diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index d2e40a2cfa..b35037e473 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -552,6 +552,7 @@ union warning_event_status_u { bool stopping_mag_use : 1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data bool vision_data_stopped : 1; ///< 9 - true when the vision system data has stopped for a significant time period bool emergency_yaw_reset_mag_stopped : 1; ///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetomer data + bool emergency_yaw_reset_gps_yaw_stopped: 1; ///< 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data } flags; uint32_t value; }; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index e4b9e14f15..ea0af3dda7 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -566,10 +566,11 @@ void Ekf::controlGpsFusion() // handle case where we are not currently using GPS, but need to align yaw angle using EKF-GSF before // we can start using GPS - const bool align_yaw_using_gsf = !_control_status.flags.gps && _do_ekfgsf_yaw_reset - && isTimedOut(_ekfgsf_yaw_reset_time, 5000000); + const bool align_yaw_using_gsf = !_control_status.flags.yaw_align + && (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE); - if (align_yaw_using_gsf) { + if ((align_yaw_using_gsf || _do_ekfgsf_yaw_reset) + && isTimedOut(_ekfgsf_yaw_reset_time, 5000000)){ if (resetYawToEKFGSF()) { _ekfgsf_yaw_reset_time = _time_last_imu; _do_ekfgsf_yaw_reset = false; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index ed98e7dfd7..433449da28 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1680,16 +1680,32 @@ bool Ekf::resetYawToEKFGSF() _flt_mag_align_start_time = _imu_sample_delayed.time_us; _control_status.flags.yaw_align = true; - if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) { + const bool is_mag_fusion_active = _control_status.flags.mag_hdg + || _control_status.flags.mag_3D; + const bool is_yaw_aiding_active = is_mag_fusion_active + || _control_status.flags.gps_yaw + || _control_status.flags.ev_yaw; + + if (!is_yaw_aiding_active) { _information_events.flags.yaw_aligned_to_imu_gps = true; ECL_INFO("Yaw aligned using IMU and GPS"); } else { - // 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; - ECL_WARN("Emergency yaw reset - mag use stopped"); + if (is_mag_fusion_active) { + // 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) { + _is_gps_yaw_faulty = true; + _warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true; + + } else if (_control_status.flags.ev_yaw) { + _inhibit_ev_yaw_use = true; + } + + ECL_WARN("Emergency yaw reset"); } return true; diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index a284ef94f6..53aceafa26 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -91,15 +91,9 @@ bool Ekf::collect_gps(const gps_message &gps) _mag_strength_gps = get_mag_strength_gauss(lat, lon); // request a reset of the yaw using the new declination - if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) { - // try to reset the yaw using the EKF-GSF yaw estimator - _do_ekfgsf_yaw_reset = true; - _ekfgsf_yaw_reset_time = 0; - - } else { - if (!declination_was_valid) { - _mag_yaw_reset_req = true; - } + if ((_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) + && !declination_was_valid) { + _mag_yaw_reset_req = true; } // save the horizontal and vertical position uncertainty of the origin