diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index de81fd1ea6..d242f6440b 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -1135,8 +1135,6 @@ private: } void stopGpsFusion(); - void stopGpsPosFusion(); - void stopGpsVelFusion(); void resetFakePosFusion(); void stopFakePosFusion(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 493c207184..6c2754760a 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1160,37 +1160,29 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) _time_last_heading_fuse = _time_delayed_us; } -// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator -// Resets the horizontal velocity and position to the default navigation sensor -// Returns true if the reset was successful bool Ekf::resetYawToEKFGSF() { if (!isYawEmergencyEstimateAvailable()) { return false; } + // don't allow reset if there's just been a yaw reset + const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); + const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat); + + if (yaw_alignment_changed || quat_reset) { + return false; + } + + ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad", + (double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw()); + resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar()); // record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight _flt_mag_align_start_time = _time_delayed_us; _control_status.flags.yaw_align = true; - - if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { - // stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around - // and cause another navigation failure - _control_status.flags.mag_fault = true; - _warning_events.flags.emergency_yaw_reset_mag_stopped = true; - - } else if (_control_status.flags.gps_yaw) { - _control_status.flags.gps_yaw_fault = true; - _warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true; - - } -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - if (_control_status.flags.ev_yaw) { - _inhibit_ev_yaw_use = true; - } -#endif // CONFIG_EKF2_EXTERNAL_VISION + _information_events.flags.yaw_aligned_to_imu_gps = true; return true; } diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 9848ad6b0b..341e041858 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -119,35 +119,26 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) _aid_src_gnss_pos); _aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS); -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - // if GPS is otherwise ready to go, but yaw_align is blocked by EV give mag a chance to start - if (_control_status.flags.tilt_align && _NED_origin_initialised - && gps_checks_passing && !gps_checks_failing) { - - if (!_control_status.flags.yaw_align) { - if (_control_status.flags.ev_yaw && !_control_status.flags.yaw_align) { - - // give mag a chance to start and yaw align if currently blocked by EV yaw - const bool mag_enabled = (_params.mag_fusion_type <= MagFuseType::MAG_3D); - const bool mag_available = (_mag_counter != 0); - - if (mag_enabled && mag_available - && !_control_status.flags.mag_field_disturbed - && !_control_status.flags.mag_fault) { - - stopEvYawFusion(); - } - } - } - } -#endif // CONFIG_EKF2_EXTERNAL_VISION - // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently - const bool mandatory_conditions_passing = ((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL)) - && _control_status.flags.tilt_align - && _control_status.flags.yaw_align - && _NED_origin_initialised; + bool mandatory_conditions_passing = false; + + if (((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL)) + && _control_status.flags.tilt_align + && _NED_origin_initialised + ) { + // if GPS is otherwise ready to go other than yaw align + if (!_control_status.flags.yaw_align && gps_checks_passing && !gps_checks_failing) { + + if (resetYawToEKFGSF()) { + ECL_INFO("GPS yaw aligned using IMU"); + } + } + + if (_control_status.flags.yaw_align) { + mandatory_conditions_passing = true; + } + } const bool continuing_conditions_passing = mandatory_conditions_passing && !gps_checks_failing; const bool starting_conditions_passing = continuing_conditions_passing && gps_checks_passing; @@ -173,6 +164,31 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) */ if (resetYawToEKFGSF()) { ECL_WARN("GPS emergency yaw reset"); + + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { + // stop using the magnetometer in the main EKF otherwise it' fusion could drag the yaw around + // and cause another navigation failure + _control_status.flags.mag_fault = true; + _warning_events.flags.emergency_yaw_reset_mag_stopped = true; + } + +#if defined(CONFIG_EKF2_GNSS_YAW) + + if (_control_status.flags.gps_yaw) { + _control_status.flags.gps_yaw_fault = true; + _warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true; + } + +#endif // CONFIG_EKF2_GNSS_YAW + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + + if (_control_status.flags.ev_yaw) { + _inhibit_ev_yaw_use = true; + } + +#endif // CONFIG_EKF2_EXTERNAL_VISION + do_vel_pos_reset = true; } } @@ -203,16 +219,6 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } else { if (starting_conditions_passing) { -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - // Do not use external vision for yaw if using GPS because yaw needs to be - // defined relative to an NED reference frame - if (_control_status.flags.ev_yaw) { - // Stop the vision for yaw fusion and do not allow it to start again - stopEvYawFusion(); - _inhibit_ev_yaw_use = true; - } -#endif // CONFIG_EKF2_EXTERNAL_VISION - ECL_INFO("starting GPS fusion"); _information_events.flags.starting_gps_fusion = true; @@ -233,23 +239,6 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; _control_status.flags.gps = true; - - } else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MagFuseType::NONE)) { - // If no mag is used, align using the yaw estimator (if available) - if (resetYawToEKFGSF()) { - _information_events.flags.yaw_aligned_to_imu_gps = true; - ECL_INFO("GPS yaw aligned using IMU, resetting vel and pos"); - - // reset velocity - _information_events.flags.reset_vel_to_gps = true; - resetVelocityTo(velocity, vel_obs_var); - _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; - - // reset position - _information_events.flags.reset_pos_to_gps = true; - resetHorizontalPositionTo(position, pos_obs_var); - _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; - } } } @@ -275,7 +264,7 @@ bool Ekf::shouldResetGpsFusion() const * with no aiding we need to do something */ bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max); + && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max); #if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -379,9 +368,18 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi } else { if (starting_conditions_passing) { // Try to activate GPS yaw fusion - startGpsYawFusion(gps_sample); + if (resetYawToGps(gps_sample.yaw)) { + + stopEvYawFusion(); + stopMagHdgFusion(); + stopMag3DFusion(); + + ECL_INFO("starting GPS yaw fusion"); + + _aid_src_gnss_yaw.time_last_fuse = _time_delayed_us; + _control_status.flags.gps_yaw = true; + _control_status.flags.yaw_align = true; - if (_control_status.flags.gps_yaw) { _nb_gps_yaw_reset_available = 1; } } @@ -393,72 +391,46 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi // 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::startGpsYawFusion(const gpsSample &gps_sample) -{ - if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) { - ECL_INFO("starting GPS yaw fusion"); - _control_status.flags.yaw_align = true; - _control_status.flags.mag_dec = false; - - stopMagHdgFusion(); - stopMag3DFusion(); - _control_status.flags.gps_yaw = true; - } } #endif // CONFIG_EKF2_GNSS_YAW void Ekf::stopGpsYawFusion() { #if defined(CONFIG_EKF2_GNSS_YAW) + if (_control_status.flags.gps_yaw) { - ECL_INFO("stopping GPS yaw fusion"); + _control_status.flags.gps_yaw = false; resetEstimatorAidStatus(_aid_src_gnss_yaw); + + // 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) { + ECL_INFO("stopping GPS yaw fusion, clearing yaw alignment"); + _control_status.flags.yaw_align = false; + + } else { + ECL_INFO("stopping GPS yaw fusion"); + } } + #endif // CONFIG_EKF2_GNSS_YAW } void Ekf::stopGpsFusion() { if (_control_status.flags.gps) { - stopGpsPosFusion(); - stopGpsVelFusion(); + ECL_INFO("stopping GPS position and velocity fusion"); + resetEstimatorAidStatus(_aid_src_gnss_pos); + resetEstimatorAidStatus(_aid_src_gnss_vel); _control_status.flags.gps = false; } - if (_control_status.flags.gps_yaw) { - stopGpsYawFusion(); - } + stopGpsHgtFusion(); + stopGpsYawFusion(); // We do not need to know the true North anymore // EV yaw can start again _inhibit_ev_yaw_use = false; } - -void Ekf::stopGpsPosFusion() -{ - if (_control_status.flags.gps) { - ECL_INFO("stopping GPS position fusion"); - _control_status.flags.gps = false; - - resetEstimatorAidStatus(_aid_src_gnss_pos); - } -} - -void Ekf::stopGpsVelFusion() -{ - ECL_INFO("stopping GPS velocity fusion"); - - resetEstimatorAidStatus(_aid_src_gnss_vel); -} diff --git a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp index 518f02a233..987b92506e 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp @@ -106,10 +106,10 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment) EXPECT_NEAR(yaw_est, yaw, tolerance_rad); EXPECT_LT(yaw_est_var, tolerance_rad); - // 2 resets: 1 after IMU+GNSS yaw alignment and 1 when starting GNSS aiding + // 1 reset when starting GNSS aiding reset_logging_checker.capturePostResetState(); - EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(2)); - EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(2)); + EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1)); + EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(1)); EXPECT_TRUE(_ekf->local_position_is_valid()); EXPECT_TRUE(_ekf->global_position_is_valid());