From de74f45e2dc3af504632f977383c41f3ca3526a6 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 12 Jul 2022 17:01:05 +0200 Subject: [PATCH] ekf2: do not run rng kinematic consistency check for fixed-wings As they are always moving horizontally, the check doesn't make sense for fixed-wings. Also don't run the check while on ground to prevent getting a failed check during pre-takeoff manipulation. --- src/modules/ekf2/EKF/control.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 75341b1290..10d366a14a 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -142,7 +142,8 @@ void Ekf::controlFusionModes() _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); // Run the kinematic consistency check when not moving horizontally - if ((sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) { + if (_control_status.flags.in_air && !_control_status.flags.fixed_wing + && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) { _rng_consistency_check.setGate(_params.range_kin_consistency_gate); _rng_consistency_check.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), _time_last_imu); }