sensors: move differential pressure aggregation to sensors/airspeed WorkItem

msg/airspeed.msg

@@ -1,9 +1,10 @@
-uint64 timestamp			# time since system start (microseconds)
+uint64 timestamp            # time since system start (microseconds)
+uint64 timestamp_sample     # the timestamp of the raw data (microseconds)
 
 float32 indicated_airspeed_m_s		# indicated airspeed in m/s
 
 float32 true_airspeed_m_s		# true filtered airspeed in m/s
 
-float32 air_temperature_celsius		# air temperature in degrees celsius, -1000 if unknown
+float32 air_temperature_celsius		# air temperature in degrees celsius, NAN if unknown
 
 float32 confidence			# confidence value from 0 to 1 for this sensor

src/lib/airspeed/airspeed.cpp

@@ -183,7 +183,6 @@ float calc_IAS(float differential_pressure)
 	} else {
 		return -sqrtf((2.0f * fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
 	}
-
 }
 
 float calc_TAS_from_CAS(float speed_calibrated, float pressure_ambient, float temperature_celsius)

src/modules/sensors/CMakeLists.txt

@@ -34,6 +34,7 @@
 add_subdirectory(data_validator)
 
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+add_subdirectory(airspeed)
 add_subdirectory(vehicle_acceleration)
 add_subdirectory(vehicle_angular_velocity)
 add_subdirectory(vehicle_air_data)
@@ -55,6 +56,7 @@ px4_add_module(
 		git_ecl
 		mathlib
 		sensor_calibration
+		sensors_airspeed
 		vehicle_acceleration
 		vehicle_angular_velocity
 		vehicle_air_data

src/modules/sensors/airspeed/Airspeed.cpp

@@ -0,0 +1,352 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "Airspeed.hpp"
+
+#include <px4_platform_common/log.h>
+#include <drivers/drv_airspeed.h>
+#include <lib/ecl/geo/geo.h>
+
+#include <lib/airspeed/airspeed.h>
+
+namespace sensors
+{
+
+using namespace matrix;
+using namespace time_literals;
+
+static constexpr uint32_t SENSOR_TIMEOUT{300_ms};
+
+/**
+ * HACK - true temperature is much less than indicated temperature in baro,
+ * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
+ */
+#define PCB_TEMP_ESTIMATE_DEG		5.0f
+
+Airspeed::Airspeed() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
+{
+	_voter.set_timeout(SENSOR_TIMEOUT);
+	_voter.set_equal_value_threshold(100);
+
+	ParametersUpdate(true);
+}
+
+Airspeed::~Airspeed()
+{
+	Stop();
+	perf_free(_cycle_perf);
+}
+
+bool Airspeed::Start()
+{
+	ScheduleNow();
+	return true;
+}
+
+void Airspeed::Stop()
+{
+	Deinit();
+
+	// clear all registered callbacks
+	for (auto &sub : _sensor_sub) {
+		sub.unregisterCallback();
+	}
+}
+
+void Airspeed::ParametersUpdate(bool force)
+{
+	// Check if parameters have changed
+	if (_parameter_update_sub.updated() || force) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		updateParams();
+
+		/* update airspeed scale */
+		int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
+
+		/* this sensor is optional, abort without error */
+		if (fd >= 0) {
+			struct airspeed_scale airscale = {
+				_param_sens_dpres_off.get(),
+				1.0f,
+			};
+
+			if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
+				PX4_ERR("failed to set offset for differential pressure sensor");
+			}
+
+			px4_close(fd);
+		}
+	}
+}
+
+void Airspeed::Run()
+{
+	perf_begin(_cycle_perf);
+
+	ParametersUpdate();
+
+	if (_vehicle_air_data_sub.updated()) {
+
+		vehicle_air_data_s air_data;
+
+		if (_vehicle_air_data_sub.copy(&air_data)) {
+			if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius)
+			    && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) {
+
+				// TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro
+				_baro_air_temperature = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG;
+			}
+		}
+	}
+
+	bool updated[MAX_SENSOR_COUNT] {};
+
+	for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
+
+		if (!_advertised[uorb_index]) {
+			// use data's timestamp to throttle advertisement checks
+			if ((_last_data[uorb_index].timestamp == 0) || (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s)) {
+				if (_sensor_sub[uorb_index].advertised()) {
+					if (uorb_index > 0) {
+						/* the first always exists, but for each further sensor, add a new validator */
+						if (!_voter.add_new_validator()) {
+							PX4_ERR("failed to add validator for %s %i", "DPRES", uorb_index);
+						}
+					}
+
+					_advertised[uorb_index] = true;
+
+					// advertise outputs in order if publishing all
+					if (!_param_sens_dpres_mode.get()) {
+						for (int instance = 0; instance < uorb_index; instance++) {
+							_airspeed_multi_pub[instance].advertise();
+						}
+					}
+
+					if (_selected_sensor_sub_index < 0) {
+						_sensor_sub[uorb_index].registerCallback();
+					}
+
+				} else {
+					_last_data[uorb_index].timestamp = hrt_absolute_time();
+				}
+			}
+		}
+
+		if (_advertised[uorb_index]) {
+			differential_pressure_s diff_pres;
+
+			while (_sensor_sub[uorb_index].update(&diff_pres)) {
+				updated[uorb_index] = true;
+
+				_device_id[uorb_index] = diff_pres.device_id;
+
+				float vect[3] {diff_pres.differential_pressure_raw_pa, diff_pres.temperature, 0.f};
+				_voter.put(uorb_index, diff_pres.timestamp, vect, diff_pres.error_count, _priority[uorb_index]);
+
+
+				float air_temperature_celsius = NAN;
+
+				// assume anything outside of a (generous) operating range of -40C to 125C is invalid
+				if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) {
+
+					air_temperature_celsius = diff_pres.temperature;
+
+				} else {
+					// differential pressure temperature invalid, use barometer temperature if available
+					if (PX4_ISFINITE(_baro_air_temperature)) {
+						air_temperature_celsius = _baro_air_temperature;
+					}
+				}
+
+				// average raw data for all instances
+				_timestamp_sample_sum[uorb_index] += diff_pres.timestamp;
+				_differential_pressure_sum[uorb_index] += diff_pres.differential_pressure_filtered_pa;
+				_temperature_sum[uorb_index] += air_temperature_celsius;
+				_sum_count[uorb_index]++;
+			}
+		}
+	}
+
+	// check for the current best sensor
+	int best_index = 0;
+	_voter.get_best(hrt_absolute_time(), &best_index);
+
+	if (best_index >= 0) {
+		if (_selected_sensor_sub_index != best_index) {
+			// clear all registered callbacks
+			for (auto &sub : _sensor_sub) {
+				sub.unregisterCallback();
+			}
+
+			if (_param_sens_dpres_mode.get()) {
+				if (_selected_sensor_sub_index >= 0) {
+					PX4_INFO("%s switch from #%u -> #%d", "DPRES", _selected_sensor_sub_index, best_index);
+				}
+			}
+
+			_selected_sensor_sub_index = best_index;
+			_sensor_sub[_selected_sensor_sub_index].registerCallback();
+		}
+	}
+
+	// Publish
+	if (_param_sens_dpres_mode.get()) {
+		// publish only best sensor
+		if ((_selected_sensor_sub_index >= 0)
+		    && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR)
+		    && updated[_selected_sensor_sub_index]) {
+
+			Publish(_selected_sensor_sub_index);
+		}
+
+	} else {
+		// publish all
+		for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
+			// publish all sensors as separate instances
+			if (updated[uorb_index] && (_device_id[uorb_index] != 0)) {
+				Publish(uorb_index, true);
+			}
+		}
+	}
+
+
+	// check failover and report
+	if (_param_sens_dpres_mode.get()) {
+		if (_last_failover_count != _voter.failover_count()) {
+			uint32_t flags = _voter.failover_state();
+			int failover_index = _voter.failover_index();
+
+			if (flags != DataValidator::ERROR_FLAG_NO_ERROR) {
+				if (failover_index != -1) {
+					const hrt_abstime now = hrt_absolute_time();
+
+					if (now - _last_error_message > 3_s) {
+						mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!",
+								      "DPRES",
+								      failover_index,
+								      ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""),
+								      ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""),
+								      ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""),
+								      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""),
+								      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : ""));
+						_last_error_message = now;
+					}
+
+					// reduce priority of failed sensor to the minimum
+					_priority[failover_index] = 1;
+				}
+			}
+
+			_last_failover_count = _voter.failover_count();
+		}
+	}
+
+	// reschedule timeout
+	ScheduleDelayed(100_ms);
+
+	perf_end(_cycle_perf);
+}
+
+void Airspeed::Publish(uint8_t instance, bool multi)
+{
+	if ((_param_sens_dpres_rate.get() > 0)
+	    && hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_dpres_rate.get())) {
+
+		const float differential_pressure = _differential_pressure_sum[instance] / _sum_count[instance];
+		const float temperature = _temperature_sum[instance] / _sum_count[instance];
+		const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _sum_count[instance];
+
+		// reset
+		_timestamp_sample_sum[instance] = 0;
+		_differential_pressure_sum[instance] = 0;
+		_temperature_sum[instance] = 0;
+		_sum_count[instance] = 0;
+
+		airspeed_s out{};
+		out.timestamp_sample = timestamp_sample;
+		out.air_temperature_celsius = temperature;
+		out.confidence = 1.f; // TODO
+
+		switch ((_device_id[instance] >> 16) & 0xFF) {
+		case DRV_DIFF_PRESS_DEVTYPE_SDP31:
+
+		// fallthrough
+		case DRV_DIFF_PRESS_DEVTYPE_SDP32:
+
+		// fallthrough
+		case DRV_DIFF_PRESS_DEVTYPE_SDP33:
+			out.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL) _param_cal_air_cmodel.get(),
+						     AIRSPEED_SENSOR_MODEL_SDP3X, _param_cal_air_tubelen.get(), _param_cal_air_tubed_mm.get(),
+						     differential_pressure, _baro_pressure_pa, temperature);
+			break;
+
+		default:
+			out.indicated_airspeed_m_s = calc_IAS(differential_pressure);
+			break;
+		}
+
+		// assume that CAS = IAS as we don't have an CAS-scale here
+		out.true_airspeed_m_s = calc_TAS_from_CAS(out.indicated_airspeed_m_s, _baro_pressure_pa, temperature);
+
+		if (PX4_ISFINITE(out.indicated_airspeed_m_s) && PX4_ISFINITE(out.true_airspeed_m_s)) {
+			out.timestamp = hrt_absolute_time();
+
+			if (multi) {
+				_airspeed_multi_pub[instance].publish(out);
+
+			} else {
+				// otherwise only ever publish the first instance
+				_airspeed_multi_pub[0].publish(out);
+			}
+
+			_last_publication_timestamp[instance] = out.timestamp;
+		}
+	}
+}
+
+void Airspeed::PrintStatus()
+{
+	if (_selected_sensor_sub_index >= 0) {
+		PX4_INFO("selected differential pressure: %d (%d)", _device_id[_selected_sensor_sub_index], _selected_sensor_sub_index);
+	}
+
+	_voter.print();
+}
+
+}; // namespace sensors

src/modules/sensors/airspeed/Airspeed.hpp

@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "data_validator/DataValidatorGroup.hpp"
+
+#include <lib/mathlib/math/Limits.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <lib/systemlib/mavlink_log.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_air_data.h>
+
+using namespace time_literals;
+
+namespace sensors
+{
+class Airspeed : public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+
+	Airspeed();
+	~Airspeed() override;
+
+	bool Start();
+	void Stop();
+
+	void PrintStatus();
+
+private:
+	void Run() override;
+
+	void ParametersUpdate(bool force = false);
+
+	void Publish(uint8_t instance, bool multi = false);
+
+	static constexpr int MAX_SENSOR_COUNT = 4;
+
+	uORB::PublicationMulti<airspeed_s> _airspeed_multi_pub[MAX_SENSOR_COUNT] {
+		{ORB_ID(airspeed)},
+		{ORB_ID(airspeed)},
+		{ORB_ID(airspeed)},
+		{ORB_ID(airspeed)},
+	};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
+
+	uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
+		{this, ORB_ID(differential_pressure), 0},
+		{this, ORB_ID(differential_pressure), 1},
+		{this, ORB_ID(differential_pressure), 2},
+		{this, ORB_ID(differential_pressure), 3},
+	};
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+
+	hrt_abstime _last_error_message{0};
+	orb_advert_t _mavlink_log_pub{nullptr};
+
+	DataValidatorGroup _voter{1};
+	unsigned _last_failover_count{0};
+
+	uint32_t _device_id[MAX_SENSOR_COUNT] {};
+	uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {0};
+	hrt_abstime _last_publication_timestamp[MAX_SENSOR_COUNT] {};
+	float _differential_pressure_sum[MAX_SENSOR_COUNT] {};
+	float _temperature_sum[MAX_SENSOR_COUNT] {};
+	int _sum_count[MAX_SENSOR_COUNT] {};
+
+	differential_pressure_s _last_data[MAX_SENSOR_COUNT] {};
+	bool _advertised[MAX_SENSOR_COUNT] {};
+
+	uint8_t _priority[MAX_SENSOR_COUNT] {100, 100, 100, 100};
+
+	int8_t _selected_sensor_sub_index{-1};
+
+	float _baro_pressure_pa{101325.f}; // Sea level standard atmospheric pressure
+	float _baro_air_temperature{15.f};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SENS_DPRES_MODE>) _param_sens_dpres_mode,
+		(ParamFloat<px4::params::SENS_DPRES_OFF>) _param_sens_dpres_off,
+		(ParamFloat<px4::params::SENS_DPRES_RATE>) _param_sens_dpres_rate,
+		(ParamInt<px4::params::CAL_AIR_CMODEL>) _param_cal_air_cmodel,
+		(ParamFloat<px4::params::CAL_AIR_TUBELEN>) _param_cal_air_tubelen,
+		(ParamFloat<px4::params::CAL_AIR_TUBED_MM>) _param_cal_air_tubed_mm
+	)
+};
+}; // namespace sensors

src/modules/sensors/airspeed/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(sensors_airspeed
+	Airspeed.cpp
+	Airspeed.hpp
+)
+target_link_libraries(sensors_airspeed
+	PRIVATE
+		airspeed
+		px4_work_queue
+)

src/modules/sensors/airspeed/params.c

@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Airspeed sensor compensation model for the SDP3x
+ *
+ * Model with Pitot
+ * 		CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed.
+ * 		CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor.
+ * Model without Pitot (1.5 mm tubes)
+ * 		CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed.
+ * 		CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor.
+ * Tube Pressure Drop
+ * 		CAL_AIR_TUBED_MM: Diameter in mm of the pitot and tubes, must have the same diameter.
+ * 		CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor and the static + dynamic port length of the pitot.
+ *
+ * @value 0 Model with Pitot
+ * @value 1 Model without Pitot (1.5 mm tubes)
+ * @value 2 Tube Pressure Drop
+ *
+ * @group Sensors
+ */
+PARAM_DEFINE_INT32(CAL_AIR_CMODEL, 0);
+
+/**
+ * Airspeed sensor tube length.
+ *
+ * See the CAL_AIR_CMODEL explanation on how this parameter should be set.
+ *
+ * @min 0.01
+ * @max 2.00
+ * @unit m
+ *
+ * @group Sensors
+ */
+PARAM_DEFINE_FLOAT(CAL_AIR_TUBELEN, 0.2f);
+
+/**
+ * Airspeed sensor tube diameter. Only used for the Tube Pressure Drop Compensation.
+ *
+ * @min 0.1
+ * @max 100
+ * @unit mm
+ *
+ * @group Sensors
+ */
+PARAM_DEFINE_FLOAT(CAL_AIR_TUBED_MM, 1.5f);
+
+/**
+ * Differential pressure sensor offset
+ *
+ * The offset (zero-reading) in Pascal
+ *
+ * @category system
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
+
+/**
+ * Sensors hub differential pressure mode
+ *
+ * @value 0 Publish all airspeeds
+ * @value 1 Publish primary airspeed
+ *
+ * @category system
+ * @reboot_required true
+ * @group Sensors
+ */
+PARAM_DEFINE_INT32(SENS_DPRES_MODE, 1);
+
+/**
+ * Differential pressure (airspeed) max rate.
+ *
+ * Airspeed data maximum publication rate. This is an upper bound,
+ * actual differential pressure data rate is still dependant on the sensor.
+ *
+ * @min 1
+ * @max 100
+ * @group Sensors
+ * @unit Hz
+ *
+ * @reboot_required true
+ *
+ */
+PARAM_DEFINE_FLOAT(SENS_DPRES_RATE, 10.0f);

src/modules/sensors/sensor_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,62 +31,6 @@
  *
  ****************************************************************************/
 
-/**
- * Airspeed sensor compensation model for the SDP3x
- *
- * Model with Pitot
- * 		CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed.
- * 		CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor.
- * Model without Pitot (1.5 mm tubes)
- * 		CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed.
- * 		CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor.
- * Tube Pressure Drop
- * 		CAL_AIR_TUBED_MM: Diameter in mm of the pitot and tubes, must have the same diameter.
- * 		CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor and the static + dynamic port length of the pitot.
- *
- * @value 0 Model with Pitot
- * @value 1 Model without Pitot (1.5 mm tubes)
- * @value 2 Tube Pressure Drop
- *
- * @group Sensors
- */
-PARAM_DEFINE_INT32(CAL_AIR_CMODEL, 0);
-
-/**
- * Airspeed sensor tube length.
- *
- * See the CAL_AIR_CMODEL explanation on how this parameter should be set.
- *
- * @min 0.01
- * @max 2.00
- * @unit m
- *
- * @group Sensors
- */
-PARAM_DEFINE_FLOAT(CAL_AIR_TUBELEN, 0.2f);
-
-/**
- * Airspeed sensor tube diameter. Only used for the Tube Pressure Drop Compensation.
- *
- * @min 0.1
- * @max 100
- * @unit mm
- *
- * @group Sensors
- */
-PARAM_DEFINE_FLOAT(CAL_AIR_TUBED_MM, 1.5f);
-
-/**
- * Differential pressure sensor offset
- *
- * The offset (zero-reading) in Pascal
- *
- * @category system
- * @group Sensor Calibration
- * @volatile
- */
-PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
-
 /**
  * Differential pressure sensor analog scaling
  *

src/modules/sensors/sensors.cpp

@@ -44,7 +44,6 @@
 #include <drivers/drv_adc.h>
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
-#include <lib/airspeed/airspeed.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
@@ -61,7 +60,6 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/airspeed.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensors_status_imu.h>
@@ -70,6 +68,7 @@
 #include <uORB/topics/vehicle_imu.h>
 
 #include "voted_sensors_update.h"
+#include "airspeed/Airspeed.hpp"
 #include "vehicle_acceleration/VehicleAcceleration.hpp"
 #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
 #include "vehicle_air_data/VehicleAirData.hpp"
@@ -80,11 +79,6 @@
 using namespace sensors;
 using namespace time_literals;
 
-/**
- * HACK - true temperature is much less than indicated temperature in baro,
- * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
- */
-#define PCB_TEMP_ESTIMATE_DEG		5.0f
 class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
@@ -126,17 +120,12 @@ private:
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)};
 	uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
 
-	uORB::Publication<airspeed_s>             _airspeed_pub{ORB_ID(airspeed)};
 	uORB::Publication<sensor_combined_s>      _sensor_pub{ORB_ID(sensor_combined)};
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-	DataValidator	_airspeed_validator;		/**< data validator to monitor airspeed */
-
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
 
 	hrt_abstime	_last_adc{0};			/**< last time we took input from the ADC */
@@ -153,9 +142,6 @@ private:
 		float diff_pres_analog_scale;
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
 
-		int32_t air_cmodel;
-		float air_tube_length;
-		float air_tube_diameter_mm;
 	} _parameters{}; /**< local copies of interesting parameters */
 
 	struct ParameterHandles {
@@ -164,15 +150,14 @@ private:
 		param_t diff_pres_analog_scale;
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
 
-		param_t air_cmodel;
-		param_t air_tube_length;
-		param_t air_tube_diameter_mm;
 	} _parameter_handles{};		/**< handles for interesting parameters */
 
 	VotedSensorsUpdate _voted_sensors_update;
 
 	VehicleAcceleration	_vehicle_acceleration;
 	VehicleAngularVelocity	_vehicle_angular_velocity;
+
+	Airspeed                *_airspeed{nullptr};
 	VehicleAirData          *_vehicle_air_data{nullptr};
 	VehicleMagnetometer     *_vehicle_magnetometer{nullptr};
 	VehicleGPSPosition	*_vehicle_gps_position{nullptr};
@@ -184,14 +169,6 @@ private:
 	 */
 	int		parameters_update();
 
-	/**
-	 * Poll the differential pressure sensor for updated data.
-	 *
-	 * @param raw			Combined sensor data structure into which
-	 *				data should be returned.
-	 */
-	void		diff_pres_poll();
-
 	/**
 	 * Check for changes in parameters.
 	 */
@@ -205,6 +182,7 @@ private:
 	 */
 	void		adc_poll();
 
+	void		InitializeAirspeed();
 	void		InitializeVehicleAirData();
 	void		InitializeVehicleGPSPosition();
 	void		InitializeVehicleIMU();
@@ -230,10 +208,6 @@ Sensors::Sensors(bool hil_enabled) :
 	_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
 
-	_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");
-
 	param_find("SYS_FAC_CAL_MODE");
 
 	// Parameters controlling the on-board sensor thermal calibrator
@@ -241,9 +215,6 @@ Sensors::Sensors(bool hil_enabled) :
 	param_find("SYS_CAL_TMAX");
 	param_find("SYS_CAL_TMIN");
 
-	_airspeed_validator.set_timeout(300000);
-	_airspeed_validator.set_equal_value_threshold(100);
-
 	_vehicle_acceleration.Start();
 	_vehicle_angular_velocity.Start();
 }
@@ -258,6 +229,11 @@ Sensors::~Sensors()
 	_vehicle_acceleration.Stop();
 	_vehicle_angular_velocity.Stop();
 
+	if (_airspeed) {
+		_airspeed->Stop();
+		delete _airspeed;
+	}
+
 	if (_vehicle_air_data) {
 		_vehicle_air_data->Stop();
 		delete _vehicle_air_data;
@@ -302,88 +278,11 @@ int Sensors::parameters_update()
 	param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale));
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
 
-	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);
-
 	_voted_sensors_update.parametersUpdate();
 
 	return PX4_OK;
 }
 
-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 = NAN;
-
-		// assume anything outside of a (generous) operating range of -40C to 125C is invalid
-		if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) {
-
-			air_temperature_celsius = diff_pres.temperature;
-
-		} else {
-			// differential pressure temperature invalid, check barometer
-			if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius)
-			    && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) {
-
-				// TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro
-				air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG;
-			}
-		}
-
-		airspeed_s airspeed{};
-		airspeed.timestamp = diff_pres.timestamp;
-
-		/* push data into validator */
-		float airspeed_input[3] = { diff_pres.differential_pressure_raw_pa, diff_pres.temperature, 0.0f };
-
-		_airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count, 100); // TODO: real priority?
-
-		airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
-
-		enum AIRSPEED_SENSOR_MODEL smodel;
-
-		switch ((diff_pres.device_id >> 16) & 0xFF) {
-		case DRV_DIFF_PRESS_DEVTYPE_SDP31:
-
-		/* fallthrough */
-		case DRV_DIFF_PRESS_DEVTYPE_SDP32:
-
-		/* fallthrough */
-		case DRV_DIFF_PRESS_DEVTYPE_SDP33:
-			/* fallthrough */
-			smodel = AIRSPEED_SENSOR_MODEL_SDP3X;
-			break;
-
-		default:
-			smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE;
-			break;
-		}
-
-		/* don't risk to feed negative airspeed into the system */
-		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, air_data.baro_pressure_pa,
-						  air_temperature_celsius);
-
-		airspeed.true_airspeed_m_s = calc_TAS_from_CAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa,
-					     air_temperature_celsius); // assume that CAS = IAS as we don't have an CAS-scale here
-
-		airspeed.air_temperature_celsius = air_temperature_celsius;
-
-		if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && PX4_ISFINITE(airspeed.true_airspeed_m_s)) {
-			_airspeed_pub.publish(airspeed);
-		}
-	}
-}
-
 void
 Sensors::parameter_update_poll(bool forced)
 {
@@ -396,23 +295,6 @@ Sensors::parameter_update_poll(bool forced)
 		// update parameters from storage
 		parameters_update();
 		updateParams();
-
-		/* update airspeed scale */
-		int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
-
-		/* this sensor is optional, abort without error */
-		if (fd >= 0) {
-			struct airspeed_scale airscale = {
-				_parameters.diff_pres_offset_pa,
-				1.0f,
-			};
-
-			if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
-				warn("WARNING: failed to set scale / offsets for airspeed sensor");
-			}
-
-			px4_close(fd);
-		}
 	}
 }
 
@@ -427,7 +309,7 @@ void Sensors::adc_poll()
 
 	if (_parameters.diff_pres_analog_scale > 0.0f) {
 
-		hrt_abstime t = hrt_absolute_time();
+		const hrt_abstime t = hrt_absolute_time();
 
 		/* rate limit to 100 Hz */
 		if (t - _last_adc >= 10000) {
@@ -456,11 +338,12 @@ void Sensors::adc_poll()
 						if (voltage > 0.4f) {
 							const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
 
-							_diff_pres.timestamp = t;
 							_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
 							_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) +
 									(diff_pres_pa_raw * 0.1f);
-							_diff_pres.temperature = -1000.0f;
+							_diff_pres.temperature = NAN;
+
+							_diff_pres.timestamp = hrt_absolute_time();
 
 							_diff_pres_pub.publish(_diff_pres);
 						}
@@ -475,6 +358,19 @@ void Sensors::adc_poll()
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
 }
 
+void Sensors::InitializeAirspeed()
+{
+	if (_airspeed == nullptr) {
+		if (orb_exists(ORB_ID(differential_pressure), 0) == PX4_OK) {
+			_airspeed = new Airspeed();
+
+			if (_airspeed) {
+				_airspeed->Start();
+			}
+		}
+	}
+}
+
 void Sensors::InitializeVehicleAirData()
 {
 	if (_param_sys_has_baro.get()) {
@@ -572,6 +468,7 @@ void Sensors::Run()
 
 	// run once
 	if (_last_config_update == 0) {
+		InitializeAirspeed();
 		InitializeVehicleAirData();
 		InitializeVehicleIMU();
 		InitializeVehicleGPSPosition();
@@ -599,8 +496,6 @@ void Sensors::Run()
 	// check analog airspeed
 	adc_poll();
 
-	diff_pres_poll();
-
 	if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {
 
 		_voted_sensors_update.setRelativeTimestamps(_sensor_combined);
@@ -612,6 +507,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();
+		InitializeAirspeed();
 		InitializeVehicleAirData();
 		InitializeVehicleIMU();
 		InitializeVehicleGPSPosition();
@@ -691,9 +587,10 @@ int Sensors::print_status()
 		_vehicle_air_data->PrintStatus();
 	}
 
-	PX4_INFO_RAW("\n");
+	if (_airspeed) {
-	PX4_INFO("Airspeed status:");
+		PX4_INFO_RAW("\n");
-	_airspeed_validator.print();
+		_airspeed->PrintStatus();
+	}
 
 	PX4_INFO_RAW("\n");
 	_vehicle_acceleration.PrintStatus();