diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-07-21 22:44:46 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-07-21 22:44:46 +0400 |
commit | bd50092dd29a83dd5044fc3027e20be91b3275e5 (patch) | |
tree | ccf96d42d3db6fcb014cbe266badf857fd2883ed /src/modules/position_estimator_inav/position_estimator_inav_main.c | |
parent | 98a4345410e91a1e3966b3364f487652af210d03 (diff) | |
download | px4-firmware-bd50092dd29a83dd5044fc3027e20be91b3275e5.tar.gz px4-firmware-bd50092dd29a83dd5044fc3027e20be91b3275e5.tar.bz2 px4-firmware-bd50092dd29a83dd5044fc3027e20be91b3275e5.zip |
position_estimator_inav: accelerometer bias estimation for Z, default weights for GPS updated
Diffstat (limited to 'src/modules/position_estimator_inav/position_estimator_inav_main.c')
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 20 |
1 files changed, 14 insertions, 6 deletions
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index a9bc09e0d..42b5fcb61 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -167,9 +167,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ - double lat_current = 0.0; //[°]] --> 47.0 - double lon_current = 0.0; //[°]] -->8.5 - double alt_current = 0.0; //[m] above MSL + double lat_current = 0.0f; //[°] --> 47.0 + double lon_current = 0.0f; //[°] --> 8.5 + double alt_current = 0.0f; //[m] above MSL uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -302,8 +302,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime pub_last = hrt_absolute_time(); uint32_t pub_interval = 4000; // limit publish rate to 250 Hz + /* acceleration in NED frame */ + float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; + /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D + float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame float baro_corr = 0.0f; // D float gps_corr[2][2] = { { 0.0f, 0.0f }, // N (pos, vel) @@ -369,9 +373,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (sensor.accelerometer_counter > accel_counter) { if (att.R_valid) { + /* correct accel bias, now only for Z */ + sensor.accelerometer_m_s2[2] -= accel_bias[2]; /* transform acceleration vector from body frame to NED frame */ - float accel_NED[3]; - for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; @@ -425,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.home_timestamp = hrt_absolute_time(); z_est[0] += sonar_corr; sonar_corr = 0.0f; - sonar_corr_filtered = 0.0; + sonar_corr_filtered = 0.0f; } } } @@ -470,6 +474,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; + /* accel bias correction, now only for Z + * not strictly correct, but stable and works */ + accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt; + /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est); |