diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 705a5f9a71..0e5525d545 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -47,6 +47,7 @@ bool cs_rng_terrain # 39 - true if we are fusing range finder data f bool cs_opt_flow_terrain # 40 - true if we are fusing flow data for terrain bool cs_valid_fake_pos # 41 - true if a valid constant position is being fused bool cs_constant_pos # 42 - true if the vehicle is at a constant position +bool cs_baro_fault # 43 - true when the current baro 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/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index 119c6fa1b3..2d20c64c81 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -65,6 +65,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) if ((_baro_counter == 0) || baro_sample.reset) { _baro_lpf.reset(measurement); _baro_counter = 1; + _control_status.flags.baro_fault = false; } else { _baro_lpf.update(measurement); @@ -113,7 +114,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) const bool continuing_conditions_passing = (_params.baro_ctrl == 1) && measurement_valid && (_baro_counter > _obs_buffer_length) - && !_baro_hgt_faulty; + && !_control_status.flags.baro_fault; const bool starting_conditions_passing = continuing_conditions_passing && isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL); @@ -148,7 +149,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) if (isRecent(_time_last_hgt_fuse, _params.hgt_fusion_timeout_max)) { // Some other height source is still working - _baro_hgt_faulty = true; + _control_status.flags.baro_fault = true; } } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 85d075e0f5..95efc971bc 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -620,6 +620,7 @@ uint64_t mag_heading_consistent : uint64_t opt_flow_terrain : 1; ///< 40 - true if we are fusing flow data for terrain uint64_t valid_fake_pos : 1; ///< 41 - true if a valid constant position is being fused uint64_t constant_pos : 1; ///< 42 - true if the vehicle is at a constant position + uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used } flags; uint64_t value; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e4a5e62ef6..47320e8c12 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -602,7 +602,6 @@ private: HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref}; - bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_MAGNETOMETER) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index ae7aad0406..9e1f8f5378 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1933,6 +1933,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_opt_flow_terrain = _ekf.control_status_flags().opt_flow_terrain; status_flags.cs_valid_fake_pos = _ekf.control_status_flags().valid_fake_pos; status_flags.cs_constant_pos = _ekf.control_status_flags().constant_pos; + status_flags.cs_baro_fault = _ekf.control_status_flags().baro_fault; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 041da27c09..24996c84da 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -141,6 +141,9 @@ void VehicleAirData::Run() AirTemperatureUpdate(); + estimator_status_flags_s estimator_status_flags; + const bool estimator_status_flags_updated = _estimator_status_flags_sub.update(&estimator_status_flags); + bool updated[MAX_SENSOR_COUNT] {}; for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { @@ -196,6 +199,11 @@ void VehicleAirData::Run() ParametersUpdate(true); } + if (estimator_status_flags_updated && _selected_sensor_sub_index >= 0 && _selected_sensor_sub_index == uorb_index + && estimator_status_flags.cs_baro_fault) { + _priority[uorb_index] = 1; // 1 is min priority while still being enabled + } + // pressure corrected with offset (if available) _calibration[uorb_index].SensorCorrectionsUpdate(); const float pressure_corrected = _calibration[uorb_index].Correct(report.pressure); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index fd28de34f8..ce7a8a117c 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -54,6 +54,7 @@ #include #include #include +#include using namespace time_literals; @@ -89,6 +90,8 @@ private: uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)}; + uORB::Subscription _estimator_status_flags_sub{ORB_ID(estimator_status_flags)}; + uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { {this, ORB_ID(sensor_baro), 0}, {this, ORB_ID(sensor_baro), 1},