sensors: throttle vehicle_air_data publication (SENS_BARO_RATE)

- don't bother running baro aggregator if SYS_HAS_BARO disabled

src/modules/sensors/sensors.cpp

@@ -168,7 +168,7 @@ private:
 
 	VehicleAcceleration	_vehicle_acceleration;
 	VehicleAngularVelocity	_vehicle_angular_velocity;
-	VehicleAirData          _vehicle_air_data;
+	VehicleAirData          *_vehicle_air_data{nullptr};
 
 	static constexpr int MAX_SENSOR_COUNT = 3;
 	VehicleIMU      *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
@@ -200,8 +200,12 @@ private:
 	 */
 	void		adc_poll();
 
+	void		InitializeVehicleAirData();
 	void		InitializeVehicleIMU();
 
+	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro
+	)
 };
 
 Sensors::Sensors(bool hil_enabled) :
@@ -218,9 +222,6 @@ Sensors::Sensors(bool hil_enabled) :
 
 	_vehicle_acceleration.Start();
 	_vehicle_angular_velocity.Start();
-	_vehicle_air_data.Start();
-
-	InitializeVehicleIMU();
 }
 
 Sensors::~Sensors()
@@ -232,11 +233,16 @@ Sensors::~Sensors()
 
 	_vehicle_acceleration.Stop();
 	_vehicle_angular_velocity.Stop();
-	_vehicle_air_data.Stop();
 
-	for (auto &i : _vehicle_imu_list) {
-		if (i != nullptr) {
-			i->Stop();
+	if (_vehicle_air_data) {
+		_vehicle_air_data->Stop();
+		delete _vehicle_air_data;
+	}
+
+	for (auto &vehicle_imu : _vehicle_imu_list) {
+		if (vehicle_imu) {
+			vehicle_imu->Stop();
+			delete vehicle_imu;
 		}
 	}
 
@@ -418,6 +424,21 @@ void Sensors::adc_poll()
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
 }
 
+void Sensors::InitializeVehicleAirData()
+{
+	if (_param_sys_has_baro.get()) {
+		if (_vehicle_air_data == nullptr) {
+			if (orb_exists(ORB_ID(sensor_baro), 0) == PX4_OK) {
+				_vehicle_air_data = new VehicleAirData();
+
+				if (_vehicle_air_data) {
+					_vehicle_air_data->Start();
+				}
+			}
+		}
+	}
+}
+
 void Sensors::InitializeVehicleIMU()
 {
 	// create a VehicleIMU instance for each accel/gyro pair
@@ -439,7 +460,6 @@ void Sensors::InitializeVehicleIMU()
 					// Start VehicleIMU instance and store
 					if (imu->Start()) {
 						_vehicle_imu_list[i] = imu;
-						ScheduleNow();
 
 					} else {
 						delete imu;
@@ -468,6 +488,8 @@ void Sensors::Run()
 
 	// run once
 	if (_last_config_update == 0) {
+		InitializeVehicleAirData();
+		InitializeVehicleIMU();
 		_voted_sensors_update.init(_sensor_combined);
 		parameter_update_poll(true);
 	}
@@ -555,6 +577,7 @@ void Sensors::Run()
 	// when not adding sensors poll for param updates
 	if (!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) {
 		_voted_sensors_update.initializeSensors();
+		InitializeVehicleAirData();
 		InitializeVehicleIMU();
 		_last_config_update = hrt_absolute_time();
 
@@ -621,8 +644,10 @@ int Sensors::print_status()
 {
 	_voted_sensors_update.printStatus();
 
-	PX4_INFO_RAW("\n");
-	_vehicle_air_data.PrintStatus();
+	if (_vehicle_air_data) {
+		PX4_INFO_RAW("\n");
+		_vehicle_air_data->PrintStatus();
+	}
 
 	PX4_INFO_RAW("\n");
 	PX4_INFO("Airspeed status:");

src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

@@ -200,46 +200,64 @@ void VehicleAirData::Run()
 
 		const sensor_baro_s &baro = _last_data[_selected_sensor_sub_index];
 
-		// populate vehicle_air_data with primary baro and publish
-		vehicle_air_data_s out{};
-		out.timestamp_sample = baro.timestamp; // TODO: baro.timestamp_sample;
-		out.baro_device_id = baro.device_id;
-		out.baro_temp_celcius = baro.temperature;
-
-		// Convert from millibar to Pa and apply temperature compensation
-		out.baro_pressure_pa = 100.0f * baro.pressure - _thermal_offset[_selected_sensor_sub_index];
-
-		// calculate altitude using the hypsometric equation
-		static constexpr float T1 = 15.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin
-		static constexpr float a = -6.5f / 1000.0f; // temperature gradient in degrees per metre
-
-		// current pressure at MSL in kPa (QNH in hPa)
-		const float p1 = _param_sens_baro_qnh.get() * 0.1f;
-
-		// measured pressure in kPa
-		const float p = out.baro_pressure_pa * 0.001f;
-
-		/*
-		 * Solve:
-		 *
-		 *     /        -(aR / g)     \
-		 *    | (p / p1)          . T1 | - T1
-		 *     \                      /
-		 * h = -------------------------------  + h1
-		 *                   a
-		 */
-		out.baro_alt_meter = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
-
-		// calculate air density
-		// estimate air density assuming typical 20degC ambient temperature
-		// TODO: use air temperature if available (differential pressure sensors)
-		static constexpr float pressure_to_density = 1.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f -
-				CONSTANTS_ABSOLUTE_NULL_CELSIUS));
-
-		out.rho = pressure_to_density * out.baro_pressure_pa;
-
-		out.timestamp = hrt_absolute_time();
-		_vehicle_air_data_pub.publish(out);
+		_baro_timestamp_sum += baro.timestamp;
+		_baro_sum += baro.pressure;
+		_baro_sum_count++;
+
+		if ((_param_sens_baro_rate.get() > 0)
+		    && hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_baro_rate.get())) {
+
+			const float pressure = _baro_sum / _baro_sum_count;
+			const hrt_abstime timestamp_sample = _baro_timestamp_sum / _baro_sum_count;
+
+			// reset
+			_baro_timestamp_sum = 0;
+			_baro_sum = 0.f;
+			_baro_sum_count = 0;
+
+			// populate vehicle_air_data with primary baro and publish
+			vehicle_air_data_s out{};
+			out.timestamp_sample = timestamp_sample; // TODO: baro.timestamp_sample;
+			out.baro_device_id = baro.device_id;
+			out.baro_temp_celcius = baro.temperature;
+
+			// Convert from millibar to Pa and apply temperature compensation
+			out.baro_pressure_pa = 100.0f * pressure - _thermal_offset[_selected_sensor_sub_index];
+
+			// calculate altitude using the hypsometric equation
+			static constexpr float T1 = 15.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin
+			static constexpr float a = -6.5f / 1000.0f; // temperature gradient in degrees per metre
+
+			// current pressure at MSL in kPa (QNH in hPa)
+			const float p1 = _param_sens_baro_qnh.get() * 0.1f;
+
+			// measured pressure in kPa
+			const float p = out.baro_pressure_pa * 0.001f;
+
+			/*
+			 * Solve:
+			 *
+			 *     /        -(aR / g)     \
+			 *    | (p / p1)          . T1 | - T1
+			 *     \                      /
+			 * h = -------------------------------  + h1
+			 *                   a
+			 */
+			out.baro_alt_meter = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
+
+			// calculate air density
+			// estimate air density assuming typical 20degC ambient temperature
+			// TODO: use air temperature if available (differential pressure sensors)
+			static constexpr float pressure_to_density = 1.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f -
+					CONSTANTS_ABSOLUTE_NULL_CELSIUS));
+
+			out.rho = pressure_to_density * out.baro_pressure_pa;
+
+			out.timestamp = hrt_absolute_time();
+			_vehicle_air_data_pub.publish(out);
+
+			_last_publication_timestamp = out.timestamp;
+		}
 	}
 
 	// check failover and report

src/modules/sensors/vehicle_air_data/VehicleAirData.hpp

@@ -86,12 +86,17 @@ private:
 
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 
+	hrt_abstime _last_publication_timestamp{0};
 	hrt_abstime _last_error_message{0};
 	orb_advert_t _mavlink_log_pub{nullptr};
 
 	DataValidatorGroup _voter{1};
 	unsigned _last_failover_count{0};
 
+	uint64_t _baro_timestamp_sum{0};
+	float _baro_sum{0.f};
+	int _baro_sum_count{0};
+
 	sensor_baro_s _last_data[MAX_SENSOR_COUNT] {};
 	bool _advertised[MAX_SENSOR_COUNT] {};
 
@@ -102,7 +107,8 @@ private:
 	int8_t _selected_sensor_sub_index{-1};
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh
+		(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh,
+		(ParamFloat<px4::params::SENS_BARO_RATE>) _param_sens_baro_rate
 	)
 };
 }; // namespace sensors

src/modules/sensors/vehicle_air_data/params.c

@@ -43,3 +43,19 @@
  *
  */
 PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
+
+/**
+ * Baro max rate.
+ *
+ * Barometric air data maximum publication rate. This is an upper bound,
+ * actual barometric data rate is still dependant on the sensor.
+ *
+ * @min 1
+ * @max 200
+ * @group Sensors
+ * @unit Hz
+ *
+ * @reboot_required true
+ *
+ */
+PARAM_DEFINE_FLOAT(SENS_BARO_RATE, 20.0f);