Browse Source

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.
main
bresch 3 years ago committed by Daniel Agar
parent
commit
de74f45e2d
  1. 3
      src/modules/ekf2/EKF/control.cpp

3
src/modules/ekf2/EKF/control.cpp

@ -142,7 +142,8 @@ void Ekf::controlFusionModes() @@ -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);
}

Loading…
Cancel
Save