Commit db3e3a8e authored by mastefan's avatar mastefan
Browse files

added integrator in new our lqr-controller: still not fully working

parent 6e5b9adc
......@@ -217,10 +217,9 @@ float startCoordinateY;
float startCoordinateZ;
// tolerances (x,y,z)
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;
float tol_takeoff[3] = {0.07, 0.07, 0.07}; //{0.03, 0.03, 0.03};
float tol_approach[3] = {0.3, 0.3, 0.07}; // added to differ between approach and land *
float tol_land[3] = {0.03, 0.03, 0.03}; // *
......@@ -469,6 +468,9 @@ void calculateControlOutput_viaLQRforRates_forLanding( float stateErrorBody[12],
// Velocity estimator
void calculateDroneXVelocity(Controller::Request &request);
// Integrator
void integrator_XYZ(float (&stateErrorBody)[12]);
// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
......
# Mass of the crazyflie
mass_CF : 28
mass_CF : 32
# Max setpoint change per second
max_setpoint_change_per_second_horizontal : 2.00 # [meters]
max_setpoint_change_per_second_vertical : 0.8 # [meters]
max_setpoint_change_per_second_vertical : 0.4 # [meters]
max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
# Frequency of the controller, in hertz
......@@ -81,7 +81,7 @@ 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 : [1, 1, 1]
gainIntegrator : [0.1, 0.1, 0.1]
# The LQR Controller
......
......@@ -374,8 +374,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
<< 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] ){
if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_approach[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_approach[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_approach[2] ){
approachedFlag = true;
ROS_INFO("approached");
}
......@@ -425,17 +425,21 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//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;
dronexSetpoint.z = request.otherCrazyflies[0].z;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.05;
}
break;
case DRONEX_STATE_LAND_ON_GROUND:
{
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;
dronexSetpoint.z = 0.0;
tookOffFlag = false;
}
}
break;
......@@ -468,8 +472,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
<< 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]) {
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]) {
ROS_INFO("took off");
tookOffFlag = true;
......@@ -578,9 +582,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
ROS_INFO("landed -> DRONEX_STATE_ON_GROUND");
flying_state = DRONEX_STATE_GROUND;
}
else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP && (abs(request.ownCrazyflie.x - request.otherCrazyflies[0].x) < tol_eps[0]) &&
(abs(request.ownCrazyflie.y - request.otherCrazyflies[0].y) < tol_eps[1]) &&
(abs(request.ownCrazyflie.z - 0.05 - request.otherCrazyflies[0].z) < tol_eps[2]) ){
else if(flying_state == DRONEX_STATE_LAND_ON_MOTHERSHIP && (abs(request.ownCrazyflie.x - request.otherCrazyflies[0].x) < tol_land[0]) &&
(abs(request.ownCrazyflie.y - request.otherCrazyflies[0].y) < tol_land[1]) &&
(abs(request.ownCrazyflie.z - 0.03 - request.otherCrazyflies[0].z) < tol_land[2]) ){
ROS_INFO("landed -> DRONEX_STATE_ON_MOTHERSHIP");
flying_state = DRONEX_STATE_ON_MOTHERSHIP;
}
......@@ -590,6 +594,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
else{
calculateControlOutputDroneX(request, response);
// for debugging:
/*ROS_INFO_STREAM("deltaX to Mothership: " << request.ownCrazyflie.x - request.otherCrazyflies[0].x);
ROS_INFO_STREAM("deltaY to Mothership: " << request.ownCrazyflie.y - request.otherCrazyflies[0].y);
ROS_INFO_STREAM("deltaZ to Mothership: " << request.ownCrazyflie.z - request.otherCrazyflies[0].z);*/
}
......@@ -660,9 +669,14 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
ms_coordinates[2] = request.otherCrazyflies[0].z;
ms_coordinates[3] = request.otherCrazyflies[0].yaw;*/
ms_coordinates[0] = dronexSetpoint.x;
/*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
......@@ -680,7 +694,6 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
float stateErrorBody[12];
// > Call the function to perform the conversion
convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate, ms_coordinates, stateErrorBody);
float rollAngle_forResponse = 0;
......@@ -690,6 +703,11 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// TODO: Do not forget to implement the yawController
float yawAngle_forResponse = 0;
integrator_XYZ(stateErrorBody);
// Perform the "-Kx" LQR computation for the rates and thrust:
for(int i = 0; i < 6; ++i){
// BODY FRAME Y CONTROLLER
......@@ -745,62 +763,62 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// 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);
}*/
// 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);
}*/
}
......@@ -1739,6 +1757,39 @@ bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
}
// ----------------------------------------------------------------------------------
// III N N TTTTT EEEEE GGGG RRRR A TTTTT OOO RRRR
// I NN N T E G R R A A T O O R R
// I N N N T EEEE G GG RRRR A A T O O RRRR
// I N NN T E G G R R AAAAA T O O R R
// III N N T EEEEE GGGG R R A A T OOO R R
// ----------------------------------------------------------------------------------
void integrator_XYZ(float (&stateErrorBody)[12])
{
if(integratorFlag == DRONEX_INTEGRATOR_ON)
{
ROS_INFO_STREAM("DRONEX_INTEGRATOR_ON");
for(int i=0; i < 3; i++)
{
integrator_var[i] += stateErrorBody[i]*(1.0/control_frequency);
}
}
if(integratorFlag == DRONEX_INTEGRATOR_RESET)
{
for(int i=0; i < 3; i++)
{
integrator_var[i] = 0;
}
}
for(int i=0; i < 3; i++)
{
stateErrorBody[i] += integrator_var[i] * gainIntegrator[i];
}
}
......
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