Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

PR: baro auto calibration #24220

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
move calibration to commander, handle gnns/gnns-denied case
haumarco committed Jan 15, 2025
commit 68b349be2fda49d1eaed97f8d936302659bc0cfa
2 changes: 2 additions & 0 deletions msg/EstimatorStatus.msg
Original file line number Diff line number Diff line change
@@ -121,3 +121,5 @@ float32 mag_inclination_deg
float32 mag_inclination_ref_deg
float32 mag_strength_gs
float32 mag_strength_ref_gs

uint8 hgt_ref # enum to indicate the current source of the height reference
121 changes: 85 additions & 36 deletions src/modules/commander/baro_calibration.cpp
Original file line number Diff line number Diff line change
@@ -57,6 +57,10 @@
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/estimator_status.h>
#include <parameters/param.h>
#include <ekf2/EKF/common.h>

using namespace matrix;
using namespace time_literals;
@@ -75,7 +79,8 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub)
float gps_altitude_sum = NAN;
int gps_altitude_sum_count = 0;


uORB::Subscription vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription estimator_status_sub(ORB_ID(estimator_status));
uORB::SubscriptionMultiArray<sensor_baro_s, MAX_SENSOR_COUNT> sensor_baro_subs{ORB_ID::sensor_baro};
calibration::Barometer calibration[MAX_SENSOR_COUNT] {};

@@ -128,73 +133,117 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub)
px4_usleep(100_ms);
}

estimator_status_s estimator_status;
estimator_status_sub.update(&estimator_status);
bool use_gps = estimator_status.hgt_ref == static_cast<uint8_t>(estimator::HeightSensor::GNSS);
bool param_save = false;

float gps_altitude = NAN;

if (PX4_ISFINITE(gps_altitude_sum) && (gps_altitude_sum_count > 0)) {
gps_altitude = gps_altitude_sum / gps_altitude_sum_count;
}
use_gps &= true;

if (!PX4_ISFINITE(gps_altitude)) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "GPS required for baro cal");
return PX4_ERROR;
} else {
use_gps = false;
}

bool param_save = false;
if (use_gps) {

float qnh = 1013.25f;
param_t param_qnh = param_find("SENS_BARO_QNH");

if (param_qnh != PARAM_INVALID) {
param_get(param_qnh, &qnh);
}

const float pressure_sealevel = 100.f * qnh;

for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
if ((calibration[instance].device_id() != 0) && (data_sum_count[instance] > 0)) {

const float pressure_pa = data_sum[instance] / data_sum_count[instance];
float pressure_altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel);

for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
if ((calibration[instance].device_id() != 0) && (data_sum_count[instance] > 0)) {
// Use GPS altitude as a reference to compute the baro bias measurement
const float baro_bias = pressure_altitude - gps_altitude;
float altitude = pressure_altitude - baro_bias;

const float pressure_pa = data_sum[instance] / data_sum_count[instance];
const float temperature = temperature_sum[instance] / data_sum_count[instance];
// find pressure offset that aligns baro altitude with GPS via binary search
float front = -10000.f;
float middle = NAN;
float last = 10000.f;
float bias = NAN;

float pressure_altitude = getAltitudeFromPressure(pressure_pa, temperature);
// perform a binary search
while (front <= last) {
middle = front + (last - front) / 2;
float altitude_calibrated = getAltitudeFromPressure(pressure_pa - middle, pressure_sealevel);

// Use GPS altitude as a reference to compute the baro bias measurement
const float baro_bias = pressure_altitude - gps_altitude;
if (altitude_calibrated > altitude + 0.1f) {
last = middle;

float altitude = pressure_altitude - baro_bias;
} else if (altitude_calibrated < altitude - 0.1f) {
front = middle;

// find pressure offset that aligns baro altitude with GPS via binary search
float front = -10000.f;
float middle = NAN;
float last = 10000.f;
} else {
bias = middle;
break;
}
}

float bias = NAN;
if (PX4_ISFINITE(bias)) {
float offset = calibration[instance].BiasCorrectedSensorOffset(bias);

// perform a binary search
while (front <= last) {
middle = front + (last - front) / 2;
float altitude_calibrated = getAltitudeFromPressure(pressure_pa - middle, temperature);
calibration[instance].set_offset(offset);

if (altitude_calibrated > altitude + 0.1f) {
last = middle;
if (calibration[instance].ParametersSave(instance, true)) {
calibration[instance].PrintStatus();
param_save = true;
}
}
}
}

} else if (altitude_calibrated < altitude - 0.1f) {
front = middle;
} else {
vehicle_air_data_s vehicle_baro;
int selected_sensor_sub_index = MAX_SENSOR_COUNT;

} else {
bias = middle;
if (vehicle_air_data_sub.update(&vehicle_baro)) {

for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
if (calibration[instance].device_id() == vehicle_baro.baro_device_id) {
selected_sensor_sub_index = instance;
break;
}
}
}

if (selected_sensor_sub_index < MAX_SENSOR_COUNT) {

if (PX4_ISFINITE(bias)) {
float offset = calibration[instance].BiasCorrectedSensorOffset(bias);
const float selected_baro_pressure = data_sum[selected_sensor_sub_index] / data_sum_count[selected_sensor_sub_index];

calibration[instance].set_offset(offset);
for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
if ((calibration[instance].device_id() != 0) && (data_sum_count[instance] > 0)
&& (instance != selected_sensor_sub_index)) {

if (calibration[instance].ParametersSave(instance, true)) {
calibration[instance].PrintStatus();
const float pressure = data_sum[instance] / data_sum_count[instance];
const float offset = pressure - selected_baro_pressure + calibration[instance].offset();

calibration[instance].set_offset(offset);
calibration[instance].ParametersSave(instance);
param_save = true;
}
}
}
}

if (param_save) {
param_notify_changes();
if (!param_save) {
return PX4_ERROR;
}

param_notify_changes();

calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
return PX4_OK;
}
2 changes: 2 additions & 0 deletions src/modules/ekf2/EKF2.cpp
Original file line number Diff line number Diff line change
@@ -1850,6 +1850,8 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
status.mag_strength_ref_gs);
#endif // CONFIG_EKF2_MAGNETOMETER

status.hgt_ref = (uint8_t)_ekf.getHeightSensorRef();

status.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_status_pub.publish(status);
}
27 changes: 0 additions & 27 deletions src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
Original file line number Diff line number Diff line change
@@ -239,33 +239,6 @@ void VehicleAirData::Run()
}
}

// when baros dont get calibrated, set the offset in respect to the current sensor
// this avoids height jumps when switching to a new sensor
if (_selected_sensor_sub_index >= 0
&& fabsf(_calibration[_selected_sensor_sub_index].offset() - _selected_baro_offset) > FLT_EPSILON) {

sensor_baro_s report;
_selected_baro_offset = _calibration[_selected_sensor_sub_index].offset();
_sensor_sub[_selected_sensor_sub_index].copy(&report);
const float selected_baro_pressure = report.pressure;

for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
if (instance != _selected_sensor_sub_index && _advertised[instance]) {

_sensor_sub[instance].copy(&report);
const float offset = report.pressure - selected_baro_pressure;

// avoid ParameterSave if difference is not significant (<10Pa)
if (fabsf(_calibration[instance].offset() - offset) > 10.f) {
_calibration[instance].set_offset(offset);
_calibration[instance].ParametersSave(instance);
}
}
}

ParametersUpdate(true);
}

// Publish
if (_param_sens_baro_rate.get() > 0) {
int interval_us = 1e6f / _param_sens_baro_rate.get();
2 changes: 0 additions & 2 deletions src/modules/sensors/vehicle_air_data/VehicleAirData.hpp
Original file line number Diff line number Diff line change
@@ -123,8 +123,6 @@ class VehicleAirData : public ModuleParams, public px4::ScheduledWorkItem

float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature

float _selected_baro_offset{0.01f};

DEFINE_PARAMETERS(
(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh,
(ParamFloat<px4::params::SENS_BARO_RATE>) _param_sens_baro_rate