From 045b300cab0fd71cd9c5d0829d7fbc242bd52d6d Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Fri, 16 Oct 2020 15:32:08 +0800 Subject: [PATCH 01/62] define feedforward usage --- src/proj_config.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/proj_config.h b/src/proj_config.h index b32ccafe..152d869a 100644 --- a/src/proj_config.h +++ b/src/proj_config.h @@ -24,6 +24,11 @@ #define QUADROTOR_USE_GEOMETRY 1 #define SELECT_CONTROLLER QUADROTOR_USE_GEOMETRY +/* feedforward control */ +#define FEEDFORWARD_USE_GEOMETRY 0 +#define FEEDFORWARD_USE_ICL 1 +#define SELECT_FEEDFORWARD FEEDFORWARD_USE_GEOMETRY + /* heading sensor */ #define NO_HEADING_SENSOR 0 #define HEADING_SENSOR_USE_COMPASS 1 From fca99b17d9990cf51f2c47c1cd9c6753822068f9 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Fri, 16 Oct 2020 16:12:38 +0800 Subject: [PATCH 02/62] multirotor_geometry: add adaptive control term for mass and moment of inertia estimation --- .../multirotor_geometry_ctrl.c | 165 +++++++++++++++++- .../multirotor_geometry_param.c | 12 ++ .../multirotor_geometry_param.h | 12 ++ 3 files changed, 182 insertions(+), 7 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 850ca4d7..b68871a4 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -22,6 +22,7 @@ #include "altitude_est.h" #include "compass.h" #include "sys_param.h" +#include "proj_config.h" #define dt 0.0025 //[s] #define MOTOR_TO_CG_LENGTH 16.25f //[cm] @@ -60,6 +61,24 @@ 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, 3, 1); +MAT_ALLOC(Y_m_clt, 1, 3); +MAT_ALLOC(Y_diag, 3, 3); +MAT_ALLOC(Y_diagt, 3, 3); +MAT_ALLOC(Y_diag_cl, 3, 3); +MAT_ALLOC(Y_diag_clt, 3, 3); +MAT_ALLOC(theta_m_hat, 1, 1); +MAT_ALLOC(theta_m_hat_dot, 1, 1); +MAT_ALLOC(theta_diag_hat, 3, 1); +MAT_ALLOC(theta_diag_hat_dot, 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(tran_ff, 3, 1); +MAT_ALLOC(rota_ff, 3, 1); float pos_error[3]; float vel_error[3]; @@ -72,6 +91,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[3]; +float k_cl_diag_gain[3]; + float uav_mass; //M = (J * W_dot) + (W X JW) @@ -128,6 +154,24 @@ 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, 3, 1); + MAT_INIT(Y_m_clt, 1, 3); + MAT_INIT(Y_diag, 3, 3); + MAT_INIT(Y_diagt, 3, 3); + MAT_INIT(Y_diag_cl, 3, 3); + MAT_INIT(Y_diag_clt, 3, 3); + MAT_INIT(theta_m_hat, 1, 1); + MAT_INIT(theta_m_hat_dot, 1, 1); + MAT_INIT(theta_diag_hat, 3, 1); + MAT_INIT(theta_diag_hat_dot, 3, 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(tran_ff, 3, 1); + MAT_INIT(rota_ff, 3, 1); /* modify local variables when user change them via ground station */ set_sys_param_update_var_addr(MR_GEO_GAIN_ROLL_P, &krx); @@ -146,6 +190,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]); + 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_X, &k_cl_m_gain[0]); + set_sys_param_update_var_addr(MR_ICL_GAIN_K_CL_M_Y, &k_cl_m_gain[1]); + set_sys_param_update_var_addr(MR_ICL_GAIN_K_CL_M_Z, &k_cl_m_gain[2]); + 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]); 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]); @@ -181,6 +237,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]); + 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_X, &k_cl_m_gain[0]); + get_sys_param_float(MR_ICL_GAIN_K_CL_M_Y, &k_cl_m_gain[1]); + get_sys_param_float(MR_ICL_GAIN_K_CL_M_Z, &k_cl_m_gain[2]); + 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]); 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]); @@ -342,13 +410,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_eun_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; @@ -357,6 +418,15 @@ 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); + float accel_ff_ned[3] = {0.0f}; + assign_vector_3x1_eun_to_ned(accel_ff_ned, autopilot.wp_now.acc_feedforward); + +#if (SELECT_FEEDFORWARD == FEEDFORWARD_USE_GEOMETRY) + float force_ff_ned[3] = {0.0f}; + 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]; + 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] + @@ -364,6 +434,39 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * 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; +#elif (SELECT_FEEDFORWARD == FEEDFORWARD_USE_ICL) + /* Y_m and Y_m transpose */ + mat_data(Y_m)[0] = accel_ff_ned[0]; + mat_data(Y_m)[1] = accel_ff_ned[1]; + mat_data(Y_m)[2] = accel_ff_ned[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_error[0] + C1_gain*pos_error[0]; + mat_data(ev_C1ex)[1] = vel_error[1] + C1_gain*pos_error[1]; + mat_data(ev_C1ex)[2] = vel_error[2] + C1_gain*pos_error[2]; + + /* theta_m update law */ + //theta_m_dot = Gamma*Y_mt*ev_C1ex + MAT_MULT(&Y_mt, &ev_C1ex, &Ymt_evC1ex); + mat_data(theta_m_hat_dot)[0] = Gamma_m_gain*mat_data(Ymt_evC1ex)[0]; + mat_data(theta_m_hat)[0] += mat_data(theta_m_hat_dot)[0] * dt; + + /* translational adaptive feedforward term */ + //Y_m*theta_m_hat + mat_data(tran_ff)[0] = mat_data(Y_m)[0]*mat_data(theta_m_hat)[0]; + mat_data(tran_ff)[1] = mat_data(Y_m)[1]*mat_data(theta_m_hat)[0]; + mat_data(tran_ff)[2] = mat_data(Y_m)[2]*mat_data(theta_m_hat)[0]; + + mat_data(kxex_kvev_mge3_mxd_dot_dot)[0] = -kpx*pos_error[0] - kvx*vel_error[0] + - tracking_error_integral[0] + mat_data(tran_ff)[0]; + mat_data(kxex_kvev_mge3_mxd_dot_dot)[1] = -kpy*pos_error[1] - kvy*vel_error[1] + - tracking_error_integral[1] + mat_data(tran_ff)[1]; + mat_data(kxex_kvev_mge3_mxd_dot_dot)[2] = -kpz*pos_error[2] - kvz*vel_error[2] + - tracking_error_integral[2] + mat_data(tran_ff)[2]; +#endif /* calculate the denominator of b3d */ float b3d_denominator; //caution: this term should not be 0 @@ -451,6 +554,7 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * MAT_MULT(&RtRd, &Wd, &RtRdWd); MAT_SUB(&W, &RtRdWd, &eW); +#if (SELECT_FEEDFORWARD == FEEDFORWARD_USE_GEOMETRY) /* calculate the inertia feedfoward term */ //W x JW MAT_MULT(&J, &W, &JW); @@ -463,6 +567,53 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * 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]; output_moments[2] = -krz*mat_data(eR)[2] -kwz*mat_data(eW)[2] + mat_data(inertia_effect)[2]; +#elif (SELECT_FEEDFORWARD == FEEDFORWARD_USE_ICL) + /* 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]; + + /* theta_diag update law */ + //theta_diag_dot = Gamma*Y_diagt*eW_C2eR + MAT_MULT(&Y_diagt, &eW_C2eR, &Ydiagt_eWC2eR); + 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]; + mat_data(theta_diag_hat)[0] += mat_data(theta_diag_hat_dot)[0] * dt; + + /* rotational adaptive feedforward term */ + //Y_diag*theta_diag_hat + mat_data(rota_ff)[0] = Gamma_diag_gain[0]*mat_data(theta_diag_hat)[0]; + mat_data(rota_ff)[1] = Gamma_diag_gain[1]*mat_data(theta_diag_hat)[1]; + mat_data(rota_ff)[2] = Gamma_diag_gain[2]*mat_data(theta_diag_hat)[2]; + + /* control input M1, M2, M3 */ + output_moments[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] + mat_data(rota_ff)[0]; + output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] + mat_data(rota_ff)[1]; + output_moments[2] = -krz*mat_data(eR)[2] -kwz*mat_data(eW)[2] + mat_data(rota_ff)[2]; +#endif } #define l_div_4 (0.25f * (1.0f / MOTOR_TO_CG_LENGTH_M)) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_param.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_param.c index 496f8d82..f7d94dab 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); + 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_X, "K_CL_M_X", 0.002); + init_sys_param_float(MR_ICL_GAIN_K_CL_M_Y, "K_CL_M_Y", 0.002); + init_sys_param_float(MR_ICL_GAIN_K_CL_M_Z, "K_CL_M_Z", 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); 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..fe4773e7 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, + 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_X, + MR_ICL_GAIN_K_CL_M_Y, + MR_ICL_GAIN_K_CL_M_Z, + MR_ICL_GAIN_K_CL_DIAG_X, + MR_ICL_GAIN_K_CL_DIAG_Y, + MR_ICL_GAIN_K_CL_DIAG_Z, MR_GEO_UAV_MASS, MR_GEO_INERTIA_JXX, MR_GEO_INERTIA_JYY, From 334bb4ada3e41180a1b44a6886cc80f6ba0997e2 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 19 Oct 2020 11:27:05 +0800 Subject: [PATCH 03/62] multirotor_geometry: rearrange feedforward term into functions --- .../multirotor_geometry_ctrl.c | 208 +++++++++--------- 1 file changed, 108 insertions(+), 100 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index b68871a4..523b60a3 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -77,8 +77,6 @@ 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(tran_ff, 3, 1); -MAT_ALLOC(rota_ff, 3, 1); float pos_error[3]; float vel_error[3]; @@ -170,8 +168,6 @@ void geometry_ctrl_init(void) MAT_INIT(eW_C2eR, 3, 1); MAT_INIT(Ymt_evC1ex, 1, 1); MAT_INIT(Ydiagt_eWC2eR, 3, 1); - MAT_INIT(tran_ff, 3, 1); - MAT_INIT(rota_ff, 3, 1); /* modify local variables when user change them via ground station */ set_sys_param_update_var_addr(MR_GEO_GAIN_ROLL_P, &krx); @@ -392,6 +388,96 @@ void geometry_manual_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *ou output_moments[2] = -_krz*mat_data(eR)[2] -_kwz*mat_data(eW)[2] + mat_data(inertia_effect)[2]; } +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_ICL(float *accel_ff, float *force_ff, float *pos_err, float *vel_err){ + /* 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]; + + /* theta_m update law */ + //theta_m_dot = Gamma*Y_mt*ev_C1ex + MAT_MULT(&Y_mt, &ev_C1ex, &Ymt_evC1ex); + mat_data(theta_m_hat_dot)[0] = Gamma_m_gain*mat_data(Ymt_evC1ex)[0]; + 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]; +} + +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_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]; + + /* theta_diag update law */ + //theta_diag_dot = Gamma*Y_diagt*eW_C2eR + MAT_MULT(&Y_diagt, &eW_C2eR, &Ydiagt_eWC2eR); + 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]; + mat_data(theta_diag_hat)[0] += mat_data(theta_diag_hat_dot)[0] * dt; + + /* rotational adaptive feedforward term */ + //Y_diag*theta_diag_hat + mom_ff[0] = Gamma_diag_gain[0]*mat_data(theta_diag_hat)[0]; + mom_ff[1] = Gamma_diag_gain[1]*mat_data(theta_diag_hat)[1]; + mom_ff[2] = Gamma_diag_gain[2]*mat_data(theta_diag_hat)[2]; +} + void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *curr_pos_ned, float *curr_vel_ned, float *output_moments, float *output_force, bool manual_flight) @@ -418,55 +504,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); + /* force feedforward control */ float accel_ff_ned[3] = {0.0f}; + float force_ff_ned[3] = {0.0f}; assign_vector_3x1_eun_to_ned(accel_ff_ned, autopilot.wp_now.acc_feedforward); #if (SELECT_FEEDFORWARD == FEEDFORWARD_USE_GEOMETRY) - float force_ff_ned[3] = {0.0f}; - 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]; - - 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_ff_ctrl_use_geometry(accel_ff_ned, accel_ff_ned); #elif (SELECT_FEEDFORWARD == FEEDFORWARD_USE_ICL) - /* Y_m and Y_m transpose */ - mat_data(Y_m)[0] = accel_ff_ned[0]; - mat_data(Y_m)[1] = accel_ff_ned[1]; - mat_data(Y_m)[2] = accel_ff_ned[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_error[0] + C1_gain*pos_error[0]; - mat_data(ev_C1ex)[1] = vel_error[1] + C1_gain*pos_error[1]; - mat_data(ev_C1ex)[2] = vel_error[2] + C1_gain*pos_error[2]; - - /* theta_m update law */ - //theta_m_dot = Gamma*Y_mt*ev_C1ex - MAT_MULT(&Y_mt, &ev_C1ex, &Ymt_evC1ex); - mat_data(theta_m_hat_dot)[0] = Gamma_m_gain*mat_data(Ymt_evC1ex)[0]; - mat_data(theta_m_hat)[0] += mat_data(theta_m_hat_dot)[0] * dt; - - /* translational adaptive feedforward term */ - //Y_m*theta_m_hat - mat_data(tran_ff)[0] = mat_data(Y_m)[0]*mat_data(theta_m_hat)[0]; - mat_data(tran_ff)[1] = mat_data(Y_m)[1]*mat_data(theta_m_hat)[0]; - mat_data(tran_ff)[2] = mat_data(Y_m)[2]*mat_data(theta_m_hat)[0]; + force_ff_ctrl_use_ICL(accel_ff_ned, force_ff_ned, pos_error, vel_error); +#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] + mat_data(tran_ff)[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] + mat_data(tran_ff)[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] + mat_data(tran_ff)[2]; -#endif + - tracking_error_integral[2] + force_ff_ned[2]; /* calculate the denominator of b3d */ float b3d_denominator; //caution: this term should not be 0 @@ -554,66 +609,19 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * MAT_MULT(&RtRd, &Wd, &RtRdWd); MAT_SUB(&W, &RtRdWd, &eW); -#if (SELECT_FEEDFORWARD == FEEDFORWARD_USE_GEOMETRY) - /* 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}; - /* 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]; - output_moments[2] = -krz*mat_data(eR)[2] -kwz*mat_data(eW)[2] + mat_data(inertia_effect)[2]; +#if (SELECT_FEEDFORWARD == FEEDFORWARD_USE_GEOMETRY) + moment_ff_ctrl_use_geometry(moment_ff); #elif (SELECT_FEEDFORWARD == FEEDFORWARD_USE_ICL) - /* 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]; - - /* theta_diag update law */ - //theta_diag_dot = Gamma*Y_diagt*eW_C2eR - MAT_MULT(&Y_diagt, &eW_C2eR, &Ydiagt_eWC2eR); - 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]; - mat_data(theta_diag_hat)[0] += mat_data(theta_diag_hat_dot)[0] * dt; - - /* rotational adaptive feedforward term */ - //Y_diag*theta_diag_hat - mat_data(rota_ff)[0] = Gamma_diag_gain[0]*mat_data(theta_diag_hat)[0]; - mat_data(rota_ff)[1] = Gamma_diag_gain[1]*mat_data(theta_diag_hat)[1]; - mat_data(rota_ff)[2] = Gamma_diag_gain[2]*mat_data(theta_diag_hat)[2]; + moment_ff_ctrl_use_ICL(moment_ff); +#endif /* control input M1, M2, M3 */ - output_moments[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] + mat_data(rota_ff)[0]; - output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] + mat_data(rota_ff)[1]; - output_moments[2] = -krz*mat_data(eR)[2] -kwz*mat_data(eW)[2] + mat_data(rota_ff)[2]; -#endif + output_moments[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] + moment_ff[0]; + output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] + moment_ff[1]; + output_moments[2] = -krz*mat_data(eR)[2] -kwz*mat_data(eW)[2] + moment_ff[2]; } #define l_div_4 (0.25f * (1.0f / MOTOR_TO_CG_LENGTH_M)) From 816ec247e667d1cc54eca41c57782d1039fb77d8 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 19 Oct 2020 18:08:01 +0800 Subject: [PATCH 04/62] rename FEEDFORWARD_USE_ICL to FEEDFORWARD_USE_ADAPTIVE_ICL and add SELECT_ADAPTIVE_W_WO_ICL --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 4 ++-- src/proj_config.h | 7 ++++++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 523b60a3..e053fed0 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -511,7 +511,7 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * #if (SELECT_FEEDFORWARD == FEEDFORWARD_USE_GEOMETRY) force_ff_ctrl_use_geometry(accel_ff_ned, accel_ff_ned); -#elif (SELECT_FEEDFORWARD == FEEDFORWARD_USE_ICL) +#elif (SELECT_FEEDFORWARD == FEEDFORWARD_USE_ADAPTIVE_ICL) force_ff_ctrl_use_ICL(accel_ff_ned, force_ff_ned, pos_error, vel_error); #endif @@ -614,7 +614,7 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * #if (SELECT_FEEDFORWARD == FEEDFORWARD_USE_GEOMETRY) moment_ff_ctrl_use_geometry(moment_ff); -#elif (SELECT_FEEDFORWARD == FEEDFORWARD_USE_ICL) +#elif (SELECT_FEEDFORWARD == FEEDFORWARD_USE_ADAPTIVE_ICL) moment_ff_ctrl_use_ICL(moment_ff); #endif diff --git a/src/proj_config.h b/src/proj_config.h index 152d869a..731d6da9 100644 --- a/src/proj_config.h +++ b/src/proj_config.h @@ -26,9 +26,14 @@ /* feedforward control */ #define FEEDFORWARD_USE_GEOMETRY 0 -#define FEEDFORWARD_USE_ICL 1 +#define FEEDFORWARD_USE_ADAPTIVE_ICL 1 #define SELECT_FEEDFORWARD FEEDFORWARD_USE_GEOMETRY +/* integral concurrent learning */ +#define ADAPTIVE_WITHOUT_ICL 0 +#define ADAPTIVE_WITH_ICL 1 +#define SELECT_ADAPTIVE_W_WO_ICL ADAPTIVE_WITHOUT_ICL + /* heading sensor */ #define NO_HEADING_SENSOR 0 #define HEADING_SENSOR_USE_COMPASS 1 From 0c182edff3e69fb4527361f8162bab1c751c506f Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Tue, 20 Oct 2020 11:07:36 +0800 Subject: [PATCH 05/62] multirotor_geometry: rename ICL function name to adaptive ICL --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index e053fed0..51747246 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -395,7 +395,7 @@ void force_ff_ctrl_use_geometry(float *accel_ff, float *force_ff){ force_ff[2] = uav_mass * (accel_ff[2] - 9.81); } -void force_ff_ctrl_use_ICL(float *accel_ff, float *force_ff, float *pos_err, float *vel_err){ +void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos_err, float *vel_err){ /* with mass of uav unknown */ /* Y_m and Y_m transpose */ mat_data(Y_m)[0] = accel_ff[0]; @@ -434,7 +434,7 @@ void moment_ff_ctrl_use_geometry(float *mom_ff){ mom_ff[2] = mat_data(WJW)[2]; } -void moment_ff_ctrl_use_ICL(float *mom_ff){ +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; @@ -512,7 +512,7 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * #if (SELECT_FEEDFORWARD == FEEDFORWARD_USE_GEOMETRY) force_ff_ctrl_use_geometry(accel_ff_ned, accel_ff_ned); #elif (SELECT_FEEDFORWARD == FEEDFORWARD_USE_ADAPTIVE_ICL) - force_ff_ctrl_use_ICL(accel_ff_ned, force_ff_ned, pos_error, vel_error); + force_ff_ctrl_use_adaptive_ICL(accel_ff_ned, force_ff_ned, pos_error, vel_error); #endif /* control input kxex_kvev_mge3_mxd_dot_dot */ @@ -615,7 +615,7 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * #if (SELECT_FEEDFORWARD == FEEDFORWARD_USE_GEOMETRY) moment_ff_ctrl_use_geometry(moment_ff); #elif (SELECT_FEEDFORWARD == FEEDFORWARD_USE_ADAPTIVE_ICL) - moment_ff_ctrl_use_ICL(moment_ff); + moment_ff_ctrl_use_adaptive_ICL(moment_ff); #endif /* control input M1, M2, M3 */ From e548c85fa133fff62c3fc23cf7bb199255051748 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Tue, 20 Oct 2020 15:49:20 +0800 Subject: [PATCH 06/62] reduce the dimension of k_m_cl_gain to scalar --- .../multirotor_geometry/multirotor_geometry_param.c | 4 +--- .../multirotor_geometry/multirotor_geometry_param.h | 4 +--- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_param.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_param.c index f7d94dab..4a2fca02 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_param.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_param.c @@ -32,9 +32,7 @@ void init_multirotor_geometry_param_list(void) 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_X, "K_CL_M_X", 0.002); - init_sys_param_float(MR_ICL_GAIN_K_CL_M_Y, "K_CL_M_Y", 0.002); - init_sys_param_float(MR_ICL_GAIN_K_CL_M_Z, "K_CL_M_Z", 0.002); + 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); diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_param.h b/src/core/controllers/multirotor_geometry/multirotor_geometry_param.h index fe4773e7..d815c157 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_param.h +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_param.h @@ -28,9 +28,7 @@ enum { MR_ICL_GAIN_GAMMA_DIAG_Z, MR_ICL_GAIN_C1, MR_ICL_GAIN_C2, - MR_ICL_GAIN_K_CL_M_X, - MR_ICL_GAIN_K_CL_M_Y, - MR_ICL_GAIN_K_CL_M_Z, + 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, From 880cb496a9d213689a2d11b2ef54b19889d1b2f1 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Wed, 21 Oct 2020 10:38:10 +0800 Subject: [PATCH 07/62] multirotor_geometry: add adaptive ICL control for mass estimation --- .../multirotor_geometry_ctrl.c | 122 +++++++++++++++--- 1 file changed, 107 insertions(+), 15 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 51747246..45c6436d 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -28,6 +28,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); @@ -63,8 +71,8 @@ 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, 3, 1); -MAT_ALLOC(Y_m_clt, 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, 3, 3); @@ -77,6 +85,14 @@ 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(last_force, 3, 1); +MAT_ALLOC(curr_force, 3, 1); +MAT_ALLOC(F_cl, 3, 1); +MAT_ALLOC(mat_m_now, 1, 1); + +float mat_m_matrix[N_m] = {0.0f}; +float mat_m_sum = 0.0f; float pos_error[3]; float vel_error[3]; @@ -93,7 +109,7 @@ float Gamma_m_gain; float Gamma_diag_gain[3]; float C1_gain; float C2_gain; -float k_cl_m_gain[3]; +float k_cl_m_gain; float k_cl_diag_gain[3]; float uav_mass; @@ -111,12 +127,27 @@ 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); @@ -154,8 +185,8 @@ void geometry_ctrl_init(void) MAT_INIT(b3d, 3, 1); MAT_INIT(Y_m, 3, 1); MAT_INIT(Y_mt, 1, 3); - MAT_INIT(Y_m_cl, 3, 1); - MAT_INIT(Y_m_clt, 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, 3, 3); @@ -168,6 +199,11 @@ void geometry_ctrl_init(void) 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(last_force, 3, 1); + MAT_INIT(curr_force, 3, 1); + MAT_INIT(F_cl, 3, 1); + MAT_INIT(mat_m_now, 1, 1); /* modify local variables when user change them via ground station */ set_sys_param_update_var_addr(MR_GEO_GAIN_ROLL_P, &krx); @@ -192,9 +228,7 @@ void geometry_ctrl_init(void) 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_X, &k_cl_m_gain[0]); - set_sys_param_update_var_addr(MR_ICL_GAIN_K_CL_M_Y, &k_cl_m_gain[1]); - set_sys_param_update_var_addr(MR_ICL_GAIN_K_CL_M_Z, &k_cl_m_gain[2]); + 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]); @@ -239,9 +273,7 @@ void geometry_ctrl_init(void) 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_X, &k_cl_m_gain[0]); - get_sys_param_float(MR_ICL_GAIN_K_CL_M_Y, &k_cl_m_gain[1]); - get_sys_param_float(MR_ICL_GAIN_K_CL_M_Z, &k_cl_m_gain[2]); + 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]); @@ -395,7 +427,7 @@ void force_ff_ctrl_use_geometry(float *accel_ff, float *force_ff){ 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){ +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]; @@ -410,10 +442,61 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos 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]; - /* theta_m update law */ - //theta_m_dot = Gamma*Y_mt*ev_C1ex + /* first term of theta_m update law */ MAT_MULT(&Y_mt, &ev_C1ex, &Ymt_evC1ex); + +#if (SELECT_ADAPTIVE_W_WO_ICL == 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_ADAPTIVE_W_WO_ICL == 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_clt_integral)[0]; + mat_data(y_m_clt_integral)[1] = mat_data(y_m_clt_integral)[1]; + mat_data(y_m_clt_integral)[2] = mat_data(y_m_clt_integral)[2]; + + /* prepare force control input used in ICL */ + MAT_SUB(&curr_force, &last_force, &F_cl); + + /* 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)[1]); + 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)[2]); + /* 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]; + + //theta_m_dot = Gamma*Y_mt*ev_C1ex + ICL update law + mat_data(theta_m_hat_dot)[0] = Gamma_m_gain*mat_data(Ymt_evC1ex)[0] + + k_cl_m_gain*Gamma_m_gain*mat_m_sum; +#endif + mat_data(theta_m_hat)[0] += mat_data(theta_m_hat_dot)[0] * dt; /* translational adaptive feedforward term */ @@ -512,7 +595,7 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * #if (SELECT_FEEDFORWARD == FEEDFORWARD_USE_GEOMETRY) force_ff_ctrl_use_geometry(accel_ff_ned, accel_ff_ned); #elif (SELECT_FEEDFORWARD == FEEDFORWARD_USE_ADAPTIVE_ICL) - force_ff_ctrl_use_adaptive_ICL(accel_ff_ned, force_ff_ned, pos_error, vel_error); + 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 */ @@ -582,6 +665,15 @@ 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 and last force for ICL */ + mat_data(last_force)[0] = mat_data(curr_force)[0]; + mat_data(last_force)[1] = mat_data(curr_force)[1]; + mat_data(last_force)[2] = mat_data(curr_force)[2]; + + 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]; From 42748cddb953af9a8aed9d7fcf4de6412ed42192 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Wed, 21 Oct 2020 10:54:43 +0800 Subject: [PATCH 08/62] multirotor_geometry: add the miss elements of theta_diag_hat --- .../controllers/multirotor_geometry/multirotor_geometry_ctrl.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 45c6436d..0680ffb9 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -553,6 +553,8 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff){ 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]; 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 From 658291f6e4ffdf9996d79b1c99b4b5393c63a38a Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Wed, 21 Oct 2020 13:50:31 +0800 Subject: [PATCH 09/62] multirotor_geometry: add adaptive ICL control for moment of inertia estimation --- .../multirotor_geometry_ctrl.c | 112 +++++++++++++++++- 1 file changed, 106 insertions(+), 6 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 0680ffb9..dbc07a1d 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -75,8 +75,8 @@ 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, 3, 3); -MAT_ALLOC(Y_diag_clt, 3, 3); +MAT_ALLOC(y_diag_cl_integral, 3, 3); +MAT_ALLOC(y_diag_clt_integral, 3, 3); MAT_ALLOC(theta_m_hat, 1, 1); MAT_ALLOC(theta_m_hat_dot, 1, 1); MAT_ALLOC(theta_diag_hat, 3, 1); @@ -90,9 +90,18 @@ MAT_ALLOC(last_force, 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(last_moment, 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]; @@ -189,8 +198,8 @@ void geometry_ctrl_init(void) 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, 3, 3); - MAT_INIT(Y_diag_clt, 3, 3); + MAT_INIT(y_diag_cl_integral, 3, 3); + MAT_INIT(y_diag_clt_integral, 3, 3); MAT_INIT(theta_m_hat, 1, 1); MAT_INIT(theta_m_hat_dot, 1, 1); MAT_INIT(theta_diag_hat, 3, 1); @@ -204,6 +213,13 @@ void geometry_ctrl_init(void) 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(last_moment, 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); @@ -468,6 +484,7 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos *(mat_data(F_cl)[1]-mat_data(y_m_cl_integral)[1]*mat_data(theta_m_hat)[1]); 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)[2]); + /* summation of past data */ if (force_ICL.index >= force_ICL.N){ force_ICL.index = 0; @@ -546,12 +563,86 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff){ 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]; - /* theta_diag update law */ - //theta_diag_dot = Gamma*Y_diagt*eW_C2eR + /* first term of theta_diag update law */ MAT_MULT(&Y_diagt, &eW_C2eR, &Ydiagt_eWC2eR); + +#if (SELECT_ADAPTIVE_W_WO_ICL == 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_ADAPTIVE_W_WO_ICL == 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] - mat_data(last_W)[0]*mat_data(last_W)[2])*dt; + mat_data(y_diag_cl_integral)[2*3 + 0] = (-mat_data(W)[0]*mat_data(W)[1] + mat_data(last_W)[0]*mat_data(last_W)[1])*dt; + mat_data(y_diag_cl_integral)[0*3 + 1] = (-mat_data(W)[1]*mat_data(W)[2] + mat_data(last_W)[1]*mat_data(last_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] - mat_data(last_W)[0]*mat_data(last_W)[1])*dt; + mat_data(y_diag_cl_integral)[0*3 + 2] = (mat_data(W)[1]*mat_data(W)[2] - mat_data(last_W)[1]*mat_data(last_W)[2])*dt; + mat_data(y_diag_cl_integral)[1*3 + 2] = (-mat_data(W)[0]*mat_data(W)[2] + mat_data(last_W)[0]*mat_data(last_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 force control input used in ICL */ + MAT_SUB(&curr_moment, &last_moment, &M_cl); + + /* 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]; + + mat_data(theta_diag_hat_dot)[0] = Gamma_diag_gain[0]*mat_data(Ydiagt_eWC2eR)[0] + + k_cl_diag_gain[0]*Gamma_diag_gain[0]*mat_diag_sum[0]; + mat_data(theta_diag_hat_dot)[1] = Gamma_diag_gain[1]*mat_data(Ydiagt_eWC2eR)[1] + + k_cl_diag_gain[1]*Gamma_diag_gain[1]*mat_diag_sum[1]; + mat_data(theta_diag_hat_dot)[2] = Gamma_diag_gain[2]*mat_data(Ydiagt_eWC2eR)[2] + + k_cl_diag_gain[2]*Gamma_diag_gain[2]*mat_diag_sum[2]; +#endif + 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; @@ -712,6 +803,15 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * moment_ff_ctrl_use_adaptive_ICL(moment_ff); #endif + /* save current moment and last moment for ICL */ + mat_data(last_moment)[0] = mat_data(curr_moment)[0]; + mat_data(last_moment)[1] = mat_data(curr_moment)[1]; + mat_data(last_moment)[2] = mat_data(curr_moment)[2]; + + mat_data(curr_moment)[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] + moment_ff[0]; + mat_data(curr_moment)[1] = -krx*mat_data(eR)[1] -kwx*mat_data(eW)[1] + moment_ff[1]; + mat_data(curr_moment)[2] = -krx*mat_data(eR)[2] -kwx*mat_data(eW)[2] + moment_ff[2]; + /* control input M1, M2, M3 */ output_moments[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] + moment_ff[0]; output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] + moment_ff[1]; From ce261096012498e881a522fcd56d7059a3c8179c Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Thu, 22 Oct 2020 11:13:04 +0800 Subject: [PATCH 10/62] correct integral of control input used ICL --- .../multirotor_geometry_ctrl.c | 26 +++++++------------ 1 file changed, 9 insertions(+), 17 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index dbc07a1d..471eca10 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -86,12 +86,10 @@ 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(last_force, 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(last_moment, 3, 1); MAT_ALLOC(curr_moment, 3, 1); MAT_ALLOC(M_cl, 3, 1); MAT_ALLOC(yDiagCl_thetaDiatHat, 3, 1); @@ -209,12 +207,10 @@ void geometry_ctrl_init(void) MAT_INIT(Ymt_evC1ex, 1, 1); MAT_INIT(Ydiagt_eWC2eR, 3, 1); MAT_INIT(last_vel, 3, 1); - MAT_INIT(last_force, 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(last_moment, 3, 1); MAT_INIT(curr_moment, 3, 1); MAT_INIT(M_cl, 3, 1); MAT_INIT(yDiagCl_thetaDiatHat, 3, 1); @@ -475,7 +471,9 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos mat_data(y_m_clt_integral)[2] = mat_data(y_m_clt_integral)[2]; /* prepare force control input used in ICL */ - MAT_SUB(&curr_force, &last_force, &F_cl); + 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] @@ -593,8 +591,10 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff){ 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 force control input used in ICL */ - MAT_SUB(&curr_moment, &last_moment, &M_cl); + /* 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); @@ -758,11 +758,7 @@ 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 and last force for ICL */ - mat_data(last_force)[0] = mat_data(curr_force)[0]; - mat_data(last_force)[1] = mat_data(curr_force)[1]; - mat_data(last_force)[2] = mat_data(curr_force)[2]; - + /* 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]; @@ -803,11 +799,7 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * moment_ff_ctrl_use_adaptive_ICL(moment_ff); #endif - /* save current moment and last moment for ICL */ - mat_data(last_moment)[0] = mat_data(curr_moment)[0]; - mat_data(last_moment)[1] = mat_data(curr_moment)[1]; - mat_data(last_moment)[2] = mat_data(curr_moment)[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] = -krx*mat_data(eR)[1] -kwx*mat_data(eW)[1] + moment_ff[1]; mat_data(curr_moment)[2] = -krx*mat_data(eR)[2] -kwx*mat_data(eW)[2] + moment_ff[2]; From 9d6b0edb3f2c2cf4bdf59b885f51c362dc371fe3 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 26 Oct 2020 10:52:46 +0800 Subject: [PATCH 11/62] multirotor_geometric: correct y_diag_cl_integral --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 471eca10..96ba70b0 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -572,13 +572,13 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff){ #elif (SELECT_ADAPTIVE_W_WO_ICL == 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] - mat_data(last_W)[0]*mat_data(last_W)[2])*dt; - mat_data(y_diag_cl_integral)[2*3 + 0] = (-mat_data(W)[0]*mat_data(W)[1] + mat_data(last_W)[0]*mat_data(last_W)[1])*dt; - mat_data(y_diag_cl_integral)[0*3 + 1] = (-mat_data(W)[1]*mat_data(W)[2] + mat_data(last_W)[1]*mat_data(last_W)[2])*dt; + 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] - mat_data(last_W)[0]*mat_data(last_W)[1])*dt; - mat_data(y_diag_cl_integral)[0*3 + 2] = (mat_data(W)[1]*mat_data(W)[2] - mat_data(last_W)[1]*mat_data(last_W)[2])*dt; - mat_data(y_diag_cl_integral)[1*3 + 2] = (-mat_data(W)[0]*mat_data(W)[2] + mat_data(last_W)[0]*mat_data(last_W)[2])*dt; + 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]; From 5b9e35bd2238549a6ac5593b8b276901551e4336 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 26 Oct 2020 11:42:59 +0800 Subject: [PATCH 12/62] add send_debug_adaptive_ICL --- src/Makefile | 1 + .../controllers/multirotor_geometry/send_debug_adaptive_ICL.c | 1 + .../controllers/multirotor_geometry/send_debug_adaptive_ICL.h | 4 ++++ 3 files changed, 6 insertions(+) create mode 100644 src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c create mode 100644 src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h diff --git a/src/Makefile b/src/Makefile index 5ece0add..a3bfeaa3 100644 --- a/src/Makefile +++ b/src/Makefile @@ -75,6 +75,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/send_debug_adaptive_ICL.c b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c new file mode 100644 index 00000000..20ee6bdc --- /dev/null +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c @@ -0,0 +1 @@ +#include "send_debug_adaptive_ICL.h" \ No newline at end of file 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..ccc96638 --- /dev/null +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h @@ -0,0 +1,4 @@ +#ifndef __SEND_DEBUG_ADAPTIVE_ICL_H__ +#define __SEND_DEBUG_ADAPTIVE_ICL_H__ + +#endif From a0c05c3acf182e848672bb38f2b4fe2d6bb63ae0 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 26 Oct 2020 14:06:09 +0800 Subject: [PATCH 13/62] multirotor_geometry: separate theta_hat_dot into adaptive part and ICL part --- .../multirotor_geometry_ctrl.c | 39 ++++++++++++++----- 1 file changed, 30 insertions(+), 9 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 96ba70b0..d1f7e303 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -79,8 +79,12 @@ MAT_ALLOC(y_diag_cl_integral, 3, 3); MAT_ALLOC(y_diag_clt_integral, 3, 3); 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); @@ -200,8 +204,12 @@ void geometry_ctrl_init(void) MAT_INIT(y_diag_clt_integral, 3, 3); 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); @@ -507,9 +515,13 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos mat_data(last_vel)[1] = curr_vel[1]; mat_data(last_vel)[2] = curr_vel[2]; - //theta_m_dot = Gamma*Y_mt*ev_C1ex + ICL update law - mat_data(theta_m_hat_dot)[0] = Gamma_m_gain*mat_data(Ymt_evC1ex)[0] - + k_cl_m_gain*Gamma_m_gain*mat_m_sum; + /* 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 mat_data(theta_m_hat)[0] += mat_data(theta_m_hat_dot)[0] * dt; @@ -635,12 +647,21 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff){ mat_data(last_W)[1] = mat_data(W)[1]; mat_data(last_W)[2] = mat_data(W)[2]; - mat_data(theta_diag_hat_dot)[0] = Gamma_diag_gain[0]*mat_data(Ydiagt_eWC2eR)[0] - + k_cl_diag_gain[0]*Gamma_diag_gain[0]*mat_diag_sum[0]; - mat_data(theta_diag_hat_dot)[1] = Gamma_diag_gain[1]*mat_data(Ydiagt_eWC2eR)[1] - + k_cl_diag_gain[1]*Gamma_diag_gain[1]*mat_diag_sum[1]; - mat_data(theta_diag_hat_dot)[2] = Gamma_diag_gain[2]*mat_data(Ydiagt_eWC2eR)[2] - + k_cl_diag_gain[2]*Gamma_diag_gain[2]*mat_diag_sum[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 mat_data(theta_diag_hat)[0] += mat_data(theta_diag_hat_dot)[0] * dt; From 9f75bbda86198f7a11265d82cecde823f8a772e7 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 26 Oct 2020 19:04:08 +0800 Subject: [PATCH 14/62] format with AStyle --- .../multirotor_geometry_ctrl.c | 53 ++++++++++--------- 1 file changed, 29 insertions(+), 24 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index d1f7e303..5c0f9376 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -31,11 +31,11 @@ #define N_m 10 #define N_diag 10 -typedef struct{ +typedef struct { bool isfull; int index; int N; -}ICL_data; +} ICL_data; MAT_ALLOC(J, 3, 3); MAT_ALLOC(R, 3, 3); @@ -141,7 +141,8 @@ bool height_ctrl_only = false; ICL_data force_ICL; ICL_data momen_ICL; -void ICL_matrix_init(void){ +void ICL_matrix_init(void) +{ force_ICL.index = 0; force_ICL.isfull = false; force_ICL.N = N_m; @@ -440,14 +441,16 @@ void geometry_manual_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *ou output_moments[2] = -_krz*mat_data(eR)[2] -_kwz*mat_data(eW)[2] + mat_data(inertia_effect)[2]; } -void force_ff_ctrl_use_geometry(float *accel_ff, float *force_ff){ +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){ +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]; @@ -485,27 +488,27 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos /* 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(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)[1]); + *(mat_data(F_cl)[1]-mat_data(y_m_cl_integral)[1]*mat_data(theta_m_hat)[1]); 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)[2]); + *(mat_data(F_cl)[2]-mat_data(y_m_cl_integral)[2]*mat_data(theta_m_hat)[2]); /* summation of past data */ - if (force_ICL.index >= force_ICL.N){ + 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){ + if (!force_ICL.isfull) { mat_m_sum = 0; - for (int i = 0; i < force_ICL.index; i++){ + for (int i = 0; i < force_ICL.index; i++) { mat_m_sum += mat_m_matrix[i]; } - }else{ + } else { mat_m_sum = 0; - for (int i = 0; i < force_ICL.N; i++){ + for (int i = 0; i < force_ICL.N; i++) { mat_m_sum += mat_m_matrix[i]; } } @@ -521,7 +524,7 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos /* 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]; + + mat_data(theta_m_hat_dot_ICL)[0]; #endif mat_data(theta_m_hat)[0] += mat_data(theta_m_hat_dot)[0] * dt; @@ -533,7 +536,8 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos force_ff[2] = mat_data(Y_m)[2]*mat_data(theta_m_hat)[0]; } -void moment_ff_ctrl_use_geometry(float *mom_ff){ +void moment_ff_ctrl_use_geometry(float *mom_ff) +{ /* with moment of inertia of uav known */ /* calculate the inertia feedfoward term */ //W x JW @@ -544,7 +548,8 @@ void moment_ff_ctrl_use_geometry(float *mom_ff){ mom_ff[2] = mat_data(WJW)[2]; } -void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff){ +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; @@ -614,7 +619,7 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff){ MAT_MULT(&y_diag_clt_integral, &M_sub_err, &mat_diag_now); /* summation of past data */ - if (momen_ICL.index >= momen_ICL.N){ + if (momen_ICL.index >= momen_ICL.N) { momen_ICL.index = 0; momen_ICL.isfull = true; } @@ -622,20 +627,20 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff){ 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){ + 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++){ + 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{ + } 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++){ + 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]; @@ -657,11 +662,11 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff){ /* 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_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_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]; + + mat_data(theta_diag_hat_dot_ICL)[2]; #endif mat_data(theta_diag_hat)[0] += mat_data(theta_diag_hat_dot)[0] * dt; From 0d1579799d966949c3d586c903a55a4dcdcd2f33 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 26 Oct 2020 19:06:21 +0800 Subject: [PATCH 15/62] add MESSAGE_ID used for adaptive-ICL-control --- src/core/debug_link/debug_link.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/core/debug_link/debug_link.h b/src/core/debug_link/debug_link.h index c0e6b9d0..07a1b38f 100644 --- a/src/core/debug_link/debug_link.h +++ b/src/core/debug_link/debug_link.h @@ -27,7 +27,15 @@ enum { MESSAGE_ID_COMPASS = 15, MESSAGE_ID_BAROMETER = 16, MESSAGE_ID_ALT_EST = 17, - MESSAGE_ID_INS_SENSOR = 18 + MESSAGE_ID_INS_SENSOR = 18, + MESSAGE_ID_ICL_THETA_M = 31, + MESSAGE_ID_ICL_THETA_M_DOT = 32, + MESSAGE_ID_ICL_THETA_M_DOT_ADAPTIVE = 33, + MESSAGE_ID_ICL_THETA_M_DOT_ICL = 34, + MESSAGE_ID_ICL_THETA_DIAG = 35, + MESSAGE_ID_ICL_THETA_DIAG_DOT = 36, + MESSAGE_ID_ICL_THETA_DIAG_DOT_ADAPTIVE = 37, + MESSAGE_ID_ICL_THETA_DIAG_DOT_ICL = 38 } MESSAGE_ID; typedef struct { From fad458d3986bae16c363f40097f99160d2117553 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 26 Oct 2020 19:08:16 +0800 Subject: [PATCH 16/62] add functions used to send debug messages for ICL --- .../send_debug_adaptive_ICL.c | 90 ++++++++++++++++++- .../send_debug_adaptive_ICL.h | 22 +++++ 2 files changed, 111 insertions(+), 1 deletion(-) diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c index 20ee6bdc..63bb38f3 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c @@ -1 +1,89 @@ -#include "send_debug_adaptive_ICL.h" \ No newline at end of file +#include "send_debug_adaptive_ICL.h" + +void send_adaptive_ICL_theta_m_debug(debug_msg_t *payload) +{ + float theta_m_esti; + theta_m_esti = mat_data(theta_m_hat)[0]; + + pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_THETA_M); + pack_debug_debug_message_float(&theta_m_esti, payload); +} + +void send_adaptive_ICL_theta_m_dot_debug(debug_msg_t *payload) +{ + float theta_m_dot_esti; + theta_m_dot_esti = mat_data(theta_m_hat_dot)[0]; + + pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_THETA_M_DOT); + pack_debug_debug_message_float(&theta_m_dot_esti, payload); +} + +void send_adaptive_ICL_theta_m_dot_adaptive_debug(debug_msg_t *payload) +{ + float theta_m_dot_adaptive_esti; + theta_m_dot_adaptive_esti = mat_data(theta_m_hat_dot_adaptive)[0]; + + pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_THETA_M_DOT_ADAPTIVE); + pack_debug_debug_message_float(&theta_m_dot_adaptive_esti, payload); +} + +void send_adaptive_ICL_theta_m_dot_ICL_debug(debug_msg_t *payload) +{ + float theta_m_dot_ICL_esti; + theta_m_dot_ICL_esti = mat_data(theta_m_hat_dot_ICL)[0]; + + pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_THETA_M_DOT_ICL); + pack_debug_debug_message_float(&theta_m_dot_ICL_esti, payload); +} + +void send_adaptive_ICL_theta_diag_debug(debug_msg_t *payload) +{ + float theta_diag_esti[3]; + 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_THETA_DIAG); + 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_theta_diag_dot_debug(debug_msg_t *payload) +{ + float theta_diag_dot_esti[3]; + 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]; + + pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_THETA_DIAG_DOT); + 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); +} + +void send_adaptive_ICL_theta_diag_dot_adaptive_debug(debug_msg_t *payload) +{ + float theta_diag_dot_adaptive_esti[3]; + theta_diag_dot_adaptive_esti[0] = mat_data(theta_diag_hat_dot_adaptive)[0]; + theta_diag_dot_adaptive_esti[1] = mat_data(theta_diag_hat_dot_adaptive)[1]; + theta_diag_dot_adaptive_esti[2] = mat_data(theta_diag_hat_dot_adaptive)[2]; + + pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_THETA_DIAG_DOT); + pack_debug_debug_message_float(&theta_diag_dot_adaptive_esti[0], payload); + pack_debug_debug_message_float(&theta_diag_dot_adaptive_esti[1], payload); + pack_debug_debug_message_float(&theta_diag_dot_adaptive_esti[2], payload); +} + +void send_adaptive_ICL_theta_diag_dot_ICL_debug(debug_msg_t *payload) +{ + float theta_diag_dot_ICL_esti[3]; + theta_diag_dot_ICL_esti[0] = mat_data(theta_diag_hat_dot_ICL)[0]; + theta_diag_dot_ICL_esti[1] = mat_data(theta_diag_hat_dot_ICL)[1]; + theta_diag_dot_ICL_esti[2] = mat_data(theta_diag_hat_dot_ICL)[2]; + + pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_THETA_DIAG_DOT); + pack_debug_debug_message_float(&theta_diag_dot_ICL_esti[0], payload); + pack_debug_debug_message_float(&theta_diag_dot_ICL_esti[1], payload); + pack_debug_debug_message_float(&theta_diag_dot_ICL_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 index ccc96638..69ebc85a 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h @@ -1,4 +1,26 @@ #ifndef __SEND_DEBUG_ADAPTIVE_ICL_H__ #define __SEND_DEBUG_ADAPTIVE_ICL_H__ +#include "arm_math.h" +#include "matrix.h" +#include "debug_link.h" + +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; + +void send_adaptive_ICL_theta_m_debug(debug_msg_t *payload); +void send_adaptive_ICL_theta_m_dot_debug(debug_msg_t *payload); +void send_adaptive_ICL_theta_m_dot_adaptive_debug(debug_msg_t *payload); +void send_adaptive_ICL_theta_m_dot_ICL_debug(debug_msg_t *payload); +void send_adaptive_ICL_theta_diag_debug(debug_msg_t *payload); +void send_adaptive_ICL_theta_diag_dot_debug(debug_msg_t *payload); +void send_adaptive_ICL_theta_diag_dot_adaptive_debug(debug_msg_t *payload); +void send_adaptive_ICL_theta_diag_dot_ICL_debug(debug_msg_t *payload); + #endif From fccde843bc64894f8dc975264ef0ef02c9a8049f Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Wed, 28 Oct 2020 11:09:39 +0800 Subject: [PATCH 17/62] add selection for sending debug messages for ICL --- src/core/tasks/debug_link_task.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/core/tasks/debug_link_task.c b/src/core/tasks/debug_link_task.c index 8d6a7ebf..02923aea 100644 --- a/src/core/tasks/debug_link_task.c +++ b/src/core/tasks/debug_link_task.c @@ -10,6 +10,7 @@ #include "ms5611.h" #include "altitude_est.h" #include "debug_msg.h" +#include "send_debug_adaptive_ICL.h" SemaphoreHandle_t debug_link_task_semphr; @@ -45,6 +46,14 @@ 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_theta_m_debug(&payload); + //send_adaptive_ICL_theta_m_dot_debug(&payload); + //send_adaptive_ICL_theta_m_dot_adaptive_debug(&payload); + //send_adaptive_ICL_theta_m_dot_ICL_debug(&payload); + //send_adaptive_ICL_theta_diag_debug(&payload); + //send_adaptive_ICL_theta_diag_dot_debug(&payload); + //send_adaptive_ICL_theta_diag_dot_adaptive_debug(&payload); + //send_adaptive_ICL_theta_diag_dot_ICL_debug(&payload); send_onboard_data(payload.s, payload.len); //freertos_task_delay(delay_time_ms); From 1726524fc110f0c5b41825f742e847aef4e7b08a Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 2 Nov 2020 11:19:12 +0800 Subject: [PATCH 18/62] multirotor_geometry: rearrange functions and add options of adaptive ICL control for manual control --- .../multirotor_geometry_ctrl.c | 159 +++++++++--------- 1 file changed, 80 insertions(+), 79 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 5c0f9376..e7d4d886 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -362,85 +362,6 @@ void reset_geometry_tracking_error_integral(void) tracking_error_integral[2] = 0.0f; } -void geometry_manual_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *output_moments, - bool heading_present) -{ - /* convert radio command (euler angle) to rotation matrix */ - euler_to_rotation_matrix(rc, mat_data(Rd), mat_data(Rtd)); - - /* W (angular velocity) */ - mat_data(W)[0] = gyro[0]; - mat_data(W)[1] = gyro[1]; - mat_data(W)[2] = gyro[2]; - - /* set Wd and Wd_dot to 0 since there is no predefined trajectory */ - mat_data(Wd)[0] = 0.0f; - mat_data(Wd)[1] = 0.0f; - mat_data(Wd)[2] = 0.0f; - mat_data(Wd_dot)[0] = 0.0f; - mat_data(Wd_dot)[1] = 0.0f; - mat_data(Wd_dot)[2] = 0.0f; - - float _krz, _kwz; //switch between full heading control and yaw rate control - - /* switch to yaw rate control mode if no heading information provided */ - if(heading_present == false) { - /* yaw rate control only */ - _krz = 0.0f; - _kwz = yaw_rate_ctrl_gain; - mat_data(Wd)[2] = rc->yaw; //set yaw rate desired value - } else { - _krz = krz; - _kwz = kwz; - } - - /* calculate attitude error eR */ - MAT_MULT(&Rtd, &R, &RtdR); - MAT_MULT(&Rt, &Rd, &RtRd); - MAT_SUB(&RtdR, &RtRd, &eR_mat); - vee_map_3x3(mat_data(eR_mat), mat_data(eR)); - mat_data(eR)[0] *= 0.5f; - mat_data(eR)[1] *= 0.5f; - mat_data(eR)[2] *= 0.5f; - - /* calculate attitude rate error eW */ - //MAT_MULT(&Rt, &Rd, &RtRd); //the term is duplicated - 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]; - -#if 0 /* inertia feedfoward term for motion planning (trajectory is known) */ - /* calculate inertia effect (trajectory is defined, Wd and Wd_dot are not zero) */ - //W * R^T * Rd * Wd - hat_map_3x3(mat_data(W), mat_data(W_hat)); - MAT_MULT(&W_hat, &Rt, &WRt); - MAT_MULT(&WRt, &Rd, &WRtRd); - MAT_MULT(&WRtRd, &Wd, &WRtRdWd); - //R^T * Rd * Wd_dot - //MAT_MULT(&Rt, &Rd, &RtRd); //the term is duplicated - MAT_MULT(&RtRd, &Wd_dot, &RtRdWddot); - //(W * R^T * Rd * Wd) - (R^T * Rd * Wd_dot) - MAT_SUB(&WRtRdWd, &RtRdWddot, &WRtRdWd_RtRdWddot); - //J*[(W * R^T * Rd * Wd) - (R^T * Rd * Wd_dot)] - MAT_MULT(&J, &WRtRdWd_RtRdWddot, &J_WRtRdWd_RtRdWddot); - //inertia effect = (W x JW) - J*[(W * R^T * Rd * Wd) - (R^T * Rd * Wd_dot)] - MAT_SUB(&WJW, &J_WRtRdWd_RtRdWddot, &inertia_effect); - -#endif - - /* 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]; - output_moments[2] = -_krz*mat_data(eR)[2] -_kwz*mat_data(eW)[2] + mat_data(inertia_effect)[2]; -} - void force_ff_ctrl_use_geometry(float *accel_ff, float *force_ff) { /* with mass of uav known */ @@ -680,6 +601,86 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff) mom_ff[2] = Gamma_diag_gain[2]*mat_data(theta_diag_hat)[2]; } +void geometry_manual_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *output_moments, + bool heading_present) +{ + /* convert radio command (euler angle) to rotation matrix */ + euler_to_rotation_matrix(rc, mat_data(Rd), mat_data(Rtd)); + + /* W (angular velocity) */ + mat_data(W)[0] = gyro[0]; + mat_data(W)[1] = gyro[1]; + mat_data(W)[2] = gyro[2]; + + /* set Wd and Wd_dot to 0 since there is no predefined trajectory */ + mat_data(Wd)[0] = 0.0f; + mat_data(Wd)[1] = 0.0f; + mat_data(Wd)[2] = 0.0f; + mat_data(Wd_dot)[0] = 0.0f; + mat_data(Wd_dot)[1] = 0.0f; + mat_data(Wd_dot)[2] = 0.0f; + + float _krz, _kwz; //switch between full heading control and yaw rate control + + /* switch to yaw rate control mode if no heading information provided */ + if(heading_present == false) { + /* yaw rate control only */ + _krz = 0.0f; + _kwz = yaw_rate_ctrl_gain; + mat_data(Wd)[2] = rc->yaw; //set yaw rate desired value + } else { + _krz = krz; + _kwz = kwz; + } + + /* calculate attitude error eR */ + MAT_MULT(&Rtd, &R, &RtdR); + MAT_MULT(&Rt, &Rd, &RtRd); + MAT_SUB(&RtdR, &RtRd, &eR_mat); + vee_map_3x3(mat_data(eR_mat), mat_data(eR)); + mat_data(eR)[0] *= 0.5f; + mat_data(eR)[1] *= 0.5f; + mat_data(eR)[2] *= 0.5f; + + /* calculate attitude rate error eW */ + //MAT_MULT(&Rt, &Rd, &RtRd); //the term is duplicated + MAT_MULT(&RtRd, &Wd, &RtRdWd); + MAT_SUB(&W, &RtRdWd, &eW); + + /* moment feedforward control */ + float moment_ff[3] = {0, 0}; + +#if (SELECT_FEEDFORWARD == FEEDFORWARD_USE_GEOMETRY) + moment_ff_ctrl_use_geometry(moment_ff); +#elif (SELECT_FEEDFORWARD ==FEEDFORWARD_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) */ + //W * R^T * Rd * Wd + hat_map_3x3(mat_data(W), mat_data(W_hat)); + MAT_MULT(&W_hat, &Rt, &WRt); + MAT_MULT(&WRt, &Rd, &WRtRd); + MAT_MULT(&WRtRd, &Wd, &WRtRdWd); + //R^T * Rd * Wd_dot + //MAT_MULT(&Rt, &Rd, &RtRd); //the term is duplicated + MAT_MULT(&RtRd, &Wd_dot, &RtRdWddot); + //(W * R^T * Rd * Wd) - (R^T * Rd * Wd_dot) + MAT_SUB(&WRtRdWd, &RtRdWddot, &WRtRdWd_RtRdWddot); + //J*[(W * R^T * Rd * Wd) - (R^T * Rd * Wd_dot)] + MAT_MULT(&J, &WRtRdWd_RtRdWddot, &J_WRtRdWd_RtRdWddot); + //inertia effect = (W x JW) - J*[(W * R^T * Rd * Wd) - (R^T * Rd * Wd_dot)] + MAT_SUB(&WJW, &J_WRtRdWd_RtRdWddot, &inertia_effect); + +#endif + + /* control input M1, M2, M3 */ + output_moments[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] + moment_ff[0]; + output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] + moment_ff[1]; + output_moments[2] = -_krz*mat_data(eR)[2] -_kwz*mat_data(eW)[2] + moment_ff[2]; +} + void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *curr_pos_ned, float *curr_vel_ned, float *output_moments, float *output_force, bool manual_flight) From 74bbd76f3f5808fabc3584df34fbbead2f819b6c Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Thu, 5 Nov 2020 12:31:46 +0800 Subject: [PATCH 19/62] comment out gains used in adaptive ICL control --- .../multirotor_geometry_ctrl.c | 18 +++++++++++++++++- .../multirotor_geometry_param.c | 2 ++ .../multirotor_geometry_param.h | 2 ++ 3 files changed, 21 insertions(+), 1 deletion(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index e7d4d886..64612d7b 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -243,6 +243,7 @@ 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]); @@ -253,6 +254,7 @@ void geometry_ctrl_init(void) 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]); @@ -288,6 +290,7 @@ 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]); @@ -298,6 +301,7 @@ void geometry_ctrl_init(void) 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]); @@ -316,6 +320,18 @@ 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] = 0.1f; + Gamma_diag_gain[1] = 0.1f; + Gamma_diag_gain[2] = 0.1f; + C1_gain = 0.1f; + C2_gain = 0.1f; + k_cl_m_gain = 0.1f; + k_cl_diag_gain[0] = 0.1f; + k_cl_diag_gain[1] = 0.1f; + k_cl_diag_gain[2] = 0.1f; + 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]); @@ -713,7 +729,7 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * assign_vector_3x1_eun_to_ned(accel_ff_ned, autopilot.wp_now.acc_feedforward); #if (SELECT_FEEDFORWARD == FEEDFORWARD_USE_GEOMETRY) - force_ff_ctrl_use_geometry(accel_ff_ned, accel_ff_ned); + force_ff_ctrl_use_geometry(accel_ff_ned, force_ff_ned); #elif (SELECT_FEEDFORWARD == FEEDFORWARD_USE_ADAPTIVE_ICL) force_ff_ctrl_use_adaptive_ICL(accel_ff_ned, force_ff_ned, pos_error, vel_error, curr_vel_ned); #endif diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_param.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_param.c index 4a2fca02..efe82185 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_param.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_param.c @@ -26,6 +26,7 @@ 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); @@ -36,6 +37,7 @@ void init_multirotor_geometry_param_list(void) 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 d815c157..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,7 @@ 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, @@ -32,6 +33,7 @@ enum { 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, From 6a177f4e3ecfa80dcdc3cae0ef7e04ae79d71d8c Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Thu, 5 Nov 2020 15:51:32 +0800 Subject: [PATCH 20/62] bound feedforward control term --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 64612d7b..422383fd 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -471,6 +471,10 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos 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], 0.8, -0.8); + bound_float(&force_ff[0], 0.8, -0.8); + bound_float(&force_ff[0], 0.8, -0.8); } void moment_ff_ctrl_use_geometry(float *mom_ff) @@ -615,6 +619,10 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff) mom_ff[0] = Gamma_diag_gain[0]*mat_data(theta_diag_hat)[0]; mom_ff[1] = Gamma_diag_gain[1]*mat_data(theta_diag_hat)[1]; mom_ff[2] = Gamma_diag_gain[2]*mat_data(theta_diag_hat)[2]; + + bound_float(&mom_ff[0], 0.1, -0.1); + bound_float(&mom_ff[0], 0.1, -0.1); + bound_float(&mom_ff[0], 0.1, -0.1); } void geometry_manual_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *output_moments, From a5de6e18dcc20e56d4c6746b5670aa4e9c55665c Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Thu, 5 Nov 2020 19:27:50 +0800 Subject: [PATCH 21/62] multirotor_geometry: fix typos --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 422383fd..4da1f1f5 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -473,8 +473,8 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos force_ff[2] = mat_data(Y_m)[2]*mat_data(theta_m_hat)[0]; bound_float(&force_ff[0], 0.8, -0.8); - bound_float(&force_ff[0], 0.8, -0.8); - bound_float(&force_ff[0], 0.8, -0.8); + bound_float(&force_ff[1], 0.8, -0.8); + bound_float(&force_ff[2], 0.8, -0.8); } void moment_ff_ctrl_use_geometry(float *mom_ff) @@ -621,8 +621,8 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff) mom_ff[2] = Gamma_diag_gain[2]*mat_data(theta_diag_hat)[2]; bound_float(&mom_ff[0], 0.1, -0.1); - bound_float(&mom_ff[0], 0.1, -0.1); - bound_float(&mom_ff[0], 0.1, -0.1); + bound_float(&mom_ff[1], 0.1, -0.1); + bound_float(&mom_ff[2], 0.1, -0.1); } void geometry_manual_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *output_moments, From 85965d88983755fbd9f3c88fbdea72abf5a27ad5 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Fri, 6 Nov 2020 15:19:49 +0800 Subject: [PATCH 22/62] multirotor_geometry: correct feedforward term of moment control of adaptive ICL control --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 4da1f1f5..2702ff2b 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -77,6 +77,7 @@ 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_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); @@ -203,6 +204,7 @@ void geometry_ctrl_init(void) 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_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); @@ -616,9 +618,10 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff) /* rotational adaptive feedforward term */ //Y_diag*theta_diag_hat - mom_ff[0] = Gamma_diag_gain[0]*mat_data(theta_diag_hat)[0]; - mom_ff[1] = Gamma_diag_gain[1]*mat_data(theta_diag_hat)[1]; - mom_ff[2] = Gamma_diag_gain[2]*mat_data(theta_diag_hat)[2]; + 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.1, -0.1); bound_float(&mom_ff[1], 0.1, -0.1); From cacb4ce12ff817d25c629c56673ed3bdef44d31c Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Fri, 6 Nov 2020 15:40:28 +0800 Subject: [PATCH 23/62] fix usage of super() function in Python2.7 --- tools/serial_plot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/serial_plot.py b/tools/serial_plot.py index 5cf5a3ec..492b1692 100644 --- a/tools/serial_plot.py +++ b/tools/serial_plot.py @@ -517,7 +517,7 @@ def run(self): serial_plotter.serial_receive() def join(self): - super(self).join() + super(serial_thread, self).join() serial_thread().start() From eec991ed734ffae380a51237c4ed6d6e1a3361a5 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 9 Nov 2020 11:58:57 +0800 Subject: [PATCH 24/62] rewrite send debug function of ICL control to concise form --- .../send_debug_adaptive_ICL.c | 100 ++++++------------ .../send_debug_adaptive_ICL.h | 10 +- src/core/debug_link/debug_link.h | 10 +- src/core/tasks/debug_link_task.c | 10 +- 4 files changed, 39 insertions(+), 91 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c index 63bb38f3..cb3135bb 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c @@ -1,89 +1,55 @@ #include "send_debug_adaptive_ICL.h" -void send_adaptive_ICL_theta_m_debug(debug_msg_t *payload) +void send_adaptive_ICL_mass_estimation_debug(debug_msg_t *payload) { float theta_m_esti; - theta_m_esti = mat_data(theta_m_hat)[0]; - - pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_THETA_M); - pack_debug_debug_message_float(&theta_m_esti, payload); -} - -void send_adaptive_ICL_theta_m_dot_debug(debug_msg_t *payload) -{ float theta_m_dot_esti; + float theta_m_dot_esti_adaptive; + float theta_m_dot_esti_ICL; + + 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_THETA_M_DOT); + pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_MASS_ESTIMATION); + 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_theta_m_dot_adaptive_debug(debug_msg_t *payload) -{ - float theta_m_dot_adaptive_esti; - theta_m_dot_adaptive_esti = mat_data(theta_m_hat_dot_adaptive)[0]; - - pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_THETA_M_DOT_ADAPTIVE); - pack_debug_debug_message_float(&theta_m_dot_adaptive_esti, payload); -} - -void send_adaptive_ICL_theta_m_dot_ICL_debug(debug_msg_t *payload) -{ - float theta_m_dot_ICL_esti; - theta_m_dot_ICL_esti = mat_data(theta_m_hat_dot_ICL)[0]; - - pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_THETA_M_DOT_ICL); - pack_debug_debug_message_float(&theta_m_dot_ICL_esti, payload); -} - -void send_adaptive_ICL_theta_diag_debug(debug_msg_t *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]; + 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_THETA_DIAG); - 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_theta_diag_dot_debug(debug_msg_t *payload) -{ - float theta_diag_dot_esti[3]; 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]; - - pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_THETA_DIAG_DOT); + 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(&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); -} - -void send_adaptive_ICL_theta_diag_dot_adaptive_debug(debug_msg_t *payload) -{ - float theta_diag_dot_adaptive_esti[3]; - theta_diag_dot_adaptive_esti[0] = mat_data(theta_diag_hat_dot_adaptive)[0]; - theta_diag_dot_adaptive_esti[1] = mat_data(theta_diag_hat_dot_adaptive)[1]; - theta_diag_dot_adaptive_esti[2] = mat_data(theta_diag_hat_dot_adaptive)[2]; - - pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_THETA_DIAG_DOT); - pack_debug_debug_message_float(&theta_diag_dot_adaptive_esti[0], payload); - pack_debug_debug_message_float(&theta_diag_dot_adaptive_esti[1], payload); - pack_debug_debug_message_float(&theta_diag_dot_adaptive_esti[2], payload); -} - -void send_adaptive_ICL_theta_diag_dot_ICL_debug(debug_msg_t *payload) -{ - float theta_diag_dot_ICL_esti[3]; - theta_diag_dot_ICL_esti[0] = mat_data(theta_diag_hat_dot_ICL)[0]; - theta_diag_dot_ICL_esti[1] = mat_data(theta_diag_hat_dot_ICL)[1]; - theta_diag_dot_ICL_esti[2] = mat_data(theta_diag_hat_dot_ICL)[2]; - - pack_debug_debug_message_header(payload, MESSAGE_ID_ICL_THETA_DIAG_DOT); - pack_debug_debug_message_float(&theta_diag_dot_ICL_esti[0], payload); - pack_debug_debug_message_float(&theta_diag_dot_ICL_esti[1], payload); - pack_debug_debug_message_float(&theta_diag_dot_ICL_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); } diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h index 69ebc85a..c8ecdb4c 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h @@ -14,13 +14,7 @@ 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; -void send_adaptive_ICL_theta_m_debug(debug_msg_t *payload); -void send_adaptive_ICL_theta_m_dot_debug(debug_msg_t *payload); -void send_adaptive_ICL_theta_m_dot_adaptive_debug(debug_msg_t *payload); -void send_adaptive_ICL_theta_m_dot_ICL_debug(debug_msg_t *payload); -void send_adaptive_ICL_theta_diag_debug(debug_msg_t *payload); -void send_adaptive_ICL_theta_diag_dot_debug(debug_msg_t *payload); -void send_adaptive_ICL_theta_diag_dot_adaptive_debug(debug_msg_t *payload); -void send_adaptive_ICL_theta_diag_dot_ICL_debug(debug_msg_t *payload); +void send_adaptive_ICL_mass_estimation_debug(debug_msg_t *payload); +void send_adaptive_ICL_inertia_estimation_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 07a1b38f..aa4304a6 100644 --- a/src/core/debug_link/debug_link.h +++ b/src/core/debug_link/debug_link.h @@ -28,14 +28,8 @@ enum { MESSAGE_ID_BAROMETER = 16, MESSAGE_ID_ALT_EST = 17, MESSAGE_ID_INS_SENSOR = 18, - MESSAGE_ID_ICL_THETA_M = 31, - MESSAGE_ID_ICL_THETA_M_DOT = 32, - MESSAGE_ID_ICL_THETA_M_DOT_ADAPTIVE = 33, - MESSAGE_ID_ICL_THETA_M_DOT_ICL = 34, - MESSAGE_ID_ICL_THETA_DIAG = 35, - MESSAGE_ID_ICL_THETA_DIAG_DOT = 36, - MESSAGE_ID_ICL_THETA_DIAG_DOT_ADAPTIVE = 37, - MESSAGE_ID_ICL_THETA_DIAG_DOT_ICL = 38 + MESSAGE_ID_ICL_MASS_ESTIMATION = 31, + MESSAGE_ID_ICL_INERTIA_ESTIMATION = 32 } MESSAGE_ID; typedef struct { diff --git a/src/core/tasks/debug_link_task.c b/src/core/tasks/debug_link_task.c index 02923aea..c15de826 100644 --- a/src/core/tasks/debug_link_task.c +++ b/src/core/tasks/debug_link_task.c @@ -46,14 +46,8 @@ 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_theta_m_debug(&payload); - //send_adaptive_ICL_theta_m_dot_debug(&payload); - //send_adaptive_ICL_theta_m_dot_adaptive_debug(&payload); - //send_adaptive_ICL_theta_m_dot_ICL_debug(&payload); - //send_adaptive_ICL_theta_diag_debug(&payload); - //send_adaptive_ICL_theta_diag_dot_debug(&payload); - //send_adaptive_ICL_theta_diag_dot_adaptive_debug(&payload); - //send_adaptive_ICL_theta_diag_dot_ICL_debug(&payload); + //send_adaptive_ICL_mass_estimation_debug(&payload); + //send_adaptive_ICL_inertia_estimation_debug(&payload); send_onboard_data(payload.s, payload.len); //freertos_task_delay(delay_time_ms); From 683f6aedc828028216cb740c3bb2abf88aeac25b Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 9 Nov 2020 12:19:00 +0800 Subject: [PATCH 25/62] add plotter for adaptive ICL control --- tools/serial_plot.py | 58 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/tools/serial_plot.py b/tools/serial_plot.py index 492b1692..62c3703b 100644 --- a/tools/serial_plot.py +++ b/tools/serial_plot.py @@ -415,6 +415,64 @@ def set_figure(self, message_id): self.create_curve('velocity', 'red') self.show_subplot() + elif (message_id == 31): + plt.subplot(411) + plt.ylabel('theta_m_hat [kg]') + plt.ylim([-2, 2]) + self.create_curve('theta_m_hat', 'red') + self.show_subplot() + + plt.subplot(412) + plt.ylabel('theta_m_hat_dot [kg/s]') + plt.ylim([-1, 1]) + self.create_curve('theta_m_hat_dot', 'red') + self.show_subplot() + + plt.subplot(413) + plt.ylabel('theta_m_hat_dot_adaptive [kg/s]') + plt.ylim([-1, 1]) + self.create_curve('theta_m_hat_dot_adaptive', 'red') + self.show_subplot() + + plt.subplot(414) + plt.ylabel('theta_m_hat_dot_adaptive [kg/s]') + plt.ylim([-1, 1]) + self.create_curve('theta_m_hat_dot_adaptive', 'red') + self.show_subplot() + + elif (message_id == 32): + plt.subplot(411) + plt.ylabel('theta_diag_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(412) + plt.ylabel('theta_diag_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(413) + plt.ylabel('adaptive updated [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(414) + plt.ylabel('ICL updated [kg*m^2/s]') + plt.ylim([-0.1, 0.1]) + self.create_curve('J_xx_hat_dot', 'red') + self.create_curve('J_yy_hat_dotMy', 'blue') + self.create_curve('J_zz_hat_dot', 'green') + self.show_subplot() + def show_graph(self): ani = animation.FuncAnimation(self.figure, self.animate, np.arange(0, 200), \ interval=0, blit=True) From b19db3285f81c5a65bb52ea44335e2bc6681725a Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 9 Nov 2020 12:36:37 +0800 Subject: [PATCH 26/62] separate feedforward selection to manual control and tracking control --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 12 ++++++------ src/proj_config.h | 13 +++++++++---- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 2702ff2b..118693a8 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -677,9 +677,9 @@ void geometry_manual_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *ou /* moment feedforward control */ float moment_ff[3] = {0, 0}; -#if (SELECT_FEEDFORWARD == FEEDFORWARD_USE_GEOMETRY) +#if (SELECT_FEEDFORWARD_MANUAL == FEEDFORWARD_MANUAL_USE_GEOMETRY) moment_ff_ctrl_use_geometry(moment_ff); -#elif (SELECT_FEEDFORWARD ==FEEDFORWARD_USE_ADAPTIVE_ICL) +#elif (SELECT_FEEDFORWARD_MANUAL == FEEDFORWARD_MANUAL_USE_ADAPTIVE_ICL) moment_ff_ctrl_use_adaptive_ICL(moment_ff); #endif @@ -739,9 +739,9 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * float force_ff_ned[3] = {0.0f}; assign_vector_3x1_eun_to_ned(accel_ff_ned, autopilot.wp_now.acc_feedforward); -#if (SELECT_FEEDFORWARD == FEEDFORWARD_USE_GEOMETRY) +#if (SELECT_FEEDFORWARD_TRACKING == FEEDFORWARD_TRACKING_USE_GEOMETRY) force_ff_ctrl_use_geometry(accel_ff_ned, force_ff_ned); -#elif (SELECT_FEEDFORWARD == FEEDFORWARD_USE_ADAPTIVE_ICL) +#elif (SELECT_FEEDFORWARD_TRACKING == FEEDFORWARD_TRACKING_USE_ADAPTIVE_ICL) force_ff_ctrl_use_adaptive_ICL(accel_ff_ned, force_ff_ned, pos_error, vel_error, curr_vel_ned); #endif @@ -847,9 +847,9 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * /* moment feedforward control */ float moment_ff[3] = {0.0}; -#if (SELECT_FEEDFORWARD == FEEDFORWARD_USE_GEOMETRY) +#if (SELECT_FEEDFORWARD_TRACKING == FEEDFORWARD_TRACKING_USE_GEOMETRY) moment_ff_ctrl_use_geometry(moment_ff); -#elif (SELECT_FEEDFORWARD == FEEDFORWARD_USE_ADAPTIVE_ICL) +#elif (SELECT_FEEDFORWARD_TRACKING == FEEDFORWARD_TRACKING_USE_ADAPTIVE_ICL) moment_ff_ctrl_use_adaptive_ICL(moment_ff); #endif diff --git a/src/proj_config.h b/src/proj_config.h index 731d6da9..2adb7208 100644 --- a/src/proj_config.h +++ b/src/proj_config.h @@ -24,10 +24,15 @@ #define QUADROTOR_USE_GEOMETRY 1 #define SELECT_CONTROLLER QUADROTOR_USE_GEOMETRY -/* feedforward control */ -#define FEEDFORWARD_USE_GEOMETRY 0 -#define FEEDFORWARD_USE_ADAPTIVE_ICL 1 -#define SELECT_FEEDFORWARD FEEDFORWARD_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 + +/* feedforward control for tracking control */ +#define FEEDFORWARD_TRACKING_USE_GEOMETRY 0 +#define FEEDFORWARD_TRACKING_USE_ADAPTIVE_ICL 1 +#define SELECT_FEEDFORWARD_TRACKING FEEDFORWARD_TRACKING_USE_GEOMETRY /* integral concurrent learning */ #define ADAPTIVE_WITHOUT_ICL 0 From 90e954c620ee50fddc0e0b62cbf207bf3a9bebfa Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 9 Nov 2020 12:46:21 +0800 Subject: [PATCH 27/62] modify boundary of force feedforward control in z direction --- .../controllers/multirotor_geometry/multirotor_geometry_ctrl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 118693a8..14993fa2 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -476,7 +476,7 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos bound_float(&force_ff[0], 0.8, -0.8); bound_float(&force_ff[1], 0.8, -0.8); - bound_float(&force_ff[2], 0.8, -0.8); + bound_float(&force_ff[2], 12, -12); } void moment_ff_ctrl_use_geometry(float *mom_ff) From ec435d8ee58055410f07a86041de5d684893dc8c Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 9 Nov 2020 13:00:35 +0800 Subject: [PATCH 28/62] multirotor_geometry: correct y_m_clt_integral in adaptive ICL control --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 14993fa2..f630eff2 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -416,9 +416,9 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos 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_clt_integral)[0]; - mat_data(y_m_clt_integral)[1] = mat_data(y_m_clt_integral)[1]; - mat_data(y_m_clt_integral)[2] = mat_data(y_m_clt_integral)[2]; + 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; From bbf7646598ef10d6b75caf861cefb6b7cd3cb5e7 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 9 Nov 2020 16:47:23 +0800 Subject: [PATCH 29/62] multirotor_geometry: test for regression matrix for adaptive ICL control --- .../multirotor_geometry_ctrl.c | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index f630eff2..8e03e5c1 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -466,6 +466,17 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos + mat_data(theta_m_hat_dot_ICL)[0]; #endif +#if 1 + 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 0 mat_data(theta_m_hat)[0] += mat_data(theta_m_hat_dot)[0] * dt; /* translational adaptive feedforward term */ @@ -477,6 +488,7 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos bound_float(&force_ff[0], 0.8, -0.8); bound_float(&force_ff[1], 0.8, -0.8); bound_float(&force_ff[2], 12, -12); +#endif } void moment_ff_ctrl_use_geometry(float *mom_ff) @@ -612,6 +624,20 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff) + mat_data(theta_diag_hat_dot_ICL)[2]; #endif +#if 1 + 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 0 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; @@ -626,6 +652,7 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff) bound_float(&mom_ff[0], 0.1, -0.1); bound_float(&mom_ff[1], 0.1, -0.1); bound_float(&mom_ff[2], 0.1, -0.1); +#endif } void geometry_manual_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *output_moments, From c73f148289cb94423d1e496910c113a223ee4b39 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 9 Nov 2020 19:45:34 +0800 Subject: [PATCH 30/62] comment out function for sending debug message --- src/core/tasks/debug_link_task.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/tasks/debug_link_task.c b/src/core/tasks/debug_link_task.c index c15de826..9dfdd218 100644 --- a/src/core/tasks/debug_link_task.c +++ b/src/core/tasks/debug_link_task.c @@ -30,7 +30,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); From 2cfcbfe1d579c2a2a7427c01a8f3edffef27ccec Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 9 Nov 2020 19:48:23 +0800 Subject: [PATCH 31/62] multirotor_geometry: finish testing regression matrix --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 8e03e5c1..f8c2874f 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -466,7 +466,7 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos + mat_data(theta_m_hat_dot_ICL)[0]; #endif -#if 1 +#if 0 mat_data(theta_m_hat)[0] = uav_mass; /* translational adaptive feedforward term */ @@ -476,7 +476,7 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos force_ff[2] = mat_data(Y_m)[2]*mat_data(theta_m_hat)[0]; #endif -#if 0 +#if 1 mat_data(theta_m_hat)[0] += mat_data(theta_m_hat_dot)[0] * dt; /* translational adaptive feedforward term */ @@ -624,7 +624,7 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff) + mat_data(theta_diag_hat_dot_ICL)[2]; #endif -#if 1 +#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]; @@ -637,7 +637,7 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff) mom_ff[2] = mat_data(M_ff)[2]; #endif -#if 0 +#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; From b828e1244de8d3336cfb9e0c82dee8a35d3e38a5 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Tue, 10 Nov 2020 19:00:59 +0800 Subject: [PATCH 32/62] separate selection of feedforwad control to force part and moment part --- .../multirotor_geometry_ctrl.c | 16 +++++------ src/proj_config.h | 28 +++++++++++++------ 2 files changed, 27 insertions(+), 17 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index f8c2874f..e1b459ba 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -407,10 +407,10 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos /* first term of theta_m update law */ MAT_MULT(&Y_mt, &ev_C1ex, &Ymt_evC1ex); -#if (SELECT_ADAPTIVE_W_WO_ICL == ADAPTIVE_WITHOUT_ICL) +#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_ADAPTIVE_W_WO_ICL == ADAPTIVE_WITH_ICL) +#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]); @@ -536,12 +536,12 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff) /* first term of theta_diag update law */ MAT_MULT(&Y_diagt, &eW_C2eR, &Ydiagt_eWC2eR); -#if (SELECT_ADAPTIVE_W_WO_ICL == ADAPTIVE_WITHOUT_ICL) +#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_ADAPTIVE_W_WO_ICL == ADAPTIVE_WITH_ICL) +#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; @@ -766,9 +766,9 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * float force_ff_ned[3] = {0.0f}; assign_vector_3x1_eun_to_ned(accel_ff_ned, autopilot.wp_now.acc_feedforward); -#if (SELECT_FEEDFORWARD_TRACKING == FEEDFORWARD_TRACKING_USE_GEOMETRY) +#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 == FEEDFORWARD_TRACKING_USE_ADAPTIVE_ICL) +#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 @@ -874,9 +874,9 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * /* moment feedforward control */ float moment_ff[3] = {0.0}; -#if (SELECT_FEEDFORWARD_TRACKING == FEEDFORWARD_TRACKING_USE_GEOMETRY) +#if (SELECT_FEEDFORWARD_TRACKING_MOMENT == FEEDFORWARD_TRACKING_MOMENT_USE_GEOMETRY) moment_ff_ctrl_use_geometry(moment_ff); -#elif (SELECT_FEEDFORWARD_TRACKING == FEEDFORWARD_TRACKING_USE_ADAPTIVE_ICL) +#elif (SELECT_FEEDFORWARD_TRACKING_MOMENT == FEEDFORWARD_TRACKING_MOMENT_USE_ADAPTIVE_ICL) moment_ff_ctrl_use_adaptive_ICL(moment_ff); #endif diff --git a/src/proj_config.h b/src/proj_config.h index 2adb7208..3b35e649 100644 --- a/src/proj_config.h +++ b/src/proj_config.h @@ -29,15 +29,25 @@ #define FEEDFORWARD_MANUAL_USE_ADAPTIVE_ICL 1 #define SELECT_FEEDFORWARD_MANUAL FEEDFORWARD_MANUAL_USE_GEOMETRY -/* feedforward control for tracking control */ -#define FEEDFORWARD_TRACKING_USE_GEOMETRY 0 -#define FEEDFORWARD_TRACKING_USE_ADAPTIVE_ICL 1 -#define SELECT_FEEDFORWARD_TRACKING FEEDFORWARD_TRACKING_USE_GEOMETRY - -/* integral concurrent learning */ -#define ADAPTIVE_WITHOUT_ICL 0 -#define ADAPTIVE_WITH_ICL 1 -#define SELECT_ADAPTIVE_W_WO_ICL ADAPTIVE_WITHOUT_ICL +/* 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 /* heading sensor */ #define NO_HEADING_SENSOR 0 From 75d26bb31c845f19577853da213bcfa0fea87c37 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Wed, 11 Nov 2020 13:12:21 +0800 Subject: [PATCH 33/62] multirotor_geometry: increase the boundary of moment feedforward control --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index e1b459ba..86c984ed 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -649,9 +649,9 @@ void moment_ff_ctrl_use_adaptive_ICL(float *mom_ff) mom_ff[1] = mat_data(M_ff)[1]; mom_ff[2] = mat_data(M_ff)[2]; - bound_float(&mom_ff[0], 0.1, -0.1); - bound_float(&mom_ff[1], 0.1, -0.1); - bound_float(&mom_ff[2], 0.1, -0.1); + 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 } From 48a7ee529138a27fc99bbb79bfd2bb95d0afafd0 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Thu, 12 Nov 2020 11:51:50 +0800 Subject: [PATCH 34/62] multirotor_geometry: modify boundary of feedforward control and initialize mass estimation --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 86c984ed..28e895ad 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -334,6 +334,9 @@ void geometry_ctrl_init(void) k_cl_diag_gain[1] = 0.1f; k_cl_diag_gain[2] = 0.1f; + /* initialize value of mass estimation */ + mat_data(theta_m_hat)[0] = 10.5; + 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]); @@ -485,9 +488,9 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos 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], 0.8, -0.8); - bound_float(&force_ff[1], 0.8, -0.8); - bound_float(&force_ff[2], 12, -12); + 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 } From b7be779106fc9d4e3dce5a3a9da1320df1f40d5a Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Thu, 12 Nov 2020 11:54:16 +0800 Subject: [PATCH 35/62] modify ylabel of mass and moment of inertia estimation --- tools/serial_plot.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/tools/serial_plot.py b/tools/serial_plot.py index 62c3703b..aac3cf4e 100644 --- a/tools/serial_plot.py +++ b/tools/serial_plot.py @@ -417,27 +417,27 @@ def set_figure(self, message_id): elif (message_id == 31): plt.subplot(411) - plt.ylabel('theta_m_hat [kg]') + plt.ylabel('m_hat [kg]') plt.ylim([-2, 2]) - self.create_curve('theta_m_hat', 'red') + self.create_curve('m_hat', 'red') self.show_subplot() plt.subplot(412) - plt.ylabel('theta_m_hat_dot [kg/s]') + plt.ylabel('m_hat_dot [kg/s]') plt.ylim([-1, 1]) - self.create_curve('theta_m_hat_dot', 'red') + self.create_curve('m_hat_dot', 'red') self.show_subplot() plt.subplot(413) - plt.ylabel('theta_m_hat_dot_adaptive [kg/s]') + plt.ylabel('m_hat_dot_adaptive [kg/s]') plt.ylim([-1, 1]) - self.create_curve('theta_m_hat_dot_adaptive', 'red') + self.create_curve('m_hat_dot_adaptive', 'red') self.show_subplot() plt.subplot(414) - plt.ylabel('theta_m_hat_dot_adaptive [kg/s]') + plt.ylabel('m_hat_dot_ICL [kg/s]') plt.ylim([-1, 1]) - self.create_curve('theta_m_hat_dot_adaptive', 'red') + self.create_curve('m_hat_dot_ICL', 'red') self.show_subplot() elif (message_id == 32): @@ -469,7 +469,7 @@ def set_figure(self, message_id): plt.ylabel('ICL updated [kg*m^2/s]') plt.ylim([-0.1, 0.1]) self.create_curve('J_xx_hat_dot', 'red') - self.create_curve('J_yy_hat_dotMy', 'blue') + self.create_curve('J_yy_hat_dot', 'blue') self.create_curve('J_zz_hat_dot', 'green') self.show_subplot() From 4e437e11d99031ca0eacb895332fd74423f82c2b Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Thu, 12 Nov 2020 14:56:48 +0800 Subject: [PATCH 36/62] multirotor_geometry: modify the initial value of mass estimation --- .../controllers/multirotor_geometry/multirotor_geometry_ctrl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 28e895ad..457c22e0 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -335,7 +335,7 @@ void geometry_ctrl_init(void) k_cl_diag_gain[2] = 0.1f; /* initialize value of mass estimation */ - mat_data(theta_m_hat)[0] = 10.5; + mat_data(theta_m_hat)[0] = 1.3; 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], From 9782f11c3c980f4b29989b02e1d4dfa8a40390e5 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Thu, 12 Nov 2020 14:58:13 +0800 Subject: [PATCH 37/62] increase the boundary of ylim() --- tools/serial_plot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/serial_plot.py b/tools/serial_plot.py index aac3cf4e..79db324a 100644 --- a/tools/serial_plot.py +++ b/tools/serial_plot.py @@ -418,7 +418,7 @@ def set_figure(self, message_id): elif (message_id == 31): plt.subplot(411) plt.ylabel('m_hat [kg]') - plt.ylim([-2, 2]) + plt.ylim([-10, 10]) self.create_curve('m_hat', 'red') self.show_subplot() From 189d85b6fdf8bf33930310987f033010bbe49c3d Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 16 Nov 2020 17:20:22 +0800 Subject: [PATCH 38/62] multirotor_geometry: change the direction of z-axis in adaptive force controller --- .../controllers/multirotor_geometry/multirotor_geometry_ctrl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 457c22e0..53965da1 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -412,7 +412,7 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos #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]; + 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]); From 7a169b57b3cf824b1f59b670320a603a4cb3d62a Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Mon, 16 Nov 2020 17:22:49 +0800 Subject: [PATCH 39/62] send mass estimation debug message by default --- src/core/tasks/debug_link_task.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/tasks/debug_link_task.c b/src/core/tasks/debug_link_task.c index 9dfdd218..481ca1e0 100644 --- a/src/core/tasks/debug_link_task.c +++ b/src/core/tasks/debug_link_task.c @@ -46,7 +46,7 @@ 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_mass_estimation_debug(&payload); //send_adaptive_ICL_inertia_estimation_debug(&payload); send_onboard_data(payload.s, payload.len); From c2642a63f2e98ad0ac8eaec6bf071281567ff91f Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Wed, 25 Nov 2020 14:35:05 +0800 Subject: [PATCH 40/62] modify label name --- tools/serial_plot.py | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/tools/serial_plot.py b/tools/serial_plot.py index 79db324a..90418f2f 100644 --- a/tools/serial_plot.py +++ b/tools/serial_plot.py @@ -418,7 +418,7 @@ def set_figure(self, message_id): elif (message_id == 31): plt.subplot(411) plt.ylabel('m_hat [kg]') - plt.ylim([-10, 10]) + plt.ylim([-4, 4]) self.create_curve('m_hat', 'red') self.show_subplot() @@ -429,20 +429,20 @@ def set_figure(self, message_id): self.show_subplot() plt.subplot(413) - plt.ylabel('m_hat_dot_adaptive [kg/s]') + plt.ylabel('adaptive [kg/s]') plt.ylim([-1, 1]) - self.create_curve('m_hat_dot_adaptive', 'red') + self.create_curve('adaptive', 'red') self.show_subplot() plt.subplot(414) - plt.ylabel('m_hat_dot_ICL [kg/s]') + plt.ylabel('ICL [kg/s]') plt.ylim([-1, 1]) - self.create_curve('m_hat_dot_ICL', 'red') + self.create_curve('ICL', 'red') self.show_subplot() elif (message_id == 32): plt.subplot(411) - plt.ylabel('theta_diag_hat [kg*m^2]') + 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') @@ -450,7 +450,7 @@ def set_figure(self, message_id): self.show_subplot() plt.subplot(412) - plt.ylabel('theta_diag_hat_dot [kg*m^2/s]') + 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') @@ -458,19 +458,19 @@ def set_figure(self, message_id): self.show_subplot() plt.subplot(413) - plt.ylabel('adaptive updated [kg*m^2/s]') + plt.ylabel('adaptive') 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.create_curve('x', 'red') + self.create_curve('y', 'blue') + self.create_curve('z', 'green') self.show_subplot() plt.subplot(414) - plt.ylabel('ICL updated [kg*m^2/s]') + plt.ylabel('ICL') 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.create_curve('x', 'red') + self.create_curve('y', 'blue') + self.create_curve('z', 'green') self.show_subplot() def show_graph(self): From 08089104b150e8ddd675d589aafaad13918681f1 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Thu, 26 Nov 2020 10:59:36 +0800 Subject: [PATCH 41/62] multirotor_geometry: initialize value of moment of inertia estimation --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 53965da1..48c7efbf 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -335,7 +335,12 @@ void geometry_ctrl_init(void) k_cl_diag_gain[2] = 0.1f; /* initialize value of mass estimation */ - mat_data(theta_m_hat)[0] = 1.3; + 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], From 93714e040c0e01c635e0b8e77f73d55635a05370 Mon Sep 17 00:00:00 2001 From: ChengChengYang1997 Date: Wed, 2 Dec 2020 18:01:06 +0800 Subject: [PATCH 42/62] correct the sign of feedforward moment control input --- .../multirotor_geometry_ctrl.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 48c7efbf..cf69e780 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -516,13 +516,13 @@ 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 + 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 + 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 @@ -738,9 +738,9 @@ void geometry_manual_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *ou #endif /* control input M1, M2, M3 */ - output_moments[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] + moment_ff[0]; - output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] + moment_ff[1]; - output_moments[2] = -_krz*mat_data(eR)[2] -_kwz*mat_data(eW)[2] + moment_ff[2]; + output_moments[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] - moment_ff[0]; + output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] - moment_ff[1]; + output_moments[2] = -_krz*mat_data(eR)[2] -_kwz*mat_data(eW)[2] - moment_ff[2]; } void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *curr_pos_ned, From 063afe02ee7dc01c5239f9b48bcf798848637502 Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Wed, 17 Feb 2021 11:53:49 +0800 Subject: [PATCH 43/62] add function for sending debug messages for mass and inertia estimation --- .../send_debug_adaptive_ICL.c | 17 +++++++++++++++++ .../send_debug_adaptive_ICL.h | 1 + src/core/debug_link/debug_link.h | 3 ++- src/core/tasks/debug_link_task.c | 3 ++- 4 files changed, 22 insertions(+), 2 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c index cb3135bb..33051ced 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c @@ -53,3 +53,20 @@ void send_adaptive_ICL_inertia_estimation_debug(debug_msg_t *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]; + + 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(&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); +} diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h index c8ecdb4c..0bd36f81 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h @@ -16,5 +16,6 @@ extern arm_matrix_instance_f32 theta_diag_hat_dot_ICL; 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); #endif diff --git a/src/core/debug_link/debug_link.h b/src/core/debug_link/debug_link.h index 58bf9309..ada983dc 100644 --- a/src/core/debug_link/debug_link.h +++ b/src/core/debug_link/debug_link.h @@ -32,7 +32,8 @@ enum { MESSAGE_ID_INS_FUSION = 20, MESSAGE_ID_AHRS_COMPASS_QUALITY_CHECK = 21, MESSAGE_ID_ICL_MASS_ESTIMATION = 31, - MESSAGE_ID_ICL_INERTIA_ESTIMATION = 32 + MESSAGE_ID_ICL_INERTIA_ESTIMATION = 32, + MESSAGE_ID_ICL_MASS_INERTIA_ESTIMATION = 33 } MESSAGE_ID; typedef struct { diff --git a/src/core/tasks/debug_link_task.c b/src/core/tasks/debug_link_task.c index ded2db37..3933a846 100644 --- a/src/core/tasks/debug_link_task.c +++ b/src/core/tasks/debug_link_task.c @@ -45,8 +45,9 @@ 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_mass_estimation_debug(&payload); //send_adaptive_ICL_inertia_estimation_debug(&payload); + send_adaptive_ICL_mass_inertia_estimation_debug(&payload); //send_ins_raw_position_debug_message(&payload); //send_ins_fusion_debug_message(&payload); //send_ahrs_compass_quality_check_debug_message(&payload); From 7393984c43caaf2bff2407fa34413a670f6e1aa8 Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Wed, 17 Feb 2021 15:45:18 +0800 Subject: [PATCH 44/62] add the condition for plotting the mass and inertia estimation --- tools/serial_plot.py | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/tools/serial_plot.py b/tools/serial_plot.py index 668ea9b4..9060353a 100644 --- a/tools/serial_plot.py +++ b/tools/serial_plot.py @@ -478,6 +478,32 @@ def set_figure(self, message_id): self.create_curve('y', 'blue') self.create_curve('z', 'green') self.show_subplot() + + elif (message_id == 33): + plt.subplot(411) + plt.ylabel('mass [kg]') + plt.ylim([-2, 2]) + self.create_curve('mass', 'red') + self.show_subplot() + + plt.subplot(412) + plt.ylabel('Jxx [kg*m^2/s]') + plt.ylim([-0.1, 0.1]) + self.create_curve('J_xx', 'red') + self.show_subplot() + + plt.subplot(413) + plt.ylabel('Jyy [kg*m^2/s]') + plt.ylim([-0.1, 0.1]) + self.create_curve('Jyy', 'red') + self.show_subplot() + + plt.subplot(414) + plt.ylabel('Jzz [kg*m^2/s]') + plt.ylim([-0.1, 0.1]) + self.create_curve('Jzz', 'red') + self.show_subplot() + elif (message_id == 19): plt.subplot(111) plt.ylabel('gps raw position [m/s]') From 241837cea8b17fe05a6efaaee65d1047ebb39827 Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Thu, 25 Mar 2021 12:00:39 +0800 Subject: [PATCH 45/62] multirotor_geometry: correct the sign of the feedforward moment control input --- .../multirotor_geometry_ctrl.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 98a0346f..5de118d4 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -506,9 +506,9 @@ void moment_ff_ctrl_use_geometry(float *mom_ff) //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]; + 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) @@ -889,14 +889,14 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * #endif /* 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] = -krx*mat_data(eR)[1] -kwx*mat_data(eW)[1] + moment_ff[1]; - mat_data(curr_moment)[2] = -krx*mat_data(eR)[2] -kwx*mat_data(eW)[2] + moment_ff[2]; + mat_data(curr_moment)[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] - moment_ff[0]; + mat_data(curr_moment)[1] = -krx*mat_data(eR)[1] -kwx*mat_data(eW)[1] - moment_ff[1]; + mat_data(curr_moment)[2] = -krx*mat_data(eR)[2] -kwx*mat_data(eW)[2] - moment_ff[2]; /* control input M1, M2, M3 */ - output_moments[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] + moment_ff[0]; - output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] + moment_ff[1]; - output_moments[2] = -krz*mat_data(eR)[2] -kwz*mat_data(eW)[2] + moment_ff[2]; + output_moments[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] - moment_ff[0]; + output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] - moment_ff[1]; + output_moments[2] = -krz*mat_data(eR)[2] -kwz*mat_data(eW)[2] - moment_ff[2]; } #define l_div_4 (0.25f * (1.0f / MOTOR_TO_CG_LENGTH_M)) From 3093ba7c3ebc0921c56685cad9c80985ff268988 Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Mon, 12 Apr 2021 11:36:04 +0800 Subject: [PATCH 46/62] add time tick in adaptive ICL debug messages --- .../multirotor_geometry_ctrl.c | 2 + .../send_debug_adaptive_ICL.c | 3 ++ .../send_debug_adaptive_ICL.h | 1 + tools/serial_plot.py | 42 +++++++++++++------ 4 files changed, 36 insertions(+), 12 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 31a7eb70..0df60fdf 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -124,6 +124,7 @@ float C1_gain; float C2_gain; float k_cl_m_gain; float k_cl_diag_gain[3]; +float t_plot = 0.0f; float uav_mass; @@ -1067,6 +1068,7 @@ void multirotor_geometry_control(radio_t *rc, float *desired_heading) } else { motor_halt(); } + t_plot += dt; } void send_geometry_moment_ctrl_debug(debug_msg_t *payload) diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c index 33051ced..f6f3b4c5 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c @@ -13,6 +13,7 @@ void send_adaptive_ICL_mass_estimation_debug(debug_msg_t *payload) 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_plot, 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); @@ -40,6 +41,7 @@ void send_adaptive_ICL_inertia_estimation_debug(debug_msg_t *payload) 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_plot, 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); @@ -65,6 +67,7 @@ void send_adaptive_ICL_mass_inertia_estimation_debug(debug_msg_t *payload) 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_plot, 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); diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h index 0bd36f81..89203276 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h @@ -13,6 +13,7 @@ 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 float t_plot; void send_adaptive_ICL_mass_estimation_debug(debug_msg_t *payload); void send_adaptive_ICL_inertia_estimation_debug(debug_msg_t *payload); diff --git a/tools/serial_plot.py b/tools/serial_plot.py index ad13adb9..dd6ddf6a 100644 --- a/tools/serial_plot.py +++ b/tools/serial_plot.py @@ -422,32 +422,44 @@ def set_figure(self, message_id): self.show_subplot() elif (message_id == 31): - plt.subplot(411) + 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('m_hat [kg]') plt.ylim([-4, 4]) self.create_curve('m_hat', 'red') self.show_subplot() - plt.subplot(412) + plt.subplot(513) plt.ylabel('m_hat_dot [kg/s]') plt.ylim([-1, 1]) self.create_curve('m_hat_dot', 'red') self.show_subplot() - plt.subplot(413) + plt.subplot(514) plt.ylabel('adaptive [kg/s]') plt.ylim([-1, 1]) self.create_curve('adaptive', 'red') self.show_subplot() - plt.subplot(414) + plt.subplot(515) plt.ylabel('ICL [kg/s]') plt.ylim([-1, 1]) self.create_curve('ICL', 'red') self.show_subplot() elif (message_id == 32): - plt.subplot(411) + 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') @@ -455,7 +467,7 @@ def set_figure(self, message_id): self.create_curve('J_zz_hat', 'green') self.show_subplot() - plt.subplot(412) + 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') @@ -463,7 +475,7 @@ def set_figure(self, message_id): self.create_curve('J_zz_hat_dot', 'green') self.show_subplot() - plt.subplot(413) + plt.subplot(514) plt.ylabel('adaptive') plt.ylim([-0.1, 0.1]) self.create_curve('x', 'red') @@ -471,7 +483,7 @@ def set_figure(self, message_id): self.create_curve('z', 'green') self.show_subplot() - plt.subplot(414) + plt.subplot(515) plt.ylabel('ICL') plt.ylim([-0.1, 0.1]) self.create_curve('x', 'red') @@ -480,25 +492,31 @@ def set_figure(self, message_id): self.show_subplot() elif (message_id == 33): - plt.subplot(411) + 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(412) + 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(413) + 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(414) + plt.subplot(515) plt.ylabel('Jzz [kg*m^2/s]') plt.ylim([-0.1, 0.1]) self.create_curve('Jzz', 'red') From 6ed7d18fd8ac88554a62e9671f6233e3f8d38046 Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Mon, 12 Apr 2021 13:39:27 +0800 Subject: [PATCH 47/62] use get_sys_time_ms() to get the system time tick --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 2 -- .../multirotor_geometry/send_debug_adaptive_ICL.c | 13 ++++++++++--- .../multirotor_geometry/send_debug_adaptive_ICL.h | 1 - 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 0df60fdf..31a7eb70 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -124,7 +124,6 @@ float C1_gain; float C2_gain; float k_cl_m_gain; float k_cl_diag_gain[3]; -float t_plot = 0.0f; float uav_mass; @@ -1068,7 +1067,6 @@ void multirotor_geometry_control(radio_t *rc, float *desired_heading) } else { motor_halt(); } - t_plot += dt; } void send_geometry_moment_ctrl_debug(debug_msg_t *payload) diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c index f6f3b4c5..1c460f58 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c @@ -1,4 +1,5 @@ #include "send_debug_adaptive_ICL.h" +#include void send_adaptive_ICL_mass_estimation_debug(debug_msg_t *payload) { @@ -6,6 +7,8 @@ void send_adaptive_ICL_mass_estimation_debug(debug_msg_t *payload) float theta_m_dot_esti; float theta_m_dot_esti_adaptive; float theta_m_dot_esti_ICL; + float current_time = get_sys_time_ms(); + 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]; @@ -13,7 +16,7 @@ void send_adaptive_ICL_mass_estimation_debug(debug_msg_t *payload) 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_plot, payload); + pack_debug_debug_message_float(¤t_time, 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); @@ -26,6 +29,8 @@ void send_adaptive_ICL_inertia_estimation_debug(debug_msg_t *payload) 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]; @@ -41,7 +46,7 @@ void send_adaptive_ICL_inertia_estimation_debug(debug_msg_t *payload) 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_plot, payload); + 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); @@ -60,6 +65,8 @@ 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]; @@ -67,7 +74,7 @@ void send_adaptive_ICL_mass_inertia_estimation_debug(debug_msg_t *payload) 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_plot, payload); + 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); diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h index 89203276..0bd36f81 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h @@ -13,7 +13,6 @@ 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 float t_plot; void send_adaptive_ICL_mass_estimation_debug(debug_msg_t *payload); void send_adaptive_ICL_inertia_estimation_debug(debug_msg_t *payload); From 4132286877d78f19ab6daf79b3aeb618a24b97ed Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Tue, 13 Apr 2021 11:26:42 +0800 Subject: [PATCH 48/62] tune gains used in adaptive ICL control --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 31a7eb70..61d8126d 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -325,15 +325,15 @@ void geometry_ctrl_init(void) /* initialize gains used in adaptive ICL control */ Gamma_m_gain = 0.1f; - Gamma_diag_gain[0] = 0.1f; - Gamma_diag_gain[1] = 0.1f; - Gamma_diag_gain[2] = 0.1f; + Gamma_diag_gain[0] = 1.0f; + Gamma_diag_gain[1] = 1.0f; + Gamma_diag_gain[2] = 1.0f; C1_gain = 0.1f; C2_gain = 0.1f; k_cl_m_gain = 0.1f; - k_cl_diag_gain[0] = 0.1f; - k_cl_diag_gain[1] = 0.1f; - k_cl_diag_gain[2] = 0.1f; + k_cl_diag_gain[0] = 2.0f; + k_cl_diag_gain[1] = 2.0f; + k_cl_diag_gain[2] = 2.0f; /* initialize value of mass estimation */ mat_data(theta_m_hat)[0] = 1.3f; From e514022cdb4e78fa61940ddb6bbe9f452227dae0 Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Thu, 15 Apr 2021 15:20:09 +0800 Subject: [PATCH 49/62] tune gamma_diag_gain to make moment of inertia converge faster --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 61d8126d..72723013 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -325,15 +325,15 @@ void geometry_ctrl_init(void) /* initialize gains used in adaptive ICL control */ Gamma_m_gain = 0.1f; - Gamma_diag_gain[0] = 1.0f; - Gamma_diag_gain[1] = 1.0f; - Gamma_diag_gain[2] = 1.0f; + 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 = 0.1f; - k_cl_diag_gain[0] = 2.0f; - k_cl_diag_gain[1] = 2.0f; - k_cl_diag_gain[2] = 2.0f; + 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; From 68ffeb6169a3b7b96be17dcda5707e40b614d126 Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Mon, 19 Apr 2021 16:52:26 +0800 Subject: [PATCH 50/62] plot z and zd in mass debug messages --- .../send_debug_adaptive_ICL.c | 7 ++++-- .../send_debug_adaptive_ICL.h | 4 ++++ tools/serial_plot.py | 22 ++++++++++++++----- 3 files changed, 26 insertions(+), 7 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c index 1c460f58..5397a588 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c @@ -1,5 +1,4 @@ #include "send_debug_adaptive_ICL.h" -#include void send_adaptive_ICL_mass_estimation_debug(debug_msg_t *payload) { @@ -8,8 +7,10 @@ void send_adaptive_ICL_mass_estimation_debug(debug_msg_t *payload) float theta_m_dot_esti_adaptive; float theta_m_dot_esti_ICL; float current_time = get_sys_time_ms(); - current_time = current_time*0.001; + 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]; @@ -17,6 +18,8 @@ void send_adaptive_ICL_mass_estimation_debug(debug_msg_t *payload) 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); diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h index 0bd36f81..1ee890b4 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h @@ -4,6 +4,9 @@ #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; @@ -13,6 +16,7 @@ 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 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); diff --git a/tools/serial_plot.py b/tools/serial_plot.py index dd6ddf6a..b805056d 100644 --- a/tools/serial_plot.py +++ b/tools/serial_plot.py @@ -422,31 +422,43 @@ def set_figure(self, message_id): self.show_subplot() elif (message_id == 31): - plt.subplot(511) + plt.subplot(711) plt.ylabel('Time [s]') plt.ylim([0.0, 100.0]) self.create_curve('Time', 'red') self.show_subplot() - plt.subplot(512) + 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(513) + 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(514) + plt.subplot(716) plt.ylabel('adaptive [kg/s]') plt.ylim([-1, 1]) self.create_curve('adaptive', 'red') self.show_subplot() - plt.subplot(515) + plt.subplot(717) plt.ylabel('ICL [kg/s]') plt.ylim([-1, 1]) self.create_curve('ICL', 'red') From a514c70d1eff9ec6810c90f99508f99dcee33b34 Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Mon, 19 Apr 2021 16:54:32 +0800 Subject: [PATCH 51/62] correct the direction of updating the estimation of the mass --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 72723013..65f9a94c 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -467,8 +467,8 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos 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; + 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] From a27a9140f66c3825c15c055e2e06342696546d67 Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Fri, 23 Apr 2021 16:51:41 +0800 Subject: [PATCH 52/62] use index 0 of theta_m_hat instead of 1 and 2, and correct the direction of the theta_m_hat_dot_ICL --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 65f9a94c..e2c483ee 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -438,9 +438,9 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos 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)[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)[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) { @@ -468,7 +468,7 @@ void force_ff_ctrl_use_adaptive_ICL(float *accel_ff, float *force_ff, float *pos /* 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; + 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] From 85debaf87ca2b07d18111a7712c2cebd12cef268 Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Fri, 23 Apr 2021 17:59:57 +0800 Subject: [PATCH 53/62] use send_adaptive_ICL_mass_inertia_estimation_debug() as default --- src/core/tasks/debug_link_task.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/tasks/debug_link_task.c b/src/core/tasks/debug_link_task.c index e50be132..d2f9ae82 100644 --- a/src/core/tasks/debug_link_task.c +++ b/src/core/tasks/debug_link_task.c @@ -31,7 +31,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); From 2776d74af6eb4668f72932817ae4b294bf6e14d1 Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Sun, 25 Apr 2021 14:01:04 +0800 Subject: [PATCH 54/62] add debug message for moment control input in adaptive ICL control --- .../multirotor_geometry_ctrl.c | 6 +++ .../send_debug_adaptive_ICL.c | 38 ++++++++++++++++++ .../send_debug_adaptive_ICL.h | 4 ++ src/core/debug_link/debug_link.h | 3 +- src/core/tasks/debug_link_task.c | 1 + tools/serial_plot.py | 39 +++++++++++++++++++ 6 files changed, 90 insertions(+), 1 deletion(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index e2c483ee..8eecc2d6 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -78,6 +78,7 @@ 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); @@ -205,6 +206,7 @@ void geometry_ctrl_init(void) 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); @@ -889,6 +891,10 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * 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] = -krx*mat_data(eR)[1] -kwx*mat_data(eW)[1] - moment_ff[1]; diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c index 5397a588..29acebcf 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c @@ -83,3 +83,41 @@ void send_adaptive_ICL_mass_inertia_estimation_debug(debug_msg_t *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(M_ff)[0]; + moment_ctrl_feedforward[1] = mat_data(M_ff)[1]; + moment_ctrl_feedforward[2] = mat_data(M_ff)[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 index 1ee890b4..ec9f5107 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h @@ -16,10 +16,14 @@ 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 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 7e95b8ee..566ab0c8 100644 --- a/src/core/debug_link/debug_link.h +++ b/src/core/debug_link/debug_link.h @@ -34,7 +34,8 @@ enum { MESSAGE_ID_INS_ESKF1_COVARIANCE = 22, MESSAGE_ID_ICL_MASS_ESTIMATION = 31, MESSAGE_ID_ICL_INERTIA_ESTIMATION = 32, - MESSAGE_ID_ICL_MASS_INERTIA_ESTIMATION = 33 + MESSAGE_ID_ICL_MASS_INERTIA_ESTIMATION = 33, + MESSAGE_ID_ICL_MOMENT_CTRL = 34 } MESSAGE_ID; typedef struct { diff --git a/src/core/tasks/debug_link_task.c b/src/core/tasks/debug_link_task.c index d2f9ae82..f73169a1 100644 --- a/src/core/tasks/debug_link_task.c +++ b/src/core/tasks/debug_link_task.c @@ -49,6 +49,7 @@ void task_debug_link(void *param) //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/tools/serial_plot.py b/tools/serial_plot.py index b805056d..1e406cb7 100644 --- a/tools/serial_plot.py +++ b/tools/serial_plot.py @@ -534,6 +534,45 @@ def set_figure(self, message_id): self.create_curve('Jzz', 'red') self.show_subplot() + elif (message_id == 34): + 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() + elif (message_id == 19): plt.subplot(111) plt.ylabel('gps raw position [m/s]') From 1668d74afdd3d28e2dc541cc8c1a7933dca6a56e Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Sun, 25 Apr 2021 15:03:30 +0800 Subject: [PATCH 55/62] multiply y, z gain in saving the y, z moment control input in adaptive-ICL control --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 8eecc2d6..283779f7 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -897,8 +897,8 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * /* 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] = -krx*mat_data(eR)[1] -kwx*mat_data(eW)[1] - moment_ff[1]; - mat_data(curr_moment)[2] = -krx*mat_data(eR)[2] -kwx*mat_data(eW)[2] - moment_ff[2]; + 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]; /* control input M1, M2, M3 */ output_moments[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] - moment_ff[0]; From 20543338b99dbedd962e460a3e35a3a6bc7bcb0a Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Mon, 26 Apr 2021 20:20:55 +0800 Subject: [PATCH 56/62] assign moment feedforward control input for sending moment ctrl debug message --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 283779f7..51f4abf0 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -740,6 +740,11 @@ void geometry_manual_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *ou #endif + /* 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] - moment_ff[0]; output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] - moment_ff[1]; @@ -900,6 +905,11 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * 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] - moment_ff[0]; output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] - moment_ff[1]; From ee1fa3ad5baa0dcf9bbc9dacb9963f6d635cecff Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Mon, 26 Apr 2021 20:23:03 +0800 Subject: [PATCH 57/62] decrease COEFFICIENT_YAW from 1.0f to 0.01f --- .../controllers/multirotor_geometry/multirotor_geometry_ctrl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 51f4abf0..406cb1ea 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -28,7 +28,7 @@ #define dt 0.0025 //[s] #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 COEFFICIENT_YAW 0.01f #define N_m 10 #define N_diag 10 From f6e71a3dd06208e987739327591ead3476adb684 Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Mon, 3 May 2021 11:46:42 +0800 Subject: [PATCH 58/62] use variable inertia_effect to replace moment_ff in output_moment caculation --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 12 ++++++------ .../multirotor_geometry/send_debug_adaptive_ICL.c | 6 +++--- .../multirotor_geometry/send_debug_adaptive_ICL.h | 1 + 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 668994e7..f3e2a6f5 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -746,9 +746,9 @@ void geometry_manual_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *ou 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] - moment_ff[0]; - output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] - moment_ff[1]; - output_moments[2] = -_krz*mat_data(eR)[2] -_kwz*mat_data(eW)[2] - moment_ff[2]; + 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]; + output_moments[2] = -_krz*mat_data(eR)[2] -_kwz*mat_data(eW)[2] + mat_data(inertia_effect)[2]; } void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float *curr_pos_ned, @@ -911,9 +911,9 @@ void geometry_tracking_ctrl(euler_t *rc, float *attitude_q, float *gyro, float * 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] - moment_ff[0]; - output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] - moment_ff[1]; - output_moments[2] = -krz*mat_data(eR)[2] -kwz*mat_data(eW)[2] - moment_ff[2]; + 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]; + output_moments[2] = -krz*mat_data(eR)[2] -kwz*mat_data(eW)[2] + mat_data(inertia_effect)[2]; } #define l_div_4 (0.25f * (1.0f / MOTOR_TO_CG_LENGTH_M)) diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c index 29acebcf..ffe44e33 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.c @@ -99,9 +99,9 @@ void send_adaptive_ICL_moment_ctrl_input_debug(debug_msg_t *payload) 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(M_ff)[0]; - moment_ctrl_feedforward[1] = mat_data(M_ff)[1]; - moment_ctrl_feedforward[2] = mat_data(M_ff)[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]; diff --git a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h index ec9f5107..9fe72f3e 100644 --- a/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h +++ b/src/core/controllers/multirotor_geometry/send_debug_adaptive_ICL.h @@ -19,6 +19,7 @@ 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); From 8ee1590bf3be911e3bac607ace7ee7881c5610c0 Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Mon, 3 May 2021 11:48:42 +0800 Subject: [PATCH 59/62] send feedback term of moment control input in manual control flight mode --- .../multirotor_geometry/multirotor_geometry_ctrl.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index f3e2a6f5..f3e869b3 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -740,6 +740,10 @@ 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]; From 0d8141214efa40ae88ca24ef6c577c1659f506b1 Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Fri, 14 May 2021 10:54:52 +0800 Subject: [PATCH 60/62] set yaw coefficient to default value --- .../controllers/multirotor_geometry/multirotor_geometry_ctrl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index f3e869b3..87be5a2d 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -28,7 +28,7 @@ #define dt 0.0025 //[s] #define MOTOR_TO_CG_LENGTH 16.25f //[cm] #define MOTOR_TO_CG_LENGTH_M (MOTOR_TO_CG_LENGTH * 0.01) //[m] -#define COEFFICIENT_YAW 0.01f +#define COEFFICIENT_YAW 1.0f #define N_m 10 #define N_diag 10 From 5b3e64db461b6a9456af4b56243ae7935bb3a6a9 Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Fri, 14 May 2021 10:57:10 +0800 Subject: [PATCH 61/62] save csv file as default while using debug link --- tools/serial_plot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/serial_plot.py b/tools/serial_plot.py index 1e406cb7..4e9c2f94 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: From 1ac2354d3bf2e9d03851fd7769c5d7162c578b5e Mon Sep 17 00:00:00 2001 From: Cheng-Cheng Yang Date: Tue, 29 Jun 2021 11:45:45 +0800 Subject: [PATCH 62/62] tune ICL gain for mass estimation --- .../controllers/multirotor_geometry/multirotor_geometry_ctrl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c index 87be5a2d..bd1f0ca4 100644 --- a/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c +++ b/src/core/controllers/multirotor_geometry/multirotor_geometry_ctrl.c @@ -332,7 +332,7 @@ void geometry_ctrl_init(void) Gamma_diag_gain[2] = 8.0f; C1_gain = 0.1f; C2_gain = 0.1f; - k_cl_m_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;