mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-28 22:03:35 +08:00
do relative calibration of baros with 1s delay
This commit is contained in:
committed by
Marco Hauswirth
parent
9ae559e311
commit
6112d59e58
@@ -254,6 +254,10 @@ void VehicleAirData::Run()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (!_relative_calibration_done) {
|
||||||
|
UpdateRelativeCalibrations(time_now_us);
|
||||||
|
}
|
||||||
|
|
||||||
// Publish
|
// Publish
|
||||||
if (_param_sens_baro_rate.get() > 0) {
|
if (_param_sens_baro_rate.get() > 0) {
|
||||||
int interval_us = 1e6f / _param_sens_baro_rate.get();
|
int interval_us = 1e6f / _param_sens_baro_rate.get();
|
||||||
@@ -325,6 +329,31 @@ void VehicleAirData::Run()
|
|||||||
perf_end(_cycle_perf);
|
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)
|
void VehicleAirData::CheckFailover(const hrt_abstime &time_now_us)
|
||||||
{
|
{
|
||||||
// check failover and report (save failover report for a cycle where parameters didn't update)
|
// check failover and report (save failover report for a cycle where parameters didn't update)
|
||||||
|
|||||||
@@ -85,6 +85,7 @@ private:
|
|||||||
void CheckFailover(const hrt_abstime &time_now_us);
|
void CheckFailover(const hrt_abstime &time_now_us);
|
||||||
bool ParametersUpdate(bool force = false);
|
bool ParametersUpdate(bool force = false);
|
||||||
void UpdateStatus();
|
void UpdateStatus();
|
||||||
|
void UpdateRelativeCalibrations(hrt_abstime time_now_us);
|
||||||
|
|
||||||
static constexpr int MAX_SENSOR_COUNT = 4;
|
static constexpr int MAX_SENSOR_COUNT = 4;
|
||||||
|
|
||||||
@@ -132,6 +133,9 @@ private:
|
|||||||
|
|
||||||
bool _last_status_baro_fault{false};
|
bool _last_status_baro_fault{false};
|
||||||
|
|
||||||
|
bool _relative_calibration_done{false};
|
||||||
|
uint64_t _calibration_t_first{0};
|
||||||
|
|
||||||
DEFINE_PARAMETERS(
|
DEFINE_PARAMETERS(
|
||||||
(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh,
|
(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
|
||||||
|
|||||||
Reference in New Issue
Block a user