UAVCAN differential pressure sensor support

* added airspeed handling (differential pressure) to uavcan and uavcannode

Co-authored-by: Jacob Crabill <jacob@flyvoly.com>

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp

@@ -69,7 +69,7 @@ static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 7200, -13};
 
 static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -14};
 
-static constexpr wq_config_t uavcan{"wq:uavcan", 2800, -15};
+static constexpr wq_config_t uavcan{"wq:uavcan", 3000, -15};
 
 static constexpr wq_config_t UART0{"wq:UART0", 1400, -16};
 static constexpr wq_config_t UART1{"wq:UART1", 1400, -17};

src/drivers/uavcan/CMakeLists.txt

@@ -124,11 +124,12 @@ px4_add_module(
 
 		# Sensors
 		sensors/sensor_bridge.cpp
+		sensors/differential_pressure.cpp
+		sensors/baro.cpp
+		sensors/battery.cpp
+		sensors/flow.cpp
 		sensors/gnss.cpp
 		sensors/mag.cpp
-		sensors/baro.cpp
-		sensors/flow.cpp
-		sensors/battery.cpp
 
 	DEPENDS
 		px4_uavcan_dsdlc

src/drivers/uavcan/sensors/differential_pressure.cpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#include "differential_pressure.hpp"
+
+#include <drivers/drv_airspeed.h>
+#include <drivers/drv_hrt.h>
+#include <lib/ecl/geo/geo.h>
+#include <parameters/param.h>
+#include <systemlib/err.h>
+
+const char *const UavcanDifferentialPressureBridge::NAME = "differential_pressure";
+
+UavcanDifferentialPressureBridge::UavcanDifferentialPressureBridge(uavcan::INode &node) :
+	UavcanCDevSensorBridgeBase("uavcan_differential_pressure", "/dev/uavcan/differential_pressure",
+				   AIRSPEED_BASE_DEVICE_PATH,
+				   ORB_ID(differential_pressure)),
+	_sub_air(node)
+{
+}
+
+int UavcanDifferentialPressureBridge::init()
+{
+	int res = device::CDev::init();
+
+	if (res < 0) {
+		return res;
+	}
+
+	// Initialize the calibration offset
+	param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
+
+	res = _sub_air.start(AirCbBinder(this, &UavcanDifferentialPressureBridge::air_sub_cb));
+
+	if (res < 0) {
+		DEVICE_LOG("failed to start uavcan sub: %d", res);
+		return res;
+	}
+
+	return 0;
+}
+
+int UavcanDifferentialPressureBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case AIRSPEEDIOCSSCALE: {
+			struct airspeed_scale *s = (struct airspeed_scale *)arg;
+			_diff_pres_offset = s->offset_pa;
+			return PX4_OK;
+		}
+
+	default: {
+			return CDev::ioctl(filp, cmd, arg);
+		}
+	}
+}
+
+void UavcanDifferentialPressureBridge::air_sub_cb(const
+		uavcan::ReceivedDataStructure<uavcan::equipment::air_data::RawAirData>
+		&msg)
+{
+	_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_UAVCAN;
+	_device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF;
+
+	float diff_press_pa = msg.differential_pressure;
+	float temperature_c = msg.static_air_temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+
+	differential_pressure_s report = {
+		.timestamp = hrt_absolute_time(),
+		.error_count = 0,
+		.differential_pressure_raw_pa = diff_press_pa - _diff_pres_offset,
+		.differential_pressure_filtered_pa = _filter.apply(diff_press_pa) - _diff_pres_offset, /// TODO: Create filter
+		.temperature = temperature_c,
+		.device_id = _device_id.devid
+	};
+
+	publish(msg.getSrcNodeID().get(), &report);
+}

src/drivers/uavcan/sensors/differential_pressure.hpp

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+#include <uORB/uORB.h>
+#include <uORB/topics/differential_pressure.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+
+#include "sensor_bridge.hpp"
+
+#include <uavcan/equipment/air_data/RawAirData.hpp>
+
+class UavcanDifferentialPressureBridge : public UavcanCDevSensorBridgeBase
+{
+public:
+	static const char *const NAME;
+
+	UavcanDifferentialPressureBridge(uavcan::INode &node);
+
+	const char *get_name() const override { return NAME; }
+
+	int init() override;
+
+private:
+	float _diff_pres_offset {0.0f};
+
+	math::LowPassFilter2p _filter{10.f, 1.1f}; /// Adapted from MS5525 driver
+
+	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
+
+	void air_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::RawAirData> &msg);
+
+	typedef uavcan::MethodBinder < UavcanDifferentialPressureBridge *,
+		void (UavcanDifferentialPressureBridge::*)
+		(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::RawAirData> &) >
+		AirCbBinder;
+
+	uavcan::Subscriber<uavcan::equipment::air_data::RawAirData, AirCbBinder> _sub_air;
+};

src/drivers/uavcan/sensors/sensor_bridge.cpp

@@ -38,11 +38,12 @@
 #include "sensor_bridge.hpp"
 #include <cassert>
 
-#include "gnss.hpp"
-#include "mag.hpp"
+#include "differential_pressure.hpp"
 #include "baro.hpp"
-#include "flow.hpp"
 #include "battery.hpp"
+#include "gnss.hpp"
+#include "flow.hpp"
+#include "mag.hpp"
 
 /*
  * IUavcanSensorBridge
@@ -54,6 +55,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 	list.add(new UavcanGnssBridge(node));
 	list.add(new UavcanFlowBridge(node));
 	list.add(new UavcanBatteryBridge(node));
+	list.add(new UavcanDifferentialPressureBridge(node));
 }
 
 /*

src/drivers/uavcannode/UavcanNode.cpp

@@ -79,6 +79,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	_power_battery_info_publisher(_node),
 	_air_data_static_pressure_publisher(_node),
 	_air_data_static_temperature_publisher(_node),
+	_raw_air_data_publisher(_node),
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
 	_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")),
 	_reset_timer(_node)
@@ -297,6 +298,7 @@ void UavcanNode::Run()
 
 		_node.setModeOperational();
 
+		_diff_pressure_sub.registerCallback();
 		_sensor_baro_sub.registerCallback();
 		_sensor_mag_sub.registerCallback();
 		_vehicle_gps_position_sub.registerCallback();
@@ -352,6 +354,25 @@ void UavcanNode::Run()
 		}
 	}
 
+	// differential_pressure -> uavcan::equipment::air_data::RawAirData
+	if (_diff_pressure_sub.updated()) {
+		differential_pressure_s diff_press;
+
+		if (_diff_pressure_sub.copy(&diff_press)) {
+
+			uavcan::equipment::air_data::RawAirData raw_air_data{};
+
+			// raw_air_data.static_pressure =
+			raw_air_data.differential_pressure = diff_press.differential_pressure_raw_pa;
+			// raw_air_data.static_pressure_sensor_temperature =
+			raw_air_data.differential_pressure_sensor_temperature = diff_press.temperature;
+			raw_air_data.static_air_temperature = diff_press.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+			// raw_air_data.pitot_temperature
+			// raw_air_data.covariance
+			_raw_air_data_publisher.broadcast(raw_air_data);
+		}
+	}
+
 	// sensor_baro -> uavcan::equipment::air_data::StaticTemperature
 	if (_sensor_baro_sub.updated()) {
 		sensor_baro_s baro;

src/drivers/uavcannode/UavcanNode.hpp

@@ -58,6 +58,7 @@
 #include <uavcan/protocol/param/ExecuteOpcode.hpp>
 #include <uavcan/protocol/RestartNode.hpp>
 #include <uavcan/equipment/ahrs/MagneticFieldStrength2.hpp>
+#include <uavcan/equipment/air_data/RawAirData.hpp>
 #include <uavcan/equipment/air_data/StaticPressure.hpp>
 #include <uavcan/equipment/air_data/StaticTemperature.hpp>
 #include <uavcan/equipment/gnss/Fix2.hpp>
@@ -71,6 +72,7 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -170,13 +172,13 @@ private:
 	uavcan::Publisher<uavcan::equipment::power::BatteryInfo> _power_battery_info_publisher;
 	uavcan::Publisher<uavcan::equipment::air_data::StaticPressure> _air_data_static_pressure_publisher;
 	uavcan::Publisher<uavcan::equipment::air_data::StaticTemperature> _air_data_static_temperature_publisher;
+	uavcan::Publisher<uavcan::equipment::air_data::RawAirData> _raw_air_data_publisher;
 	hrt_abstime _last_static_temperature_publish{0};
 
-
-
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 
 	uORB::SubscriptionCallbackWorkItem _battery_status_sub{this, ORB_ID(battery_status)};
+	uORB::SubscriptionCallbackWorkItem _diff_pressure_sub{this, ORB_ID(differential_pressure)};
 	uORB::SubscriptionCallbackWorkItem _sensor_baro_sub{this, ORB_ID(sensor_baro)};
 	uORB::SubscriptionCallbackWorkItem _sensor_mag_sub{this, ORB_ID(sensor_mag)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_gps_position_sub{this, ORB_ID(vehicle_gps_position)};