Skip to content

Commit

Permalink
move calibration to commander, handle gnns/gnns-denied case
Browse files Browse the repository at this point in the history
  • Loading branch information
@haumarco
haumarco committed Jan 14, 2025
1 parent 5172dd0 commit 8f7dabc
Show file tree
Hide file tree
Showing 4 changed files with 100 additions and 64 deletions.
2 changes: 2 additions & 0 deletions msg/EstimatorStatus.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,3 +121,5 @@ float32 mag_inclination_deg
float32 mag_inclination_ref_deg
float32 mag_strength_gs
float32 mag_strength_ref_gs

uint8 hgt_reference # enum to indicate the current source of the height reference
133 changes: 96 additions & 37 deletions src/modules/commander/baro_calibration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -75,6 +79,11 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub)
float gps_altitude_sum = NAN;
int gps_altitude_sum_count = 0;

uORB::SubscriptionData<vehicle_air_data_s> vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
vehicle_air_data_s vehicle_baro;
int selected_sensor_sub_index = MAX_SENSOR_COUNT;

uORB::SubscriptionData<estimator_status_s> 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] {};
Expand Down Expand Up @@ -128,73 +137,123 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub)
px4_usleep(100_ms);
}

estimator_status_sub.update();
bool use_gps = estimator_status_sub.get().hgt_ref == (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 {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG,
"GNSS required for absolute baro cal, performing relative calibration");
use_gps = false;
}

bool param_save = false;
if (use_gps) {

for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
if ((calibration[instance].device_id() != 0) && (data_sum_count[instance] > 0)) {
float qnh = 1013.25f;
param_t param_qnh = param_find("SENS_BARO_QNH");

const float pressure_pa = data_sum[instance] / data_sum_count[instance];
const float temperature = temperature_sum[instance] / data_sum_count[instance];
if (param_qnh != PARAM_INVALID) {
param_get(param_qnh, &qnh);
}

float pressure_altitude = getAltitudeFromPressure(pressure_pa, temperature);
const float pressure_sealevel = 100.f * qnh;

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

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 pressure_altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel);

float bias = NAN;
// Use GPS altitude as a reference to compute the baro bias measurement
const float baro_bias = pressure_altitude - gps_altitude;

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

if (altitude_calibrated > altitude + 0.1f) {
last = 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 if (altitude_calibrated < altitude - 0.1f) {
front = middle;
float bias = NAN;

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

if (altitude_calibrated > altitude + 0.1f) {
last = middle;

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

} else {
bias = middle;
break;
}
}

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

calibration[instance].set_offset(offset);

if (calibration[instance].ParametersSave(instance, true)) {
calibration[instance].PrintStatus();
param_save = true;
}
}
}
}

} else {
if (vehicle_air_data_sub.update()) {
vehicle_baro = vehicle_air_data_sub.get();

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

if (selected_sensor_sub_index < MAX_SENSOR_COUNT) {

const float selected_baro_pressure = data_sum[selected_sensor_sub_index] / data_sum_count[selected_sensor_sub_index];

if (PX4_ISFINITE(bias)) {
float offset = calibration[instance].BiasCorrectedSensorOffset(bias);
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)) {

calibration[instance].set_offset(offset);
const float pressure = data_sum[instance] / data_sum_count[instance];
const float offset = pressure - selected_baro_pressure;

if (calibration[instance].ParametersSave(instance, true)) {
calibration[instance].PrintStatus();
param_save = true;
// avoid ParameterSave if difference is not significant (<10Pa)
// if (fabsf(calibration[instance].offset() - offset) > 10.f) {
if (fabsf(calibration[instance].offset() - offset) > 10.f) {
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
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down
27 changes: 0 additions & 27 deletions src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down

0 comments on commit 8f7dabc

Please sign in to comment.