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 b508726d authored by roangel's avatar roangel
Browse files

added enable/disable integrator approach to remove model mismatch in the beginning

parent 8c4366ca
......@@ -521,8 +521,8 @@ void do_MPC(int* publish_rate)
MPC_mutex.unlock();
// Do the MPC step:
// U_0 = mympc_varying_another_ADP(A_tray, B_tray, g_tray, Q, R, P, N, x0_local, X_ref, U_ref, 0, 0.1391*4, 0.7887);
U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0_local, X_ref, U_ref, 0, 0.1391*4, 0.7887);
U_0 = mympc_varying_another_ADP(A_tray, B_tray, g_tray, Q, R, P, N, x0_local, X_ref, U_ref, 0, 0.1391*4, 0.7887);
// U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0_local, X_ref, U_ref, 0, 0.1391*4, 0.7887);
VectorUtype u = U_0.tail(4); // this is the input to apply next
// prediction of next N states:
......@@ -595,6 +595,16 @@ LogMsg log_temp;
ros::Time t_temp;
const double z_offset = 0.09;
double sum_integrator_x = 0.0;
double sum_integrator_y = 0.0;
double sum_integrator_z = 0.0;
double Ki_x = 0.5;
double Ki_y = 0.5;
double Ki_z = 0.1;
bool integrator_enabled = false;
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
......@@ -630,20 +640,35 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
MPC_mutex.lock();
x0 << stateInertialEstimate[0] - setpoint[0],
stateInertialEstimate[1] - setpoint[1],
stateInertialEstimate[2] - (setpoint[2] - z_offset),
stateInertialEstimate[2] - (setpoint[2]),
stateInertialEstimate[3],
stateInertialEstimate[4],
stateInertialEstimate[5];
MPC_mutex.unlock();
if(integrator_enabled)
{
sum_integrator_x += (setpoint[0] - stateInertialEstimate[0])*1/200;
sum_integrator_y += (setpoint[1] - stateInertialEstimate[1])*1/200;
sum_integrator_z += (setpoint[2] - stateInertialEstimate[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;
// read raw_input from shared_raw_input atomic variable
float local_input[4];
MPC_mutex.lock();
local_input[0] = raw_input[0];
local_input[1] = raw_input[1];
local_input[0] = raw_input[0]+offset_input_roll;
local_input[1] = raw_input[1]+offset_input_pitch;
local_input[2] = raw_input[2];
local_input[3] = raw_input[3];
local_input[3] = raw_input[3]+offset_input_thrust;
MPC_mutex.unlock();
// create a function that takes as input angle references, like a crazyflie entity with input angles
......@@ -830,6 +855,15 @@ void setpointCallback(const Setpoint& newSetpoint)
setpoint[2] = newSetpoint.z;
setpoint[3] = newSetpoint.yaw;
if(setpoint[3] == 0)
{
integrator_enabled = false;
}
else
{
integrator_enabled = true;
}
MPC_mutex.lock();
x0 << stateInertialEstimate[0] - setpoint[0],
......
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