To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit c4512715 authored by roangel's avatar roangel
Browse files

added integrator to LQR to get offsetless graphs

parent ac1c4f37
......@@ -168,6 +168,16 @@ rosbag::Bag bag;
LogMsgLong log_temp;
ros::Time t_temp;
double sum_integrator_x = 0.0;
double sum_integrator_y = 0.0;
double sum_integrator_z = 0.0;
double Ki_x = 0.1;
double Ki_y = 0.1;
double Ki_z = 0.05;
bool integrator_enabled = false;
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
......@@ -206,7 +216,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// > Call the function to perform the conversion
convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate,setpoint,current_bodyFrameError);
// CARRY OUT THE CONTROLLER COMPUTATIONS
calculateControlOutput_viaLQR(current_bodyFrameError,request,response);
......@@ -846,6 +855,8 @@ void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller
void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
{
// PERFORM THE NESTED "u=Kx" LQR CONTROLLER COMPUTATION
......@@ -909,6 +920,25 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
bag.write("log_topic", t_temp, log_temp);
if(integrator_enabled)
{
sum_integrator_x += -(stateErrorBody[0])*1/200;
sum_integrator_y += -(stateErrorBody[1])*1/200;
sum_integrator_z += -(stateErrorBody[2])*1/200;
std::cout << "sum_integrator_x: " << sum_integrator_x << std::endl;
std::cout << "sum_integrator_y: " << sum_integrator_y << std::endl;
std::cout << "sum_integrator_z: " << sum_integrator_z << std::endl;
}
double offset_input_roll = -Ki_y*sum_integrator_y;
double offset_input_pitch = Ki_x*sum_integrator_x;
double offset_input_thrust = Ki_z*sum_integrator_z;
//ROS_INFO("Inner called");
......@@ -945,14 +975,15 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
// 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.roll = rollRate_forResponse + offset_input_roll;
response.controlOutput.pitch = pitchRate_forResponse + offset_input_pitch;
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.
float thrustAdjustment = lqr_angleRateNested_prev_thrustAdjustment / 4.0;
float thrustAdjustment = lqr_angleRateNested_prev_thrustAdjustment / 4.0 + offset_input_thrust/4.0;
// > NOTE: the "gravity_force_quarter" value was already divided by 4 when
// it was loaded/processes from the .yaml file.
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
......@@ -1345,7 +1376,17 @@ void setpointCallback(const Setpoint& newSetpoint)
setpoint[0] = newSetpoint.x;
setpoint[1] = newSetpoint.y;
setpoint[2] = newSetpoint.z;
setpoint[3] = newSetpoint.yaw;
setpoint[3] = 0;
if(newSetpoint.yaw == 0)
{
integrator_enabled = false;
}
else
{
integrator_enabled = true;
}
}
......
Markdown is supported
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