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

fix(commander): fix baro calibration infinite loop (#26752)

Browse Source 
In dd2322d622, the local PressureToAltitude(pressure_pa, temperature)
was replaced with the shared getAltitudeFromPressure(pressure_pa,
pressure_sealevel_pa), but the call sites continued passing temperature
where sea-level pressure was expected. This caused the binary search to
never converge, hanging "commander calibrate baro" indefinitely.

The original function used measured temperature in its hypsometric
equation. The replacement uses standard atmosphere temperature (15C)
internally, which is sufficient since the calibration computes a
relative offset against GPS altitude.

- Pass kPressRefSeaLevelPa as the second argument instead of temperature
- Remove the now-unused temperature accumulation
- Replace unbounded while loop with iteration-capped for loop to prevent
  hangs from float precision stalls, matching VehicleAirData.cpp
This commit is contained in:
Jacob Dahl 2026-03-16 14:51:12 -08:00 committed by GitHub
parent d04858efe0
commit a107179ce7
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 13 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/modules/commander/baro_calibration.cpp
View File

@ -81,7 +81,6 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub)
uint64_t timestamp_sample_sum[MAX_SENSOR_COUNT] {0};
float data_sum[MAX_SENSOR_COUNT] {};
float temperature_sum[MAX_SENSOR_COUNT] {};
int data_sum_count[MAX_SENSOR_COUNT] {};
const hrt_abstime time_start_us = hrt_absolute_time();
@ -99,7 +98,6 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub)
timestamp_sample_sum[instance] += sensor_baro.timestamp_sample;
data_sum[instance] += pressure_corrected;
temperature_sum[instance] += sensor_baro.temperature;
data_sum_count[instance]++;
}
}
@ -145,9 +143,8 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub)
if ((calibration[instance].device_id() != 0) && (data_sum_count[instance] > 0)) {
const float pressure_pa = data_sum[instance] / data_sum_count[instance];
const float temperature = temperature_sum[instance] / data_sum_count[instance];
float pressure_altitude = getAltitudeFromPressure(pressure_pa, temperature);
float pressure_altitude = getAltitudeFromPressure(pressure_pa, kPressRefSeaLevelPa);
// Use GPS altitude as a reference to compute the baro bias measurement
const float baro_bias = pressure_altitude - gps_altitude;
@ -155,25 +152,25 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub)
float altitude = pressure_altitude - baro_bias;
// find pressure offset that aligns baro altitude with GPS via binary search
float front = -10000.f;
float middle = NAN;
float last = 10000.f;
float low = -10000.f;
float high = 10000.f;
static constexpr float kTolerance = 0.1f;
static constexpr int kMaxIterations = 100;
float bias = NAN;
// perform a binary search
while (front <= last) {
middle = front + (last - front) / 2;
float altitude_calibrated = getAltitudeFromPressure(pressure_pa - middle, temperature);
for (int i = 0; i < kMaxIterations; ++i) {
float mid = low + (high - low) / 2.f;
float altitude_calibrated = getAltitudeFromPressure(pressure_pa - mid, kPressRefSeaLevelPa);
if (altitude_calibrated > altitude + 0.1f) {
last = middle;
if (altitude_calibrated > altitude + kTolerance) {
high = mid;
} else if (altitude_calibrated < altitude - 0.1f) {
front = middle;
} else if (altitude_calibrated < altitude - kTolerance) {
low = mid;
} else {
bias = middle;
bias = mid;
break;
}
}