Commit 6e5b9adc authored by mastefan's avatar mastefan
Browse files

Completely Implemented the nested Controller. Need to adapt the gain matrix values.

Flight not really stable enough.
parent 78ef3c78
......@@ -217,7 +217,7 @@ float startCoordinateY;
float startCoordinateZ;
// tolerances (x,y,z)
float tol_eps[3] = {0.03, 0.03, 0.03};
float tol_eps[3] = {0.08, 0.08, 0.08};//{0.03, 0.03, 0.03};
float tol_approach[3] = {0.2, 0.2, 0.05}; // added to differ between approach and land *
float tol_land[3] = {0.03, 0.03, 0.03}; // *
float eps_height = 0.05;
......@@ -230,7 +230,7 @@ float eps_height = 0.05;
// > Total mass of the Crazyflie plus whatever it is carrying, in [grams]
float m_mass_total_grams;
float gravity = 9.81;
// > The setpoints for (x,y,z) position and yaw angle, in that order
float m_setpoint[4] = {0.0,0.0,0.0,0.0};
......@@ -278,7 +278,7 @@ 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> gainMatrixThrust_SixStateVector (9,0.0);
std::vector<float> gainMatrixRollAngle (9,0.0);
std::vector<float> gainMatrixPitchAngle (9,0.0);
......@@ -314,6 +314,7 @@ float previous_xzy_rpy_measurement[6];
// difference state estimator
float stateInterialEstimate_viaFiniteDifference[12];
// > The full 12 state estimate maintained by the point mass
// kalman filter state estimator
float stateInterialEstimate_viaPointMassKalmanFilter[12];
......
......@@ -56,11 +56,7 @@ shouldDisplayDebugInfo : false
estimator_method : 1
# The LQR Controller parameters for "mode = 3"
gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
gainMatrixRollRate : [ 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00]
gainMatrixPitchRate : [ 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00]
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
# The max and minimum thrust for a 16-bit command
......@@ -88,10 +84,25 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
gainIntegrator : [1, 1, 1]
# The LQR Controller
gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
gainMatrixRollRate : [ 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00]
gainMatrixPitchRate : [ 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00]
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
# LQR: get Angles from position error
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]
#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]
\ No newline at end of file
......@@ -368,6 +368,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.4;//0.6;
ROS_INFO_STREAM("APPROACH: (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_eps[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_eps[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_eps[2] ){
approachedFlag = true;
......@@ -415,6 +421,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.y = request.otherCrazyflies[0].y;
......@@ -424,7 +431,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
case 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.y = request.ownCrazyflie.y;
......@@ -454,13 +461,19 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
dronexSetpoint.x = startCoordinateX;
dronexSetpoint.y = startCoordinateY;
dronexSetpoint.z = startCoordinateZ + 0.6;
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 << ")");
if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_eps[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_eps[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_eps[2]) {
ROS_INFO("took off");
tookOffFlag = true;
ROS_INFO_STREAM("Entering: DRONEX_STATE_HOVER");
flying_state = DRONEX_STATE_HOVER;
}
}
......@@ -468,6 +481,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
case DRONEX_STATE_HOVER:
{
//ROS_INFO_STREAM("DRONEX_STATE_HOVER");
// keep setpoint constant
/*
dronexSetpoint.x = dronexSetpoint.x;
......@@ -485,12 +499,16 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// flightSeqeunce 1: simple approaching and landing on static mothership
if (flightSequence == SEQUENCE_LAND_ON_MOTHERSHIP){
//ROS_INFO_STREAM("Entering: DRONEX_STATE_TAKING_OFF");
flying_state = DRONEX_STATE_TAKING_OFF;
//ROS_INFO_STREAM("Flight sequence: Landing on mothership");
if(tookOffFlag){
//ROS_INFO_STREAM("Entering: DRONEX_STATE_APPROACH");
flying_state = DRONEX_STATE_APPROACH;
if(approachedFlag){
ROS_INFO_STREAM("Entering: DRONEX_STATE_LAND_ON_MOTHERSHIP");
flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
}
......@@ -498,6 +516,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
/*
// flightSequence 2: approach and land with velocity optimized controller
// TODO: define SEQUENCE names in .h (maybe rename sequences)
if (flightSequence == SEQUENCE_2){
......@@ -518,7 +537,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
}
*/
calculateDroneXVelocity(request);
......@@ -591,6 +610,23 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
// 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
// DroneX Controller
void calculateControlOutputDroneX(Controller::Request &request, Controller::Response &response){
......@@ -619,17 +655,21 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// 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;
/*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
float ms_velocity[3];
ms_velocity[0] = drone_X_vel[0];
ms_velocity[1] = drone_X_vel[1]:
ms_velocity[1] = drone_X_vel[1];
ms_velocity[2] = drone_X_vel[2];
......@@ -639,7 +679,7 @@ 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_coordintes, stateErrorBody);
convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, ms_coordinates, stateErrorBody);
......@@ -657,10 +697,12 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// BODY FRAME X CONTROLLER
pitchAngle_forResponse -= gainMatrixPitchAngle[i] * stateErrorBody[i];
// BODY FRAME X CONTROLLER
thrustAdjustment -= gainMatrixThrustAdjustment[i] * stateErrorBody[i];
//thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
}
// Calculcate Roll and Pitch Baseline, which comes from the moving mothership
float rollAngle_baseline = 0;
float pitchAngle_baseline = 0;
......@@ -671,7 +713,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// Calculate the Force Feedforward
float F = (gravity * mass_CF)/(cos(rollAngle_forResponse)*cos(pitchAngle_forResponse));
float F = (gravity * m_mass_total_grams)/(cos(rollAngle_forResponse)*cos(pitchAngle_forResponse));
......@@ -682,7 +724,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
float AngleError[3] = {
stateErrorBody[6] - rollAngle_forResponse,
stateErrorBody[7] - pitchAngle_forResponse,
yawAngle_forResponse
stateErrorBody[8]
};
float rollRate_forResponse = 0;
......@@ -694,6 +736,72 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
yawRate_forResponse -= gainMatrixYawRatefromAngle[i] * AngleError[i];
}
for(int i = 0; i < 9; ++i){
thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
}
//thrustAdjustment -= F;
// 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.
// > 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.
thrustAdjustment = thrustAdjustment / 4.0;
// > Compute the feed-forward force
float feed_forward_thrust_per_motor = 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);
// 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 0:" << motorPoly[1]);
ROS_INFO_STREAM("motorPoly 0:" << 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);
}*/
}
......@@ -758,7 +866,7 @@ void calculateDroneXVelocity(Controller::Request &request){
drone_X_vel[1] = (current_DroneX_pos[1] - prev_DroneX_pos[1])*m_vicon_frequency;
drone_X_vel[2] = (current_DroneX_pos[2] - prev_DroneX_pos[2])*m_vicon_frequency;
ROS_INFO_STREAM("Velocity: vx " << drone_X_vel[0] << ", vy " << drone_X_vel[1] << ", vz " << drone_X_vel[2]);
//ROS_INFO_STREAM("Velocity: vx " << drone_X_vel[0] << ", vy " << drone_X_vel[1] << ", vz " << drone_X_vel[2]);
}
......@@ -917,19 +1025,7 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
// ----------------------------------------------------------------------------------
//
// 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
......@@ -1477,13 +1573,13 @@ 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");
getParameterFloatVector(nodeHandle_for_dronexController, "gainMatrixYawRatefromAngle", gainMatrixYawRatefromAngle, 3);
getParameterFloatVector(nodeHandle_for_dronexController, "gainMatrixPitchRatefromAngle", gainMatrixPitchRatefromAngle, 3);
getParameterFloatVector(nodeHandle_for_dronexController, "gainMatrixRollRatefromAngle", gainMatrixRollRatefromAngle,3);
gainMatrixThrust_SixStateVector = getParameterFloatVector(nodeHandle_for_dronexController, "gainMatrixThrust_SixStateVector");
gainMatrixRollAngle = getParameterFloatVector(nodeHandle_for_dronexController, "gainMatrixRollAngle");
gainMatrixPitchAngle = getParameterFloatVector(nodeHandle_for_dronexController, "gainMatrixPitchAngle");
getParameterFloatVector(nodeHandle_for_dronexController, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
getParameterFloatVector(nodeHandle_for_dronexController, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
getParameterFloatVector(nodeHandle_for_dronexController, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
// 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