Commit 77f48a8f authored by muelmarc's avatar muelmarc
Browse files

changed the function convertIntoBodyFrame(request, est, state, yaw_measured)...

changed the function convertIntoBodyFrame(request, est, state, yaw_measured) such that it uses the measured yaw without considering the setpoint - eliminates the drifting problem but is probably still wrong regarding the controlling logic. Added an rqt-plot to the launch file.
parent c23733a8
......@@ -21,6 +21,9 @@
</node>
<node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" />
<!-- When we have a GUI, this has to be adapted and included
<node pkg="d_fall_pps" name="GUI" output="screen" type="GUI">
<rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" />
......
TeamName: 'Two'
CrazyFlieName: "cfThree"
CrazyFlieAddress: "radio://0/72/2M"
CrazyFlieName: "cfFour"
CrazyFlieAddress: "radio://0/99/2M"
SafeController: ""
SafeControllerType: ""
CustomController: ""
......
......@@ -15,5 +15,5 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
#setpoint in meters (x, y, z, yaw)
setpoint: [0.2, 0.2, 0.6, 0.7]
setpoint: [0.0, 0.0, 0.3, 1]
......@@ -122,7 +122,7 @@ void viconCallback(const ViconData& data){
if(data.crazyflieName == cflie){
ROS_INFO_STREAM(data);
//forward data to safety check
bool autocontrolOn = false;//safetyCheck(data);
bool autocontrolOn = safetyCheck(data);
ppsClientToController(data, autocontrolOn);
}
else {
......
......@@ -81,6 +81,7 @@ float computeMotorPolyBackward(float thrust) {
return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
}
//Kalman
void estimateState(Controller::Request &request, float (&est)[9]) {
// attitude
......@@ -150,14 +151,16 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
ROS_INFO_STREAM("est vy: " << est[4]);
ROS_INFO_STREAM("est vz: " << est[5]);
ROS_INFO_STREAM("est y: " << est[8]);
ROS_INFO_STREAM("est r: " << est[6]);
ROS_INFO_STREAM("est p: " << est[7]);
ROS_INFO_STREAM("est y: " << est[8]);
}
//simple derivative
/*void estimateState(Controller::Request &request, float (&est)[9]) {
/*
void estimateState(Controller::Request &request, float (&est)[9]) {
est[0] = request.crazyflieLocation.x;
est[1] = request.crazyflieLocation.y;
est[2] = request.crazyflieLocation.z;
......@@ -173,11 +176,14 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
est[6] = request.crazyflieLocation.roll;
est[7] = request.crazyflieLocation.pitch;
est[8] = request.crazyflieLocation.yaw;
}*/
}
*/
void convertIntoBodyFrame(Controller::Request &request, float est[9], float (&state)[9]) {
float sinYaw = sin(request.crazyflieLocation.yaw);
float cosYaw = cos(request.crazyflieLocation.yaw);
void convertIntoBodyFrame(Controller::Request &request, float est[9], float (&state)[9], int yaw_measured) {
//float sinYaw = sin(request.crazyflieLocation.yaw);
//float cosYaw = cos(request.crazyflieLocation.yaw);
float sinYaw = sin(yaw_measured);
float cosYaw = cos(yaw_measured);
state[0] = est[0] * cosYaw + est[1] * sinYaw;
state[1] = -est[0] * sinYaw + est[1] * cosYaw;
......@@ -194,6 +200,10 @@ void convertIntoBodyFrame(Controller::Request &request, float est[9], float (&st
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
ViconData vicon = request.crazyflieLocation;
//trial>>>>>>>
int yaw_measured = request.crazyflieLocation.yaw;
//<<<<<<
//move coordinate system to make setpoint origin
request.crazyflieLocation.x -= setpoint[0];
......@@ -209,7 +219,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
estimateState(request, est);
float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
convertIntoBodyFrame(request, est, state);
convertIntoBodyFrame(request, est, state, yaw_measured);
//convertIntoBodyFrame(request, est, state, yaw);
//calculate feedback
float outRoll = 0;
......@@ -225,7 +236,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//HINWEIS: übersteuern beim outYaw wenn man 180 Grad zum yaw-Setpoint startet
//nach Multiplikation mit 0.5 gibt es den Effekt nicht mehr -> mit Paul besprechen....
outYaw *= 0.5;
//outYaw *= 0.5;
response.controlOutput.roll = outRoll;
response.controlOutput.pitch = outPitch;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment