diff --git a/src/Makefile b/src/Makefile index d62a32ce..350d8bd9 100644 --- a/src/Makefile +++ b/src/Makefile @@ -83,6 +83,7 @@ SRC+=./core/main.c \ ./core/controllers/multirotor_pid/multirotor_pid_param.c \ ./core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c \ ./core/controllers/multirotor_geometry/multirotor_geometry_param.c \ + ./core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c \ ./core/controllers/actuator/motor_thrust_fitting.c \ ./core/controllers/autopilot.c \ ./core/tasks/flight_ctrl_task.c \ diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index d64c3271..bd1f0ca4 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -21,6 +21,7 @@ #include "barometer.h" #include "compass.h" #include "sys_param.h" +#include "proj_config.h" #include "led.h" #include "attitude_state.h" @@ -28,6 +29,14 @@ #define MOTOR_TO_CG_LENGTH 16.25f //[cm] #define MOTOR_TO_CG_LENGTH_M (MOTOR_TO_CG_LENGTH * 0.01) //[m] #define COEFFICIENT_YAW 1.0f +#define N_m 10 +#define N_diag 10 + +typedef struct { + bool isfull; + int index; + int N; +} ICL_data; MAT_ALLOC(J, 3, 3); MAT_ALLOC(R, 3, 3); @@ -61,6 +70,43 @@ MAT_ALLOC(kxex_kvev_mge3_mxd_dot_dot, 3, 1); MAT_ALLOC(b1d, 3, 1); MAT_ALLOC(b2d, 3, 1); MAT_ALLOC(b3d, 3, 1); +MAT_ALLOC(Y_m, 3, 1); +MAT_ALLOC(Y_mt, 1, 3); +MAT_ALLOC(y_m_cl_integral, 3, 1); +MAT_ALLOC(y_m_clt_integral, 1, 3); +MAT_ALLOC(Y_diag, 3, 3); +MAT_ALLOC(Y_diagt, 3, 3); +MAT_ALLOC(y_diag_cl_integral, 3, 3); +MAT_ALLOC(y_diag_clt_integral, 3, 3); +MAT_ALLOC(M_fb, 3, 1); +MAT_ALLOC(M_ff, 3, 1); +MAT_ALLOC(theta_m_hat, 1, 1); +MAT_ALLOC(theta_m_hat_dot, 1, 1); +MAT_ALLOC(theta_m_hat_dot_adaptive, 1, 1); +MAT_ALLOC(theta_m_hat_dot_ICL, 1, 1); +MAT_ALLOC(theta_diag_hat, 3, 1); +MAT_ALLOC(theta_diag_hat_dot, 3, 1); +MAT_ALLOC(theta_diag_hat_dot_adaptive, 3, 1); +MAT_ALLOC(theta_diag_hat_dot_ICL, 3, 1); +MAT_ALLOC(ev_C1ex, 3, 1); +MAT_ALLOC(eW_C2eR, 3, 1); +MAT_ALLOC(Ymt_evC1ex, 1, 1); +MAT_ALLOC(Ydiagt_eWC2eR, 3, 1); +MAT_ALLOC(last_vel, 3, 1); +MAT_ALLOC(curr_force, 3, 1); +MAT_ALLOC(F_cl, 3, 1); +MAT_ALLOC(mat_m_now, 1, 1); +MAT_ALLOC(last_W, 3, 1); +MAT_ALLOC(curr_moment, 3, 1); +MAT_ALLOC(M_cl, 3, 1); +MAT_ALLOC(yDiagCl_thetaDiatHat, 3, 1); +MAT_ALLOC(M_sub_err, 3, 1); +MAT_ALLOC(mat_diag_now, 3, 1); + +float mat_m_matrix[N_m] = {0.0f}; +float mat_m_sum = 0.0f; +float mat_diag_matrix[3][N_diag]; +float mat_diag_sum[3]; float pos_error[3]; float vel_error[3]; @@ -73,6 +119,13 @@ float kvx, kvy, kvz; float yaw_rate_ctrl_gain; float k_tracking_i_gain[3]; +float Gamma_m_gain; +float Gamma_diag_gain[3]; +float C1_gain; +float C2_gain; +float k_cl_m_gain; +float k_cl_diag_gain[3]; + float uav_mass; //M = (J * W_dot) + (W X JW) @@ -88,12 +141,28 @@ autopilot_t autopilot; bool height_ctrl_only = false; +ICL_data force_ICL; +ICL_data momen_ICL; + +void ICL_matrix_init(void) +{ + force_ICL.index = 0; + force_ICL.isfull = false; + force_ICL.N = N_m; + + momen_ICL.index = 0; + momen_ICL.isfull = false; + momen_ICL.N = N_diag; +} + void geometry_ctrl_init(void) { init_multirotor_geometry_param_list(); autopilot_init(&autopilot); + ICL_matrix_init(); + float geo_fence_origin[3] = {0.0f, 0.0f, 0.0f}; autopilot_set_enu_rectangular_fence(geo_fence_origin, 2.5f, 1.3f, 3.0f); @@ -129,6 +198,38 @@ void geometry_ctrl_init(void) MAT_INIT(b1d, 3, 1); MAT_INIT(b2d, 3, 1); MAT_INIT(b3d, 3, 1); + MAT_INIT(Y_m, 3, 1); + MAT_INIT(Y_mt, 1, 3); + MAT_INIT(y_m_cl_integral, 3, 1); + MAT_INIT(y_m_clt_integral, 1, 3); + MAT_INIT(Y_diag, 3, 3); + MAT_INIT(Y_diagt, 3, 3); + MAT_INIT(y_diag_cl_integral, 3, 3); + MAT_INIT(y_diag_clt_integral, 3, 3); + MAT_INIT(M_fb, 3, 1); + MAT_INIT(M_ff, 3, 1); + MAT_INIT(theta_m_hat, 1, 1); + MAT_INIT(theta_m_hat_dot, 1, 1); + MAT_INIT(theta_m_hat_dot_adaptive, 1, 1); + MAT_INIT(theta_m_hat_dot_ICL, 1, 1); + MAT_INIT(theta_diag_hat, 3, 1); + MAT_INIT(theta_diag_hat_dot, 3, 1); + MAT_INIT(theta_diag_hat_dot_adaptive, 1, 1); + MAT_INIT(theta_diag_hat_dot_ICL, 1, 1); + MAT_INIT(ev_C1ex, 3, 1); + MAT_INIT(eW_C2eR, 3, 1); + MAT_INIT(Ymt_evC1ex, 1, 1); + MAT_INIT(Ydiagt_eWC2eR, 3, 1); + MAT_INIT(last_vel, 3, 1); + MAT_INIT(curr_force, 3, 1); + MAT_INIT(F_cl, 3, 1); + MAT_INIT(mat_m_now, 1, 1); + MAT_INIT(last_W, 3, 1); + MAT_INIT(curr_moment, 3, 1); + MAT_INIT(M_cl, 3, 1); + MAT_INIT(yDiagCl_thetaDiatHat, 3, 1); + MAT_INIT(M_sub_err, 3, 1); + MAT_INIT(mat_diag_now, 3, 1); /* modify local variables when user change them via ground station */ set_sys_param_update_var_addr(MR_GEO_GAIN_ROLL_P, &krx); @@ -147,6 +248,18 @@ void geometry_ctrl_init(void) set_sys_param_update_var_addr(MR_GEO_GAIN_POS_X_I, &k_tracking_i_gain[0]); set_sys_param_update_var_addr(MR_GEO_GAIN_POS_Y_I, &k_tracking_i_gain[1]); set_sys_param_update_var_addr(MR_GEO_GAIN_POS_Z_I, &k_tracking_i_gain[2]); +#if 0 + set_sys_param_update_var_addr(MR_ICL_GAIN_GAMMA_M, &Gamma_m_gain); + set_sys_param_update_var_addr(MR_ICL_GAIN_GAMMA_DIAG_X, &Gamma_diag_gain[0]); + set_sys_param_update_var_addr(MR_ICL_GAIN_GAMMA_DIAG_Y, &Gamma_diag_gain[1]); + set_sys_param_update_var_addr(MR_ICL_GAIN_GAMMA_DIAG_Z, &Gamma_diag_gain[2]); + set_sys_param_update_var_addr(MR_ICL_GAIN_C1, &C1_gain); + set_sys_param_update_var_addr(MR_ICL_GAIN_C2, &C2_gain); + set_sys_param_update_var_addr(MR_ICL_GAIN_K_CL_M, &k_cl_m_gain); + set_sys_param_update_var_addr(MR_ICL_GAIN_K_CL_DIAG_X, &k_cl_diag_gain[0]); + set_sys_param_update_var_addr(MR_ICL_GAIN_K_CL_DIAG_Y, &k_cl_diag_gain[1]); + set_sys_param_update_var_addr(MR_ICL_GAIN_K_CL_DIAG_Z, &k_cl_diag_gain[2]); +#endif set_sys_param_update_var_addr(MR_GEO_UAV_MASS, &uav_mass); set_sys_param_update_var_addr(MR_GEO_INERTIA_JXX, &mat_data(J)[0*3 + 0]); set_sys_param_update_var_addr(MR_GEO_INERTIA_JYY, &mat_data(J)[1*3 + 1]); @@ -182,6 +295,18 @@ void geometry_ctrl_init(void) get_sys_param_float(MR_GEO_GAIN_POS_X_I, &k_tracking_i_gain[0]); get_sys_param_float(MR_GEO_GAIN_POS_Y_I, &k_tracking_i_gain[1]); get_sys_param_float(MR_GEO_GAIN_POS_Z_I, &k_tracking_i_gain[2]); +#if 0 + get_sys_param_float(MR_ICL_GAIN_GAMMA_M, &Gamma_m_gain); + get_sys_param_float(MR_ICL_GAIN_K_CL_DIAG_X, &Gamma_diag_gain[0]); + get_sys_param_float(MR_ICL_GAIN_K_CL_DIAG_Y, &Gamma_diag_gain[1]); + get_sys_param_float(MR_ICL_GAIN_K_CL_DIAG_Z, &Gamma_diag_gain[2]); + get_sys_param_float(MR_ICL_GAIN_C1, &C1_gain); + get_sys_param_float(MR_ICL_GAIN_C2, &C2_gain); + get_sys_param_float(MR_ICL_GAIN_K_CL_M, &k_cl_m_gain); + get_sys_param_float(MR_ICL_GAIN_K_CL_DIAG_X, &k_cl_diag_gain[0]); + get_sys_param_float(MR_ICL_GAIN_K_CL_DIAG_Y, &k_cl_diag_gain[1]); + get_sys_param_float(MR_ICL_GAIN_K_CL_DIAG_Z, &k_cl_diag_gain[2]); +#endif get_sys_param_float(MR_GEO_UAV_MASS, &uav_mass); get_sys_param_float(MR_GEO_INERTIA_JXX, &mat_data(J)[0*3 + 0]); get_sys_param_float(MR_GEO_INERTIA_JYY, &mat_data(J)[1*3 + 1]); @@ -200,6 +325,26 @@ void geometry_ctrl_init(void) get_sys_param_float(THRUST_TO_PWM_C6, &coeff_thrust_to_cmd[5]); get_sys_param_float(THRUST_MAX, &motor_thrust_max); + /* initialize gains used in adaptive ICL control */ + Gamma_m_gain = 0.1f; + Gamma_diag_gain[0] = 8.0f; + Gamma_diag_gain[1] = 8.0f; + Gamma_diag_gain[2] = 8.0f; + C1_gain = 0.1f; + C2_gain = 0.1f; + k_cl_m_gain = 7.5f; + k_cl_diag_gain[0] = 5.0f; + k_cl_diag_gain[1] = 5.0f; + k_cl_diag_gain[2] = 5.0f; + + /* initialize value of mass estimation */ + mat_data(theta_m_hat)[0] = 1.3f; + + /* initialize value of moment of inertia estimation */ + mat_data(theta_diag_hat)[0] = 0.01f; + mat_data(theta_diag_hat)[1] = 0.01f; + mat_data(theta_diag_hat)[2] = 0.01f; + set_motor_max_thrust(motor_thrust_max); set_motor_cmd_to_thrust_coeff(coeff_cmd_to_thrust[0], coeff_cmd_to_thrust[1], coeff_cmd_to_thrust[2], coeff_cmd_to_thrust[3], coeff_cmd_to_thrust[4], coeff_cmd_to_thrust[5]); @@ -246,6 +391,281 @@ void reset_geometry_tracking_error_integral(void) tracking_error_integral[2] = 0.0f; } +void force_ff_ctrl_use_geometry(float *accel_ff, float *force_ff) +{ + /* with mass of uav known */ + force_ff[0] = uav_mass * accel_ff[0]; + force_ff[1] = uav_mass * accel_ff[1]; + force_ff[2] = uav_mass * (accel_ff[2] - 9.81); +} + +void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos_err, float *vel_err, float *curr_vel) +{ + /* with mass of uav unknown */ + /* Y_m and Y_m transpose */ + mat_data(Y_m)[0] = accel_ff[0]; + mat_data(Y_m)[1] = accel_ff[1]; + mat_data(Y_m)[2] = accel_ff[2] - 9.81; + mat_data(Y_mt)[0] = mat_data(Y_m)[0]; + mat_data(Y_mt)[1] = mat_data(Y_m)[1]; + mat_data(Y_mt)[2] = mat_data(Y_m)[2]; + + //ev_C1ex = ev + C1*ex + mat_data(ev_C1ex)[0] = vel_err[0] + C1_gain*pos_err[0]; + mat_data(ev_C1ex)[1] = vel_err[1] + C1_gain*pos_err[1]; + mat_data(ev_C1ex)[2] = vel_err[2] + C1_gain*pos_err[2]; + + /* first term of theta_m update law */ + MAT_MULT(&Y_mt, &ev_C1ex, &Ymt_evC1ex); + +#if (SELECT_FORCE_ADAPTIVE_W_WO_ICL == FORCE_ADAPTIVE_WITHOUT_ICL) + //theta_m_dot = Gamma*Y_mt*ev_C1ex + mat_data(theta_m_hat_dot)[0] = -(Gamma_m_gain*mat_data(Ymt_evC1ex)[0]); +#elif (SELECT_FORCE_ADAPTIVE_W_WO_ICL == FORCE_ADAPTIVE_WITH_ICL) + /* y_m_cl_integral and y_m_cl_integral transpose */ + mat_data(y_m_cl_integral)[0] = -(curr_vel[0] - mat_data(last_vel)[0]); + mat_data(y_m_cl_integral)[1] = -(curr_vel[1] - mat_data(last_vel)[1]); + mat_data(y_m_cl_integral)[2] = -(curr_vel[2] - mat_data(last_vel)[2]) + 9.81*dt; + + mat_data(y_m_clt_integral)[0] = mat_data(y_m_cl_integral)[0]; + mat_data(y_m_clt_integral)[1] = mat_data(y_m_cl_integral)[1]; + mat_data(y_m_clt_integral)[2] = mat_data(y_m_cl_integral)[2]; + + /* prepare force control input used in ICL */ + mat_data(F_cl)[0] = mat_data(curr_force)[0]*dt; + mat_data(F_cl)[1] = mat_data(curr_force)[1]*dt; + mat_data(F_cl)[2] = mat_data(curr_force)[2]*dt; + + /* prepare past data */ + mat_data(mat_m_now)[0] = mat_data(y_m_clt_integral)[0] + *(mat_data(F_cl)[0]-mat_data(y_m_cl_integral)[0]*mat_data(theta_m_hat)[0]); + mat_data(mat_m_now)[0] += mat_data(y_m_clt_integral)[1] + *(mat_data(F_cl)[1]-mat_data(y_m_cl_integral)[1]*mat_data(theta_m_hat)[0]); + mat_data(mat_m_now)[0] += mat_data(y_m_clt_integral)[2] + *(mat_data(F_cl)[2]-mat_data(y_m_cl_integral)[2]*mat_data(theta_m_hat)[0]); + + /* summation of past data */ + if (force_ICL.index >= force_ICL.N) { + force_ICL.index = 0; + force_ICL.isfull = true; + } + mat_m_matrix[force_ICL.index] = mat_data(mat_m_now)[0]; + force_ICL.index++; + if (!force_ICL.isfull) { + mat_m_sum = 0; + for (int i = 0; i < force_ICL.index; i++) { + mat_m_sum += mat_m_matrix[i]; + } + } else { + mat_m_sum = 0; + for (int i = 0; i < force_ICL.N; i++) { + mat_m_sum += mat_m_matrix[i]; + } + } + + /* save current velocity as last velocity in the next loop */ + mat_data(last_vel)[0] = curr_vel[0]; + mat_data(last_vel)[1] = curr_vel[1]; + mat_data(last_vel)[2] = curr_vel[2]; + + /* prepare components of theta_m_hat_dot */ + mat_data(theta_m_hat_dot_adaptive)[0] = -Gamma_m_gain*mat_data(Ymt_evC1ex)[0]; + mat_data(theta_m_hat_dot_ICL)[0] = k_cl_m_gain*Gamma_m_gain*mat_m_sum; + + /* theta_m_dot = adaptive law + ICL update law */ + mat_data(theta_m_hat_dot)[0] = mat_data(theta_m_hat_dot_adaptive)[0] + + mat_data(theta_m_hat_dot_ICL)[0]; +#endif + +#if 0 + mat_data(theta_m_hat)[0] = uav_mass; + + /* translational adaptive feedforward term */ + //Y_m*theta_m_hat + force_ff[0] = mat_data(Y_m)[0]*mat_data(theta_m_hat)[0]; + force_ff[1] = mat_data(Y_m)[1]*mat_data(theta_m_hat)[0]; + force_ff[2] = mat_data(Y_m)[2]*mat_data(theta_m_hat)[0]; +#endif + +#if 1 + mat_data(theta_m_hat)[0] += mat_data(theta_m_hat_dot)[0] * dt; + + /* translational adaptive feedforward term */ + //Y_m*theta_m_hat + force_ff[0] = mat_data(Y_m)[0]*mat_data(theta_m_hat)[0]; + force_ff[1] = mat_data(Y_m)[1]*mat_data(theta_m_hat)[0]; + force_ff[2] = mat_data(Y_m)[2]*mat_data(theta_m_hat)[0]; + + bound_float(&force_ff[0], 2.8, -2.8); + bound_float(&force_ff[1], 2.8, -2.8); + bound_float(&force_ff[2], 15, -15); +#endif +} + +void moment_ff_ctrl_use_geometry(float *mom_ff) +{ + /* with moment of inertia of uav known */ + /* calculate the inertia feedfoward term */ + //W x JW + MAT_MULT(&J, &W, &JW); + cross_product_3x1(mat_data(W), mat_data(JW), mat_data(WJW)); + mom_ff[0] = -mat_data(WJW)[0]; + mom_ff[1] = -mat_data(WJW)[1]; + mom_ff[2] = -mat_data(WJW)[2]; +} + +void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff) +{ + /* with moment of inertia of uav unknown */ + /* Y_diag from angular velocity */ + mat_data(Y_diag)[0*3 + 0] = 0; + mat_data(Y_diag)[1*3 + 0] = -mat_data(W)[0]*mat_data(W)[2]; + mat_data(Y_diag)[2*3 + 0] = mat_data(W)[0]*mat_data(W)[1]; + mat_data(Y_diag)[0*3 + 1] = mat_data(W)[1]*mat_data(W)[2]; + mat_data(Y_diag)[1*3 + 1] = 0; + mat_data(Y_diag)[2*3 + 1] = -mat_data(W)[0]*mat_data(W)[1]; + mat_data(Y_diag)[0*3 + 2] = -mat_data(W)[1]*mat_data(W)[2]; + mat_data(Y_diag)[1*3 + 2] = mat_data(W)[0]*mat_data(W)[2]; + mat_data(Y_diag)[2*3 + 2] = 0; + + //Y_diagt = transpose of Y_diag + mat_data(Y_diagt)[0*3 + 0] = mat_data(Y_diag)[0*3 + 0]; + mat_data(Y_diagt)[1*3 + 0] = mat_data(Y_diag)[0*3 + 1]; + mat_data(Y_diagt)[2*3 + 0] = mat_data(Y_diag)[0*3 + 2]; + mat_data(Y_diagt)[0*3 + 1] = mat_data(Y_diag)[1*3 + 0]; + mat_data(Y_diagt)[1*3 + 1] = mat_data(Y_diag)[1*3 + 1]; + mat_data(Y_diagt)[2*3 + 1] = mat_data(Y_diag)[1*3 + 2]; + mat_data(Y_diagt)[0*3 + 2] = mat_data(Y_diag)[2*3 + 0]; + mat_data(Y_diagt)[1*3 + 2] = mat_data(Y_diag)[2*3 + 1]; + mat_data(Y_diagt)[2*3 + 2] = mat_data(Y_diag)[2*3 + 2]; + + //eW_C2eR = eW + C2*eR + mat_data(eW_C2eR)[0] = mat_data(eW)[0] + C2_gain*mat_data(eR)[0]; + mat_data(eW_C2eR)[1] = mat_data(eW)[1] + C2_gain*mat_data(eR)[1]; + mat_data(eW_C2eR)[2] = mat_data(eW)[2] + C2_gain*mat_data(eR)[2]; + + /* first term of theta_diag update law */ + MAT_MULT(&Y_diagt, &eW_C2eR, &Ydiagt_eWC2eR); + +#if (SELECT_MOMENT_ADAPTIVE_W_WO_ICL == MOMENT_ADAPTIVE_WITHOUT_ICL) + //theta_diag_dot = Gamma*Y_diagt*eW_C2eR + mat_data(theta_diag_hat_dot)[0] = Gamma_diag_gain[0]*mat_data(Ydiagt_eWC2eR)[0]; + mat_data(theta_diag_hat_dot)[1] = Gamma_diag_gain[1]*mat_data(Ydiagt_eWC2eR)[1]; + mat_data(theta_diag_hat_dot)[2] = Gamma_diag_gain[2]*mat_data(Ydiagt_eWC2eR)[2]; +#elif (SELECT_MOMENT_ADAPTIVE_W_WO_ICL == MOMENT_ADAPTIVE_WITH_ICL) + /* y_diag_cl_integral and y_diag_cl_integral transpose */ + mat_data(y_diag_cl_integral)[0*3 + 0] = mat_data(W)[0] - mat_data(last_W)[0]; + mat_data(y_diag_cl_integral)[1*3 + 0] = (mat_data(W)[0]*mat_data(W)[2])*dt; + mat_data(y_diag_cl_integral)[2*3 + 0] = (-mat_data(W)[0]*mat_data(W)[1])*dt; + mat_data(y_diag_cl_integral)[0*3 + 1] = (-mat_data(W)[1]*mat_data(W)[2])*dt; + mat_data(y_diag_cl_integral)[1*3 + 1] = mat_data(W)[1] - mat_data(last_W)[1]; + mat_data(y_diag_cl_integral)[2*3 + 1] = (mat_data(W)[0]*mat_data(W)[1])*dt; + mat_data(y_diag_cl_integral)[0*3 + 2] = (mat_data(W)[1]*mat_data(W)[2])*dt; + mat_data(y_diag_cl_integral)[1*3 + 2] = (-mat_data(W)[0]*mat_data(W)[2])*dt; + mat_data(y_diag_cl_integral)[2*3 + 2] = mat_data(W)[2] - mat_data(last_W)[2]; + + mat_data(y_diag_clt_integral)[0*3 + 0] = mat_data(y_diag_cl_integral)[0*3 + 0]; + mat_data(y_diag_clt_integral)[1*3 + 0] = mat_data(y_diag_cl_integral)[0*3 + 1]; + mat_data(y_diag_clt_integral)[2*3 + 0] = mat_data(y_diag_cl_integral)[0*3 + 2]; + mat_data(y_diag_clt_integral)[0*3 + 1] = mat_data(y_diag_cl_integral)[1*3 + 0]; + mat_data(y_diag_clt_integral)[1*3 + 1] = mat_data(y_diag_cl_integral)[1*3 + 1]; + mat_data(y_diag_clt_integral)[2*3 + 1] = mat_data(y_diag_cl_integral)[1*3 + 2]; + mat_data(y_diag_clt_integral)[0*3 + 2] = mat_data(y_diag_cl_integral)[2*3 + 0]; + mat_data(y_diag_clt_integral)[1*3 + 2] = mat_data(y_diag_cl_integral)[2*3 + 1]; + mat_data(y_diag_clt_integral)[2*3 + 2] = mat_data(y_diag_cl_integral)[2*3 + 2]; + + /* prepare moment control input used in ICL */ + mat_data(M_cl)[0] = mat_data(curr_moment)[0]*dt; + mat_data(M_cl)[1] = mat_data(curr_moment)[1]*dt; + mat_data(M_cl)[2] = mat_data(curr_moment)[2]*dt; + + /* prepare past data */ + MAT_MULT(&y_diag_cl_integral, &theta_diag_hat, &yDiagCl_thetaDiatHat); + MAT_SUB(&M_cl, &yDiagCl_thetaDiatHat, &M_sub_err); + MAT_MULT(&y_diag_clt_integral, &M_sub_err, &mat_diag_now); + + /* summation of past data */ + if (momen_ICL.index >= momen_ICL.N) { + momen_ICL.index = 0; + momen_ICL.isfull = true; + } + mat_diag_matrix[0][momen_ICL.index] = mat_data(mat_diag_now)[0]; + mat_diag_matrix[1][momen_ICL.index] = mat_data(mat_diag_now)[1]; + mat_diag_matrix[2][momen_ICL.index] = mat_data(mat_diag_now)[2]; + momen_ICL.index++; + if (!momen_ICL.isfull) { + mat_diag_sum[0] = 0.0f; + mat_diag_sum[1] = 0.0f; + mat_diag_sum[2] = 0.0f; + for (int i = 0; i < momen_ICL.index; i++) { + mat_diag_sum[0] += mat_diag_matrix[0][i]; + mat_diag_sum[1] += mat_diag_matrix[1][i]; + mat_diag_sum[2] += mat_diag_matrix[2][i]; + } + } else { + mat_diag_sum[0] = 0.0f; + mat_diag_sum[1] = 0.0f; + mat_diag_sum[2] = 0.0f; + for (int i = 0; i < momen_ICL.N; i++) { + mat_diag_sum[0] += mat_diag_matrix[0][i]; + mat_diag_sum[1] += mat_diag_matrix[1][i]; + mat_diag_sum[2] += mat_diag_matrix[2][i]; + } + } + + /* save current angular velocity as last velocity in the next loop */ + mat_data(last_W)[0] = mat_data(W)[0]; + mat_data(last_W)[1] = mat_data(W)[1]; + mat_data(last_W)[2] = mat_data(W)[2]; + + /* prepare components of theta_diag_hat_dot */ + mat_data(theta_diag_hat_dot_adaptive)[0] = Gamma_diag_gain[0]*mat_data(Ydiagt_eWC2eR)[0]; + mat_data(theta_diag_hat_dot_adaptive)[1] = Gamma_diag_gain[1]*mat_data(Ydiagt_eWC2eR)[1]; + mat_data(theta_diag_hat_dot_adaptive)[2] = Gamma_diag_gain[2]*mat_data(Ydiagt_eWC2eR)[2]; + mat_data(theta_diag_hat_dot_ICL)[0] = k_cl_diag_gain[0]*Gamma_diag_gain[0]*mat_diag_sum[0]; + mat_data(theta_diag_hat_dot_ICL)[1] = k_cl_diag_gain[1]*Gamma_diag_gain[1]*mat_diag_sum[1]; + mat_data(theta_diag_hat_dot_ICL)[2] = k_cl_diag_gain[2]*Gamma_diag_gain[2]*mat_diag_sum[2]; + + /* theta_diag_dot = adaptive law + ICL update law */ + mat_data(theta_diag_hat_dot)[0] = mat_data(theta_diag_hat_dot_adaptive)[0] + + mat_data(theta_diag_hat_dot_ICL)[0]; + mat_data(theta_diag_hat_dot)[1] = mat_data(theta_diag_hat_dot_adaptive)[1] + + mat_data(theta_diag_hat_dot_ICL)[1]; + mat_data(theta_diag_hat_dot)[2] = mat_data(theta_diag_hat_dot_adaptive)[2] + + mat_data(theta_diag_hat_dot_ICL)[2]; +#endif + +#if 0 + mat_data(theta_diag_hat)[0] = mat_data(J)[0]; + mat_data(theta_diag_hat)[1] = mat_data(J)[4]; + mat_data(theta_diag_hat)[2] = mat_data(J)[8]; + + /* rotational adaptive feedforward term */ + //Y_diag*theta_diag_hat + MAT_MULT(&Y_diag, &theta_diag_hat, &M_ff); + mom_ff[0] = mat_data(M_ff)[0]; + mom_ff[1] = mat_data(M_ff)[1]; + mom_ff[2] = mat_data(M_ff)[2]; +#endif + +#if 1 + mat_data(theta_diag_hat)[0] += mat_data(theta_diag_hat_dot)[0] * dt; + mat_data(theta_diag_hat)[1] += mat_data(theta_diag_hat_dot)[1] * dt; + mat_data(theta_diag_hat)[2] += mat_data(theta_diag_hat_dot)[2] * dt; + + /* rotational adaptive feedforward term */ + //Y_diag*theta_diag_hat + MAT_MULT(&Y_diag, &theta_diag_hat, &M_ff); + mom_ff[0] = mat_data(M_ff)[0]; + mom_ff[1] = mat_data(M_ff)[1]; + mom_ff[2] = mat_data(M_ff)[2]; + + bound_float(&mom_ff[0], 0.3, -0.3); + bound_float(&mom_ff[1], 0.3, -0.3); + bound_float(&mom_ff[2], 0.3, -0.3); +#endif +} + void geometry_manual_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *output_moments, bool heading_present) { @@ -292,13 +712,14 @@ void geometry_manual_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *ou MAT_MULT(&RtRd, &Wd, &RtRdWd); MAT_SUB(&W, &RtRdWd, &eW); - /* calculate the inertia feedfoward term */ - //W x JW - MAT_MULT(&J, &W, &JW); - cross_product_3x1(mat_data(W), mat_data(JW), mat_data(WJW)); - mat_data(inertia_effect)[0] = mat_data(WJW)[0]; - mat_data(inertia_effect)[1] = mat_data(WJW)[1]; - mat_data(inertia_effect)[2] = mat_data(WJW)[2]; + /* moment feedforward control */ + float moment_ff[3] = {0, 0}; + +#if (SELECT_FEEDFORWARD_MANUAL == FEEDFORWARD_MANUAL_USE_GEOMETRY) + moment_ff_ctrl_use_geometry(moment_ff); +#elif (SELECT_FEEDFORWARD_MANUAL == FEEDFORWARD_MANUAL_USE_ADAPTIVE_ICL) + moment_ff_ctrl_use_adaptive_ICL(moment_ff); +#endif #if 0 /* inertia feedfoward term for motion planning (trajectory is known) */ /* calculate inertia effect (trajectory is defined, Wd and Wd_dot are not zero) */ @@ -319,6 +740,15 @@ void geometry_manual_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *ou #endif + mat_data(M_fb)[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0]; + mat_data(M_fb)[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1]; + mat_data(M_fb)[2] = -krz*mat_data(eR)[2] -kwz*mat_data(eW)[2]; + + /* for debug messages */ + mat_data(inertia_effect)[0] = -moment_ff[0]; + mat_data(inertia_effect)[1] = -moment_ff[1]; + mat_data(inertia_effect)[2] = -moment_ff[2]; + /* control input M1, M2, M3 */ output_moments[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] + mat_data(inertia_effect)[0]; output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] + mat_data(inertia_effect)[1]; @@ -343,13 +773,6 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * vel_error[1] = curr_vel_ned[1] - vel_des_ned[1]; vel_error[2] = curr_vel_ned[2] - vel_des_ned[2]; - float force_ff_ned[3] = {0.0f}; - float accel_ff_ned[3] = {0.0f}; - assign_vector_3x1_enu_to_ned(accel_ff_ned, autopilot.wp_now.acc_feedforward); - force_ff_ned[0] = uav_mass * accel_ff_ned[0]; - force_ff_ned[1] = uav_mass * accel_ff_ned[1]; - force_ff_ned[2] = uav_mass * accel_ff_ned[2]; - tracking_error_integral[0] += k_tracking_i_gain[0] * (pos_error[0]) * dt; tracking_error_integral[1] += k_tracking_i_gain[1] * (pos_error[1]) * dt; tracking_error_integral[2] += k_tracking_i_gain[2] * (pos_error[2]) * dt; @@ -358,13 +781,24 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * bound_float(&tracking_error_integral[1], 150, -150); bound_float(&tracking_error_integral[2], 50, -50); - mat_data(kxex_kvev_mge3_mxd_dot_dot)[0] = -kpx*pos_error[0] - kvx*vel_error[0] + - force_ff_ned[0] - tracking_error_integral[0]; - mat_data(kxex_kvev_mge3_mxd_dot_dot)[1] = -kpy*pos_error[1] - kvy*vel_error[1] + - force_ff_ned[1] - tracking_error_integral[1]; - mat_data(kxex_kvev_mge3_mxd_dot_dot)[2] = -kpz*pos_error[2] - kvz*vel_error[2] + - force_ff_ned[2] - tracking_error_integral[2] - - uav_mass * 9.81; + /* force feedforward control */ + float accel_ff_ned[3] = {0.0f}; + float force_ff_ned[3] = {0.0f}; + assign_vector_3x1_enu_to_ned(accel_ff_ned, autopilot.wp_now.acc_feedforward); + +#if (SELECT_FEEDFORWARD_TRACKING_FORCE == FEEDFORWARD_TRACKING_FORCE_USE_GEOMETRY) + force_ff_ctrl_use_geometry(accel_ff_ned, force_ff_ned); +#elif (SELECT_FEEDFORWARD_TRACKING_FORCE == FEEDFORWARD_TRACKING_FORCE_USE_ADAPTIVE_ICL) + force_ff_ctrl_use_adaptive_ICL(accel_ff_ned, force_ff_ned, pos_error, vel_error, curr_vel_ned); +#endif + + /* control input kxex_kvev_mge3_mxd_dot_dot */ + mat_data(kxex_kvev_mge3_mxd_dot_dot)[0] = -kpx*pos_error[0] - kvx*vel_error[0] + - tracking_error_integral[0] + force_ff_ned[0]; + mat_data(kxex_kvev_mge3_mxd_dot_dot)[1] = -kpy*pos_error[1] - kvy*vel_error[1] + - tracking_error_integral[1] + force_ff_ned[1]; + mat_data(kxex_kvev_mge3_mxd_dot_dot)[2] = -kpz*pos_error[2] - kvz*vel_error[2] + - tracking_error_integral[2] + force_ff_ned[2]; /* calculate the denominator of b3d */ float b3d_denominator; //caution: this term should not be 0 @@ -425,6 +859,11 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * neg_kxex_kvev_mge3_mxd_dot_dot[2] = -mat_data(kxex_kvev_mge3_mxd_dot_dot)[2]; arm_dot_prod_f32(neg_kxex_kvev_mge3_mxd_dot_dot, mat_data(Re3), 3, output_force); + /* save current force for ICL */ + mat_data(curr_force)[0] = (*output_force)*mat_data(Re3)[0]; + mat_data(curr_force)[1] = (*output_force)*mat_data(Re3)[1]; + mat_data(curr_force)[2] = (*output_force)*mat_data(Re3)[2]; + /* W (angular velocity) */ mat_data(W)[0] = gyro[0]; mat_data(W)[1] = gyro[1]; @@ -452,13 +891,28 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * MAT_MULT(&RtRd, &Wd, &RtRdWd); MAT_SUB(&W, &RtRdWd, &eW); - /* calculate the inertia feedfoward term */ - //W x JW - MAT_MULT(&J, &W, &JW); - cross_product_3x1(mat_data(W), mat_data(JW), mat_data(WJW)); - mat_data(inertia_effect)[0] = mat_data(WJW)[0]; - mat_data(inertia_effect)[1] = mat_data(WJW)[1]; - mat_data(inertia_effect)[2] = mat_data(WJW)[2]; + /* moment feedforward control */ + float moment_ff[3] = {0.0}; + +#if (SELECT_FEEDFORWARD_TRACKING_MOMENT == FEEDFORWARD_TRACKING_MOMENT_USE_GEOMETRY) + moment_ff_ctrl_use_geometry(moment_ff); +#elif (SELECT_FEEDFORWARD_TRACKING_MOMENT == FEEDFORWARD_TRACKING_MOMENT_USE_ADAPTIVE_ICL) + moment_ff_ctrl_use_adaptive_ICL(moment_ff); +#endif + + mat_data(M_fb)[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0]; + mat_data(M_fb)[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1]; + mat_data(M_fb)[2] = -krz*mat_data(eR)[2] -kwz*mat_data(eW)[2]; + + /* save current moment for ICL */ + mat_data(curr_moment)[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] - moment_ff[0]; + mat_data(curr_moment)[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] - moment_ff[1]; + mat_data(curr_moment)[2] = -krz*mat_data(eR)[2] -kwz*mat_data(eW)[2] - moment_ff[2]; + + /* for debug messages */ + mat_data(inertia_effect)[0] = -moment_ff[0]; + mat_data(inertia_effect)[1] = -moment_ff[1]; + mat_data(inertia_effect)[2] = -moment_ff[2]; /* control input M1, M2, M3 */ output_moments[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] + mat_data(inertia_effect)[0]; diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_param.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_param.c index fe3e7e16..e08d0626 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_param.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_param.c @@ -26,6 +26,18 @@ void init_multirotor_geometry_param_list(void) init_sys_param_float(MR_GEO_GAIN_POS_X_I, "POS_I_GAIN_X", 0.0f); init_sys_param_float(MR_GEO_GAIN_POS_Y_I, "POS_I_GAIN_Y", 0.0f); init_sys_param_float(MR_GEO_GAIN_POS_Z_I, "POS_I_GAIN_Z", 0.0f); +#if 0 + init_sys_param_float(MR_ICL_GAIN_GAMMA_M, "GAMMA_M_GAIN", 0.1); + init_sys_param_float(MR_ICL_GAIN_K_CL_DIAG_X, "GAMMA_DIAG_GAIN_X", 0.1); + init_sys_param_float(MR_ICL_GAIN_K_CL_DIAG_Y, "GAMMA_DIAG_GAIN_Y", 0.1); + init_sys_param_float(MR_ICL_GAIN_K_CL_DIAG_Z, "GAMMA_DIAG_GAIN_Z", 0.1); + init_sys_param_float(MR_ICL_GAIN_C1, "C1_GAIN", 0.6); + init_sys_param_float(MR_ICL_GAIN_C2, "C2_GAIN", 0.01); + init_sys_param_float(MR_ICL_GAIN_K_CL_M, "K_CL_M", 0.002); + init_sys_param_float(MR_ICL_GAIN_K_CL_DIAG_X, "K_CL_GIAG_X", 0.02); + init_sys_param_float(MR_ICL_GAIN_K_CL_DIAG_Y, "K_CL_GIAG_Y", 0.0065); + init_sys_param_float(MR_ICL_GAIN_K_CL_DIAG_Z, "K_CL_GIAG_Z", 0.47); +#endif init_sys_param_float(MR_GEO_UAV_MASS, "UAV_MASS", 1.15f); init_sys_param_float(MR_GEO_INERTIA_JXX, "INERTIA_JXX", 0.01466f); init_sys_param_float(MR_GEO_INERTIA_JYY, "INERTIA_JYY", 0.01466f); diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_param.h b/src/core/controllers/multirotor_geometry/multirotor_geometry_param.h index 98b96fac..6761996c 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_param.h +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_param.h @@ -22,6 +22,18 @@ enum { MR_GEO_GAIN_POS_X_I, MR_GEO_GAIN_POS_Y_I, MR_GEO_GAIN_POS_Z_I, +#if 0 + MR_ICL_GAIN_GAMMA_M, + MR_ICL_GAIN_GAMMA_DIAG_X, + MR_ICL_GAIN_GAMMA_DIAG_Y, + MR_ICL_GAIN_GAMMA_DIAG_Z, + MR_ICL_GAIN_C1, + MR_ICL_GAIN_C2, + MR_ICL_GAIN_K_CL_M, + MR_ICL_GAIN_K_CL_DIAG_X, + MR_ICL_GAIN_K_CL_DIAG_Y, + MR_ICL_GAIN_K_CL_DIAG_Z, +#endif MR_GEO_UAV_MASS, MR_GEO_INERTIA_JXX, MR_GEO_INERTIA_JYY, diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c new file mode 100644 index 00000000..ffe44e33 --- /dev/null +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c @@ -0,0 +1,123 @@ +#include "send_debug_adaptive_ICL.h" + +void send_adaptive_ICL_mass_estimation_debug(debug_msg_t *payload) +{ + float theta_m_esti; + float theta_m_dot_esti; + float theta_m_dot_esti_adaptive; + float theta_m_dot_esti_ICL; + float current_time = get_sys_time_ms(); + float curr_pos[3] = {0.0f}; + + get_enu_position(curr_pos); + current_time = current_time*0.001; + theta_m_esti = mat_data(theta_m_hat)[0]; + theta_m_dot_esti = mat_data(theta_m_hat_dot)[0]; + theta_m_dot_esti_adaptive = mat_data(theta_m_hat_dot_adaptive)[0]; + theta_m_dot_esti_ICL = mat_data(theta_m_hat_dot_ICL)[0]; + + pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_MASS_ESTIMATION); + pack_debug_debug_message_float(¤t_time, payload); + pack_debug_debug_message_float(&curr_pos[2], payload); + pack_debug_debug_message_float(&autopilot.wp_now.pos[2], payload); + pack_debug_debug_message_float(&theta_m_esti, payload); + pack_debug_debug_message_float(&theta_m_dot_esti, payload); + pack_debug_debug_message_float(&theta_m_dot_esti_adaptive, payload); + pack_debug_debug_message_float(&theta_m_dot_esti_ICL, payload); +} + +void send_adaptive_ICL_inertia_estimation_debug(debug_msg_t *payload) +{ + float theta_diag_esti[3]; + float theta_diag_dot_esti[3]; + float theta_diag_dot_esti_adaptive[3]; + float theta_diag_dot_esti_ICL[3]; + float current_time = get_sys_time_ms(); + current_time = current_time*0.001; + + theta_diag_esti[0] = mat_data(theta_diag_hat)[0]; + theta_diag_esti[1] = mat_data(theta_diag_hat)[1]; + theta_diag_esti[2] = mat_data(theta_diag_hat)[2]; + theta_diag_dot_esti[0] = mat_data(theta_diag_hat_dot)[0]; + theta_diag_dot_esti[1] = mat_data(theta_diag_hat_dot)[1]; + theta_diag_dot_esti[2] = mat_data(theta_diag_hat_dot)[2]; + theta_diag_dot_esti_adaptive[0] = mat_data(theta_diag_hat_dot_adaptive)[0]; + theta_diag_dot_esti_adaptive[1] = mat_data(theta_diag_hat_dot_adaptive)[1]; + theta_diag_dot_esti_adaptive[2] = mat_data(theta_diag_hat_dot_adaptive)[2]; + theta_diag_dot_esti_ICL[0] = mat_data(theta_diag_hat_dot_ICL)[0]; + theta_diag_dot_esti_ICL[1] = mat_data(theta_diag_hat_dot_ICL)[1]; + theta_diag_dot_esti_ICL[2] = mat_data(theta_diag_hat_dot_ICL)[2]; + + pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_INERTIA_ESTIMATION); + pack_debug_debug_message_float(¤t_time, payload); + pack_debug_debug_message_float(&theta_diag_esti[0], payload); + pack_debug_debug_message_float(&theta_diag_esti[1], payload); + pack_debug_debug_message_float(&theta_diag_esti[2], payload); + pack_debug_debug_message_float(&theta_diag_dot_esti[0], payload); + pack_debug_debug_message_float(&theta_diag_dot_esti[1], payload); + pack_debug_debug_message_float(&theta_diag_dot_esti[2], payload); + pack_debug_debug_message_float(&theta_diag_dot_esti_adaptive[0], payload); + pack_debug_debug_message_float(&theta_diag_dot_esti_adaptive[1], payload); + pack_debug_debug_message_float(&theta_diag_dot_esti_adaptive[2], payload); + pack_debug_debug_message_float(&theta_diag_dot_esti_ICL[0], payload); + pack_debug_debug_message_float(&theta_diag_dot_esti_ICL[1], payload); + pack_debug_debug_message_float(&theta_diag_dot_esti_ICL[2], payload); +} + +void send_adaptive_ICL_mass_inertia_estimation_debug(debug_msg_t *payload) +{ + float theta_m_esti; + float theta_diag_esti[3]; + float current_time = get_sys_time_ms(); + current_time = current_time*0.001; + + theta_m_esti = mat_data(theta_m_hat)[0]; + theta_diag_esti[0] = mat_data(theta_diag_hat)[0]; + theta_diag_esti[1] = mat_data(theta_diag_hat)[1]; + theta_diag_esti[2] = mat_data(theta_diag_hat)[2]; + + pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_MASS_INERTIA_ESTIMATION); + pack_debug_debug_message_float(¤t_time, payload); + pack_debug_debug_message_float(&theta_m_esti, payload); + pack_debug_debug_message_float(&theta_diag_esti[0], payload); + pack_debug_debug_message_float(&theta_diag_esti[1], payload); + pack_debug_debug_message_float(&theta_diag_esti[2], payload); +} + +void send_adaptive_ICL_moment_ctrl_input_debug(debug_msg_t *payload) +{ + float moment_ctrl[3]; + float moment_ctrl_feedback[3]; + float moment_ctrl_feedforward[3]; + float theta_diag_esti[3]; + float current_time = get_sys_time_ms(); + current_time = current_time*0.001; + + moment_ctrl[0] = mat_data(curr_moment)[0]; + moment_ctrl[1] = mat_data(curr_moment)[1]; + moment_ctrl[2] = mat_data(curr_moment)[2]; + moment_ctrl_feedback[0] = mat_data(M_fb)[0]; + moment_ctrl_feedback[1] = mat_data(M_fb)[1]; + moment_ctrl_feedback[2] = mat_data(M_fb)[2]; + moment_ctrl_feedforward[0] = mat_data(inertia_effect)[0]; + moment_ctrl_feedforward[1] = mat_data(inertia_effect)[1]; + moment_ctrl_feedforward[2] = mat_data(inertia_effect)[2]; + theta_diag_esti[0] = mat_data(theta_diag_hat)[0]; + theta_diag_esti[1] = mat_data(theta_diag_hat)[1]; + theta_diag_esti[2] = mat_data(theta_diag_hat)[2]; + + pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_MOMENT_CTRL); + pack_debug_debug_message_float(¤t_time, payload); + pack_debug_debug_message_float(&moment_ctrl[0], payload); + pack_debug_debug_message_float(&moment_ctrl[1], payload); + pack_debug_debug_message_float(&moment_ctrl[2], payload); + pack_debug_debug_message_float(&moment_ctrl_feedback[0], payload); + pack_debug_debug_message_float(&moment_ctrl_feedback[1], payload); + pack_debug_debug_message_float(&moment_ctrl_feedback[2], payload); + pack_debug_debug_message_float(&moment_ctrl_feedforward[0], payload); + pack_debug_debug_message_float(&moment_ctrl_feedforward[1], payload); + pack_debug_debug_message_float(&moment_ctrl_feedforward[2], payload); + pack_debug_debug_message_float(&theta_diag_esti[0], payload); + pack_debug_debug_message_float(&theta_diag_esti[1], payload); + pack_debug_debug_message_float(&theta_diag_esti[2], payload); +} diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h new file mode 100644 index 00000000..9fe72f3e --- /dev/null +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h @@ -0,0 +1,30 @@ +#ifndef __SEND_DEBUG_ADAPTIVE_ICL_H__ +#define __SEND_DEBUG_ADAPTIVE_ICL_H__ + +#include "arm_math.h" +#include "matrix.h" +#include "debug_link.h" +#include "autopilot.h" +#include "position_state.h" +#include + +extern arm_matrix_instance_f32 theta_m_hat; +extern arm_matrix_instance_f32 theta_m_hat_dot; +extern arm_matrix_instance_f32 theta_m_hat_dot_adaptive; +extern arm_matrix_instance_f32 theta_m_hat_dot_ICL; +extern arm_matrix_instance_f32 theta_diag_hat; +extern arm_matrix_instance_f32 theta_diag_hat_dot; +extern arm_matrix_instance_f32 theta_diag_hat_dot_adaptive; +extern arm_matrix_instance_f32 theta_diag_hat_dot_ICL; +extern arm_matrix_instance_f32 curr_moment; +extern arm_matrix_instance_f32 M_fb; +extern arm_matrix_instance_f32 M_ff; +extern arm_matrix_instance_f32 inertia_effect; +extern autopilot_t autopilot; + +void send_adaptive_ICL_mass_estimation_debug(debug_msg_t *payload); +void send_adaptive_ICL_inertia_estimation_debug(debug_msg_t *payload); +void send_adaptive_ICL_mass_inertia_estimation_debug(debug_msg_t *payload); +void send_adaptive_ICL_moment_ctrl_input_debug(debug_msg_t *payload); + +#endif diff --git a/src/core/debug_link/debug_link.h b/src/core/debug_link/debug_link.h index 75403772..8167ea41 100644 --- a/src/core/debug_link/debug_link.h +++ b/src/core/debug_link/debug_link.h @@ -34,7 +34,11 @@ enum { MESSAGE_ID_INS_ESKF1_COVARIANCE = 22, MESSAGE_ID_VINS_MONO_POSITION = 30, MESSAGE_ID_VINS_MONO_QUATERNION = 31, - MESSAGE_ID_VINS_MONO_VELOCITY = 32 + MESSAGE_ID_VINS_MONO_VELOCITY = 32, + MESSAGE_ID_ICL_MASS_ESTIMATION = 41, + MESSAGE_ID_ICL_INERTIA_ESTIMATION = 42, + MESSAGE_ID_ICL_MASS_INERTIA_ESTIMATION = 43, + MESSAGE_ID_ICL_MOMENT_CTRL = 44 } MESSAGE_ID; typedef struct { diff --git a/src/core/tasks/debug_link_task.c b/src/core/tasks/debug_link_task.c index fb26597f..6834cbe7 100644 --- a/src/core/tasks/debug_link_task.c +++ b/src/core/tasks/debug_link_task.c @@ -10,6 +10,7 @@ #include "flight_ctrl_task.h" #include "ms5611.h" #include "debug_msg.h" +#include "send_debug_adaptive_ICL.h" #include "compass.h" #include "ins_eskf.h" @@ -31,7 +32,7 @@ void task_debug_link(void *param) while(1) { //while(xSemaphoreTake(debug_link_task_semphr, portMAX_DELAY) != pdTRUE); //send_imu_debug_message(&payload); - send_attitude_euler_debug_message(&payload); + //send_attitude_euler_debug_message(&payload); //send_attitude_quaternion_debug_message(&payload); //send_attitude_imu_debug_message(&payload); //send_pid_debug_message(&payload); @@ -46,6 +47,10 @@ void task_debug_link(void *param) //send_barometer_debug_message(&payload); //send_alt_est_debug_message(&payload); //send_ins_sensor_debug_message(&payload); + //send_adaptive_ICL_mass_estimation_debug(&payload); + //send_adaptive_ICL_inertia_estimation_debug(&payload); + send_adaptive_ICL_mass_inertia_estimation_debug(&payload); + //send_adaptive_ICL_moment_ctrl_input_debug(&payload); //send_ins_raw_position_debug_message(&payload); //send_ins_fusion_debug_message(&payload); //send_ahrs_compass_quality_check_debug_message(&payload); diff --git a/src/proj_config.h b/src/proj_config.h index f8c53629..ec337ae5 100644 --- a/src/proj_config.h +++ b/src/proj_config.h @@ -47,6 +47,31 @@ #define QUADROTOR_USE_GEOMETRY 1 #define SELECT_CONTROLLER QUADROTOR_USE_GEOMETRY +/* feedforward control for manual control */ +#define FEEDFORWARD_MANUAL_USE_GEOMETRY 0 +#define FEEDFORWARD_MANUAL_USE_ADAPTIVE_ICL 1 +#define SELECT_FEEDFORWARD_MANUAL FEEDFORWARD_MANUAL_USE_GEOMETRY + +/* force feedforward control for tracking control */ +#define FEEDFORWARD_TRACKING_FORCE_USE_GEOMETRY 0 +#define FEEDFORWARD_TRACKING_FORCE_USE_ADAPTIVE_ICL 1 +#define SELECT_FEEDFORWARD_TRACKING_FORCE FEEDFORWARD_TRACKING_FORCE_USE_GEOMETRY + +/* moment feedforward control for tracking control */ +#define FEEDFORWARD_TRACKING_MOMENT_USE_GEOMETRY 0 +#define FEEDFORWARD_TRACKING_MOMENT_USE_ADAPTIVE_ICL 1 +#define SELECT_FEEDFORWARD_TRACKING_MOMENT FEEDFORWARD_TRACKING_MOMENT_USE_GEOMETRY + +/* force integral concurrent learning */ +#define FORCE_ADAPTIVE_WITHOUT_ICL 0 +#define FORCE_ADAPTIVE_WITH_ICL 1 +#define SELECT_FORCE_ADAPTIVE_W_WO_ICL FORCE_ADAPTIVE_WITHOUT_ICL + +/* moment integral concurrent learning */ +#define MOMENT_ADAPTIVE_WITHOUT_ICL 0 +#define MOMENT_ADAPTIVE_WITH_ICL 1 +#define SELECT_MOMENT_ADAPTIVE_W_WO_ICL MOMENT_ADAPTIVE_WITHOUT_ICL + /*===================* * hardware settings * *===================*/ diff --git a/tools/serial_plot.py b/tools/serial_plot.py index d727decd..4d10ee1a 100644 --- a/tools/serial_plot.py +++ b/tools/serial_plot.py @@ -17,7 +17,7 @@ bytesize=serial.EIGHTBITS,\ timeout=100) -save_csv = False +save_csv = True csv_file = 'serial_log.csv' if save_csv == True: @@ -439,7 +439,7 @@ def set_figure(self, message_id): self.create_curve('py', 'blue') self.create_curve('pz', 'green') self.show_subplot() - + elif (message_id == 20): plt.subplot(421) plt.ylabel('time [ms]') @@ -474,7 +474,7 @@ def set_figure(self, message_id): self.create_curve('py', 'blue') self.create_curve('pz', 'green') self.show_subplot() - + plt.subplot(426) plt.ylabel('gps velocity [m/s]') plt.ylim([-5.0, 5.0]) @@ -492,55 +492,205 @@ def set_figure(self, message_id): self.show_subplot() elif (message_id == 21): - plt.subplot(511) - plt.ylabel('mag [uT]') - plt.ylim([-100, 100]) - self.create_curve('mx', 'red') - self.create_curve('my', 'blue') - self.create_curve('mz', 'green') - self.show_subplot() - - plt.subplot(512) - plt.ylabel('mag strength [uT]') - plt.ylim([0, 100]) - self.create_curve('mx', 'red') - self.show_subplot() - - plt.subplot(513) - plt.ylabel('update freq') - plt.ylim([0, 100]) - self.create_curve('mx', 'red') - self.show_subplot() - - plt.subplot(514) - plt.ylabel('quality') - plt.ylim([-1.2, 1.2]) - self.create_curve('1: good 0: bad', 'red') - self.show_subplot() - - plt.subplot(515) - plt.ylabel('deg') - plt.ylim([-200, 200]) - self.create_curve('compass yaw', 'red') - self.create_curve('ahrs yaw', 'blue') - self.show_subplot() + plt.subplot(511) + plt.ylabel('mag [uT]') + plt.ylim([-100, 100]) + self.create_curve('mx', 'red') + self.create_curve('my', 'blue') + self.create_curve('mz', 'green') + self.show_subplot() + + plt.subplot(512) + plt.ylabel('mag strength [uT]') + plt.ylim([0, 100]) + self.create_curve('mx', 'red') + self.show_subplot() + + plt.subplot(513) + plt.ylabel('update freq') + plt.ylim([0, 100]) + self.create_curve('mx', 'red') + self.show_subplot() + + plt.subplot(514) + plt.ylabel('quality') + plt.ylim([-1.2, 1.2]) + self.create_curve('1: good 0: bad', 'red') + self.show_subplot() + + plt.subplot(515) + plt.ylabel('deg') + plt.ylim([-200, 200]) + self.create_curve('compass yaw', 'red') + self.create_curve('ahrs yaw', 'blue') + self.show_subplot() elif (message_id == 22): - plt.subplot(111) - plt.ylabel('eskf1 P matrix') - plt.ylim([-100, 100]) - self.create_curve('P00', 'blue') - self.create_curve('P11', 'orange') - self.create_curve('P22', 'green') - self.create_curve('P33', 'red') - self.create_curve('P44', 'purple') - self.create_curve('P55', 'brown') - self.create_curve('P66', 'pink') - self.create_curve('P77', 'gray') - self.create_curve('P88', 'cyan') - self.show_subplot() - - + plt.subplot(111) + plt.ylabel('eskf1 P matrix') + plt.ylim([-100, 100]) + self.create_curve('P00', 'blue') + self.create_curve('P11', 'orange') + self.create_curve('P22', 'green') + self.create_curve('P33', 'red') + self.create_curve('P44', 'purple') + self.create_curve('P55', 'brown') + self.create_curve('P66', 'pink') + self.create_curve('P77', 'gray') + self.create_curve('P88', 'cyan') + self.show_subplot() + + elif (message_id == 41): + plt.subplot(711) + plt.ylabel('Time [s]') + plt.ylim([0.0, 100.0]) + self.create_curve('Time', 'red') + self.show_subplot() + + plt.subplot(712) + plt.ylabel('mass [kg]') + plt.ylim([-2, 2]) + self.create_curve('mass', 'red') + self.show_subplot() + + plt.subplot(713) + plt.ylabel('z enu [m]') + plt.ylim([-2, 2]) + self.create_curve('z enu', 'red') + self.show_subplot() + + plt.subplot(714) + plt.ylabel('m_hat [kg]') + plt.ylim([-4, 4]) + self.create_curve('m_hat', 'red') + self.show_subplot() + + plt.subplot(715) + plt.ylabel('m_hat_dot [kg/s]') + plt.ylim([-1, 1]) + self.create_curve('m_hat_dot', 'red') + self.show_subplot() + + plt.subplot(716) + plt.ylabel('adaptive [kg/s]') + plt.ylim([-1, 1]) + self.create_curve('adaptive', 'red') + self.show_subplot() + + plt.subplot(717) + plt.ylabel('ICL [kg/s]') + plt.ylim([-1, 1]) + self.create_curve('ICL', 'red') + self.show_subplot() + + elif (message_id == 42): + plt.subplot(511) + plt.ylabel('Time [s]') + plt.ylim([0.0, 100.0]) + self.create_curve('Time', 'red') + self.show_subplot() + + plt.subplot(512) + plt.ylabel('J_hat [kg*m^2]') + plt.ylim([-0.1, 0.1]) + self.create_curve('J_xx_hat', 'red') + self.create_curve('J_yy_hat', 'blue') + self.create_curve('J_zz_hat', 'green') + self.show_subplot() + + plt.subplot(513) + plt.ylabel('hat_dot [kg*m^2/s]') + plt.ylim([-0.1, 0.1]) + self.create_curve('J_xx_hat_dot', 'red') + self.create_curve('J_yy_hat_dot', 'blue') + self.create_curve('J_zz_hat_dot', 'green') + self.show_subplot() + + plt.subplot(514) + plt.ylabel('adaptive') + plt.ylim([-0.1, 0.1]) + self.create_curve('x', 'red') + self.create_curve('y', 'blue') + self.create_curve('z', 'green') + self.show_subplot() + + plt.subplot(515) + plt.ylabel('ICL') + plt.ylim([-0.1, 0.1]) + self.create_curve('x', 'red') + self.create_curve('y', 'blue') + self.create_curve('z', 'green') + self.show_subplot() + + elif (message_id == 43): + plt.subplot(511) + plt.ylabel('Time [s]') + plt.ylim([0.0, 100.0]) + self.create_curve('Time', 'red') + self.show_subplot() + + plt.subplot(512) + plt.ylabel('mass [kg]') + plt.ylim([-2, 2]) + self.create_curve('mass', 'red') + self.show_subplot() + + plt.subplot(513) + plt.ylabel('Jxx [kg*m^2/s]') + plt.ylim([-0.1, 0.1]) + self.create_curve('J_xx', 'red') + self.show_subplot() + + plt.subplot(514) + plt.ylabel('Jyy [kg*m^2/s]') + plt.ylim([-0.1, 0.1]) + self.create_curve('Jyy', 'red') + self.show_subplot() + + plt.subplot(515) + plt.ylabel('Jzz [kg*m^2/s]') + plt.ylim([-0.1, 0.1]) + self.create_curve('Jzz', 'red') + self.show_subplot() + + elif (message_id == 44): + plt.subplot(511) + plt.ylabel('Time [s]') + plt.ylim([0.0, 500.0]) + self.create_curve('Time', 'red') + self.show_subplot() + + plt.subplot(512) + plt.ylabel('M [N*m]') + plt.ylim([-4.0, 4.0]) + self.create_curve('Mx', 'red') + self.create_curve('My', 'blue') + self.create_curve('Mz', 'green') + self.show_subplot() + + plt.subplot(513) + plt.ylabel('M_fb [N*m]') + plt.ylim([-4.0, 4.0]) + self.create_curve('M_fb_x', 'red') + self.create_curve('M_fb_y', 'blue') + self.create_curve('M_fb_z', 'green') + self.show_subplot() + + plt.subplot(514) + plt.ylabel('M_ff [N*m]') + plt.ylim([-0.06, 0.06]) + self.create_curve('M_ff_x', 'red') + self.create_curve('M_ff_y', 'blue') + self.create_curve('M_ff_z', 'green') + self.show_subplot() + + plt.subplot(515) + plt.ylabel('J_hat [kg*m^2/s]') + plt.ylim([-0.1, 0.1]) + self.create_curve('Jxx', 'red') + self.create_curve('Jyy', 'blue') + self.create_curve('Jzz', 'green') + self.show_subplot() def show_graph(self): ani = animation.FuncAnimation(self.figure, self.animate, np.arange(0, 200), \