Commit 0f49931a authored by maruggv's avatar maruggv
Browse files

VM: did some changes for readability

parent db7970ff
...@@ -320,10 +320,12 @@ std::vector<float> gainMatrixPitchAngle (9,0.0); ...@@ -320,10 +320,12 @@ std::vector<float> gainMatrixPitchAngle (9,0.0);
// Integrator parameters // Integrator parameters
std::vector<float> gainIntegrator (3,0.0); // in case we want to go back to old integrator concept, else TODO: delete
std::vector<float> gainIntegratorRate (3,0.0); std::vector<float> gainIntegratorRate (3,0.0);
std::vector<float> gainIntegratorAngle (3,0.0); std::vector<float> gainIntegratorAngle (3,0.0);
// Feedforward gain (velocity -> angle)
std::vector<float> gainFeedforwardAnglefromVelocity (3,0.0);
// The 16-bit command limits // The 16-bit command limits
float cmd_sixteenbit_min; float cmd_sixteenbit_min;
......
...@@ -82,7 +82,6 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342] ...@@ -82,7 +82,6 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
#PMKF_Kinf_for_angles : [ 0.3277,12.9648] #PMKF_Kinf_for_angles : [ 0.3277,12.9648]
# for our integrator (so far just random values) # for our integrator (so far just random values)
gainIntegrator : [ 0.50, 0.50, 0.50] # [x,y,z] (in case we want to go back to old integrator concept, else TODO: delete)
gainIntegratorRate : [-5.00, 5.00, 5.00] # [roll, pitch, z] gainIntegratorRate : [-5.00, 5.00, 5.00] # [roll, pitch, z]
gainIntegratorAngle : [-0.20, 0.20, 0.20] # [roll, pitch, z] gainIntegratorAngle : [-0.20, 0.20, 0.20] # [roll, pitch, z]
...@@ -104,7 +103,11 @@ gainMatrixRollAngle : [ 0.00,-0.31, 0.00, 0.00,-0.25, 0.00] ...@@ -104,7 +103,11 @@ gainMatrixRollAngle : [ 0.00,-0.31, 0.00, 0.00,-0.25, 0.00]
gainMatrixPitchAngle : [ 0.31, 0.00, 0.00, 0.25, 0.00, 0.00] gainMatrixPitchAngle : [ 0.31, 0.00, 0.00, 0.25, 0.00, 0.00]
#LQR: get Rates from Angle error # LQR: get Rates from Angle error
gainMatrixRollRatefromAngle : [ 4.00, 0.00, 0.00] gainMatrixRollRatefromAngle : [ 4.00, 0.00, 0.00]
gainMatrixPitchRatefromAngle : [ 0.00, 4.00, 0.00] gainMatrixPitchRatefromAngle : [ 0.00, 4.00, 0.00]
gainMatrixYawRatefromAngle : [ 0.00, 0.00, 2.30] gainMatrixYawRatefromAngle : [ 0.00, 0.00, 2.30]
# LQR: get baseline Angle from Velocity for feedforward (roll, pitch)
gainFeedforwardAnglefromVelocity: [-(20.0 * DEG2RAD), (20.0 * DEG2RAD)]
\ No newline at end of file
...@@ -394,16 +394,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -394,16 +394,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
m_time_ticks++; m_time_ticks++;
m_time_seconds = float(m_time_ticks) / m_vicon_frequency; m_time_seconds = float(m_time_ticks) / m_vicon_frequency;
ROS_INFO_STREAM("Mass of crazyflie " << m_mass_total_grams );
switch(flying_state){ switch(flying_state){
case DRONEX_STATE_APPROACH: case DRONEX_STATE_APPROACH:
{ {
//ROS_INFO("DRONEX_STATE_APPROACH"); //ROS_INFO("DRONEX_STATE_APPROACH");
dronexSetpoint.x = request.otherCrazyflies[0].x; dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y; dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.4;//0.6; dronexSetpoint.z = request.otherCrazyflies[0].z + 0.4;
ROS_INFO_STREAM("APPROACH: (x,y,z) Difference: (" ROS_INFO_STREAM("APPROACH: (x,y,z) Difference: ("
...@@ -416,7 +413,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -416,7 +413,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
approachedFlag = true; approachedFlag = true;
ROS_INFO("approached"); ROS_INFO("approached");
} }
//setpointCallback(dronexSetpoint);
} }
break; break;
...@@ -459,8 +455,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -459,8 +455,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
case DRONEX_STATE_LAND_ON_MOTHERSHIP: case DRONEX_STATE_LAND_ON_MOTHERSHIP:
{ {
//ROS_INFO("DRONEX_STATE_LAND_ON_MOTHERSHIP"); //ROS_INFO("DRONEX_STATE_LAND_ON_MOTHERSHIP");
//to land on mothership (17.10. vm) dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.x = request.otherCrazyflies[0].x; //set setpoint to droneX x y and z coordinates
dronexSetpoint.y = request.otherCrazyflies[0].y; dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.05; dronexSetpoint.z = request.otherCrazyflies[0].z + 0.05;
} }
...@@ -470,7 +465,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -470,7 +465,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
{ {
if(tookOffFlag){ if(tookOffFlag){
ROS_INFO("DRONEX_STATE_LAND_ON_GROUND"); ROS_INFO("DRONEX_STATE_LAND_ON_GROUND");
// to land on ground (17.10. vm)
dronexSetpoint.x = request.ownCrazyflie.x; dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y; dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = 0.0; dronexSetpoint.z = 0.0;
...@@ -487,12 +481,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -487,12 +481,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if(!savedStartCoordinates) if(!savedStartCoordinates)
{ {
startCoordinateX = request.ownCrazyflie.x; // Nöd sicher öbs das brucht. Idee: Afangscoordinate abspeichere zum in Hover state z ende. startCoordinateX = request.ownCrazyflie.x;
startCoordinateY = request.ownCrazyflie.y; startCoordinateY = request.ownCrazyflie.y;
startCoordinateZ = request.ownCrazyflie.z; startCoordinateZ = request.ownCrazyflie.z;
savedStartCoordinates = true; savedStartCoordinates = true;
//setpointCallback(dronexSetpoint);
ROS_INFO_STREAM("DRONEX: saved start Coordinates"); ROS_INFO_STREAM("DRONEX: saved start Coordinates");
ROS_INFO_STREAM("x = " << startCoordinateX); ROS_INFO_STREAM("x = " << startCoordinateX);
ROS_INFO_STREAM("y = " << startCoordinateY); ROS_INFO_STREAM("y = " << startCoordinateY);
...@@ -504,10 +498,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -504,10 +498,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.y = startCoordinateY; dronexSetpoint.y = startCoordinateY;
dronexSetpoint.z = startCoordinateZ + 0.4; dronexSetpoint.z = startCoordinateZ + 0.4;
ROS_INFO_STREAM("TO: (x,y,z) Difference: (" // For debugging the integrator
<< request.ownCrazyflie.x-dronexSetpoint.x << ", " // ROS_INFO_STREAM("TO: (x,y,z) Difference: ("
<< request.ownCrazyflie.y-dronexSetpoint.y << ", " // << request.ownCrazyflie.x-dronexSetpoint.x << ", "
<< request.ownCrazyflie.z-dronexSetpoint.z << ")"); // << request.ownCrazyflie.y-dronexSetpoint.y << ", "
// << request.ownCrazyflie.z-dronexSetpoint.z << ")");
if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_takeoff[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_takeoff[1] && if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_takeoff[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_takeoff[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_takeoff[2]) { abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_takeoff[2]) {
...@@ -525,10 +520,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -525,10 +520,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//ROS_INFO_STREAM("DRONEX_STATE_HOVER"); //ROS_INFO_STREAM("DRONEX_STATE_HOVER");
// keep setpoint constant // keep setpoint constant
// for testing hover over mothership
/*
dronexSetpoint.x = request.otherCrazyflies[0].x; dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y; dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z+0.3; dronexSetpoint.z = request.otherCrazyflies[0].z+0.3;
*/
} }
break; break;
...@@ -589,10 +586,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -589,10 +586,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
setpointCallback(dronexSetpoint); setpointCallback(dronexSetpoint);
/*dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.3*/
//setpointCallback(dronexSetpoint); //setpointCallback(dronexSetpoint);
// CALL THE FUNCTION FOR PER CYLCE OPERATIONS // CALL THE FUNCTION FOR PER CYLCE OPERATIONS
...@@ -704,6 +697,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp ...@@ -704,6 +697,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
integrator_XYZ(current_bodyFrameError); integrator_XYZ(current_bodyFrameError);
calculateControlOutput_viaLQRforRates(current_bodyFrameError,request,response); calculateControlOutput_viaLQRforRates(current_bodyFrameError,request,response);
}else if(controller_mode == 1){ }else if(controller_mode == 1){
...@@ -713,28 +707,19 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp ...@@ -713,28 +707,19 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// Read Mothership coordinates // Read Mothership coordinates
// x,y,z,yaw // x,y,z,yaw
float ms_coordinates[4]; float m_setpoint_for_controller_1[4];
/*ms_coordinates[0] = request.otherCrazyflies[0].x;
ms_coordinates[1] = request.otherCrazyflies[0].y; m_setpoint_for_controller_1[0] = m_setpoint_for_controller[0];
ms_coordinates[2] = request.otherCrazyflies[0].z; m_setpoint_for_controller_1[1] = m_setpoint_for_controller[1];
ms_coordinates[3] = request.otherCrazyflies[0].yaw;*/ m_setpoint_for_controller_1[2] = m_setpoint_for_controller[2];
m_setpoint_for_controller_1[3] = request.otherCrazyflies[0].yaw;
/*ms_coordinates[0] = dronexSetpoint.x;
ms_coordinates[1] = dronexSetpoint.y;
ms_coordinates[2] = dronexSetpoint.z;
ms_coordinates[3] = request.otherCrazyflies[0].yaw;*/
ms_coordinates[0] = m_setpoint_for_controller[0];
ms_coordinates[1] = m_setpoint_for_controller[1];
ms_coordinates[2] = m_setpoint_for_controller[2];
ms_coordinates[3] = request.otherCrazyflies[0].yaw;
// Load Mothership velocity // Load Mothership velocity
// x dot, y dot, z dot // x dot, y dot, z dot
float ms_velocity[3]; float m_velocity_for_controller_1[3];
ms_velocity[0] = drone_X_vel[0]; m_velocity_for_controller_1[0] = drone_X_vel[0];
ms_velocity[1] = drone_X_vel[1]; m_velocity_for_controller_1[1] = drone_X_vel[1];
ms_velocity[2] = drone_X_vel[2]; m_velocity_for_controller_1[2] = drone_X_vel[2];
...@@ -743,7 +728,13 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp ...@@ -743,7 +728,13 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// > Define a local array to fill in with the body frame error // > Define a local array to fill in with the body frame error
float stateErrorBody[12]; float stateErrorBody[12];
// > Call the function to perform the conversion // > Call the function to perform the conversion
convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, ms_coordinates, stateErrorBody); convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, m_setpoint_for_controller_1, stateErrorBody);
integrator_XYZ(stateErrorBody);
// Compute control output via Nested LQR controller:
float rollAngle_forResponse = 0; float rollAngle_forResponse = 0;
...@@ -753,56 +744,38 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp ...@@ -753,56 +744,38 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// TODO: Do not forget to implement the yawController // TODO: Do not forget to implement the yawController
float yawAngle_forResponse = 0; float yawAngle_forResponse = 0;
// integrator:
integrator_XYZ(stateErrorBody);
// integrator
// BODY FRAME Y CONTROLLER // BODY FRAME Y CONTROLLER
rollAngle_forResponse -= gainIntegratorAngle[0] * integrator_sum_XYZ[1]; rollAngle_forResponse -= gainIntegratorAngle[0] * integrator_sum_XYZ[1];
// BODY FRAME X CONTROLLER // BODY FRAME X CONTROLLER
pitchAngle_forResponse -= gainIntegratorAngle[1] * integrator_sum_XYZ[0]; pitchAngle_forResponse -= gainIntegratorAngle[1] * integrator_sum_XYZ[0];
// > ALITUDE CONTROLLER (i.e., z-controller): // ALITUDE CONTROLLER (i.e., z-controller):
thrustAdjustment -= gainIntegratorAngle[2] * integrator_sum_XYZ[2]; thrustAdjustment -= gainIntegratorAngle[2] * integrator_sum_XYZ[2];
// Perform the "-Kx" LQR computation for the rates and thrust: // Perform the "-Kx" LQR computation for the rates:
for(int i = 0; i < 6; ++i){ for(int i = 0; i < 6; ++i){
// BODY FRAME Y CONTROLLER // BODY FRAME Y CONTROLLER
rollAngle_forResponse -= 1.1*gainMatrixRollAngle[i] * stateErrorBody[i]; rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i];
// BODY FRAME X CONTROLLER
pitchAngle_forResponse -= 1.1*gainMatrixPitchAngle[i] * stateErrorBody[i];
// BODY FRAME X CONTROLLER // BODY FRAME X CONTROLLER
//thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i]; pitchAngle_forResponse -= gainMatrixPitchAngle[i] * stateErrorBody[i];
} }
// Calculate the angle in x-y plane of DroneX for transformation to local coordinates // Calculate the angle in x-y plane of DroneX for transformation to local coordinates
float droneXAngle;
float ms_velocity_norm = sqrt(pow(ms_velocity[0], 2.0) + pow(ms_velocity[1], 2.0)); //(ms_velocity[0])^2 * (ms_velocity[1])^2);
// if(ms_velocity_norm != 0){
// if (ms_velocity[1] >= 0)
// {
// droneXAngle = acos(ms_velocity[0] / ms_velocity_norm);
// }else{
// droneXAngle = -acos(ms_velocity[0] / ms_velocity_norm);
// }
// }
float sinYaw = sin(request.ownCrazyflie.yaw); float sinYaw = sin(request.ownCrazyflie.yaw);
float cosYaw = cos(request.ownCrazyflie.yaw); float cosYaw = cos(request.ownCrazyflie.yaw);
// Fill in the (x,y,z) position estimates to be returned // Fill in the (x,y,z) position estimates to be returned
float cf_vel_baseline_body_x = ms_velocity[0] * cosYaw + ms_velocity[1] * sinYaw; float cf_vel_baseline_body_x = m_velocity_for_controller_1[0] * cosYaw + m_velocity_for_controller_1[1] * sinYaw;
float cf_vel_baseline_body_y = ms_velocity[1] * cosYaw - ms_velocity[0] * sinYaw; float cf_vel_baseline_body_y = m_velocity_for_controller_1[1] * cosYaw - m_velocity_for_controller_1[0] * sinYaw;
// Calculcate Roll and Pitch Baseline, which comes from the moving mothership // Calculcate Roll and Pitch Baseline, which comes from the moving mothership
rollAngle_forResponse -= (20.0 * DEG2RAD) * cf_vel_baseline_body_y; rollAngle_forResponse += gainFeedforwardAnglefromVelocity[0] * cf_vel_baseline_body_y;
pitchAngle_forResponse += (20.0 * DEG2RAD) * cf_vel_baseline_body_x; pitchAngle_forResponse += gainFeedforwardAnglefromVelocity[1] * cf_vel_baseline_body_x;
// Calculate the Force Feedforward // Calculate the Force Feedforward
float F_in_newtons = (gravity * m_mass_total_grams)/(cos(request.ownCrazyflie.roll)*cos(request.ownCrazyflie.pitch)*1000.0); float F_in_newtons = (gravity * m_mass_total_grams)/(cos(request.ownCrazyflie.roll)*cos(request.ownCrazyflie.pitch)*1000.0);
...@@ -811,8 +784,8 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp ...@@ -811,8 +784,8 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// LQR for Rates // LQR for Rates
// Calculate the Roll and Pitch Angle error
// Calculate the Roll and Pitch Angle error
float AngleError[3] = { float AngleError[3] = {
stateErrorBody[6] - rollAngle_forResponse, stateErrorBody[6] - rollAngle_forResponse,
stateErrorBody[7] - pitchAngle_forResponse, stateErrorBody[7] - pitchAngle_forResponse,
...@@ -822,24 +795,20 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp ...@@ -822,24 +795,20 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
float rollRate_forResponse = 0; float rollRate_forResponse = 0;
float pitchRate_forResponse = 0; float pitchRate_forResponse = 0;
float yawRate_forResponse = 0; float yawRate_forResponse = 0;
// LQR for roll, pitch, yaw
for(int i = 0; i < 4; i++){ for(int i = 0; i < 4; i++){
rollRate_forResponse -= gainMatrixRollRatefromAngle[i] * AngleError[i]; rollRate_forResponse -= gainMatrixRollRatefromAngle[i] * AngleError[i];
pitchRate_forResponse -= gainMatrixPitchRatefromAngle[i] * AngleError[i]; pitchRate_forResponse -= gainMatrixPitchRatefromAngle[i] * AngleError[i];
yawRate_forResponse -= gainMatrixYawRatefromAngle[i] * AngleError[i]; yawRate_forResponse -= gainMatrixYawRatefromAngle[i] * AngleError[i];
} }
/*for(int i = 0; i < 9; ++i){ // LQR for thrust (z-controller)
thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
}*/
for(int i = 0; i < 6; ++i){ for(int i = 0; i < 6; ++i){
thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i]; thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
} }
//thrustAdjustment -= F;
// UPDATE THE "RETURN" THE VARIABLE NAMED "response" // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
// Put the computed rates and thrust into the "response" variable // Put the computed rates and thrust into the "response" variable
...@@ -847,25 +816,16 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp ...@@ -847,25 +816,16 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
response.controlOutput.roll = rollRate_forResponse; response.controlOutput.roll = rollRate_forResponse;
response.controlOutput.pitch = pitchRate_forResponse; response.controlOutput.pitch = pitchRate_forResponse;
response.controlOutput.yaw = yawRate_forResponse; response.controlOutput.yaw = yawRate_forResponse;
// > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
// > NOTE: remember that the thrust is commanded per motor, so you sohuld // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity (1/4 for each motor)
// consider whether the "thrustAdjustment" computed by your
// controller needed to be divided by 4 or not.
thrustAdjustment = thrustAdjustment / 4.0; thrustAdjustment = thrustAdjustment / 4.0;
// > Compute the feed-forward force float feed_forward_thrust_per_motor = F_in_newtons / 4.0;
float feed_forward_thrust_per_motor = F_in_newtons / 4.0; //m_mass_total_grams * 9.81/(1000*4);
// > Put in the per motor commands
// Only for motor wearing test // Only for motor wearing test
// if(tookOffFlag && thrust_factor > 0){ // if(tookOffFlag && thrust_factor > 0){
// thrust_factor -= 1e-4; // thrust_factor -= 1e-4;
// } // }
thrust_factor = 1.0;
response.controlOutput.motorCmd1 = thrust_factor * computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor); response.controlOutput.motorCmd1 = thrust_factor * computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
...@@ -902,8 +862,8 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp ...@@ -902,8 +862,8 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// An example of "printing out" the "thrust-to-command" conversion parameters. // An example of "printing out" the "thrust-to-command" conversion parameters.
ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]); ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
ROS_INFO_STREAM("motorPoly 0:" << motorPoly[1]); ROS_INFO_STREAM("motorPoly 1:" << motorPoly[1]);
ROS_INFO_STREAM("motorPoly 0:" << motorPoly[2]); ROS_INFO_STREAM("motorPoly 2:" << motorPoly[2]);
// An example of "printing out" the per motor 16-bit command computed. // An example of "printing out" the per motor 16-bit command computed.
ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1); ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
...@@ -914,77 +874,62 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp ...@@ -914,77 +874,62 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
}else if(controller_mode == 2){// Trajectory Follower }else if(controller_mode == 2){// Trajectory Follower
// to implement the trajectory tracking
//TODO
// Pragash: I've added this controller mode 2 to implement the trajectory tracking in the future
// At the moment it's the same as in controller mode 1
// LQR for Angles // LQR for Angles
// Read Mothership coordinates // Get coordinates, velocity for setpoint
// x,y,z,yaw // x,y,z,yaw
float ms_coordinates[4]; float m_setpoint_for_controller_2[4];
/*ms_coordinates[0] = request.otherCrazyflies[0].x;
ms_coordinates[1] = request.otherCrazyflies[0].y;
ms_coordinates[2] = request.otherCrazyflies[0].z;
ms_coordinates[3] = request.otherCrazyflies[0].yaw;*/
/*ms_coordinates[0] = dronexSetpoint.x;
ms_coordinates[1] = dronexSetpoint.y;
ms_coordinates[2] = dronexSetpoint.z;
ms_coordinates[3] = request.otherCrazyflies[0].yaw;*/
// Load Mothership velocity
// x dot, y dot, z dot // x dot, y dot, z dot
float ms_velocity[3]; float m_velocity_for_controller_2[3];
if(flying_state == DRONEX_FOLLOW_TRAJECTORY){ if(flying_state == DRONEX_FOLLOW_TRAJECTORY){
calculateTrajectory(); calculateTrajectory();
ms_coordinates[0] = trajectory_setpoint[0]; m_setpoint_for_controller_2[0] = trajectory_setpoint[0];
ms_coordinates[1] = trajectory_setpoint[1]; m_setpoint_for_controller_2[1] = trajectory_setpoint[1];
ms_coordinates[2] = trajectory_setpoint[2]; m_setpoint_for_controller_2[2] = trajectory_setpoint[2];
ms_coordinates[3] = trajectory_setpoint[3]; m_setpoint_for_controller_2[3] = trajectory_setpoint[3];
/* /*
ms_velocity[0] = trajectory_velocity[0]; m_velocity_for_controller_2[0] = trajectory_velocity[0];
ms_velocity[1] = trajectory_velocity[1]; m_velocity_for_controller_2[1] = trajectory_velocity[1];
ms_velocity[2] = trajectory_velocity[2]; m_velocity_for_controller_2[2] = trajectory_velocity[2];
*/ */
ms_velocity[0] = 0; m_velocity_for_controller_2[0] = 0;
ms_velocity[1] = 0; m_velocity_for_controller_2[1] = 0;
ms_velocity[2] = 0; m_velocity_for_controller_2[2] = 0;
}else{ }else{
ms_coordinates[0] = m_setpoint_for_controller[0]; m_setpoint_for_controller_2[0] = m_setpoint_for_controller[0];
ms_coordinates[1] = m_setpoint_for_controller[1]; m_setpoint_for_controller_2[1] = m_setpoint_for_controller[1];
ms_coordinates[2] = m_setpoint_for_controller[2]; m_setpoint_for_controller_2[2] = m_setpoint_for_controller[2];
ms_coordinates[3] = request.otherCrazyflies[0].yaw; m_setpoint_for_controller_2[3] = request.otherCrazyflies[0].yaw;
ms_velocity[0] = drone_X_vel[0]; m_velocity_for_controller_2[0] = drone_X_vel[0];
ms_velocity[1] = drone_X_vel[1]; m_velocity_for_controller_2[1] = drone_X_vel[1];
ms_velocity[2] = drone_X_vel[2]; m_velocity_for_controller_2[2] = drone_X_vel[2];
} }
// CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO // CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO
// THE BODY FRAME ERROR REQUIRED BY THE CONTROLLER // THE BODY FRAME ERROR REQUIRED BY THE CONTROLLER
// > Define a local array to fill in with the body frame error // > Define a local array to fill in with the body frame error
float stateErrorBody[12]; float stateErrorBody[12];
// > Call the function to perform the conversion // > Call the function to perform the conversion
convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, ms_coordinates, stateErrorBody); convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, m_setpoint_for_controller_2, stateErrorBody);
integrator_XYZ(stateErrorBody);
// Compute control output via Nested LQR controller: