feat(sensors): keep calibrating the baro on the GNSS altitude until arming (#29876)

This commit is contained in:
Marco Hauswirth authored and GitHub committed 2026-11-19 17:10:35 +01:00
1 parent 67e8a34802
commit 829f0f412e
5 files changed
+145 -14

No files matched your search

+12 -5
View File
@@ -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
@@ -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<float>(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<float>(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;
}
}
@@ -56,7 +56,9 @@
#include <uORB/topics/sensors_status.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/estimator_status_flags.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_gnss.h>
#include <uORB/topics/vehicle_status.h>
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};
@@ -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
@@ -48,7 +48,9 @@
#include <uORB/topics/estimator_status_flags.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_gnss.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORBManager.hpp>
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_s> _parameter_update_pub;
static uORB::Publication<estimator_status_flags_s> _estimator_status_flags_pub;
static uORB::Publication<vehicle_gnss_s> _vehicle_gnss_pub;
static uORB::Publication<vehicle_status_s> _vehicle_status_pub;
static uORB::Publication<vehicle_command_s> _vehicle_command_pub;
static constexpr const char *kOffsetParams[kSensorCount] {"CAL_BARO0_OFF"};
@@ -249,6 +283,8 @@ uORB::PublicationMulti<sensor_baro_s> VehicleAirDataTest::_sensor_baro_pubs[kSen
uORB::Publication<parameter_update_s> VehicleAirDataTest::_parameter_update_pub{ORB_ID(parameter_update)};
uORB::Publication<estimator_status_flags_s> VehicleAirDataTest::_estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
uORB::Publication<vehicle_gnss_s> VehicleAirDataTest::_vehicle_gnss_pub{ORB_ID(vehicle_gnss)};
uORB::Publication<vehicle_status_s> VehicleAirDataTest::_vehicle_status_pub{ORB_ID(vehicle_status)};
uORB::Publication<vehicle_command_s> 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);
}