From a685987082e03cef9263c35c8ec20adc708343fa Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 7 May 2021 15:29:53 +0200 Subject: [PATCH] GNSS yaw: unset yaw_align if fusion stops before takeoff We do not want to rely on a potentially badly initialized heading as it could lead to a flyaway directly after takeoff Also remove the resetMagHeading() call from the GPS fusion control logic as this is properly handled in mag_control. --- EKF/control.cpp | 35 +++++++++++++++++------------------ EKF/mag_control.cpp | 38 ++++++++++++++++++++++++++++++++++---- test/test_EKF_gps_yaw.cpp | 19 +++++++++++++++++++ 3 files changed, 70 insertions(+), 22 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 8a4f23a050..0556f4391b 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -524,27 +524,20 @@ void Ekf::controlGpsFusion() const bool want_to_reset_mag_heading = !_control_status.flags.yaw_align || _control_status.flags.ev_yaw || _mag_inhibit_yaw_reset_req; - if (want_to_reset_mag_heading && canResetMagHeading()) { - _control_status.flags.ev_yaw = false; - _control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState()); - // Handle the special case where we have not been constraining yaw drift or learning yaw bias due - // to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor. - if (_mag_inhibit_yaw_reset_req) { - _mag_inhibit_yaw_reset_req = false; - // Zero the yaw bias covariance and set the variance to the initial alignment uncertainty - P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S)); - } - } - // If the heading is valid start using gps aiding - if (_control_status.flags.yaw_align) { + if (want_to_reset_mag_heading) { + _mag_yaw_reset_req = true; + _control_status.flags.ev_yaw = false; + + } else { + // If the heading is valid start using gps aiding startGpsFusion(); } } - } else if (!(_params.fusion_mode & MASK_USE_GPS)) { - _control_status.flags.gps = false; - + } else if (_control_status.flags.gps + && (!(_params.fusion_mode & MASK_USE_GPS) || !_control_status.flags.yaw_align)) { + stopGpsFusion(); } // Handle the case where we are using GPS and another source of aiding and GPS is failing checks @@ -725,14 +718,12 @@ void Ekf::controlGpsFusion() } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { stopGpsFusion(); - stopGpsYawFusion(); _warning_events.flags.gps_data_stopped = true; ECL_WARN("GPS data stopped"); } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { // Handle the case where we are fusing another position source along GPS, // stop waiting for GPS after 1 s of lost signal stopGpsFusion(); - stopGpsYawFusion(); _warning_events.flags.gps_data_stopped_using_alternate = true; ECL_WARN("GPS data stopped, using only EV, OF or air data" ); } @@ -812,6 +803,14 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) // No yaw data in the message anymore. Stop until it comes back. stopGpsYawFusion(); } + + // Before takeoff, we do not want to continue to rely on the current heading + // if we had to stop the fusion + if (!_control_status.flags.in_air + && !_control_status.flags.gps_yaw + && _control_status_prev.flags.gps_yaw) { + _control_status.flags.yaw_align = false; + } } void Ekf::controlHeightSensorTimeouts() diff --git a/EKF/mag_control.cpp b/EKF/mag_control.cpp index fc3309e5be..1132bb0bf5 100644 --- a/EKF/mag_control.cpp +++ b/EKF/mag_control.cpp @@ -67,12 +67,14 @@ void Ekf::controlMagFusion() _num_bad_flight_yaw_events = 0; } - if (_control_status.flags.mag_fault || !_control_status.flags.yaw_align) { + if (_control_status.flags.mag_fault || !_control_status.flags.tilt_align) { stopMagFusion(); return; } _mag_yaw_reset_req |= otherHeadingSourcesHaveStopped(); + _mag_yaw_reset_req |= !_control_status.flags.yaw_align; + _mag_yaw_reset_req |= _mag_inhibit_yaw_reset_req; if (noOtherYawAidingThanMag() && _mag_data_ready) { if (_control_status.flags.in_air) { @@ -84,6 +86,11 @@ void Ekf::controlMagFusion() runOnGroundYawReset(); } + if (!_control_status.flags.yaw_align) { + // Having the yaw aligned is mandatory to continue + return; + } + // Determine if we should use simple magnetic heading fusion which works better when // there are large external disturbances or the more accurate 3-axis fusion switch (_params.mag_fusion_type) { @@ -139,7 +146,18 @@ void Ekf::runOnGroundYawReset() ? resetMagHeading(_mag_lpf.getState()) : false; - _mag_yaw_reset_req = !has_realigned_yaw; + if (has_realigned_yaw) { + _mag_yaw_reset_req = false; + _control_status.flags.yaw_align = true; + + // Handle the special case where we have not been constraining yaw drift or learning yaw bias due + // to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor. + if (_mag_inhibit_yaw_reset_req) { + _mag_inhibit_yaw_reset_req = false; + // Zero the yaw bias covariance and set the variance to the initial alignment uncertainty + P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S)); + } + } } } @@ -156,8 +174,20 @@ void Ekf::runInAirYawReset() if (canRealignYawUsingGps()) { has_realigned_yaw = realignYawGPS(); } else if (canResetMagHeading()) { has_realigned_yaw = resetMagHeading(_mag_lpf.getState()); } - _mag_yaw_reset_req = !has_realigned_yaw; - _control_status.flags.mag_aligned_in_flight = has_realigned_yaw; + if (has_realigned_yaw) { + _mag_yaw_reset_req = false; + _control_status.flags.yaw_align = true; + _control_status.flags.mag_aligned_in_flight = true; + + // Handle the special case where we have not been constraining yaw drift or learning yaw bias due + // to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor. + if (_mag_inhibit_yaw_reset_req) { + _mag_inhibit_yaw_reset_req = false; + // Zero the yaw bias covariance and set the variance to the initial alignment uncertainty + P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S)); + } + } + } } diff --git a/test/test_EKF_gps_yaw.cpp b/test/test_EKF_gps_yaw.cpp index c35d241995..4cbe4021e6 100644 --- a/test/test_EKF_gps_yaw.cpp +++ b/test/test_EKF_gps_yaw.cpp @@ -239,3 +239,22 @@ TEST_F(EkfGpsHeadingTest, yaw_jump) EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); } + +TEST_F(EkfGpsHeadingTest, stop_on_ground) +{ + // GIVEN: the GPS yaw fusion activated and there is no mag data + _sensor_simulator._mag.stop(); + float gps_heading = _ekf_wrapper.getYawAngle(); + _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator.runSeconds(5); + + // WHEN: the measurement stops + gps_heading = NAN; + _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator.runSeconds(7.5); + + // THEN: the fusion should stop and the GPS pos/vel aiding + // should stop as well because the yaw is not aligned anymore + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); +}