diff --git a/msg/estimator_event_flags.msg b/msg/estimator_event_flags.msg index 677fd9e60c..c540855b89 100644 --- a/msg/estimator_event_flags.msg +++ b/msg/estimator_event_flags.msg @@ -30,3 +30,4 @@ bool bad_yaw_using_gps_course # 7 - true when the fiter has detected bool stopping_mag_use # 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data bool vision_data_stopped # 9 - true when the vision system data has stopped for a significant time period bool emergency_yaw_reset_mag_stopped # 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 # 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 diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index 7f8dc28cb0..e96087a7a5 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -46,6 +46,10 @@ uint8 CS_GND_EFFECT = 20 # 20 - true when when protection from ground effect ind uint8 CS_RNG_STUCK = 21 # 21 - true when a stuck range finder sensor has been detected uint8 CS_GPS_YAW = 22 # 22 - true when yaw (not ground course) data from a GPS receiver is being fused uint8 CS_MAG_ALIGNED = 23 # 23 - true when the in-flight mag field alignment has been completed +uint8 CS_EV_VEL = 24 # 24 - true when local frame velocity data fusion from external vision measurements is intended +uint8 CS_SYNTHETIC_MAG_Z = 25 # 25 - true when we are using a synthesized measurement for the magnetometer Z component +uint8 CS_VEHICLE_AT_REST = 26 # 26 - true when the vehicle is at rest +uint8 CS_GPS_YAW_FAULT = 27 # 27 - true when the GNSS heading has been declared faulty and is no longer being used uint32 filter_fault_flags # Bitmask to indicate EKF internal faults # 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error diff --git a/msg/estimator_status_flags.msg b/msg/estimator_status_flags.msg index 2e9c737119..34984e38ea 100644 --- a/msg/estimator_status_flags.msg +++ b/msg/estimator_status_flags.msg @@ -31,7 +31,7 @@ bool cs_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment bool cs_ev_vel # 24 - true when local frame velocity data fusion from external vision measurements is intended bool cs_synthetic_mag_z # 25 - true when we are using a synthesized measurement for the magnetometer Z component bool cs_vehicle_at_rest # 26 - true when the vehicle is at rest - +bool cs_gps_yaw_fault # 27 - true when the GNSS heading has been declared faulty and is no longer being used # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 4a68d0f990..83b17a254d 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3786,6 +3786,8 @@ void Commander::estimator_check() } const bool mag_fault_prev = (_estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)); + const bool gnss_heading_fault_prev = (_estimator_status_sub.get().control_mode_flags & + (1 << estimator_status_s::CS_GPS_YAW_FAULT)); // use primary estimator_status if (_estimator_selector_status_sub.updated()) { @@ -3803,12 +3805,18 @@ void Commander::estimator_check() // Check for a magnetomer fault and notify the user const bool mag_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)); + const bool gnss_heading_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS_YAW_FAULT)); if (!mag_fault_prev && mag_fault) { mavlink_log_critical(&_mavlink_log_pub, "Stopping compass use! Check calibration on landing"); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, true, true, false, _status); } + if (!gnss_heading_fault_prev && gnss_heading_fault) { + mavlink_log_critical(&_mavlink_log_pub, "Stopping GNSS heading use! Check configuration on landing"); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, false, _status); + } + /* Check estimator status for signs of bad yaw induced post takeoff navigation failure * for a short time interval after takeoff. * Most of the time, the drone can recover from a bad initial yaw using GPS-inertial diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index b35037e473..5b099953d3 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -487,6 +487,7 @@ union filter_control_status_u { uint32_t ev_vel : 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended uint32_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component uint32_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest + uint32_t gps_yaw_fault : 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used } flags; uint32_t value; }; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index ea0af3dda7..cc55440909 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -751,7 +751,7 @@ void Ekf::controlGpsFusion() void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) { if (!(_params.fusion_mode & MASK_USE_GPSYAW) - || _is_gps_yaw_faulty) { + || _control_status.flags.gps_yaw_fault) { stopGpsYawFusion(); return; @@ -793,7 +793,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) } 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 - _is_gps_yaw_faulty = true; + _control_status.flags.gps_yaw_fault = true; stopGpsYawFusion(); } else { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e929755088..60bb53ce4a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -536,7 +536,6 @@ 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/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 433449da28..82518e2eef 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1698,7 +1698,7 @@ bool Ekf::resetYawToEKFGSF() _warning_events.flags.emergency_yaw_reset_mag_stopped = true; } else if (_control_status.flags.gps_yaw) { - _is_gps_yaw_faulty = true; + _control_status.flags.gps_yaw_fault = true; _warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true; } else if (_control_status.flags.ev_yaw) { diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index cc33604eb6..2ebb1cfd6d 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -641,6 +641,7 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp) event_flags.stopping_mag_use = _ekf.warning_event_flags().stopping_mag_use; event_flags.vision_data_stopped = _ekf.warning_event_flags().vision_data_stopped; event_flags.emergency_yaw_reset_mag_stopped = _ekf.warning_event_flags().emergency_yaw_reset_mag_stopped; + event_flags.emergency_yaw_reset_gps_yaw_stopped = _ekf.warning_event_flags().emergency_yaw_reset_gps_yaw_stopped; event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_event_flags_pub.publish(event_flags); @@ -1172,6 +1173,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_ev_vel = _ekf.control_status_flags().ev_vel; status_flags.cs_synthetic_mag_z = _ekf.control_status_flags().synthetic_mag_z; status_flags.cs_vehicle_at_rest = _ekf.control_status_flags().vehicle_at_rest; + status_flags.cs_gps_yaw_fault = _ekf.control_status_flags().gps_yaw_fault; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;