diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 97d669612b..6e8c0ab5fe 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -280,6 +280,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; + float accel_offs_est[3] = { 0.0f, 0.0f, 0.0f }; int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ @@ -471,36 +472,45 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { hrt_abstime t = hrt_absolute_time(); float dt = (t - last_time) / 1000000.0; last_time = t; - /* calculate corrected acceleration vector in UAV frame */ - float accel_corr[3]; - acceleration_correct(accel_corr, sensor.accelerometer_raw, - params.acc_T, params.acc_offs); - /* transform acceleration vector from UAV frame to NED frame */ - float accel_NED[3]; - for (int i = 0; i < 3; i++) { - accel_NED[i] = 0.0f; - for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * accel_corr[j]; + if (att.R_valid) { + /* calculate corrected acceleration vector in UAV frame */ + float accel_corr[3]; + acceleration_correct(accel_corr, sensor.accelerometer_raw, + params.acc_T, params.acc_offs); + /* transform acceleration vector from UAV frame to NED frame */ + float accel_NED[3]; + for (int i = 0; i < 3; i++) { + accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { + accel_NED[i] += att.R[i][j] * (accel_corr[j] - accel_offs_est[j]); + } + } + accel_NED[2] += const_earth_gravity; + /* accelerometers offset drift correction: rotate acceleration error back to UAV frame and integrate */ + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + /* the inverse of a rotation matrix is its transpose, just swap i and j */ + accel_offs_est[i] += att.R[j][i] * accel_NED[j] * params.acc_offs_w * dt; + } + } + /* kalman filter prediction */ + kalman_filter_inertial_predict(dt, z_est); + /* prepare vectors for kalman filter correction */ + float z_meas[2]; // position, acceleration + bool use_z[2] = { false, false }; + if (local_flag_baroINITdone && baro_updated) { + z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt + use_z[0] = true; + } + if (accelerometer_updated) { + z_meas[1] = accel_NED[2]; + use_z[1] = true; + } + if (use_z[0] || use_z[1]) { + /* correction */ + kalman_filter_inertial_update(z_est, z_meas, params.k, + use_z); } - } - accel_NED[2] += const_earth_gravity; - /* kalman filter prediction */ - kalman_filter_inertial_predict(dt, z_est); - /* prepare vectors for kalman filter correction */ - float z_meas[2]; // position, acceleration - bool use_z[2] = { false, false }; - if (local_flag_baroINITdone && baro_updated) { - z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt - use_z[0] = true; - } - if (accelerometer_updated) { - z_meas[1] = accel_NED[2]; - use_z[1] = true; - } - if (use_z[0] || use_z[1]) { - /* correction */ - kalman_filter_inertial_update(z_est, z_meas, params.k, - use_z); } if (verbose_mode) { /* print updates rate */ @@ -521,9 +531,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - pos.x = 0.0f; - pos.vx = 0.0f; - pos.y = 0.0f; + pos.x = accel_offs_est[0]; + pos.vx = accel_offs_est[1]; + pos.y = accel_offs_est[2]; pos.vy = 0.0f; pos.z = z_est[0]; pos.vz = z_est[1]; diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index d3a165947c..8cd9844c7d 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -39,6 +39,7 @@ */ #include "position_estimator_inav_params.h" +#include "acceleration_transform.h" /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ @@ -51,6 +52,8 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_OFFS_W, 0.0f); + PARAM_DEFINE_INT32(INAV_ACC_OFFS_0, 0); PARAM_DEFINE_INT32(INAV_ACC_OFFS_1, 0); PARAM_DEFINE_INT32(INAV_ACC_OFFS_2, 0); @@ -75,6 +78,8 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_alt_20 = param_find("INAV_K_ALT_20"); h->k_alt_21 = param_find("INAV_K_ALT_21"); + h->acc_offs_w = param_find("INAV_ACC_OFFS_W"); + h->acc_offs_0 = param_find("INAV_ACC_OFFS_0"); h->acc_offs_1 = param_find("INAV_ACC_OFFS_1"); h->acc_offs_2 = param_find("INAV_ACC_OFFS_2"); @@ -101,6 +106,8 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_alt_20, &(p->k[2][0])); param_get(h->k_alt_21, &(p->k[2][1])); + param_get(h->acc_offs_w, &(p->acc_offs_w)); + /* read int32 and cast to int16 */ int32_t t; param_get(h->acc_offs_0, &t); @@ -119,5 +126,6 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->acc_t_20, &(p->acc_T[2][0])); param_get(h->acc_t_21, &(p->acc_T[2][1])); param_get(h->acc_t_22, &(p->acc_T[2][2])); + return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index 51e57a9146..2c6fb3df2e 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -43,6 +43,7 @@ struct position_estimator_inav_params { int use_gps; float k[3][2]; + float acc_offs_w; int16_t acc_offs[3]; float acc_T[3][3]; }; @@ -57,6 +58,8 @@ struct position_estimator_inav_param_handles { param_t k_alt_20; param_t k_alt_21; + param_t acc_offs_w; + param_t acc_offs_0; param_t acc_offs_1; param_t acc_offs_2;