diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg index 3c7985e630..9b9f3c657d 100644 --- a/msg/EstimatorStatus.msg +++ b/msg/EstimatorStatus.msg @@ -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 diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index ab5267d2d6..0a22ec1bbc 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -57,6 +57,10 @@ #include #include #include +#include +#include +#include +#include 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_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(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; + + } else { + use_gps = false; } - if (!PX4_ISFINITE(gps_altitude)) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "GPS required for baro cal"); - return PX4_ERROR; - } + if (use_gps) { - bool param_save = false; + float qnh = 1013.25f; + param_t param_qnh = param_find("SENS_BARO_QNH"); - for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) { - if ((calibration[instance].device_id() != 0) && (data_sum_count[instance] > 0)) { + if (param_qnh != PARAM_INVALID) { + param_get(param_qnh, &qnh); + } - const float pressure_pa = data_sum[instance] / data_sum_count[instance]; - const float temperature = temperature_sum[instance] / data_sum_count[instance]; + const float pressure_sealevel = 100.f * qnh; - float pressure_altitude = getAltitudeFromPressure(pressure_pa, temperature); + 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; + const float pressure_pa = data_sum[instance] / data_sum_count[instance]; + float pressure_altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel); - float altitude = pressure_altitude - baro_bias; + // 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; - // find pressure offset that aligns baro altitude with GPS via binary search - float front = -10000.f; - float middle = NAN; - float last = 10000.f; + // 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 bias = NAN; + // perform a binary search + while (front <= last) { + middle = front + (last - front) / 2; + float altitude_calibrated = getAltitudeFromPressure(pressure_pa - middle, pressure_sealevel); - // perform a binary search - while (front <= last) { - middle = front + (last - front) / 2; - float altitude_calibrated = getAltitudeFromPressure(pressure_pa - middle, temperature); + if (altitude_calibrated > altitude + 0.1f) { + last = middle; - if (altitude_calibrated > altitude + 0.1f) { - last = middle; + } else if (altitude_calibrated < altitude - 0.1f) { + front = middle; - } else if (altitude_calibrated < altitude - 0.1f) { - front = middle; + } else { + bias = middle; + break; + } + } - } else { - bias = middle; + 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 { + vehicle_air_data_s vehicle_baro; + int selected_sensor_sub_index = MAX_SENSOR_COUNT; + + 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 (PX4_ISFINITE(bias)) { - float offset = calibration[instance].BiasCorrectedSensorOffset(bias); + if (selected_sensor_sub_index < MAX_SENSOR_COUNT) { - calibration[instance].set_offset(offset); + const float selected_baro_pressure = data_sum[selected_sensor_sub_index] / data_sum_count[selected_sensor_sub_index]; - if (calibration[instance].ParametersSave(instance, true)) { - calibration[instance].PrintStatus(); + 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)) { + + 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; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c85527b912..8ea0f69031 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1850,6 +1850,8 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) 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); } diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 33b72aae54..041da27c09 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -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(); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index eb26d1c14b..fd28de34f8 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -123,8 +123,6 @@ private: float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature - float _selected_baro_offset{0.01f}; - DEFINE_PARAMETERS( (ParamFloat) _param_sens_baro_qnh, (ParamFloat) _param_sens_baro_rate