sensors: move baro aggregation to new sensors/vehicle_air_data

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -128,8 +128,6 @@ then
 	param set CAL_MAG0_ID 197388
 	param set CAL_MAG_PRIME 197388
 
-	param set CAL_BARO_PRIME 6620172
-
 	param set CBRK_AIRSPD_CHK 0
 
 	param set COM_DISARM_LAND 0.5

msg/sensor_selection.msg

@@ -4,6 +4,5 @@
 #
 uint64 timestamp		# time since system start (microseconds)
 uint32 accel_device_id		# unique device ID for the selected accelerometers
-uint32 baro_device_id		# unique device ID for the selected barometer
 uint32 gyro_device_id		# unique device ID for the selected rate gyros
 uint32 mag_device_id		# unique device ID for the selected magnetometer

msg/vehicle_air_data.msg

@@ -1,4 +1,9 @@
-uint64 timestamp			# time since system start (microseconds)
+
+uint64 timestamp            # time since system start (microseconds)
+
+uint64 timestamp_sample     # the timestamp of the raw data (microseconds)
+
+uint32 baro_device_id       # unique device ID for the selected barometer
 
 float32 baro_alt_meter			# Altitude above MSL calculated from temperature compensated baro sensor data using an ISA corrected for sea level pressure SENS_BARO_QNH.
 float32 baro_temp_celcius		# Temperature in degrees celsius

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp

@@ -221,11 +221,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 	/* ---- BARO ---- */
 	if (checkSensors) {
-		//bool prime_found = false;
-
-		int32_t prime_id = -1;
-		param_get(param_find("CAL_BARO_PRIME"), &prime_id);
-
 		int32_t sys_has_baro = 1;
 		param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
 
@@ -238,29 +233,13 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 			int32_t device_id = -1;
 
-			if (baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
-				if ((prime_id > 0) && (device_id == prime_id)) {
-					//prime_found = true;
-				}
-
-			} else {
+			if (!baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
 				if (required) {
 					failed = true;
 					baro_fail_reported = true;
 				}
 			}
 		}
-
-		// TODO there is no logic in place to calibrate the primary baro yet
-		// // check if the primary device is present
-		// if (false) {
-		// 	if (reportFailures && !failed) {
-		// 		mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational");
-		// 	}
-
-		// 	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, true, false, status);
-		// 	failed = true;
-		// }
 	}
 
 	/* ---- IMU CONSISTENCY ---- */

src/modules/commander/CMakeLists.txt

@@ -41,7 +41,6 @@ px4_add_module(
 	SRCS
 		accelerometer_calibration.cpp
 		airspeed_calibration.cpp
-		baro_calibration.cpp
 		calibration_routines.cpp
 		Commander.cpp
 		commander_helper.cpp

src/modules/commander/Commander.cpp

@@ -50,7 +50,6 @@
 #include "Arming/HealthFlags/HealthFlags.h"
 #include "accelerometer_calibration.h"
 #include "airspeed_calibration.h"
-#include "baro_calibration.h"
 #include "calibration_routines.h"
 #include "commander_helper.h"
 #include "esc_calibration.h"

src/modules/ekf2/ekf2_main.cpp

@@ -911,7 +911,7 @@ void Ekf2::Run()
 
 			if (_airdata_sub.copy(&airdata)) {
 				_ekf.set_air_density(airdata.rho);
-				const baroSample baro_sample {airdata.baro_alt_meter, airdata.timestamp};
+				const baroSample baro_sample {airdata.baro_alt_meter, airdata.timestamp_sample};
 				_ekf.setBaroData(baro_sample);
 				ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
 						(int64_t)ekf2_timestamps.timestamp / 100);

src/modules/sensors/CMakeLists.txt

@@ -35,6 +35,7 @@ add_subdirectory(sensor_corrections) # used by vehicle_{acceleration, angular_ve
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 add_subdirectory(vehicle_acceleration)
 add_subdirectory(vehicle_angular_velocity)
+add_subdirectory(vehicle_air_data)
 add_subdirectory(vehicle_imu)
 
 px4_add_module(
@@ -54,5 +55,6 @@ px4_add_module(
 		mathlib
 		vehicle_acceleration
 		vehicle_angular_velocity
+		vehicle_air_data
 		vehicle_imu
 	)

src/modules/sensors/common.h

@@ -46,11 +46,7 @@ namespace sensors
 constexpr uint8_t MAG_COUNT_MAX = 4;
 constexpr uint8_t GYRO_COUNT_MAX = 3;
 constexpr uint8_t ACCEL_COUNT_MAX = 3;
-constexpr uint8_t BARO_COUNT_MAX = 3;
 
-constexpr uint8_t SENSOR_COUNT_MAX = math::max(MAG_COUNT_MAX,
-				     math::max(GYRO_COUNT_MAX,
-						     math::max(ACCEL_COUNT_MAX,
-								     BARO_COUNT_MAX)));
+constexpr uint8_t SENSOR_COUNT_MAX = math::max(MAG_COUNT_MAX, math::max(GYRO_COUNT_MAX, ACCEL_COUNT_MAX));
 
 } /* namespace sensors */

src/modules/sensors/parameters.cpp

@@ -58,9 +58,6 @@ void initialize_parameter_handles(ParameterHandles &parameter_handles)
 	parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
 	parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
 
-	/* Barometer QNH */
-	parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
-
 	parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL");
 	parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
 	parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
@@ -107,8 +104,6 @@ void update_parameters(const ParameterHandles &parameter_handles, Parameters &pa
 	param_get(parameter_handles.board_offset[1], &(parameters.board_offset[1]));
 	param_get(parameter_handles.board_offset[2], &(parameters.board_offset[2]));
 
-	param_get(parameter_handles.baro_qnh, &(parameters.baro_qnh));
-
 	param_get(parameter_handles.air_cmodel, &parameters.air_cmodel);
 	param_get(parameter_handles.air_tube_length, &parameters.air_tube_length);
 	param_get(parameter_handles.air_tube_diameter_mm, &parameters.air_tube_diameter_mm);

src/modules/sensors/parameters.h

@@ -56,8 +56,6 @@ struct Parameters {
 
 	float board_offset[3];
 
-	float baro_qnh;
-
 	int32_t air_cmodel;
 	float air_tube_length;
 	float air_tube_diameter_mm;
@@ -73,8 +71,6 @@ struct ParameterHandles {
 
 	param_t board_offset[3];
 
-	param_t baro_qnh;
-
 	param_t air_cmodel;
 	param_t air_tube_length;
 	param_t air_tube_diameter_mm;

src/modules/sensors/sensor_params.c

@@ -31,14 +31,6 @@
  *
  ****************************************************************************/
 
-/**
- * Primary baro ID
- *
- * @category system
- * @group Sensor Calibration
- */
-PARAM_DEFINE_INT32(CAL_BARO_PRIME, 0);
-
 /**
  * Airspeed sensor compensation model for the SDP3x
  *
@@ -108,19 +100,6 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
 
-/**
- * QNH for barometer
- *
- * @min 500
- * @max 1500
- * @group Sensors
- * @unit hPa
- *
- * @reboot_required true
- *
- */
-PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
-
 /**
  * Board rotation
  *

src/modules/sensors/sensors.cpp

@@ -74,6 +74,7 @@
 #include "voted_sensors_update.h"
 #include "vehicle_acceleration/VehicleAcceleration.hpp"
 #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
+#include "vehicle_air_data/VehicleAirData.hpp"
 #include "vehicle_imu/VehicleIMU.hpp"
 
 using namespace sensors;
@@ -112,7 +113,6 @@ class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::Sch
 	bool		_armed{false};				/**< arming status of the vehicle */
 
 	hrt_abstime     _last_config_update{0};
-	hrt_abstime     _airdata_prev_timestamp{0};
 	hrt_abstime     _sensor_combined_prev_timestamp{0};
 	hrt_abstime     _magnetometer_prev_timestamp{0};
 
@@ -123,11 +123,11 @@ class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::Sch
 	uORB::Subscription	_diff_pres_sub{ORB_ID(differential_pressure)};			/**< raw differential pressure subscription */
 	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};				/**< notification of parameter updates */
 	uORB::Subscription	_vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle control mode subscription */
+	uORB::Subscription	_vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
 
 	uORB::Publication<airspeed_s>			_airspeed_pub{ORB_ID(airspeed)};			/**< airspeed */
 	uORB::Publication<sensor_combined_s>		_sensor_pub{ORB_ID(sensor_combined)};			/**< combined sensor data topic */
 	uORB::Publication<sensor_preflight_s>		_sensor_preflight_pub{ORB_ID(sensor_preflight)};		/**< sensor preflight topic */
-	uORB::Publication<vehicle_air_data_s>		_airdata_pub{ORB_ID(vehicle_air_data)};			/**< combined sensor data topic */
 	uORB::Publication<vehicle_magnetometer_s>	_magnetometer_pub{ORB_ID(vehicle_magnetometer)};	/**< combined sensor data topic */
 
 	uORB::SubscriptionCallbackWorkItem _sensor_gyro_integrated_sub[GYRO_COUNT_MAX] {
@@ -159,6 +159,7 @@ class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::Sch
 
 	VehicleAcceleration	_vehicle_acceleration;
 	VehicleAngularVelocity	_vehicle_angular_velocity;
+	VehicleAirData          _vehicle_air_data;
 
 	static constexpr int MAX_SENSOR_COUNT = 3;
 	VehicleIMU      *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
@@ -175,7 +176,7 @@ class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::Sch
 	 * @param raw			Combined sensor data structure into which
 	 *				data should be returned.
 	 */
-	void		diff_pres_poll(const vehicle_air_data_s &airdata);
+	void		diff_pres_poll();
 
 	/**
 	 * Check for changes in parameters.
@@ -213,6 +214,7 @@ Sensors::Sensors(bool hil_enabled) :
 
 	_vehicle_acceleration.Start();
 	_vehicle_angular_velocity.Start();
+	_vehicle_air_data.Start();
 
 	InitializeVehicleIMU();
 }
@@ -221,6 +223,7 @@ Sensors::~Sensors()
 {
 	_vehicle_acceleration.Stop();
 	_vehicle_angular_velocity.Stop();
+	_vehicle_air_data.Stop();
 
 	for (auto &i : _vehicle_imu_list) {
 		if (i != nullptr) {
@@ -268,15 +271,17 @@ int Sensors::adc_init()
 	return OK;
 }
 
-void
-Sensors::diff_pres_poll(const vehicle_air_data_s &raw)
+void Sensors::diff_pres_poll()
 {
 	differential_pressure_s diff_pres{};
 
 	if (_diff_pres_sub.update(&diff_pres)) {
 
+		vehicle_air_data_s air_data{};
+		_vehicle_air_data_sub.copy(&air_data);
+
 		float air_temperature_celsius = (diff_pres.temperature > -300.0f) ? diff_pres.temperature :
-						(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
+						(air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
 
 		airspeed_s airspeed{};
 		airspeed.timestamp = diff_pres.timestamp;
@@ -312,10 +317,10 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw)
 		airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)
 						  _parameters.air_cmodel,
 						  smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
-						  diff_pres.differential_pressure_filtered_pa, raw.baro_pressure_pa,
+						  diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa,
 						  air_temperature_celsius);
 
-		airspeed.true_airspeed_m_s = calc_TAS_from_EAS(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa,
+		airspeed.true_airspeed_m_s = calc_TAS_from_EAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa,
 					     air_temperature_celsius); // assume that EAS = IAS as we don't have an EAS-scale here
 
 		airspeed.air_temperature_celsius = air_temperature_celsius;
@@ -482,14 +487,13 @@ void Sensors::Run()
 		}
 	}
 
-	vehicle_air_data_s airdata{};
 	vehicle_magnetometer_s magnetometer{};
-	_voted_sensors_update.sensorsPoll(_sensor_combined, airdata, magnetometer);
+	_voted_sensors_update.sensorsPoll(_sensor_combined, magnetometer);
 
 	// check analog airspeed
 	adc_poll();
 
-	diff_pres_poll(airdata);
+	diff_pres_poll();
 
 
 	// check failover and update subscribed sensor (if necessary)
@@ -517,11 +521,6 @@ void Sensors::Run()
 		}
 	}
 
-	if ((airdata.timestamp != 0) && (airdata.timestamp != _airdata_prev_timestamp)) {
-		_airdata_pub.publish(airdata);
-		_airdata_prev_timestamp = airdata.timestamp;
-	}
-
 	if ((magnetometer.timestamp != 0) && (magnetometer.timestamp != _magnetometer_prev_timestamp)) {
 		_magnetometer_pub.publish(magnetometer);
 		_magnetometer_prev_timestamp = magnetometer.timestamp;
@@ -618,6 +617,8 @@ int Sensors::print_status()
 {
 	_voted_sensors_update.printStatus();
 
+	PX4_INFO_RAW("\n");
+
 	PX4_INFO("Airspeed status:");
 	_airspeed_validator.print();
 
@@ -627,6 +628,11 @@ int Sensors::print_status()
 	PX4_INFO_RAW("\n");
 	_vehicle_angular_velocity.PrintStatus();
 
+	PX4_INFO_RAW("\n");
+	_vehicle_air_data.PrintStatus();
+
+	PX4_INFO_RAW("\n");
+
 	for (auto &i : _vehicle_imu_list) {
 		if (i != nullptr) {
 			PX4_INFO_RAW("\n");