UAVCAN Air Data Support

src/drivers/uavcan/CMakeLists.txt

@@ -127,6 +127,7 @@ px4_add_module(
 		sensors/differential_pressure.cpp
 		sensors/baro.cpp
 		sensors/battery.cpp
+		sensors/airspeed.cpp
 		sensors/flow.cpp
 		sensors/gnss.cpp
 		sensors/mag.cpp

src/drivers/uavcan/sensors/airspeed.cpp

@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   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 RJ Gritter <rjgritter657@gmail.com>
+ */
+
+#include <drivers/drv_hrt.h>
+#include "airspeed.hpp"
+#include <math.h>
+#include <lib/ecl/geo/geo.h> // For CONSTANTS_*
+
+const char *const UavcanAirspeedBridge::NAME = "airspeed";
+
+UavcanAirspeedBridge::UavcanAirspeedBridge(uavcan::INode &node) :
+	UavcanCDevSensorBridgeBase("uavcan_airspeed", "/dev/uavcan/airspeed", AIRSPEED_BASE_DEVICE_PATH, ORB_ID(airspeed)),
+	_sub_ias_data(node),
+	_sub_tas_data(node),
+	_sub_oat_data(node)
+{ }
+
+int UavcanAirspeedBridge::init()
+{
+	int res = device::CDev::init();
+
+	if (res < 0) {
+		return res;
+	}
+
+	res = _sub_ias_data.start(IASCbBinder(this, &UavcanAirspeedBridge::ias_sub_cb));
+
+	if (res < 0) {
+		DEVICE_LOG("failed to start uavcan sub: %d", res);
+		return res;
+	}
+
+	res = _sub_tas_data.start(TASCbBinder(this, &UavcanAirspeedBridge::tas_sub_cb));
+
+	if (res < 0) {
+		DEVICE_LOG("failed to start uavcan sub: %d", res);
+		return res;
+	}
+
+	res = _sub_oat_data.start(OATCbBinder(this, &UavcanAirspeedBridge::oat_sub_cb));
+
+	if (res < 0) {
+		DEVICE_LOG("failed to start uavcan sub: %d", res);
+		return res;
+	}
+
+	return 0;
+}
+
+void
+UavcanAirspeedBridge::oat_sub_cb(const
+				 uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
+{
+	_last_outside_air_temp_k = msg.static_temperature;
+}
+
+void
+UavcanAirspeedBridge::tas_sub_cb(const
+				 uavcan::ReceivedDataStructure<uavcan::equipment::air_data::TrueAirspeed> &msg)
+{
+	_last_tas_m_s = msg.true_airspeed;
+}
+
+void
+UavcanAirspeedBridge::ias_sub_cb(const
+				 uavcan::ReceivedDataStructure<uavcan::equipment::air_data::IndicatedAirspeed> &msg)
+{
+	airspeed_s report{};
+
+	/*
+	 * FIXME HACK
+	 * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT.
+	 * It stopped working when the time sync feature has been introduced, because it caused libuavcan
+	 * to use an independent time source (based on hardware TIM5) instead of HRT.
+	 * The proper solution is to be developed.
+	 */
+	report.timestamp   		= hrt_absolute_time();
+	report.indicated_airspeed_m_s   = msg.indicated_airspeed;
+	report.true_airspeed_m_s   	= _last_tas_m_s;
+	report.air_temperature_celsius 	= _last_outside_air_temp_k + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+
+	publish(msg.getSrcNodeID().get(), &report);
+}

src/drivers/uavcan/sensors/airspeed.hpp

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   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 RJ Gritter <rjgritter657@gmail.com>
+ */
+
+#pragma once
+
+#include "sensor_bridge.hpp"
+#include <drivers/drv_airspeed.h>
+#include <uORB/topics/airspeed.h>
+
+#include <uavcan/equipment/air_data/IndicatedAirspeed.hpp>
+#include <uavcan/equipment/air_data/TrueAirspeed.hpp>
+#include <uavcan/equipment/air_data/StaticTemperature.hpp>
+
+class UavcanAirspeedBridge : public UavcanCDevSensorBridgeBase
+{
+public:
+	static const char *const NAME;
+
+	UavcanAirspeedBridge(uavcan::INode &node);
+
+	const char *get_name() const override { return NAME; }
+
+	int init() override;
+
+private:
+
+	void ias_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::IndicatedAirspeed> &msg);
+	void tas_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::TrueAirspeed> &msg);
+	void oat_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg);
+
+	typedef uavcan::MethodBinder < UavcanAirspeedBridge *,
+		void (UavcanAirspeedBridge::*)
+		(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::IndicatedAirspeed> &) >
+		IASCbBinder;
+
+	typedef uavcan::MethodBinder < UavcanAirspeedBridge *,
+		void (UavcanAirspeedBridge::*)
+		(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::TrueAirspeed> &) >
+		TASCbBinder;
+
+	typedef uavcan::MethodBinder < UavcanAirspeedBridge *,
+		void (UavcanAirspeedBridge::*)
+		(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &) >
+		OATCbBinder;
+
+	uavcan::Subscriber<uavcan::equipment::air_data::IndicatedAirspeed, IASCbBinder> _sub_ias_data;
+	uavcan::Subscriber<uavcan::equipment::air_data::TrueAirspeed, TASCbBinder> _sub_tas_data;
+	uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, OATCbBinder> _sub_oat_data;
+
+	float _last_tas_m_s{0.0f};
+	float _last_outside_air_temp_k{0.0f};
+
+};

src/drivers/uavcan/sensors/sensor_bridge.cpp

@@ -41,6 +41,7 @@
 #include "differential_pressure.hpp"
 #include "baro.hpp"
 #include "battery.hpp"
+#include "airspeed.hpp"
 #include "gnss.hpp"
 #include "flow.hpp"
 #include "mag.hpp"
@@ -55,6 +56,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 UavcanAirspeedBridge(node));
 	list.add(new UavcanDifferentialPressureBridge(node));
 }