From f753b92cff32cb185d58f9b8eb6644c17f8a7867 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 15 Sep 2022 15:29:51 +0200 Subject: [PATCH] ekf2: start fake pos fusion after dead-reckoning timeout Starting the fake position as soon as all the aiding sources stop makes the local position immediately invalid while we could continue to navigate for a couple of second with inertial dead-reckoning --- src/modules/ekf2/EKF/fake_height_control.cpp | 4 +++- src/modules/ekf2/EKF/fake_pos_control.cpp | 3 ++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/fake_height_control.cpp b/src/modules/ekf2/EKF/fake_height_control.cpp index 10f5d01a40..9f671b3219 100644 --- a/src/modules/ekf2/EKF/fake_height_control.cpp +++ b/src/modules/ekf2/EKF/fake_height_control.cpp @@ -50,7 +50,9 @@ void Ekf::controlFakeHgtFusion() if (fake_hgt_data_ready) { const bool continuing_conditions_passing = !isVerticalAidingActive(); - const bool starting_conditions_passing = continuing_conditions_passing; + const bool starting_conditions_passing = continuing_conditions_passing + && _vertical_velocity_deadreckon_time_exceeded + && _vertical_position_deadreckon_time_exceeded; if (_control_status.flags.fake_hgt) { if (continuing_conditions_passing) { diff --git a/src/modules/ekf2/EKF/fake_pos_control.cpp b/src/modules/ekf2/EKF/fake_pos_control.cpp index 74bcdb9878..f111840aba 100644 --- a/src/modules/ekf2/EKF/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/fake_pos_control.cpp @@ -51,7 +51,8 @@ void Ekf::controlFakePosFusion() if (fake_pos_data_ready) { const bool continuing_conditions_passing = !isHorizontalAidingActive(); - const bool starting_conditions_passing = continuing_conditions_passing; + const bool starting_conditions_passing = continuing_conditions_passing + && _horizontal_deadreckon_time_exceeded; if (_control_status.flags.fake_pos) { if (continuing_conditions_passing) {