diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index a12634055f..dba21446c6 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -254,6 +254,10 @@ void VehicleAirData::Run() } } + if (!_relative_calibration_done) { + UpdateRelativeCalibrations(time_now_us); + } + // Publish if (_param_sens_baro_rate.get() > 0) { int interval_us = 1e6f / _param_sens_baro_rate.get(); @@ -325,6 +329,31 @@ void VehicleAirData::Run() perf_end(_cycle_perf); } +void VehicleAirData::UpdateRelativeCalibrations(const hrt_abstime time_now_us) +{ + // delay calibration to allow all drivers to start up + _calibration_t_first = _calibration_t_first == 0 ? time_now_us : _calibration_t_first; + + if (time_now_us - _calibration_t_first > 1_s) { + _relative_calibration_done = true; + 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) { + + if (instance != _selected_sensor_sub_index + && _calibration[instance].device_id() != 0 + && _data_sum_count[instance] > 0) { + const float pressure_secondary = _data_sum[instance] / _data_sum_count[instance]; + const float new_offset = pressure_secondary - pressure_primary + _calibration[instance].offset(); + _calibration[instance].set_offset(new_offset); + _calibration[instance].ParametersSave(instance); + param_notify_changes(); + ParametersUpdate(true); + } + } + } +} + void VehicleAirData::CheckFailover(const hrt_abstime &time_now_us) { // check failover and report (save failover report for a cycle where parameters didn't update) diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index 1e6555fcdd..fb50138d0c 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -85,6 +85,7 @@ private: void CheckFailover(const hrt_abstime &time_now_us); bool ParametersUpdate(bool force = false); void UpdateStatus(); + void UpdateRelativeCalibrations(hrt_abstime time_now_us); static constexpr int MAX_SENSOR_COUNT = 4; @@ -132,6 +133,9 @@ private: bool _last_status_baro_fault{false}; + bool _relative_calibration_done{false}; + uint64_t _calibration_t_first{0}; + DEFINE_PARAMETERS( (ParamFloat) _param_sens_baro_qnh, (ParamFloat) _param_sens_baro_rate