automaically calibrate other baros in respect to main baro

This commit is contained in:
Marco Hauswirth
2025-01-15 15:28:00 +01:00
parent e01fef755a
commit 6816310432
2 changed files with 29 additions and 0 deletions
@@ -239,6 +239,33 @@ 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);
const 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);
const float offset = report.pressure - selected_baro_pressure;
// avoid ParameterSave if difference is not significant (<10Pa)
if (fabsf(_calibration[instance].offset() - offset) > 10.f) {
_calibration[instance].set_offset(offset);
_calibration[instance].ParametersSave(instance);
}
}
}
ParametersUpdate(true);
}
// Publish
if (_param_sens_baro_rate.get() > 0) {
int interval_us = 1e6f / _param_sens_baro_rate.get();
@@ -123,6 +123,8 @@ private:
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