diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index a34e5672a4..efbb3ed43c 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -45,6 +45,7 @@ uint8 CS_EV_VEL = 24 # 24 - true when local frame velocity data fusion from ext 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 +uint8 CS_RNG_FAULT = 28 # 28 - true when the range finder 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 34984e38ea..3aedeb1002 100644 --- a/msg/estimator_status_flags.msg +++ b/msg/estimator_status_flags.msg @@ -32,6 +32,7 @@ bool cs_ev_vel # 24 - true when local frame velocity data fusion 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 +bool cs_rng_fault # 28 - true when the range finder 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/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 9fd3aceff9..a045e8180e 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -490,6 +490,7 @@ union filter_control_status_u { 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 + uint32_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used } flags; uint32_t value; }; diff --git a/src/modules/ekf2/EKF/sensor_range_finder.cpp b/src/modules/ekf2/EKF/sensor_range_finder.cpp index b4ea510566..94d3b891ea 100644 --- a/src/modules/ekf2/EKF/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/sensor_range_finder.cpp @@ -62,7 +62,7 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us) { updateDtDataLpf(current_time_us); - if (isSampleOutOfDate(current_time_us) || !isDataContinuous()) { + if (_is_faulty || isSampleOutOfDate(current_time_us) || !isDataContinuous()) { _is_sample_valid = false; _is_regularly_sending_data = false; return; diff --git a/src/modules/ekf2/EKF/sensor_range_finder.hpp b/src/modules/ekf2/EKF/sensor_range_finder.hpp index f8935358a2..6fae8cb328 100644 --- a/src/modules/ekf2/EKF/sensor_range_finder.hpp +++ b/src/modules/ekf2/EKF/sensor_range_finder.hpp @@ -106,6 +106,8 @@ public: float getValidMinVal() const { return _rng_valid_min_val; } float getValidMaxVal() const { return _rng_valid_max_val; } + void setFaulty() { _is_faulty = true; } + private: void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth); @@ -123,6 +125,7 @@ private: bool _is_sample_valid{}; ///< true if range finder sample retrieved from buffer is valid bool _is_regularly_sending_data{false}; ///< true if the interval between two samples is less than the maximum expected interval uint64_t _time_last_valid_us{}; ///< time the last range finder measurement was ready (uSec) + bool _is_faulty{false}; ///< the sensor should not be used anymore /* * Stuck check diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 1b294b0ba0..b38abe9d61 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1262,6 +1262,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) 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.cs_rng_fault = _ekf.control_status_flags().rng_fault; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;