From 51cd63d62648ea33253c37c5ec617f6b694a40ca Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 22 Jun 2020 15:57:53 +0200 Subject: [PATCH] GPS Yaw: fall back to other yaw aiding source in case of bad data If the user selects GPS yaw fusion but that there is no GPS yaw data in the GPS message or if the fusion is rejected for some time, the GPS yaw data is declared faulty and the fusion is stopped to allow an other source of yaw aiding to start. --- EKF/control.cpp | 13 ++++++++++++- EKF/ekf.h | 1 + test/sensor_simulator/ekf_wrapper.cpp | 14 ++++++++++++++ test/sensor_simulator/ekf_wrapper.h | 3 +++ test/test_EKF_gps_yaw.cpp | 27 +++++++++++++++++++++++++++ 5 files changed, 57 insertions(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 9ebd9976e0..e9e6aa5ff4 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -518,7 +518,10 @@ void Ekf::controlGpsFusion() // Detect if coming back after significant time without GPS data bool gps_signal_was_lost = isTimedOut(_time_prev_gps_us, 1000000); - if (!(_params.fusion_mode & MASK_USE_GPSYAW)) { + + if (!(_params.fusion_mode & MASK_USE_GPSYAW) + || _is_gps_yaw_faulty) { + stopGpsYawFusion(); } else { @@ -541,6 +544,14 @@ void Ekf::controlGpsFusion() fuseGpsAntYaw(); } } + + // Check if the data is constantly fused by the estimator, + // if not, declare the sensor faulty and stop the fusion + // By doing this, another source of yaw aiding is allowed to start + if (isTimedOut(_time_last_gps_yaw_fuse, (uint64_t)5e6)) { + _is_gps_yaw_faulty = true; + stopGpsYawFusion(); + } } // Determine if we should use GPS aiding for velocity and horizontal position diff --git a/EKF/ekf.h b/EKF/ekf.h index 6e5108ab61..a2f73053c7 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -499,6 +499,7 @@ private: // height sensor status bool _baro_hgt_faulty{false}; ///< true if valid baro data is unavailable for use bool _gps_hgt_intermittent{false}; ///< true if gps height into the buffer is intermittent + bool _is_gps_yaw_faulty{false}; ///< true if gps yaw data is rejected by the filter for too long // imu fault status uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec) diff --git a/test/sensor_simulator/ekf_wrapper.cpp b/test/sensor_simulator/ekf_wrapper.cpp index 1d56c799c3..b40dad9773 100644 --- a/test/sensor_simulator/ekf_wrapper.cpp +++ b/test/sensor_simulator/ekf_wrapper.cpp @@ -170,6 +170,20 @@ void EkfWrapper::disableExternalVisionAlignment() _ekf_params->fusion_mode &= ~MASK_ROTATE_EV; } +bool EkfWrapper::isIntendingMagHeadingFusion() const +{ + filter_control_status_u control_status; + _ekf->get_control_mode(&control_status.value); + return control_status.flags.mag_hdg; +} + +bool EkfWrapper::isIntendingMag3DFusion() const +{ + filter_control_status_u control_status; + _ekf->get_control_mode(&control_status.value); + return control_status.flags.mag_3D; +} + bool EkfWrapper::isWindVelocityEstimated() const { filter_control_status_u control_status; diff --git a/test/sensor_simulator/ekf_wrapper.h b/test/sensor_simulator/ekf_wrapper.h index ca3128f1ac..1808d00cea 100644 --- a/test/sensor_simulator/ekf_wrapper.h +++ b/test/sensor_simulator/ekf_wrapper.h @@ -84,6 +84,9 @@ public: void disableExternalVisionHeadingFusion(); bool isIntendingExternalVisionHeadingFusion() const; + bool isIntendingMagHeadingFusion() const; + bool isIntendingMag3DFusion() const; + void enableExternalVisionAlignment(); void disableExternalVisionAlignment(); diff --git a/test/test_EKF_gps_yaw.cpp b/test/test_EKF_gps_yaw.cpp index b44073e03d..50629470e5 100644 --- a/test/test_EKF_gps_yaw.cpp +++ b/test/test_EKF_gps_yaw.cpp @@ -117,3 +117,30 @@ TEST_F(EkfGpsHeadingTest, yawConvergence) EXPECT_NEAR(yaw_est, gps_heading, math::radians(0.5f)) << "yaw est: " << math::degrees(yaw_est) << "gps yaw: " << math::degrees(gps_heading); } + +TEST_F(EkfGpsHeadingTest, fallBackToMag) +{ + // GIVEN: an initial GPS yaw, not aligned with the current one + float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(10.f); + _sensor_simulator._gps.setYaw(gps_heading); + + // WHEN: the GPS yaw fusion is activated + _ekf_wrapper.enableGpsHeadingFusion(); + _sensor_simulator.runSeconds(1); + + // THEN: GPS heading fusion should have started, and mag + // fusion should be disabled + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); + + // BUT WHEN: the GPS yaw is suddenly invalid + gps_heading = NAN; + _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator.runSeconds(6); + + // THEN: after a few seconds, the fusion should stop and + // the estimator should fall back to mag fusion + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); +}