From 0c5291d194e7ba100552a81fa2a74238ca7b166f Mon Sep 17 00:00:00 2001 From: isidroas Date: Tue, 16 Feb 2021 14:37:34 +0100 Subject: [PATCH] Heading reset to magnetometer from GPS or EV (#969) --- EKF/control.cpp | 2 +- EKF/ekf.h | 2 ++ EKF/mag_control.cpp | 11 +++++++++++ test/test_EKF_fusionLogic.cpp | 11 ++++++++--- test/test_EKF_gps_yaw.cpp | 3 +++ 5 files changed, 25 insertions(+), 4 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 49704d9c78..1f598c6bf4 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -333,7 +333,7 @@ void Ekf::controlExternalVisionFusion() fuseHeading(); } - } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel) + } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw) && isTimedOut(_time_last_ext_vision, (uint64_t)_params.reset_timeout_max)) { // Turn off EV fusion mode if no data has been received diff --git a/EKF/ekf.h b/EKF/ekf.h index a5a08e826f..964e07121c 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -382,6 +382,7 @@ private: bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event. bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements + bool _non_mag_yaw_aiding_running_prev{false}; ///< true when heading is being fused from other sources that are not the magnetometer (for example EV or GPS). bool _is_yaw_fusion_inhibited{false}; ///< true when yaw sensor use is being inhibited @@ -778,6 +779,7 @@ private: void controlMagFusion(); bool noOtherYawAidingThanMag() const; + bool otherHeadingSourcesHaveStopped(); void checkHaglYawResetReq(); float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; } diff --git a/EKF/mag_control.cpp b/EKF/mag_control.cpp index ac38c3f052..fc3309e5be 100644 --- a/EKF/mag_control.cpp +++ b/EKF/mag_control.cpp @@ -72,6 +72,8 @@ void Ekf::controlMagFusion() return; } + _mag_yaw_reset_req |= otherHeadingSourcesHaveStopped(); + if (noOtherYawAidingThanMag() && _mag_data_ready) { if (_control_status.flags.in_air) { checkHaglYawResetReq(); @@ -327,3 +329,12 @@ void Ekf::run3DMagAndDeclFusions() } } +bool Ekf::otherHeadingSourcesHaveStopped() +{ + // detect rising edge of noOtherYawAidingThanMag() + bool result = noOtherYawAidingThanMag() && _non_mag_yaw_aiding_running_prev; + + _non_mag_yaw_aiding_running_prev = !noOtherYawAidingThanMag(); + + return result; +} diff --git a/test/test_EKF_fusionLogic.cpp b/test/test_EKF_fusionLogic.cpp index ad08da3ea8..4b22e53a3d 100644 --- a/test/test_EKF_fusionLogic.cpp +++ b/test/test_EKF_fusionLogic.cpp @@ -270,6 +270,7 @@ TEST_F(EkfFusionLogicTest, doVisionVelocityFusion) TEST_F(EkfFusionLogicTest, doVisionHeadingFusion) { // WHEN: allow vision position to be fused and we send vision data + const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); _ekf_wrapper.enableExternalVisionHeadingFusion(); _sensor_simulator.startExternalVision(); _sensor_simulator.runSeconds(4); @@ -281,19 +282,23 @@ TEST_F(EkfFusionLogicTest, doVisionHeadingFusion) EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); EXPECT_FALSE(_ekf->local_position_is_valid()); EXPECT_FALSE(_ekf->global_position_is_valid()); + // THEN: Yaw state should be reset to vision + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); // WHEN: stop sending vision data _sensor_simulator.stopExternalVision(); - _sensor_simulator.runSeconds(6); + _sensor_simulator.runSeconds(7.1); // THEN: EKF should stop to intend to fuse vision position estimate // and EKF should not have a valid local position estimate anymore EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); - EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - // TODO: it is still intending to fuse ev_yaw. There should be some fallback to mag if possible + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); EXPECT_FALSE(_ekf->local_position_is_valid()); EXPECT_FALSE(_ekf->global_position_is_valid()); + // THEN: Yaw state shoud be reset to mag + EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2); } TEST_F(EkfFusionLogicTest, doBaroHeightFusion) diff --git a/test/test_EKF_gps_yaw.cpp b/test/test_EKF_gps_yaw.cpp index dc4ca24ecd..56627817f1 100644 --- a/test/test_EKF_gps_yaw.cpp +++ b/test/test_EKF_gps_yaw.cpp @@ -195,6 +195,8 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag) EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); + const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); + // BUT WHEN: the GPS yaw is suddenly invalid gps_heading = NAN; _sensor_simulator._gps.setYaw(gps_heading); @@ -204,4 +206,5 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag) // the estimator should fall back to mag fusion EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); }