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

fixed some bug with the ADP_matrices file. Added one forgotten mutex

parent a7ec2e32
......@@ -6,9 +6,9 @@
#define N_V_fun 9
std::array<Eigen::Matrix<double, N_x, N_x>, N_V_fun> V_functions_P;
std::array<Eigen::Matrix<double, N_x, 1>, N_V_fun> V_functions_p;
std::array<double, N_V_fun> V_functions_q;
extern std::array<Eigen::Matrix<double, N_x, N_x>, N_V_fun> V_functions_P;
extern std::array<Eigen::Matrix<double, N_x, 1>, N_V_fun> V_functions_p;
extern std::array<double, N_V_fun> V_functions_q;
void fill_V_function_variables();
......
#include "ADP_matrices.h"
std::array<Eigen::Matrix<double, N_x, N_x>, N_V_fun> V_functions_P;
std::array<Eigen::Matrix<double, N_x, 1>, N_V_fun> V_functions_p;
std::array<double, N_V_fun> V_functions_q;
void fill_V_function_variables()
{
......
......@@ -68,6 +68,7 @@
#include <std_msgs/Int32.h>
#include "MPC_functions.h"
#include "ADP_matrices.h"
#include <boost/thread/thread.hpp>
......@@ -772,16 +773,30 @@ void setpointCallback(const Setpoint& newSetpoint)
setpoint[2] = newSetpoint.z;
setpoint[3] = newSetpoint.yaw;
MPC_mutex.lock();
x0 << stateInertialEstimate[0] - setpoint[0],
stateInertialEstimate[1] - setpoint[1],
stateInertialEstimate[2] - setpoint[2],
stateInertialEstimate[3],
stateInertialEstimate[4],
stateInertialEstimate[5];
MPC_mutex.unlock();
initialize_MPC_variables(N, x0);
// debug
// fill_V_function_variables();
// std::cout << "V_functions_P[0]" << std::endl;
// std::cout << V_functions_P[0].format(CleanFmt) << std::endl;
// std::cout << "V_functions_p[0]" << std::endl;
// std::cout << V_functions_p[0].format(CleanFmt) << std::endl;
// std::cout << "V_functions_q[0]" << std::endl;
// std::cout << V_functions_q[0] << std::endl;
// clock_t begin_time = clock();
......@@ -1414,6 +1429,8 @@ int main(int argc, char* argv[]) {
0 , 12731, 0 , 0 , 7027 , 0 ,
0 , 0 , 31321 , 0 , 0 , 22707;
fill_V_function_variables();
// X_ref and U_ref. These don't change at all
X_ref.resize(N_x*(N+1));
U_ref.resize(N_u*N);
......
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