dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf: add range finder "faulty" status 

When delclared faulty, the range finder cannot be used anymore
This commit is contained in:
bresch 2022-02-07 10:31:28 +01:00 committed by Daniel Agar
parent a3b2550f07
commit 5818974f0f
6 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/estimator_status.msg
View File

@ -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

1
msg/estimator_status_flags.msg
View File

@ -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

1
src/modules/ekf2/EKF/common.h
View File

@ -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;
};

2
src/modules/ekf2/EKF/sensor_range_finder.cpp
View File

@ -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;

3
src/modules/ekf2/EKF/sensor_range_finder.hpp
View File

@ -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

1
src/modules/ekf2/EKF2.cpp
View File

@ -1262,6 +1262,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
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;