move analog differential pressure to standalone optional driver

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -133,6 +133,12 @@ then
 			ms5525 start -X -q
 
 			ets_airspeed start -X -q
+
+			# analog differential pressure
+			if param greater -s SENS_DPRES_ANSC 0
+			then
+				analog_diffpres_main start
+			fi
 		fi
 	fi
 fi

boards/px4/raspberrypi/src/board_config.h

@@ -51,7 +51,5 @@
 #define ADC_BATTERY_CURRENT_CHANNEL	-1
 #define ADC_AIRSPEED_VOLTAGE_CHANNEL 2
 
-#define ADC_DP_V_DIV 1.0f
-
 #include <system_config.h>
 #include <px4_platform_common/board_common.h>

boards/scumaker/pilotpi/src/board_config.h

@@ -52,7 +52,6 @@
 #define ADC_AIRSPEED_VOLTAGE_CHANNEL 2
 
 #define BOARD_BATTERY1_V_DIV 5.7f	// 1K + 4.7K
-#define ADC_DP_V_DIV 1.0f
 
 #define BOARD_ADC_OPEN_CIRCUIT_V 5.3f	// Powered from USB
 

src/drivers/differential_pressure/CMakeLists.txt

@@ -31,6 +31,7 @@
 #
 ############################################################################
 
+add_subdirectory(analog_diffpres)
 add_subdirectory(ets)
 add_subdirectory(ms4515)
 add_subdirectory(ms4525)

src/drivers/differential_pressure/analog_diffpres/AnalogDifferentialPressure.cpp

@@ -0,0 +1,205 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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
+ * 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 "AnalogDifferentialPressure.hpp"
+
+#include <px4_platform_common/getopt.h>
+
+AnalogDifferentialPressure::AnalogDifferentialPressure(int16_t adc_channel) :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
+	_adc_channel(adc_channel)
+{
+}
+
+int AnalogDifferentialPressure::init()
+{
+	_adc_report_sub.registerCallback();
+	ScheduleNow();
+
+	return PX4_OK;
+}
+
+void AnalogDifferentialPressure::Run()
+{
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		updateParams();
+	}
+
+	perf_begin(_sample_perf);
+
+	if (_param_sens_dpres_ansc.get() > 0.0f) {
+		adc_report_s adc;
+
+		if (_adc_report_sub.update(&adc)) {
+			/* Read add channels we got */
+			for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) {
+				if (_adc_channel == adc.channel_id[i]) {
+
+					// The scale factor defined by HW's resistive divider (Rt+Rb)/ Rb
+#if defined(ADC_DP_V_DIV)
+					static constexpr float scale = ADC_DP_V_DIV;
+#else
+					static constexpr float scale = 1.0f;
+#endif
+
+					/* calculate airspeed, raw is the difference from */
+					const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * scale;
+
+					/**
+					 * The voltage divider pulls the signal down, only act on
+					 * a valid voltage from a connected sensor. Also assume a non-
+					 * zero offset from the sensor if its connected.
+					 *
+					 * Notice: This won't work on devices which have PGA controlled
+					 * vref. Those devices require no divider at all.
+					 */
+					if (voltage > 0.4f) {
+						const float differential_pressure_pa = voltage * _param_sens_dpres_ansc.get();
+
+						if (fabsf(differential_pressure_pa - _differential_pressure_pa_last) > 0.01f) {
+							// only publish changes
+							sensor_differential_pressure_s diff_pres{};
+							diff_pres.device_id = 0xAA; // TODO DRV_DIFF_PRESS_DEVTYPE_MS4525
+							diff_pres.timestamp_sample = adc.timestamp;
+							diff_pres.differential_pressure_pa = differential_pressure_pa;
+							diff_pres.temperature = NAN;
+							diff_pres.timestamp = hrt_absolute_time();
+							_diff_pres_pub.publish(diff_pres);
+
+							_differential_pressure_pa_last = differential_pressure_pa;
+						}
+					}
+
+					break;
+				}
+			}
+
+			PX4_DEBUG("ADC channel %d not found", _adc_channel);
+		}
+	}
+
+	perf_end(_sample_perf);
+}
+
+int AnalogDifferentialPressure::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int AnalogDifferentialPressure::task_spawn(int argc, char *argv[])
+{
+#if defined(ADC_AIRSPEED_VOLTAGE_CHANNEL)
+	int16_t adc_channel = ADC_AIRSPEED_VOLTAGE_CHANNEL;
+#else
+	int16_t adc_channel = -1;
+#endif
+
+	bool error_flag = false;
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "c:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'c':
+			adc_channel = strtoul(myoptarg, nullptr, 10);
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+	if (error_flag) {
+		return PX4_ERROR;
+	}
+
+	AnalogDifferentialPressure *instance = new AnalogDifferentialPressure(adc_channel);
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init() == PX4_OK) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int AnalogDifferentialPressure::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+ADC driver.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("analog_diffpres", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_INT('c', -1, 0, 32, "ADC channel", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int analog_diffpres_main(int argc, char *argv[])
+{
+	return AnalogDifferentialPressure::main(argc, argv);
+}

src/drivers/differential_pressure/analog_diffpres/AnalogDifferentialPressure.hpp

@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ *
+ * Driver for analog differential pressure sensor
+ *
+ */
+
+#pragma once
+
+#include <drivers/drv_adc.h>
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/adc_report.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_differential_pressure.h>
+
+using namespace time_literals;
+
+class AnalogDifferentialPressure : public ModuleBase<AnalogDifferentialPressure>, public ModuleParams,
+	public px4::ScheduledWorkItem
+{
+public:
+	AnalogDifferentialPressure(int16_t adc_channel = -1);
+	~AnalogDifferentialPressure() override = default;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	int init();
+
+private:
+
+	void Run() override;
+
+	uORB::SubscriptionCallbackWorkItem _adc_report_sub{this, ORB_ID(adc_report)};
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::PublicationMulti<sensor_differential_pressure_s> _diff_pres_pub{ORB_ID(sensor_differential_pressure)};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::SENS_DPRES_ANSC>) _param_sens_dpres_ansc
+	)
+
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
+
+	float _differential_pressure_pa_last{0.f};
+
+	int16_t _adc_channel{-1};
+};

src/drivers/differential_pressure/analog_diffpres/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2013-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
+# 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_module(
+	MODULE drivers__differential_pressure__analog_diffpres
+	MAIN analog_diffpres
+	SRCS
+		AnalogDifferentialPressure.cpp
+		AnalogDifferentialPressure.hpp
+	DEPENDS
+		px4_work_queue
+	)

src/drivers/differential_pressure/analog_diffpres/params.c

@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Differential pressure sensor analog scaling
+ *
+ * Pick the appropriate scaling from the datasheet.
+ * this number defines the (linear) conversion from voltage
+ * to Pascal (pa). For the MPXV7002DP this is 1000.
+ *
+ * NOTE: If the sensor always registers zero, try switching
+ * the static and dynamic tubes.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
+
+
+// SENS_DPRES_ANLG_EN
+// SENS_DPRES_ANLG_CHANNEL?

src/modules/sensors/sensor_params.c

@@ -31,20 +31,6 @@
  *
  ****************************************************************************/
 
-/**
- * Differential pressure sensor analog scaling
- *
- * Pick the appropriate scaling from the datasheet.
- * this number defines the (linear) conversion from voltage
- * to Pascal (pa). For the MPXV7002DP this is 1000.
- *
- * NOTE: If the sensor always registers zero, try switching
- * the static and dynamic tubes.
- *
- * @group Sensor Calibration
- */
-PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
-
 /**
  * Board rotation
  *

src/modules/sensors/sensors.cpp

@@ -41,7 +41,6 @@
  * @author Beat Küng <beat-kueng@gmx.net>
  */
 
-#include <drivers/drv_adc.h>
 #include <drivers/drv_hrt.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
@@ -59,7 +58,6 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_differential_pressure.h>
 #include <uORB/topics/sensors_status_imu.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_imu.h>
@@ -124,11 +122,6 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-#if defined(ADC_AIRSPEED_VOLTAGE_CHANNEL)
-	uORB::Subscription _adc_report_sub {ORB_ID(adc_report)};
-	uORB::PublicationMulti<sensor_differential_pressure_s> _diff_pres_pub{ORB_ID(sensor_differential_pressure)};		/**< differential_pressure */
-#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
-
 	VotedSensorsUpdate _voted_sensors_update;
 
 	VehicleAcceleration	_vehicle_acceleration;
@@ -146,14 +139,6 @@ private:
 	 */
 	void 		parameter_update_poll(bool forced = false);
 
-	/**
-	 * Poll the ADC and update readings to suit.
-	 *
-	 * @param raw			Combined sensor data structure into which
-	 *				data should be returned.
-	 */
-	void		adc_poll();
-
 	void		InitializeAirspeed();
 	void		InitializeVehicleAirData();
 	void		InitializeVehicleGPSPosition();
@@ -161,10 +146,6 @@ private:
 	void		InitializeVehicleMagnetometer();
 
 	DEFINE_PARAMETERS(
-#if defined(ADC_AIRSPEED_VOLTAGE_CHANNEL)
-		(ParamFloat<px4::params::SENS_DPRES_ANSC>) _param_sens_dpres_ansc,
-#endif // ADC_AIRSPEED_VOLTAGE_CHANNEL
-
 		(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro,
 		(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
 		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
@@ -252,49 +233,6 @@ void Sensors::parameter_update_poll(bool forced)
 	}
 }
 
-void Sensors::adc_poll()
-{
-	/* only read if not in HIL mode */
-	if (_hil_enabled) {
-		return;
-	}
-
-#if defined(ADC_AIRSPEED_VOLTAGE_CHANNEL)
-
-	if (_param_sens_dpres_ansc.get() > 0.0f) {
-		adc_report_s adc;
-
-		if (_adc_report_sub.update(&adc)) {
-			/* Read add channels we got */
-			for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) {
-				if (ADC_AIRSPEED_VOLTAGE_CHANNEL == adc.channel_id[i]) {
-					/* calculate airspeed, raw is the difference from */
-					const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * ADC_DP_V_DIV;
-
-					/**
-					 * The voltage divider pulls the signal down, only act on
-					 * a valid voltage from a connected sensor. Also assume a non-
-					 * zero offset from the sensor if its connected.
-					 *
-					 * Notice: This won't work on devices which have PGA controlled
-					 * vref. Those devices require no divider at all.
-					 */
-					if (voltage > 0.4f) {
-						sensor_differential_pressure_s diff_pres{};
-						diff_pres.timestamp_sample = adc.timestamp;
-						diff_pres.differential_pressure_pa = voltage * _param_sens_dpres_ansc.get();
-						diff_pres.temperature = NAN;
-						diff_pres.timestamp = hrt_absolute_time();
-						_diff_pres_pub.publish(diff_pres);
-					}
-				}
-			}
-		}
-	}
-
-#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
-}
-
 void Sensors::InitializeAirspeed()
 {
 	if (_airspeed == nullptr) {
@@ -437,9 +375,6 @@ void Sensors::Run()
 
 	_voted_sensors_update.sensorsPoll(_sensor_combined);
 
-	// check analog airspeed
-	adc_poll();
-
 	if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {
 
 		_voted_sensors_update.setRelativeTimestamps(_sensor_combined);