automaically calibrate other baros in respect to main baro

PX4 · Jan 7, 2025 · bc07585 · bc07585
1 parent 8070a9b
commit bc07585
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 0 deletions.
diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
@@ -239,6 +239,28 @@ void VehicleAirData::Run()
 		}
 	}
 
+	// when baros dont get calibrated, set the offset in respect to the current sensor
+	// this avoids height jumps when switching to a new sensor
+	if (_selected_sensor_sub_index >= 0
+	    && fabsf(_calibration[_selected_sensor_sub_index].offset() - _selected_baro_offset) > FLT_EPSILON) {
+
+		sensor_baro_s report;
+		_selected_baro_offset = _calibration[_selected_sensor_sub_index].offset();
+		_sensor_sub[_selected_sensor_sub_index].copy(&report);
+		float selected_baro_pressure = report.pressure;
+
+		for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
+			if (instance != _selected_sensor_sub_index && _advertised[instance]) {
+
+				_sensor_sub[instance].copy(&report);
+				_calibration[instance].set_offset(report.pressure - selected_baro_pressure);
+				_calibration[instance].ParametersSave(instance);
+			}
+		}
+
+		ParametersUpdate(true);
+	}
+
 	// Publish
 	if (_param_sens_baro_rate.get() > 0) {
 		int interval_us = 1e6f / _param_sens_baro_rate.get();

diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp
@@ -123,6 +123,8 @@ class VehicleAirData : public ModuleParams, public px4::ScheduledWorkItem
 
 	float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature
 
+	float _selected_baro_offset{0.01f};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh,
 		(ParamFloat<px4::params::SENS_BARO_RATE>) _param_sens_baro_rate