diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg index 3c7985e630bb..9b9f3c657d6d 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 ab5267d2d6b4..0a22ec1bbc23 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; - 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; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c85527b9121a..8ea0f69031cd 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 041da27c099e..c24303244699 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -37,6 +37,7 @@ #include #include #include +#include namespace sensors { @@ -293,6 +294,26 @@ void VehicleAirData::Run() } } } + + if (!_calibration_done) { + // allow all drivers to start up + _calibration_delay = _calibration_delay == 0 ? time_now_us : _calibration_delay; + + if (time_now_us - _calibration_delay > 1_s) { + _calibration_done = true; + uORB::Publication _vehicle_command_sub{ORB_ID(vehicle_command)}; + vehicle_command_s vcmd{}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION; + vcmd.param1 = 0; + vcmd.param2 = 0; + vcmd.param3 = 1; + vcmd.param4 = 0; + vcmd.param5 = 0; + vcmd.param6 = 0; + vcmd.param7 = 0; + _vehicle_command_sub.publish(vcmd); + } + } } if (!parameter_update) { diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index fd28de34f8c6..7fdcd8826691 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -123,6 +123,9 @@ class VehicleAirData : public ModuleParams, public px4::ScheduledWorkItem float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature + bool _calibration_done{false}; + uint64_t _calibration_delay{0}; + DEFINE_PARAMETERS( (ParamFloat) _param_sens_baro_qnh, (ParamFloat) _param_sens_baro_rate