Skip to content
GitLab
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
Hide whitespace changes
Inline Side-by-side
pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
View file @ 0d2187dd
......@@ -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
......
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