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) {