diff --git a/docs/en/sensor/barometer.md b/docs/en/sensor/barometer.md index 369c4d531ee..200a8a7bc6b 100644 --- a/docs/en/sensor/barometer.md +++ b/docs/en/sensor/barometer.md @@ -63,15 +63,22 @@ GNSS-baro calibration adjusts baro sensor offsets to align with absolute altitud This calibration is controlled by the [SENS_BAR_AUTOCAL](../advanced_config/parameter_reference.md#SENS_BAR_AUTOCAL) parameter (enabled by default). The algorithm monitors GNSS quality, collects altitude differences over a 2-second filtered window, and verifies stability within 4m tolerance. -Once stable, it uses binary search to calculate pressure offsets that align baro altitude with GNSS altitude (0.1m precision), then applies the offset to all sensors and saves the parameters. +Once stable, it uses binary search to calculate pressure offsets that align the baro altitude with the filtered GNSS altitude (0.1m precision), then applies the offset to all sensors and saves the parameters. + +While the vehicle stays on the ground and disarmed, the calibration keeps running: a baro drifting away from the GNSS altitude (for example from sensor warm-up or weather changes between boot and takeoff) is recalibrated once its offset, filtered over 5 seconds, exceeds 0.5 m. +This tracking stops on arming and resumes after landing and disarming. +It also stops until the next reboot once the altitude is set externally (`VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE`, for example the manual position reset of a ground station), as the GNSS altitude is then not trusted. Notes: -- **EKF Independence**: GNSS-baro calibration operates independently of EKF2 altitude fusion settings. -- **Execution Timing**: Calibration runs even when [EKF2_GPS_CTRL](../advanced_config/parameter_reference.md#EKF2_GPS_CTRL) altitude fusion is disabled. -- **One-Time Process**: Each calibration session completes once per system startup. +- **GNSS Height Fusion**: Calibration requires GNSS height fusion to be active ([EKF2_GPS_CTRL](../advanced_config/parameter_reference.md#EKF2_GPS_CTRL)). +- **Ground Only**: Calibration only runs while landed. + The first calibration after boot also completes while armed, so arming early does not leave the baro uncalibrated. + After that the baro is only recalibrated while disarmed. +- **Height Reset**: EKF2 resets its height to each new baro calibration when the baro is the height reference ([EKF2_HGT_REF](../advanced_config/parameter_reference.md#EKF2_HGT_REF)), so the estimated altitude follows the GNSS altitude until arming. - **Persistence**: Calibration offsets are saved to parameters and persist across reboots. -- **Faulty GNSS Vulnerability**: If GNSS data is faulty during boot, the calibration will use incorrect altitude reference. +- **Faulty GNSS Vulnerability**: The baro follows the GNSS altitude for as long as the vehicle is landed and disarmed. + A wrong GNSS altitude during boot is corrected once the GNSS altitude recovers, but a GNSS altitude that is still faulty at arming is the reference for the flight. See [Faulty GNSS Data During Boot](../advanced_config/tuning_the_ecl_ekf.md#faulty-gnss-data-during-boot) for mitigation strategies. ## See Also diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 41fe7d7cd0b..5714445e01f 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -262,6 +262,22 @@ void VehicleAirData::Run() } } + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + } + + vehicle_command_s vehicle_command; + + while (_vehicle_command_sub.update(&vehicle_command)) { + // an externally set altitude overrides the GNSS one + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE + && PX4_ISFINITE(vehicle_command.param7)) { + _external_altitude_set = true; + } + } + estimator_status_flags_s latest_estimator_status_flags; bool baro_gnss_calibration_running = false; @@ -270,9 +286,14 @@ void VehicleAirData::Run() if (!_relative_calibration_done) { _relative_calibration_done = UpdateRelativeCalibrations(time_now_us); - } else if (!_baro_gnss_calibration_done && _param_sens_bar_autocal.get() && latest_estimator_status_flags.cs_gps_hgt) { + } else if (_param_sens_bar_autocal.get() && latest_estimator_status_flags.cs_gps_hgt + && (!_armed || !_baro_gnss_calibration_done) && !_external_altitude_set) { + // first calibration may complete armed, later drift is only tracked while disarmed baro_gnss_calibration_running = true; - _baro_gnss_calibration_done = BaroGNSSAltitudeOffset(); + + if (BaroGNSSAltitudeOffset()) { + _baro_gnss_calibration_done = true; + } } } @@ -522,6 +543,12 @@ bool VehicleAirData::BaroGNSSAltitudeOffset() { static constexpr float kDeltaOffsetTolerance = 4.f; static constexpr uint64_t kLpfWindow = 2_s; + // time constant of the baro-GNSS offset filter after the first calibration + static constexpr uint64_t kTrackingTimeConstant = 5_s; + // wait after a filter reset before evaluating the offset, 3 time constants = 95% settled + static constexpr uint64_t kTrackingSettleTime = 3 * kTrackingTimeConstant; + // recalibrate when the filtered baro-GNSS offset exceeds this + static constexpr float kTrackingThreshold = 0.5f; vehicle_gnss_s vehicle_gnss; @@ -532,9 +559,9 @@ bool VehicleAirData::BaroGNSSAltitudeOffset() 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(vehicle_gnss.receiver.altitude_msl); + const float baro_altitude = getAltitudeFromPressure(baro_pressure, pressure_sealevel); - const float delta_alt = getAltitudeFromPressure(baro_pressure, pressure_sealevel) - target_altitude; + const float delta_alt = baro_altitude - static_cast(vehicle_gnss.receiver.altitude_msl); bool gnss_baro_offset_stable = false; if (vehicle_gnss.failed_strict_checks || _t_first_gnss_sample == 0) { @@ -546,14 +573,19 @@ bool VehicleAirData::BaroGNSSAltitudeOffset() if (_calibration_t_first == 0) { _calibration_t_first = vehicle_gnss.timestamp; - _delta_baro_gnss_lpf.setParameters(_calibration_t_first - _t_first_gnss_sample, kLpfWindow); + _delta_baro_gnss_lpf.setParameters(_calibration_t_first - _t_first_gnss_sample, + _baro_gnss_calibration_done ? kTrackingTimeConstant : kLpfWindow); _delta_baro_gnss_lpf.reset(delta_alt); } else { _delta_baro_gnss_lpf.update(delta_alt); } - if (vehicle_gnss.timestamp - _calibration_t_first > kLpfWindow && !PX4_ISFINITE(_baro_gnss_offset_t1)) { + if (_baro_gnss_calibration_done) { + gnss_baro_offset_stable = (vehicle_gnss.timestamp - _calibration_t_first > kTrackingSettleTime) + && (fabsf(_delta_baro_gnss_lpf.getState()) > kTrackingThreshold); + + } else if (vehicle_gnss.timestamp - _calibration_t_first > kLpfWindow && !PX4_ISFINITE(_baro_gnss_offset_t1)) { _baro_gnss_offset_t1 = _delta_baro_gnss_lpf.getState(); } else if (vehicle_gnss.timestamp - _calibration_t_first > 2 * kLpfWindow && PX4_ISFINITE(_baro_gnss_offset_t1)) { @@ -571,6 +603,11 @@ bool VehicleAirData::BaroGNSSAltitudeOffset() return false; } + // next check starts a new window + _t_first_gnss_sample = 0; + + const float target_altitude = baro_altitude - _delta_baro_gnss_lpf.getState(); + // Binary search float low = -10000.0f; float high = 10000.0f; @@ -594,12 +631,20 @@ bool VehicleAirData::BaroGNSSAltitudeOffset() } } - // add new offset to existing relative offsets + if (!PX4_ISFINITE(offset)) { + return false; + } + + // add new offset to existing relative offsets, also of a sensor without a sample since the last publication for (int instance = 0; instance < MAX_SENSOR_COUNT; ++instance) { - if (_calibration[instance].device_id() != 0 && _data_sum_count[instance] > 0) { + if (_calibration[instance].device_id() != 0) { _calibration[instance].set_offset(_calibration[instance].offset() + offset); _calibration[instance].ParametersSave(instance); param_notify_changes(); + + // apply to samples accumulated with the old offset + _data_sum[instance] -= offset * _data_sum_count[instance]; + _last_data[instance] -= offset; } } diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index f56c29b76d3..d579f76fcdb 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -56,7 +56,9 @@ #include #include #include +#include #include +#include using namespace time_literals; @@ -116,6 +118,8 @@ private: }; uORB::Subscription _vehicle_gnss_sub{ORB_ID(vehicle_gnss)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; calibration::Barometer _calibration[MAX_SENSOR_COUNT]; @@ -143,6 +147,8 @@ private: int8_t _selected_sensor_sub_index{-1}; bool _last_status_baro_fault{false}; + bool _armed{false}; + bool _external_altitude_set{false}; bool _relative_calibration_done{false}; bool _baro_gnss_calibration_done{false}; diff --git a/src/modules/sensors/vehicle_air_data/params.yaml b/src/modules/sensors/vehicle_air_data/params.yaml index bfdf106ada4..6a95e685de0 100644 --- a/src/modules/sensors/vehicle_air_data/params.yaml +++ b/src/modules/sensors/vehicle_air_data/params.yaml @@ -24,7 +24,9 @@ parameters: SENS_BAR_AUTOCAL: description: short: Barometer auto calibration - long: Automatically calibrate barometer based on the GNSS height + long: |- + Automatically calibrate barometer based on the GNSS height. + Keeps recalibrating while landed and disarmed when the baro drifts away from the GNSS height. category: System type: boolean default: 1 diff --git a/src/modules/sensors/vehicle_air_data/test/VehicleAirDataTest.cpp b/src/modules/sensors/vehicle_air_data/test/VehicleAirDataTest.cpp index b1ab60ef6b0..54873007458 100644 --- a/src/modules/sensors/vehicle_air_data/test/VehicleAirDataTest.cpp +++ b/src/modules/sensors/vehicle_air_data/test/VehicleAirDataTest.cpp @@ -48,7 +48,9 @@ #include #include #include +#include #include +#include #include class VehicleAirDataTest : public ::testing::Test @@ -165,6 +167,26 @@ public: _vehicle_gnss_pub.publish(gnss); } + static void publishExternalPositionEstimate(float altitude) + { + vehicle_command_s command{}; + command.timestamp = hrt_absolute_time(); + command.command = vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE; + command.param2 = 0.f; + command.param5 = 47.; + command.param6 = 8.; + command.param7 = altitude; + _vehicle_command_pub.publish(command); + } + + static void publishVehicleStatus(bool armed) + { + vehicle_status_s status{}; + status.timestamp = hrt_absolute_time(); + status.arming_state = armed ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_DISARMED; + _vehicle_status_pub.publish(status); + } + // a run of GNSS samples 100 ms apart, all with the same solution and estimator state struct GnssSegment { int samples; @@ -172,6 +194,9 @@ public: uint16_t failed_strict_checks; bool in_air = false; bool gnss_height_fused = true; + bool armed = false; + // sent with the first sample, NAN for none + float external_altitude = NAN; }; // brings the module up on the given altitude, then feeds it the given GNSS segments in order, and returns @@ -192,6 +217,7 @@ public: runFor(module, 1500_ms, [&]() { publishSample(0, noisy_pressure()); publishEstimatorStatusFlags(); + publishVehicleStatus(false); }); EXPECT_EQ(module.selectedIndex(), 0); @@ -204,6 +230,12 @@ public: gnss_timestamp += 100_ms; publishSample(0, noisy_pressure()); publishEstimatorStatusFlags(segment.in_air, segment.gnss_height_fused); + publishVehicleStatus(segment.armed); + + if ((i == 0) && PX4_ISFINITE(segment.external_altitude)) { + publishExternalPositionEstimate(segment.external_altitude); + } + publishGnss(gnss_timestamp, segment.altitude_msl, segment.failed_strict_checks); module.RunPublic(); px4_usleep(5_ms); @@ -237,6 +269,8 @@ public: static uORB::Publication _parameter_update_pub; static uORB::Publication _estimator_status_flags_pub; static uORB::Publication _vehicle_gnss_pub; + static uORB::Publication _vehicle_status_pub; + static uORB::Publication _vehicle_command_pub; static constexpr const char *kOffsetParams[kSensorCount] {"CAL_BARO0_OFF"}; @@ -249,6 +283,8 @@ uORB::PublicationMulti VehicleAirDataTest::_sensor_baro_pubs[kSen uORB::Publication VehicleAirDataTest::_parameter_update_pub{ORB_ID(parameter_update)}; uORB::Publication VehicleAirDataTest::_estimator_status_flags_pub{ORB_ID(estimator_status_flags)}; uORB::Publication VehicleAirDataTest::_vehicle_gnss_pub{ORB_ID(vehicle_gnss)}; +uORB::Publication VehicleAirDataTest::_vehicle_status_pub{ORB_ID(vehicle_status)}; +uORB::Publication VehicleAirDataTest::_vehicle_command_pub{ORB_ID(vehicle_command)}; constexpr const char *VehicleAirDataTest::kOffsetParams[]; TEST_F(VehicleAirDataTest, BarometerDisabledFromBootDoesNotDriveRun) @@ -352,3 +388,38 @@ TEST_F(VehicleAirDataTest, GnssBaroCalibrationCompletesAfterFlight) EXPECT_GT(offset, 150.f); EXPECT_LT(offset, 210.f); } + +TEST_F(VehicleAirDataTest, GnssBaroCalibrationCompletesWhileArmedOnGround) +{ + // arming before the first calibration does not hold it back + const float offset = runGnssCalibration({{60, 15., 0, false, true, true}}); + + EXPECT_GT(offset, 150.f); + EXPECT_LT(offset, 210.f); +} + +TEST_F(VehicleAirDataTest, GnssBaroCalibrationTracksBaroDriftUntilArmed) +{ + // a steady 1.5 m drift on the ground is recalibrated onto 16.5 m, drift while armed is ignored + const float offset = runGnssCalibration({ + {60, 15., 0}, + {300, 16.5, 0}, + {300, 25., 0, false, true, true}, + }); + + // roughly 12 Pa per metre + EXPECT_GT(offset, 194.f); + EXPECT_LT(offset, 204.f); +} + +TEST_F(VehicleAirDataTest, GnssBaroCalibrationStopsTrackingAfterExternalAltitude) +{ + // an externally set altitude stops the tracking, the calibration stays on 15 m + const float offset = runGnssCalibration({ + {60, 15., 0}, + {300, 25., 0, false, true, false, 30.f}, + }); + + EXPECT_GT(offset, 150.f); + EXPECT_LT(offset, 210.f); +}