diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 49bbbcc90a..916f4802d5 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/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(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(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 diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index 431e0867bb..d7620d48d0 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -36,6 +36,7 @@ #include "data_validator/DataValidatorGroup.hpp" #include +#include #include #include #include @@ -55,6 +56,7 @@ #include #include #include +#include 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 _delta_baro_gnss_lpf{}; + float _baro_gnss_offset_t1{NAN}; + uint64_t _t_first_gnss_sample{0}; DEFINE_PARAMETERS( (ParamFloat) _param_sens_baro_qnh, - (ParamFloat) _param_sens_baro_rate + (ParamFloat) _param_sens_baro_rate, + (ParamBool) _param_sens_baro_autocal ) }; }; // namespace sensors diff --git a/src/modules/sensors/vehicle_air_data/params.c b/src/modules/sensors/vehicle_air_data/params.c index fee8ac6263..0f32fee2f1 100644 --- a/src/modules/sensors/vehicle_air_data/params.c +++ b/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);