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);
// 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> gainIntegratorAngle (3,0.0);
// Feedforward gain (velocity -> angle)
std::vector<float> gainFeedforwardAnglefromVelocity (3,0.0);
// The 16-bit command limits
float cmd_sixteenbit_min;
......
......@@ -82,7 +82,6 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
#PMKF_Kinf_for_angles : [ 0.3277,12.9648]
# 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]
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]
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]
gainMatrixPitchRatefromAngle : [ 0.00, 4.00, 0.00]
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 &
m_time_ticks++;
m_time_seconds = float(m_time_ticks) / m_vicon_frequency;
ROS_INFO_STREAM("Mass of crazyflie " << m_mass_total_grams );
switch(flying_state){
case DRONEX_STATE_APPROACH:
{
//ROS_INFO("DRONEX_STATE_APPROACH");
dronexSetpoint.x = request.otherCrazyflies[0].x;
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: ("
......@@ -416,7 +413,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
approachedFlag = true;
ROS_INFO("approached");
}
//setpointCallback(dronexSetpoint);
}
break;
......@@ -459,8 +455,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
case 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; //set setpoint to droneX x y and z coordinates
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.05;
}
......@@ -470,7 +465,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
{
if(tookOffFlag){
ROS_INFO("DRONEX_STATE_LAND_ON_GROUND");
// to land on ground (17.10. vm)
dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = 0.0;
......@@ -487,12 +481,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
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;
startCoordinateZ = request.ownCrazyflie.z;
savedStartCoordinates = true;
//setpointCallback(dronexSetpoint);
ROS_INFO_STREAM("DRONEX: saved start Coordinates");
ROS_INFO_STREAM("x = " << startCoordinateX);
ROS_INFO_STREAM("y = " << startCoordinateY);
......@@ -504,10 +498,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.y = startCoordinateY;
dronexSetpoint.z = startCoordinateZ + 0.4;
ROS_INFO_STREAM("TO: (x,y,z) Difference: ("
<< request.ownCrazyflie.x-dronexSetpoint.x << ", "
<< request.ownCrazyflie.y-dronexSetpoint.y << ", "
<< request.ownCrazyflie.z-dronexSetpoint.z << ")");
// For debugging the integrator
// ROS_INFO_STREAM("TO: (x,y,z) Difference: ("
// << request.ownCrazyflie.x-dronexSetpoint.x << ", "
// << 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] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_takeoff[2]) {
......@@ -525,10 +520,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//ROS_INFO_STREAM("DRONEX_STATE_HOVER");
// keep setpoint constant
// for testing hover over mothership
/*
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z+0.3;
*/
}
break;
......@@ -589,10 +586,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
setpointCallback(dronexSetpoint);
/*dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.3*/
//setpointCallback(dronexSetpoint);
// CALL THE FUNCTION FOR PER CYLCE OPERATIONS
......@@ -704,6 +697,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
integrator_XYZ(current_bodyFrameError);
calculateControlOutput_viaLQRforRates(current_bodyFrameError,request,response);
}else if(controller_mode == 1){
......@@ -713,28 +707,19 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// Read Mothership coordinates
// x,y,z,yaw
float ms_coordinates[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;*/
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;
float m_setpoint_for_controller_1[4];
m_setpoint_for_controller_1[0] = m_setpoint_for_controller[0];
m_setpoint_for_controller_1[1] = m_setpoint_for_controller[1];
m_setpoint_for_controller_1[2] = m_setpoint_for_controller[2];
m_setpoint_for_controller_1[3] = request.otherCrazyflies[0].yaw;
// Load Mothership velocity
// x dot, y dot, z dot
float ms_velocity[3];
ms_velocity[0] = drone_X_vel[0];
ms_velocity[1] = drone_X_vel[1];
ms_velocity[2] = drone_X_vel[2];
float m_velocity_for_controller_1[3];
m_velocity_for_controller_1[0] = drone_X_vel[0];
m_velocity_for_controller_1[1] = drone_X_vel[1];
m_velocity_for_controller_1[2] = drone_X_vel[2];
......@@ -743,7 +728,13 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// > Define a local array to fill in with the body frame error
float stateErrorBody[12];
// > 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;
......@@ -753,56 +744,38 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// TODO: Do not forget to implement the yawController
float yawAngle_forResponse = 0;
integrator_XYZ(stateErrorBody);
// integrator
// integrator:
// BODY FRAME Y CONTROLLER
rollAngle_forResponse -= gainIntegratorAngle[0] * integrator_sum_XYZ[1];
// BODY FRAME X CONTROLLER
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];
// 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){
// BODY FRAME Y CONTROLLER
rollAngle_forResponse -= 1.1*gainMatrixRollAngle[i] * stateErrorBody[i];
// BODY FRAME X CONTROLLER
pitchAngle_forResponse -= 1.1*gainMatrixPitchAngle[i] * stateErrorBody[i];
rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i];
// 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
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 cosYaw = cos(request.ownCrazyflie.yaw);
// 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_y = ms_velocity[1] * cosYaw - ms_velocity[0] * 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 = 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
rollAngle_forResponse -= (20.0 * DEG2RAD) * cf_vel_baseline_body_y;
pitchAngle_forResponse += (20.0 * DEG2RAD) * cf_vel_baseline_body_x;
rollAngle_forResponse += gainFeedforwardAnglefromVelocity[0] * cf_vel_baseline_body_y;
pitchAngle_forResponse += gainFeedforwardAnglefromVelocity[1] * cf_vel_baseline_body_x;
// Calculate the Force Feedforward
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
// LQR for Rates
// Calculate the Roll and Pitch Angle error
// Calculate the Roll and Pitch Angle error
float AngleError[3] = {
stateErrorBody[6] - rollAngle_forResponse,
stateErrorBody[7] - pitchAngle_forResponse,
......@@ -822,24 +795,20 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
float rollRate_forResponse = 0;
float pitchRate_forResponse = 0;
float yawRate_forResponse = 0;
// LQR for roll, pitch, yaw
for(int i = 0; i < 4; i++){
rollRate_forResponse -= gainMatrixRollRatefromAngle[i] * AngleError[i];
pitchRate_forResponse -= gainMatrixPitchRatefromAngle[i] * AngleError[i];
yawRate_forResponse -= gainMatrixYawRatefromAngle[i] * AngleError[i];
}
/*for(int i = 0; i < 9; ++i){
thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
}*/
// LQR for thrust (z-controller)
for(int i = 0; i < 6; ++i){
thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
}
//thrustAdjustment -= F;
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
// Put the computed rates and thrust into the "response" variable
......@@ -847,25 +816,16 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
response.controlOutput.roll = rollRate_forResponse;
response.controlOutput.pitch = pitchRate_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
// consider whether the "thrustAdjustment" computed by your
// controller needed to be divided by 4 or not.
// > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity (1/4 for each motor)
thrustAdjustment = thrustAdjustment / 4.0;
// > Compute the feed-forward force
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
float feed_forward_thrust_per_motor = F_in_newtons / 4.0;
// Only for motor wearing test
// if(tookOffFlag && thrust_factor > 0){
// thrust_factor -= 1e-4;
// }
thrust_factor = 1.0;
response.controlOutput.motorCmd1 = thrust_factor * computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
......@@ -902,8 +862,8 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// An example of "printing out" the "thrust-to-command" conversion parameters.
ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
ROS_INFO_STREAM("motorPoly 0:" << motorPoly[1]);
ROS_INFO_STREAM("motorPoly 0:" << motorPoly[2]);
ROS_INFO_STREAM("motorPoly 1:" << motorPoly[1]);
ROS_INFO_STREAM("motorPoly 2:" << motorPoly[2]);
// An example of "printing out" the per motor 16-bit command computed.
ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
......@@ -914,77 +874,62 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
}else if(controller_mode == 2){// Trajectory Follower
//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
// to implement the trajectory tracking
// LQR for Angles
// Read Mothership coordinates
// Get coordinates, velocity for setpoint
// x,y,z,yaw
float ms_coordinates[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
float m_setpoint_for_controller_2[4];
// x dot, y dot, z dot
float ms_velocity[3];
float m_velocity_for_controller_2[3];
if(flying_state == DRONEX_FOLLOW_TRAJECTORY){
calculateTrajectory();
ms_coordinates[0] = trajectory_setpoint[0];
ms_coordinates[1] = trajectory_setpoint[1];
ms_coordinates[2] = trajectory_setpoint[2];
ms_coordinates[3] = trajectory_setpoint[3];
m_setpoint_for_controller_2[0] = trajectory_setpoint[0];
m_setpoint_for_controller_2[1] = trajectory_setpoint[1];
m_setpoint_for_controller_2[2] = trajectory_setpoint[2];
m_setpoint_for_controller_2[3] = trajectory_setpoint[3];
/*
ms_velocity[0] = trajectory_velocity[0];
ms_velocity[1] = trajectory_velocity[1];
ms_velocity[2] = trajectory_velocity[2];
m_velocity_for_controller_2[0] = trajectory_velocity[0];
m_velocity_for_controller_2[1] = trajectory_velocity[1];
m_velocity_for_controller_2[2] = trajectory_velocity[2];
*/
ms_velocity[0] = 0;
ms_velocity[1] = 0;
ms_velocity[2] = 0;
m_velocity_for_controller_2[0] = 0;
m_velocity_for_controller_2[1] = 0;
m_velocity_for_controller_2[2] = 0;
}else{
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;
m_setpoint_for_controller_2[0] = m_setpoint_for_controller[0];
m_setpoint_for_controller_2[1] = m_setpoint_for_controller[1];
m_setpoint_for_controller_2[2] = m_setpoint_for_controller[2];
m_setpoint_for_controller_2[3] = request.otherCrazyflies[0].yaw;
ms_velocity[0] = drone_X_vel[0];
ms_velocity[1] = drone_X_vel[1];
ms_velocity[2] = drone_X_vel[2];
m_velocity_for_controller_2[0] = drone_X_vel[0];
m_velocity_for_controller_2[1] = drone_X_vel[1];
m_velocity_for_controller_2[2] = drone_X_vel[2];
}
// CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO
// THE BODY FRAME ERROR REQUIRED BY THE CONTROLLER
// > Define a local array to fill in with the body frame error
float stateErrorBody[12];
// > 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:
float rollAngle_forResponse = 0;
......@@ -994,61 +939,48 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// TODO: Do not forget to implement the yawController
float yawAngle_forResponse = 0;
integrator_XYZ(stateErrorBody);
// integrator
// integrator:
// BODY FRAME Y CONTROLLER
rollAngle_forResponse -= gainIntegratorAngle[0] * integrator_sum_XYZ[1];
// BODY FRAME X CONTROLLER
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];
// 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){
// 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
//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
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);
}
}
// Calculcate Roll and Pitch Baseline, which comes from the moving mothership
float rollAngle_baseline = 1;
float pitchAngle_baseline = 1;
// TODO
rollAngle_forResponse += rollAngle_baseline * (cos(droneXAngle) * ms_velocity[0]- sin(droneXAngle) * ms_velocity[1]);
pitchAngle_forResponse += pitchAngle_forResponse * (sin(droneXAngle) * ms_velocity[0] + cos(droneXAngle) * ms_velocity[1]);
// Calculate the angle in x-y plane of DroneX for transformation to local coordinates
float sinYaw = sin(request.ownCrazyflie.yaw);
float cosYaw = cos(request.ownCrazyflie.yaw);
// Fill in the (x,y,z) position estimates to be returned
float cf_vel_baseline_body_x = m_velocity_for_controller_2[0] * cosYaw + m_velocity_for_controller_2[1] * sinYaw;
float cf_vel_baseline_body_y = m_velocity_for_controller_2[1] * cosYaw - m_velocity_for_controller_2[0] * sinYaw;
// Calculcate Roll and Pitch Baseline, which comes from the moving mothership
rollAngle_forResponse += gainFeedforwardAnglefromVelocity[0] * cf_vel_baseline_body_y;
pitchAngle_forResponse += gainFeedforwardAnglefromVelocity[1] * cf_vel_baseline_body_x;
// Calculate the Force Feedforward
float F_in_newtons = (gravity * m_mass_total_grams)/(cos(rollAngle_forResponse)*cos(pitchAngle_forResponse)*1000.0);
float F_in_newtons = (gravity * m_mass_total_grams)/(cos(request.ownCrazyflie.roll)*cos(request.ownCrazyflie.pitch)*1000.0);
// LQR for Rates
// Calculate the Roll and Pitch Angle error
// Calculate the Roll and Pitch Angle error
float AngleError[3] = {
stateErrorBody[6] - rollAngle_forResponse,
stateErrorBody[7] - pitchAngle_forResponse,
......@@ -1058,24 +990,20 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
float rollRate_forResponse = 0;
float pitchRate_forResponse = 0;
float yawRate_forResponse = 0;
// LQR for roll, pitch, yaw
for(int i = 0; i < 4; i++){
rollRate_forResponse -= gainMatrixRollRatefromAngle[i] * AngleError[i];
pitchRate_forResponse -= gainMatrixPitchRatefromAngle[i] * AngleError[i];
yawRate_forResponse -= gainMatrixYawRatefromAngle[i] * AngleError[i];
}
/*for(int i = 0; i < 9; ++i){
thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
}*/
// LQR for thrust (z-controller)
for(int i = 0; i < 6; ++i){
thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
}
//thrustAdjustment -= F;
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
// Put the computed rates and thrust into the "response" variable
......@@ -1083,18 +1011,22 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
response.controlOutput.roll = rollRate_forResponse;
response.controlOutput.pitch = pitchRate_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
// consider whether the "thrustAdjustment" computed by your
// controller needed to be divided by 4 or not.
// > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity (1/4 for each motor)
thrustAdjustment = thrustAdjustment / 4.0;
// > Compute the feed-forward force
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
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
float feed_forward_thrust_per_motor = F_in_newtons / 4.0;
// Only for motor wearing test
// if(tookOffFlag && thrust_factor > 0){
// thrust_factor -= 1e-4;
// }
response.controlOutput.motorCmd1 = thrust_factor * computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd2 = thrust_factor * computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd3 = thrust_factor * computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd4 = thrust_factor * computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
// Specify that this controller is a rate controller
......@@ -1103,18 +1035,37 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
// to see setpoint in lqr plots
Setpoint msg_setpoint;
msg_setpoint.x = ms_coordinates[0];
msg_setpoint.y = ms_coordinates[1];
msg_setpoint.z = ms_coordinates[2];
msg_setpoint.yaw = ms_coordinates[3];
//dronexSetpointToGUIPublisher.publish(msg_setpoint);
// An alternate debugging technique is to print out data directly to the