From 589d58ce8782764c8afce14b0cb9f0c21d219d7f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 30 Oct 2017 15:47:12 +1100 Subject: [PATCH] ekf2: Fix bug preventing first IMU failover bias reset --- src/modules/ekf2/ekf2_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index de5548233e..2e5ac08066 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -561,7 +561,8 @@ void Ekf2::run() orb_check(sensor_selection_sub, &sensor_selection_updated); - if (sensor_selection_updated) { + // Always update sensor selction first time through if time stamp is non zero + if (sensor_selection_updated || (sensor_selection.timestamp == 0)) { sensor_selection_s sensor_selection_prev = sensor_selection; orb_copy(ORB_ID(sensor_selection), sensor_selection_sub, &sensor_selection);