Commit 78ef3c78 authored by mastefan's avatar mastefan
Browse files

Implemented the Controller for Landing with nested LQR

parent 55f12735
......@@ -199,7 +199,8 @@ int integratorFlag = DRONEX_INTEGRATOR_OFF;
float integrator_var[3] = {0,0,0};
// Controller Mode
int controller_mode = 1;
// Variable for choosing flight sequence
int flightSequence = 0;
......@@ -273,6 +274,15 @@ std::vector<float> gainMatrixRollRate (9,0.0);
std::vector<float> gainMatrixPitchRate (9,0.0);
std::vector<float> gainMatrixYawRate (9,0.0);
std::vector<float> gainMatrixYawRatefromAngle (9,0.0);
std::vector<float> gainMatrixPitchRatefromAngle (9,0.0);
std::vector<float> gainMatrixRollRatefromAngle (9,0.0);
std::vector<float> gainMatrixThrust_SixStateVector (9,0.0);
std::vector<float> gainMatrixRollAngle (9,0.0);
std::vector<float> gainMatrixPitchAngle (9,0.0);
// Integrator parameters
std::vector<float> gainIntegrator (3,0.0);
......@@ -442,7 +452,7 @@ void motorsOFF(Controller::Response &response);
// CONTROLLER COMPUTATIONS
// > 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);
......@@ -453,6 +463,8 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
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);
// Velocity estimator
void calculateDroneXVelocity(Controller::Request &request);
......
......@@ -12,8 +12,9 @@ vicon_frequency : 200
# controller_mode : 0: Basic Controller
# controller_mode : 1: Angle Controller
controller_mode : 1
......@@ -85,3 +86,12 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
# for our integrator (so far just random values)
gainIntegrator : [1, 1, 1]
gainMatrixThrust_SixStateVector: [ 0.00, 0.00, 0.82, 0.00, 0.00, 0.22]
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]
gainMatrixRollRatefromAngle : [ 4.00, 0.00, 0.00]
gainMatrixPitchRatefromAngle : [ 0.00, 4.00, 0.00]
gainMatrixYawRatefromAngle : [ 0.00, 0.00, 2.30]
\ No newline at end of file
......@@ -358,6 +358,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
m_time_seconds = float(m_time_ticks) / m_vicon_frequency;
switch(flying_state){
case DRONEX_STATE_APPROACH:
{
......@@ -390,10 +392,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = request.ownCrazyflie.z;
/* std_msgs::Int32 flying_state_msg;
flying_state_msg.data = STATE_MOTORS_OFF;
flyingStatePublisher.publish(flying_state_msg);
return true;*/
}
break;
......@@ -412,10 +410,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = request.ownCrazyflie.z;
/* std_msgs::Int32 flying_state_msg;
flying_state_msg.data = STATE_MOTORS_OFF;
flyingStatePublisher.publish(flying_state_msg);
return true;*/
}
break;
......@@ -430,46 +424,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
case DRONEX_STATE_LAND_ON_GROUND:
{
//ROS_INFO_STREAM("DRONEX_STATE_LAND_ON_GROUND: Crazyflie z coordinate " << request.ownCrazyflie.z);
//ROS_INFO_STREAM("DRONEX_STATE_LAND_ON_GROUND: Mothership z coordinate " << request.otherCrazyflies[0].z);
// to land on mothership: -> commented out (17.10. vm)
//dronexSetpoint.x = request.otherCrazyflies[0].x; //set setpoint to droneX x y and z coordinates
//dronexSetpoint.y = request.otherCrazyflies[0].y;
//dronexSetpoint.z = request.otherCrazyflies[0].z+0.05;
// to land on ground (17.10. vm)
dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = 0;
/*if(request.ownCrazyflie.z < request.otherCrazyflies[0].z+0.25){ // >0.1
std_msgs::Int32 flying_state_msg;
flying_state_msg.data = STATE_MOTORS_OFF;
//flyingStatePublisher.publish(flying_state_msg);
ROS_INFO_STREAM("DRONEX_MOTORS_OFF..." << request.ownCrazyflie.z - request.otherCrazyflies[0].z);
return true;
}*/
/*
}else{
//if(dronexSetpoint.z>=request.otherCrazyflies[0].z){
float down_per_cycle = 0.003;
if(dronexSetpoint.z < request.otherCrazyflies[0].z+0.5){
down_per_cycle = 0.0005;
}
dronexSetpoint.z -= down_per_cycle;
ROS_INFO_STREAM("DRONEX_L: z" << dronexSetpoint.z);
//}else{
//}
}*/
}
break;
......@@ -586,12 +546,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
performEstimatorUpdate_forStateInterial(request);
// 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);
......@@ -615,7 +570,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
motorsOFF(response);
}
else{
calculateControlOutput_viaLQRforRates(current_bodyFrameError,request,response);
calculateControlOutputDroneX(request, response);
}
......@@ -634,6 +589,144 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// RETURN "true" TO INDICATE THAT THE COMPUTATIONS WERE SUCCESSFUL
return true;
}
// DroneX Controller
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response){
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);
calculateControlOutput_viaLQRforRates(current_bodyFrameError,request,response);
}else{
// LQR for Angles
// Read Mothership coordinates
// x,y,z,yaw
float ms_coordinates[4];
ms_coordintes[0] = request.otherCrazyflies[0].x;
ms_coordintes[1] = request.otherCrazyflies[0].y;
ms_coordintes[2] = request.otherCrazyflies[0].z;
ms_coordintes[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];
// 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_coordintes, stateErrorBody);
float rollAngle_forResponse = 0;
float pitchAngle_forResponse = 0;
float thrustAdjustment = 0;
// TODO: Do not forget to implement the yawController
float yawAngle_forResponse = 0;
// Perform the "-Kx" LQR computation for the rates and thrust:
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];
// BODY FRAME X CONTROLLER
thrustAdjustment -= gainMatrixThrustAdjustment[i] * stateErrorBody[i];
}
// Calculcate Roll and Pitch Baseline, which comes from the moving mothership
float rollAngle_baseline = 0;
float pitchAngle_baseline = 0;
// TODO
rollAngle_forResponse += rollAngle_baseline;
pitchAngle_forResponse += pitchAngle_forResponse;
// Calculate the Force Feedforward
float F = (gravity * mass_CF)/(cos(rollAngle_forResponse)*cos(pitchAngle_forResponse));
// LQR for Rates
// Calculate the Roll and Pitch Angle error
float AngleError[3] = {
stateErrorBody[6] - rollAngle_forResponse,
stateErrorBody[7] - pitchAngle_forResponse,
yawAngle_forResponse
};
float rollRate_forResponse = 0;
float pitchRate_forResponse = 0;
float yawRate_forResponse = 0;
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];
}
}
}
// Set motors Output to 0
void motorsOFF(Controller::Response &response){
......@@ -672,6 +765,8 @@ void calculateDroneXVelocity(Controller::Request &request){
// ------------------------------------------------------------------------------
// EEEEE SSSS TTTTT III M M A TTTTT III OOO N N
// E S T I MM MM A A T I O O NN N
......@@ -813,6 +908,38 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
// ----------------------------------------------------------------------------------
// L QQQ RRRR
// L Q Q R R
// L Q Q RRRR 1111111111111111111111111111111
// L Q Q R R
// LLLLL QQ Q R R
// ----------------------------------------------------------------------------------
//
// State Error Body
// 1) x Error
// 2) y Error
// 3) z Error
// 4) x dot Error
// 5) y dot Error
// 6) z dot Error
// 7) Roll
// 8) Pitch
// 9) yaw
// 10) Roll dot
// 11) Pitch dot
// 12) Yaw dot
void calculateControlOutput_viaLQRforRates_forLanding(float stateErrorBody[12], Controller::Request &request, Controller::Response &response){
}
// ----------------------------------------------------------------------------------
// L QQQ RRRR
// L Q Q R R
......@@ -821,6 +948,7 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
// LLLLL QQ Q R R
// ----------------------------------------------------------------------------------
void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
{
// PERFORM THE "u=Kx" LQR CONTROLLER COMPUTATION
......@@ -1347,6 +1475,15 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
controller_mode = getParameterInt(nodeHandle_for_dronexController, "controller_mode");
gainMatrixYawRatefromAngle = getParameterFloatVector(nodeHandle_for_dronexController, "gainMatrixYawRatefromAngle");
gainMatrixPitchRatefromAngle = getParameterFloatVector(nodeHandle_for_dronexController, "gainMatrixPitchRatefromAngle");
gainMatrixRollRatefromAngle = getParameterFloatVector(nodeHandle_for_dronexController, "gainMatrixRollRatefromAngle");
gainMatrixThrust_SixStateVector = getParameterFloatVector(nodeHandle_for_dronexController, "gainMatrixThrust_SixStateVector");
gainMatrixRollAngle = getParameterFloatVector(nodeHandle_for_dronexController, "gainMatrixRollAngle");
gainMatrixPitchAngle = getParameterFloatVector(nodeHandle_for_dronexController, "gainMatrixPitchAngle");
// Max setpoint change per second
m_max_setpoint_change_per_second_horizontal = getParameterFloat(nodeHandle_for_dronexController , "max_setpoint_change_per_second_horizontal");
......
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