Commit 12c2fc73 authored by maruggv's avatar maruggv
Browse files

Major changes how controllers are called, setpoint handling

parent 142c6f9c
......@@ -197,6 +197,7 @@ float m_time_seconds;
float m_mass_CF_grams;
Setpoint dronexSetpoint;
Setpoint dronexVelocity;
int flying_state = DRONEX_STATE_GROUND;
int integratorFlag = DRONEX_INTEGRATOR_OFF;
......@@ -306,12 +307,17 @@ float gravity = 9.81;
float m_setpoint[4] = {0.0,0.0,0.0,0.0};
float m_setpoint_for_controller[4] = {0.0,0.0,0.0,0.0};
// > The velocities for (x,y,z) position, in that order
float m_velocity[3] = {0.0,0.0,0.0,0.0};
float m_velocity_for_controller[3] = {0.0,0.0,0.0,0.0};
// > Small adjustments to the x-y setpoint
float m_xAdjustment = 0.0f;
float m_yAdjustment = 0.0f;
// Boolean for whether to limit rate of change of the setpoint
bool m_shouldSmoothSetpointChanges = true;
bool m_shouldClipVelocity = true;
// Max setpoint change per second
float m_max_setpoint_change_per_second_horizontal;
......@@ -541,17 +547,10 @@ float calculate_distance_in_xyz(std::vector<float> p1, std::vector<float> p2);
// > The function that is called to "start" all estimation and control computations
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response);
// > The various functions that implement an LQR controller
void calculateControlOutput_viaLQR( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforMotors( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforActuators( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforRates( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforAngles( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforRates_forLanding( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
// > The various functions that implement an LQR controller
void calculateControlOutput_viaLQRforRates( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
void calculateControlOutput_viaLQRforRates_Nested(float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
// Velocity estimator
void calculateMSVelocity(Controller::Request &request);
......
......@@ -181,6 +181,26 @@ void perControlCycleOperations()
m_setpoint_for_controller[2] = m_setpoint[2];
m_setpoint_for_controller[3] = m_setpoint[3];
}
if (m_shouldClipVelocity)
{
// clip if velocity is too big
m_velocity_for_controller[0] = m_velocity[0];
m_velocity_for_controller[1] = m_velocity[1];
m_velocity_for_controller[2] = m_velocity[2];
float tot_velocity_xy = sqrt(abs(pow(m_velocity[0],2))+abs(pow(m_velocity[1],2)));
if(tot_velocity_xy > 1.0){
m_velocity_for_controller[0] = 1.0 * m_velocity[0]/tot_velocity_xy;
m_velocity_for_controller[1] = 1.0 * m_velocity[1]/tot_velocity_xy;
}
if(m_velocity[3]<-0.8){
m_velocity_for_controller[3] = -0.8;
}else if(m_velocity[3]>0.8){
m_velocity_for_controller[3] = 0.8;
}
}
}
......@@ -596,13 +616,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
calculateMSVelocity(request);
setpointCallback(dronexSetpoint);
//setpointCallback(dronexSetpoint);
// CALL THE FUNCTION FOR PER CYLCE OPERATIONS
perControlCycleOperations();
// THIS IS THE START OF THE "OUTER" CONTROL LOOP
// > i.e., this is the control loop run on this laptop
// > this function is called at the frequency specified
......@@ -616,6 +629,71 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
performEstimatorUpdate_forStateInterial(request);
if(controller_mode == 0){
// do not change
}else if(controller_mode == 1){// controller that uses the mothership velocity
//dronexSetpoint.x = dronexSetpoint.x;
//dronexSetpoint.y = dronexSetpoint.y;
//dronexSetpoint.z = dronexSetpoint.z;
dronexSetpoint.yaw = request.otherCrazyflies[0].yaw; // same yaw as mothership
// Load Mothership velocity
// x dot, y dot, z dot
dronexVelocity.x = mothership_vel[0];
dronexVelocity.y = mothership_vel[1];
dronexVelocity.z = mothership_vel[2];
}else if(controller_mode == 2){// Trajectory Follower
// to implement the trajectory tracking
if(flying_state == DRONEX_STATE_FOLLOWING_TRAJECTORY){
calculateTrajectory(request);
dronexSetpoint.x = trajectory_setpoint[0];
dronexSetpoint.y = trajectory_setpoint[1];
dronexSetpoint.z = trajectory_setpoint[2];
//m_setpoint_for_controller_2[3] = trajectory_setpoint[3];
dronexSetpoint.yaw = request.otherCrazyflies[0].yaw; // same yaw as mothership
dronexVelocity.x = trajectory_velocity[0];
dronexVelocity.y = trajectory_velocity[1];
dronexVelocity.z = trajectory_velocity[2];
/*
m_setpoint_for_controller_2[0] = request.owncraazyflie.x;
m_setpoint_for_controller_2[1] = request.owncraazyflie.y;
m_setpoint_for_controller_2[2] = request.owncraazyflie.z;
m_velocity_for_controller_2[0] = 1;
m_velocity_for_controller_2[1] = 0;
m_velocity_for_controller_2[2] = 0;
*/
}else{
//dronexSetpoint.x = dronexSetpoint.x;
//dronexSetpoint.y = dronexSetpoint.y;
//dronexSetpoint.z = dronexSetpoint.z;
dronexSetpoint.yaw = request.otherCrazyflies[0].yaw; // same yaw as mothership
dronexVelocity.x = mothership_vel[0];
dronexVelocity.y = mothership_vel[1];
dronexVelocity.z = mothership_vel[2];
}
}
setpointCallback(dronexSetpoint, dronexVelocity);
// CALL THE FUNCTION FOR PER CYLCE OPERATIONS
perControlCycleOperations();
float stateErrorBody[12];
// > Call the function to perform the conversion
convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, m_setpoint_for_controller, stateErrorBody);
......@@ -640,7 +718,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
motorsOFF(response);
}
else{
calculateControlOutputDroneX(request, response);
calculateControlOutputDroneX(request, response, stateErrorBody);
// for debugging:
/*ROS_INFO_STREAM("deltaX to Mothership: " << request.ownCrazyflie.x - request.otherCrazyflies[0].x);
......@@ -691,395 +769,32 @@ void crazyfliecontextRefresh(d_fall_pps::CrazyflieContext context){
// DroneX Controller
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response){
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response, float stateErrorBody[12]){
if(controller_mode == 0){
// 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 current_bodyFrameError[12];
// > Call the function to perform the conversion
convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, m_setpoint_for_controller, current_bodyFrameError);
integrator_XYZ(current_bodyFrameError);
calculateControlOutput_viaLQRforRates(current_bodyFrameError,request,response);
integrator_XYZ(stateErrorBody);
calculateControlOutput_viaLQRforRates(stateErrorBody,request,response);
}else if(controller_mode == 1){
// LQR for Angles
// Read Mothership coordinates
// x,y,z,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 m_velocity_for_controller_1[3];
m_velocity_for_controller_1[0] = mothership_vel[0];
m_velocity_for_controller_1[1] = mothership_vel[1];
m_velocity_for_controller_1[2] = mothership_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, m_setpoint_for_controller_1, stateErrorBody);
//integrator
integrator_XYZ(stateErrorBody);
// Compute control output via Nested LQR controller:
float rollAngle_forResponse = 0;
float pitchAngle_forResponse = 0;
float thrustAdjustment = 0;
// TODO: Do not forget to implement the yawController
float yawAngle_forResponse = 0;
// 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):
thrustAdjustment -= gainIntegratorAngle[2] * integrator_sum_XYZ[2];
// Perform the "-Kx" LQR computation for the rates:
for(int i = 0; i < 6; ++i){
// BODY FRAME Y CONTROLLER
rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i];
// BODY FRAME X CONTROLLER
pitchAngle_forResponse -= gainMatrixPitchAngle[i] * stateErrorBody[i];
}
// 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_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 += 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);
// LQR for Rates
// Calculate the Roll and Pitch Angle error
float AngleError[3] = {
stateErrorBody[6] - rollAngle_forResponse,
stateErrorBody[7] - pitchAngle_forResponse,
stateErrorBody[8]
};
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];
}
// LQR for thrust (z-controller)
for(int i = 0; i < 6; ++i){
thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
}
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
// Put the computed rates and thrust into the "response" variable
// > For roll, pitch, and yaw:
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 (1/4 for each motor)
thrustAdjustment = thrustAdjustment / 4.0;
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
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
// An alternate debugging technique is to print out data directly to the
// command line from which this node was launched.
/*if (shouldDisplayDebugInfo)
{
// An example of "printing out" the data from the "request" argument to the
// command line. This might be useful for debugging.
ROS_INFO_STREAM("x-coordinate [m]: " << request.ownCrazyflie.x);
ROS_INFO_STREAM("y-coordinate [m]: " << request.ownCrazyflie.y);
ROS_INFO_STREAM("z-coordinate [m]: " << request.ownCrazyflie.z);
ROS_INFO_STREAM("roll [deg]: " << request.ownCrazyflie.roll);
ROS_INFO_STREAM("pitch [deg]: " << request.ownCrazyflie.pitch);
ROS_INFO_STREAM("yaw [deg]: " << request.ownCrazyflie.yaw);
ROS_INFO_STREAM("Delta t [s]: " << request.ownCrazyflie.acquiringTime);
// An example of "printing out" the control actions computed.
ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
// An example of "printing out" the "thrust-to-command" conversion parameters.
ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
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);
ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
}*/
calculateControlOutput_viaLQRforRates_Nested(stateErrorBody, request, response);
}else if(controller_mode == 2){// Trajectory Follower
// to implement the trajectory tracking
// LQR for Angles
// Get coordinates, velocity for setpoint
// x,y,z,yaw
float m_setpoint_for_controller_2[4];
// x dot, y dot, z dot
float m_velocity_for_controller_2[3];
if(flying_state == DRONEX_STATE_FOLLOWING_TRAJECTORY){
calculateTrajectory(request);
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];
m_setpoint_for_controller_2[3] = request.otherCrazyflies[0].yaw;
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];
/*
m_velocity_for_controller_2[0] = 0;
m_velocity_for_controller_2[1] = 0;
m_velocity_for_controller_2[2] = 0;
*/
}else{
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;
m_velocity_for_controller_2[0] = mothership_vel[0];
m_velocity_for_controller_2[1] = mothership_vel[1];
m_velocity_for_controller_2[2] = mothership_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, m_setpoint_for_controller_2, stateErrorBody);
// integrator
integrator_XYZ(stateErrorBody);
// Compute control output via Nested LQR controller:
float rollAngle_forResponse = 0;
float pitchAngle_forResponse = 0;
float thrustAdjustment = 0;
// TODO: Do not forget to implement the yawController
float yawAngle_forResponse = 0;
// 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):
thrustAdjustment -= gainIntegratorAngle[2] * integrator_sum_XYZ[2];
// Perform the "-Kx" LQR computation for the rates:
for(int i = 0; i < 6; ++i){
// BODY FRAME Y CONTROLLER
rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i];
// BODY FRAME X CONTROLLER
pitchAngle_forResponse -= gainMatrixPitchAngle[i] * stateErrorBody[i];
}
// 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(request.ownCrazyflie.roll)*cos(request.ownCrazyflie.pitch)*1000.0);
// LQR for Rates
// Calculate the Roll and Pitch Angle error
float AngleError[3] = {
stateErrorBody[6] - rollAngle_forResponse,
stateErrorBody[7] - pitchAngle_forResponse,
stateErrorBody[8]
};
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];
}
// LQR for thrust (z-controller)
for(int i = 0; i < 6; ++i){
thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
}
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
// Put the computed rates and thrust into the "response" variable
// > For roll, pitch, and yaw:
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 (1/4 for each motor)
thrustAdjustment = thrustAdjustment / 4.0;
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
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
// An alternate debugging technique is to print out data directly to the
// command line from which this node was launched.
/*if (shouldDisplayDebugInfo)
{
// An example of "printing out" the data from the "request" argument to the
// command line. This might be useful for debugging.
ROS_INFO_STREAM("x-coordinate [m]: " << request.ownCrazyflie.x);
ROS_INFO_STREAM("y-coordinate [m]: " << request.ownCrazyflie.y);
ROS_INFO_STREAM("z-coordinate [m]: " << request.ownCrazyflie.z);
ROS_INFO_STREAM("roll [deg]: " << request.ownCrazyflie.roll);
ROS_INFO_STREAM("pitch [deg]: " << request.ownCrazyflie.pitch);
ROS_INFO_STREAM("yaw [deg]: " << request.ownCrazyflie.yaw);
ROS_INFO_STREAM("Delta t [s]: " << request.ownCrazyflie.acquiringTime);
// An example of "printing out" the control actions computed.
ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
// An example of "printing out" the "thrust-to-command" conversion parameters.
ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
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);
ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
}*/
calculateControlOutput_viaLQRforRates_Nested(stateErrorBody, request, response);
}else{ // Don't know what to do ^^
......@@ -1124,18 +839,6 @@ void calculateTrajectory(Controller::Request &request){
trajectory_velocity[0] = (trajectory_to_ms(request, total_time_since_start + deltaT)[0]-request.ownCrazyflie.x)/deltaT;
trajectory_velocity[1] = (trajectory_to_ms(request, total_time_since_start + deltaT)[1]-request.ownCrazyflie.y)/deltaT;
trajectory_velocity[2] = 0;
// clip if velocity is too big
float tot_velocity_xy = sqrt(abs(pow(trajectory_velocity[0],2))+abs(pow(trajectory_velocity[1],2)));
if(tot_velocity_xy > 1.0){
trajectory_velocity[0] = 1.0 * trajectory_velocity[0]/tot_velocity_xy;
trajectory_velocity[1] = 1.0 * trajectory_velocity[1]/tot_velocity_xy;
}
if(trajectory_velocity[3]<-0.8){
trajectory_velocity[3] = -0.8;
}else if(trajectory_velocity[3]>0.8){
trajectory_velocity[3] = 0.8;
}
}
......@@ -1453,11 +1156,141 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
//