autom. trigger baro calib on boot

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

src/modules/commander/baro_calibration.cpp

@@ -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;
+
+	} 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;
 }

src/modules/ekf2/EKF2.cpp

@@ -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);
 }

src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

@@ -37,6 +37,7 @@
 #include <px4_platform_common/events.h>
 #include <lib/geo/geo.h>
 #include <lib/atmosphere/atmosphere.h>
+#include <uORB/topics/vehicle_command.h>
 
 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_s> _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) {

src/modules/sensors/vehicle_air_data/VehicleAirData.hpp

@@ -123,6 +123,9 @@ private:
 
 	float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature
 
+	bool _calibration_done{false};
+	uint64_t _calibration_delay{0};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh,
 		(ParamFloat<px4::params::SENS_BARO_RATE>) _param_sens_baro_rate