commander: report GNSS yaw fault to user

This commit is contained in:
bresch
2021-08-05 11:10:02 +02:00
committed by Mathieu Bresciani
parent 44219e9f45
commit 01d0b8800e
9 changed files with 20 additions and 5 deletions
+1
View File
@@ -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;
};
+2 -2
View File
@@ -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 {
-1
View File
@@ -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)
+1 -1
View File
@@ -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) {