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

instead of atomic, use mutexes. Everything is kind of dirty, need to clean in the future

parent dbd779d0
......@@ -201,7 +201,7 @@ std::atomic<array<float,N_x>> shared_x0;
std::mutex MPC_mutex;
int N = 2;
int N = 4;
// ROS Publisher for debugging variables
......@@ -504,18 +504,10 @@ void do_MPC(int* publish_rate)
while(ros::ok())
{
// Put the MPC loop here:
// load x0 from shared one
std::array<float, N_x> x0_loaded = shared_x0.load(std::memory_order_acquire);
VectorXtype x0_local;
x0_local << x0_loaded[0],
x0_loaded[1],
x0_loaded[2],
x0_loaded[3],
x0_loaded[4],
x0_loaded[5];
// load x0 protected
MPC_mutex.lock();
VectorXtype x0_local = x0;
MPC_mutex.unlock();
// Do the MPC step:
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);
......@@ -530,6 +522,8 @@ void do_MPC(int* publish_rate)
{
euler_method_forward(F_crazyflie_6_states, 0, params.Ts/4, params.Ts, x_approx_next, U_0.segment(j,N_u), params, x_approx_temp);
x_approx_next = x_approx_temp.back();
MPC_mutex.lock();
X_tray[j] = x_approx_next;
// Now update U_tray using the same loop
......@@ -537,20 +531,23 @@ void do_MPC(int* publish_rate)
U_tray[j] = U_0.segment(N_u*(j+1), N_u);
else
U_tray[j] = U_0.segment(N_u*j, N_u); // last element of the new U_tray is assumed to be the same as second to last
MPC_mutex.unlock();
}
// X_tray and U_tray have been completely updated with the new states coming from the new sequence of inputs
// Relinearize around new trayectory:
MPC_mutex.lock();
get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
MPC_mutex.unlock();
// update the current input to the middle loop
std::array<float,4> temp_input;
temp_input[0] = (float)u(0);
temp_input[1] = (float)u(1);
temp_input[2] = (float)u(2);
temp_input[3] = (float)u(3);
shared_raw_input.store(temp_input,std::memory_order_release);
MPC_mutex.lock();
raw_input[0] = (float)u(0);
raw_input[1] = (float)u(1);
raw_input[2] = (float)u(2);
raw_input[3] = (float)u(3);
MPC_mutex.unlock();
// end MPC loop
......@@ -600,23 +597,14 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// read new x0
MPC_mutex.lock();
x0 << stateInertialEstimate[0] - setpoint[0],
stateInertialEstimate[1] - setpoint[1],
stateInertialEstimate[2] - setpoint[2],
stateInertialEstimate[3],
stateInertialEstimate[4],
stateInertialEstimate[5];
std::array<float, N_x> x0_temp;
x0_temp[0] = x0(0);
x0_temp[1] = x0(1);
x0_temp[2] = x0(2);
x0_temp[3] = x0(3);
x0_temp[4] = x0(4);
x0_temp[5] = x0(5);
shared_x0.store(x0_temp, std::memory_order_release);
MPC_mutex.unlock();
if(outer_loop_counter == 0)
{
......@@ -657,12 +645,14 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
outer_loop_counter = 0;
// read raw_input from shared_raw_input atomic variable
std::array<float,4> temp_input = shared_raw_input.load(std::memory_order_acquire);
float local_input[4];
raw_input[0] = temp_input[0];
raw_input[1] = temp_input[1];
raw_input[2] = temp_input[2];
raw_input[3] = temp_input[3];
MPC_mutex.lock();
local_input[0] = raw_input[0];
local_input[1] = raw_input[1];
local_input[2] = raw_input[2];
local_input[3] = raw_input[3];
MPC_mutex.unlock();
// ============= END MPC PART ===============
......@@ -673,7 +663,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// create a function that takes as input angle references, like a crazyflie entity with input angles
// This function has to run at 200Hz
angleControlledCrazyflie(stateInertialEstimate, raw_input, response);
angleControlledCrazyflie(stateInertialEstimate, local_input, response);
......@@ -1092,16 +1082,14 @@ void setpointCallback(const Setpoint& newSetpoint)
void initialize_MPC_variables(int N, VectorXtype x0)
{
// params:
params.m = cf_mass/1000.0;
params.g = 9.81;
params.Ts = 1/50.0;
// Initialize initial state, 6 states
VectorUtype u_temp;
u_temp << 0, 0, 0, params.m*params.g;
// X_tray and U_tray
MPC_mutex.lock();
X_tray.clear();
U_tray.clear();
for(int i = 0; i < N; i++)
......@@ -1109,6 +1097,7 @@ void initialize_MPC_variables(int N, VectorXtype x0)
X_tray.push_back(x0);
U_tray.push_back(u_temp);
}
MPC_mutex.unlock();
// cout << "filled X_tray and U_tray:" << endl;
......@@ -1121,7 +1110,9 @@ void initialize_MPC_variables(int N, VectorXtype x0)
// cout << U_tray[i].format(CleanFmt) << endl;
// fill A_tray, B_tray, g_tray with initial values
MPC_mutex.lock();
get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
MPC_mutex.unlock();
// cout << "filled A_tray B_tray and g_tray:" << endl;
......@@ -1137,36 +1128,6 @@ void initialize_MPC_variables(int N, VectorXtype x0)
// for(int i=0; i < g_tray.size(); i++)
// cout << g_tray[i].format(CleanFmt) << endl;
VectorXtype q_vec;
q_vec << 5000, 5000, 5000, 500, 500, 500;
VectorUtype r_vec;
r_vec << 2000, 2000, 10000, 150000;
Q = q_vec.asDiagonal();
R = r_vec.asDiagonal();
P <<97395, 0 , 0 , 12731, 0 , 0 ,
0 , 97395, 0 , 0 , 12731, 0 ,
0 , 0 , 124811, 0 , 0 , 31321,
12731, 0 , 0 , 7027 , 0 , 0 ,
0 , 12731, 0 , 0 , 7027 , 0 ,
0 , 0 , 31321 , 0 , 0 , 22707;
// X_ref and U_ref. These don't change at all
X_ref.resize(N_x*(N+1));
U_ref.resize(N_u*N);
VectorXtype x_ref;
x_ref << 0,0,0,0,0,0;
VectorUtype u_ref;
u_ref << 0,0,0,params.m*params.g;
for(int i = 0; i < N+1; i++)
{
X_ref.segment(i*N_x, N_x) = x_ref;
if(i < N)
U_ref.segment(i*N_u, N_u) = u_ref;
}
}
......@@ -1505,6 +1466,43 @@ int main(int argc, char* argv[]) {
stateInertialEstimate[4],
stateInertialEstimate[5];
// params:
params.m = cf_mass/1000.0;
params.g = 9.81;
params.Ts = 1/50.0;
VectorXtype q_vec;
q_vec << 5000, 5000, 5000, 500, 500, 500;
VectorUtype r_vec;
r_vec << 2000, 2000, 10000, 150000;
Q = q_vec.asDiagonal();
R = r_vec.asDiagonal();
P <<97395, 0 , 0 , 12731, 0 , 0 ,
0 , 97395, 0 , 0 , 12731, 0 ,
0 , 0 , 124811, 0 , 0 , 31321,
12731, 0 , 0 , 7027 , 0 , 0 ,
0 , 12731, 0 , 0 , 7027 , 0 ,
0 , 0 , 31321 , 0 , 0 , 22707;
// X_ref and U_ref. These don't change at all
X_ref.resize(N_x*(N+1));
U_ref.resize(N_u*N);
VectorXtype x_ref;
x_ref << 0,0,0,0,0,0;
VectorUtype u_ref;
u_ref << 0,0,0,params.m*params.g;
for(int i = 0; i < N+1; i++)
{
X_ref.segment(i*N_x, N_x) = x_ref;
if(i < N)
U_ref.segment(i*N_u, N_u) = u_ref;
}
initialize_MPC_variables(N, x0);
int rate_MPC = 50; //50 Hz
......
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