add cs_baro_fault to switch to fallback baro if available (#24260)

PX4 · Jan 28, 2025 · 4df65c1 · 4df65c1
1 parent 92b1f51
commit 4df65c1
Show file tree

Hide file tree

Showing 7 changed files with 17 additions and 3 deletions.
diff --git 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
@@ -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
@@ -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
@@ -602,7 +602,6 @@ class Ekf final : public EstimatorInterface
 
 	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
@@ -1933,6 +1933,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		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
@@ -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
@@ -54,6 +54,7 @@
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensors_status.h>
 #include <uORB/topics/vehicle_air_data.h>
+#include <uORB/topics/estimator_status_flags.h>
 
 using namespace time_literals;
 
@@ -89,6 +90,8 @@ class VehicleAirData : public ModuleParams, public px4::ScheduledWorkItem
 
 	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},