forked from Archive/PX4-Autopilot
ekf: add range finder "faulty" status
When delclared faulty, the range finder cannot be used anymore
This commit is contained in:
parent
a3b2550f07
commit
5818974f0f
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
};
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue