From 0c9e07c95a874ef08b9afe766df40baca69ebdb9 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 27 Oct 2017 15:45:53 +1100 Subject: [PATCH] ekf2: repeat IMU bias reset until successful --- src/modules/ekf2/ekf2_main.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 072b782ec3..de5548233e 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -457,6 +457,8 @@ void Ekf2::run() int sensor_selection_sub = orb_subscribe(ORB_ID(sensor_selection)); int sensor_baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + bool imu_bias_reset_request = false; + // because we can have several distance sensor instances with different orientations int range_finder_subs[ORB_MULTI_MAX_INSTANCES]; int range_finder_sub_index = -1; // index for downward-facing range finder subscription @@ -566,16 +568,21 @@ void Ekf2::run() if ((sensor_selection_prev.timestamp > 0) && (sensor_selection.timestamp > sensor_selection_prev.timestamp)) { if (sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) { PX4_WARN("accel id changed, resetting IMU bias"); - _ekf.reset_imu_bias(); + imu_bias_reset_request = true; } if (sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) { PX4_WARN("gyro id changed, resetting IMU bias"); - _ekf.reset_imu_bias(); + imu_bias_reset_request = true; } } } + // attempt reset until successful + if (imu_bias_reset_request) { + imu_bias_reset_request = !_ekf.reset_imu_bias(); + } + orb_check(optical_flow_sub, &optical_flow_updated); if (optical_flow_updated) {