fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

do relative calibration of baros with 1s delay

Browse Source
This commit is contained in:
Marco Hauswirth 2025-02-11 07:26:20 +01:00 committed by Marco Hauswirth
parent 9ae559e311
commit 6112d59e58
2 changed files with 33 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
View File

@ -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)

4
src/modules/sensors/vehicle_air_data/VehicleAirData.hpp
View File

@ -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<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh,
(ParamFloat<px4::params::SENS_BARO_RATE>) _param_sens_baro_rate