Baro offset calibration based on GNSS height (#24859)

* apply offset to baro sensors based on gnss measurements when gnss is selected as hgt-ref

* always calibrate baro with gnss if new param SENS_BAR_AUTOCAL is set accordingly

* always do baro-gnss calibration when AUTOCALIB is set, but ensure epv is small enough and gnss data is consistent with baro

* avoid update and reset in the same step

* minor change of constexpr usage

src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

@@ -256,6 +256,9 @@ void VehicleAirData::Run()
 
 	if (!_relative_calibration_done) {
 		_relative_calibration_done = UpdateRelativeCalibrations(time_now_us);
+
+	} else if (!_baro_gnss_calibration_done && _param_sens_baro_autocal.get()) {
+		_baro_gnss_calibration_done = BaroGNSSAltitudeOffset();
 	}
 
 	// Publish
@@ -336,7 +339,7 @@ bool VehicleAirData::UpdateRelativeCalibrations(const hrt_abstime time_now_us)
 		_calibration_t_first = time_now_us;
 	}
 
-	if (time_now_us - _calibration_t_first > 1_s) {
+	if (time_now_us - _calibration_t_first > 1_s && _data_sum_count[_selected_sensor_sub_index] > 0) {
 		const float pressure_primary = _data_sum[_selected_sensor_sub_index] / _data_sum_count[_selected_sensor_sub_index];
 
 		for (int instance = 0; instance < MAX_SENSOR_COUNT; ++instance) {
@@ -469,4 +472,93 @@ void VehicleAirData::PrintStatus()
 	}
 }
 
+bool VehicleAirData::BaroGNSSAltitudeOffset()
+{
+	static constexpr float kEpvReq = 8.f;
+	static constexpr float kDeltaOffsetTolerance = 4.f;
+	static constexpr uint64_t kLpfWindow = 2_s;
+	static constexpr float kLpfTimeConstant = static_cast<float>(kLpfWindow) * 1.e-6f;
+
+	sensor_gps_s gps_pos;
+
+	if (!_vehicle_gps_position_sub.update(&gps_pos)) {
+		return false;
+	}
+
+	const float pressure_sealevel = _param_sens_baro_qnh.get() * 100.0f;
+	const float baro_pressure = _data_sum[_selected_sensor_sub_index] / _data_sum_count[_selected_sensor_sub_index];
+	const float target_altitude = static_cast<float>(gps_pos.altitude_msl_m);
+
+	const float delta_alt =  getAltitudeFromPressure(baro_pressure, pressure_sealevel) - target_altitude;
+	bool gnss_baro_offset_stable = false;
+
+	if (gps_pos.epv > kEpvReq || _t_first_gnss_sample == 0) {
+		_calibration_t_first = 0;
+		_t_first_gnss_sample = gps_pos.timestamp;
+		return false;
+	}
+
+	if (_calibration_t_first == 0) {
+		_calibration_t_first = gps_pos.timestamp;
+		const float dt = (_calibration_t_first - _t_first_gnss_sample) * 1.e-6f;
+		_delta_baro_gnss_lpf.setParameters(dt, kLpfTimeConstant);
+		_delta_baro_gnss_lpf.reset(delta_alt);
+
+	} else {
+		_delta_baro_gnss_lpf.update(delta_alt);
+	}
+
+	if (gps_pos.timestamp - _calibration_t_first > kLpfWindow && !PX4_ISFINITE(_baro_gnss_offset_t1)) {
+		_baro_gnss_offset_t1 = _delta_baro_gnss_lpf.getState();
+
+	} else if (gps_pos.timestamp - _calibration_t_first > 2 * kLpfWindow && PX4_ISFINITE(_baro_gnss_offset_t1)) {
+		if (fabsf(_delta_baro_gnss_lpf.getState() - _baro_gnss_offset_t1) > kDeltaOffsetTolerance) {
+			_baro_gnss_offset_t1 = NAN;
+			_calibration_t_first = 0;
+			_t_first_gnss_sample = 0;
+
+		} else {
+			gnss_baro_offset_stable = true;
+		}
+	}
+
+	if (!gnss_baro_offset_stable) {
+		return false;
+	}
+
+	// Binary search
+	float low = -10000.0f;
+	float high = 10000.0f;
+	float offset = NAN;
+	static constexpr float kTolerance = 0.1f;
+	static constexpr int kIterations = 100;
+
+	for (int i = 0; i < kIterations; ++i) {
+		float mid = low + (high - low) / 2.0f;
+		float calibrated_altitude = getAltitudeFromPressure(baro_pressure - mid, pressure_sealevel);
+
+		if (calibrated_altitude > target_altitude + kTolerance) {
+			high = mid;
+
+		} else if (calibrated_altitude < target_altitude - kTolerance) {
+			low = mid;
+
+		} else {
+			offset = mid;
+			break;
+		}
+	}
+
+	// add new offset to existing relative offsets
+	for (int instance = 0; instance < MAX_SENSOR_COUNT; ++instance) {
+		if (_calibration[instance].device_id() != 0 && _data_sum_count[instance] > 0) {
+			_calibration[instance].set_offset(_calibration[instance].offset() + offset);
+			_calibration[instance].ParametersSave(instance);
+			param_notify_changes();
+		}
+	}
+
+	return true;
+}
+
 }; // namespace sensors

src/modules/sensors/vehicle_air_data/VehicleAirData.hpp

@@ -36,6 +36,7 @@
 #include "data_validator/DataValidatorGroup.hpp"
 
 #include <lib/sensor_calibration/Barometer.hpp>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
@@ -55,6 +56,7 @@
 #include <uORB/topics/sensors_status.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/estimator_status_flags.h>
+#include <uORB/topics/sensor_gps.h>
 
 using namespace time_literals;
 
@@ -86,6 +88,7 @@ private:
 	bool ParametersUpdate(bool force = false);
 	void UpdateStatus();
 	bool UpdateRelativeCalibrations(hrt_abstime time_now_us);
+	bool BaroGNSSAltitudeOffset();
 
 	static constexpr int MAX_SENSOR_COUNT = 4;
 
@@ -106,6 +109,8 @@ private:
 		{this, ORB_ID(sensor_baro), 3},
 	};
 
+	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+
 	calibration::Barometer _calibration[MAX_SENSOR_COUNT];
 
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
@@ -134,11 +139,16 @@ private:
 	bool _last_status_baro_fault{false};
 
 	bool _relative_calibration_done{false};
+	bool _baro_gnss_calibration_done{false};
 	uint64_t _calibration_t_first{0};
+	AlphaFilter<float> _delta_baro_gnss_lpf{};
+	float _baro_gnss_offset_t1{NAN};
+	uint64_t _t_first_gnss_sample{0};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh,
-		(ParamFloat<px4::params::SENS_BARO_RATE>) _param_sens_baro_rate
+		(ParamFloat<px4::params::SENS_BARO_RATE>) _param_sens_baro_rate,
+		(ParamBool<px4::params::SENS_BAR_AUTOCAL>) _param_sens_baro_autocal
 	)
 };
 }; // namespace sensors

src/modules/sensors/vehicle_air_data/params.c

@@ -53,3 +53,15 @@ PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
  * @unit Hz
  */
 PARAM_DEFINE_FLOAT(SENS_BARO_RATE, 20.0f);
+
+/**
+ * Barometer auto calibration
+ *
+ * Automatically calibrate barometer based on the GNSS height
+ *
+ * @boolean
+ *
+ * @category system
+ * @group Sensors
+ */
+PARAM_DEFINE_INT32(SENS_BAR_AUTOCAL, 0);