diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b0ca385d90..fd3cdbd18c 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -672,7 +672,6 @@ private: # if defined(CONFIG_EKF2_GNSS_YAW) estimator_aid_source1d_s _aid_src_gnss_yaw{}; - uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source # endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index d8a6b26522..a88bb5739c 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -358,7 +358,6 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample) && !_gps_intermittent; if (_control_status.flags.gps_yaw) { - if (continuing_conditions_passing) { fuseGpsYaw(gps_sample.yaw_offset); @@ -366,27 +365,7 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample) const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max); if (is_fusion_failing) { - if (_nb_gps_yaw_reset_available > 0) { - // Data seems good, attempt a reset - resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset); - - if (_control_status.flags.in_air) { - _nb_gps_yaw_reset_available--; - } - - } else if (starting_conditions_passing) { - // Data seems good, but previous reset did not fix the issue - // something else must be wrong, declare the sensor faulty and stop the fusion - _control_status.flags.gps_yaw_fault = true; - stopGpsYawFusion(); - - } else { - // A reset did not fix the issue but all the starting checks are not passing - // This could be a temporary issue, stop the fusion without declaring the sensor faulty - stopGpsYawFusion(); - } - - // TODO: should we give a new reset credit when the fusion does not fail for some time? + stopGpsYawFusion(); } } else { @@ -397,14 +376,27 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample) } else { if (starting_conditions_passing) { // Try to activate GPS yaw fusion - if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) { - ECL_INFO("starting GPS yaw fusion"); + const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos; - _aid_src_gnss_yaw.time_last_fuse = _time_delayed_us; + if (!_control_status.flags.in_air + || !_control_status.flags.yaw_align + || not_using_ne_aiding) { + + // Reset before starting the fusion + if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) { + _aid_src_gnss_yaw.time_last_fuse = _time_delayed_us; + _control_status.flags.gps_yaw = true; + _control_status.flags.yaw_align = true; + } + + } else if (!_aid_src_gnss_yaw.innovation_rejected) { + // Do not force a reset but wait for the consistency check to pass _control_status.flags.gps_yaw = true; - _control_status.flags.yaw_align = true; + fuseGpsYaw(gps_sample.yaw_offset); + } - _nb_gps_yaw_reset_available = 1; + if (_control_status.flags.gps_yaw) { + ECL_INFO("starting GPS yaw fusion"); } } } diff --git a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp b/src/modules/ekf2/test/test_EKF_gps_yaw.cpp index 2c3afc655a..9fd8c4b179 100644 --- a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp +++ b/src/modules/ekf2/test/test_EKF_gps_yaw.cpp @@ -275,9 +275,15 @@ TEST_F(EkfGpsHeadingTest, yawJmpOnGround) _sensor_simulator._gps.setYaw(gps_heading); _sensor_simulator.runSeconds(8); - // THEN: the fusion should reset - EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion()); + // THEN: the fusion should stop, reset to mag + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); + + // AND THEN: restart GNSS yaw fusion + _sensor_simulator.runSeconds(5); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion()); + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2); EXPECT_LT(fabsf(matrix::wrap_pi(_ekf_wrapper.getYawAngle() - gps_heading)), math::radians(1.f)); } @@ -285,7 +291,7 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir) { // GIVEN: the GPS yaw fusion activated float gps_heading = _ekf_wrapper.getYawAngle(); - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps.setYaw(gps_heading + math::radians(90.f)); _sensor_simulator.runSeconds(5); _ekf->set_in_air_status(true); @@ -295,20 +301,14 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir) _sensor_simulator._gps.setYaw(gps_heading); _sensor_simulator.runSeconds(7.5); - // THEN: the fusion should reset - EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); - - // BUT WHEN: the measurement jumps a 2nd time - gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(180.f)); - _sensor_simulator._gps.setYaw(gps_heading); - _sensor_simulator.runSeconds(7.5); + // THEN: the fusion should not reset as heading is still observable through GNSS vel/pos fusion + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter); // THEN: after a few seconds, the fusion should stop and // the estimator doesn't fall back to mag fusion because it has // been declared inconsistent with the filter states EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); EXPECT_FALSE(_ekf_wrapper.isMagHeadingConsistent()); - //TODO: should we force a reset to mag if the GNSS yaw fusion was forced to stop? EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); }