Upgrade libcanard to 3.0 and rename uavcan_v1 to cyphal

Did some prep work for redundant interfaces by introducing CanardHandle class to decouple physical interfaces from cyphal.cpp

src/drivers/cyphal/Subscribers/BaseSubscriber.hpp

@@ -0,0 +1,154 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file BaseSubscriber.hpp
+ *
+ * Defines basic functionality of Cyphal subscriber class
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+
+#include <lib/parameters/param.h>
+
+#include "../CanardHandle.hpp"
+#include "../CanardInterface.hpp"
+#include "../ParamManager.hpp"
+
+class UavcanBaseSubscriber
+{
+public:
+	UavcanBaseSubscriber(CanardHandle &handle, const char *prefix_name, const char *subject_name, uint8_t instance = 0) :
+		_canard_handle(handle), _prefix_name(prefix_name), _instance(instance)
+	{
+		_subj_sub._subject_name = subject_name;
+		_subj_sub._canard_sub.user_reference = this;
+		_subj_sub._canard_sub.port_id = CANARD_PORT_ID_UNSET;
+	}
+
+	virtual ~UavcanBaseSubscriber()
+	{
+		unsubscribe();
+	}
+
+	bool isValidPortId(int32_t id) const { return id >= 0 && id <= CANARD_PORT_ID_MAX; }
+
+	virtual void subscribe() = 0;
+	virtual void unsubscribe()
+	{
+		SubjectSubscription *curSubj = &_subj_sub;
+
+		while (curSubj != nullptr) {
+			_canard_handle.RxUnsubscribe(CanardTransferKindMessage, curSubj->_canard_sub.port_id);
+			curSubj = curSubj->next;
+		}
+	};
+
+	virtual void callback(const CanardRxTransfer &msg) = 0;
+
+	CanardPortID id(uint32_t instance = 0)
+	{
+		uint32_t i = 0;
+		SubjectSubscription *curSubj = &_subj_sub;
+
+		while (curSubj != nullptr) {
+			if (instance == i) {
+				return curSubj->_canard_sub.port_id;
+			}
+
+			curSubj = curSubj->next;
+			i++;
+		}
+
+		return CANARD_PORT_ID_UNSET; // Wrong id return unset
+	}
+
+	bool hasPortID(CanardPortID port_id)
+	{
+		if (!isValidPortId((int32_t)port_id)) {
+			return false;
+		}
+
+		SubjectSubscription *curSubj = &_subj_sub;
+
+		while (curSubj != nullptr) {
+			if (port_id == curSubj->_canard_sub.port_id) {
+				return true;
+			}
+
+			curSubj = curSubj->next;
+		}
+
+		return false;
+	}
+
+	const char *getSubjectName()
+	{
+		return _subj_sub._subject_name;
+	}
+
+	uint8_t getInstance()
+	{
+		return _instance;
+	}
+
+	void printInfo()
+	{
+		SubjectSubscription *curSubj = &_subj_sub;
+
+		while (curSubj != nullptr) {
+			if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) {
+				PX4_INFO("Subscribed %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub.port_id);
+			}
+
+			curSubj = curSubj->next;
+		}
+	}
+
+protected:
+	struct SubjectSubscription {
+		CanardRxSubscription _canard_sub;
+		const char *_subject_name;
+		struct SubjectSubscription *next {nullptr};
+	};
+
+	CanardHandle &_canard_handle;
+	const char *_prefix_name;
+	SubjectSubscription _subj_sub;
+	uint8_t _instance {0};
+	/// TODO: 'type' parameter? uavcan.pub.PORT_NAME.type (see 384.Access.1.0.uavcan)
+};

src/drivers/cyphal/Subscribers/DynamicPortSubscriber.hpp

@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file DynamicPortSubscriber.hpp
+ *
+ * Defines basic functionality of Cyphal subscriber class with Non-fixed unregulated port identifier
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+
+#include <lib/parameters/param.h>
+
+#include <uavcan/_register/Access_1_0.h>
+
+#include "../CanardInterface.hpp"
+#include "../ParamManager.hpp"
+#include "BaseSubscriber.hpp"
+
+class UavcanDynamicPortSubscriber : public UavcanBaseSubscriber
+{
+public:
+	UavcanDynamicPortSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, const char *prefix_name,
+				    const char *subject_name,  uint8_t instance = 0) :
+		UavcanBaseSubscriber(handle, prefix_name, subject_name, instance), _param_manager(pmgr) { };
+
+	void updateParam()
+	{
+		SubjectSubscription *curSubj = &_subj_sub;
+
+		while (curSubj != nullptr) {
+			char uavcan_param[90];
+			snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.sub.%s%s.%d.id", _prefix_name, curSubj->_subject_name, _instance);
+
+			// Set _port_id from _uavcan_param
+			uavcan_register_Value_1_0 value;
+
+			if (_param_manager.GetParamByName(uavcan_param, value)) {
+				uint16_t new_id = value.natural16.value.elements[0];
+
+				/* FIXME how about partial subscribing */
+				if (curSubj->_canard_sub.port_id != new_id) {
+					if (new_id == CANARD_PORT_ID_UNSET) {
+						// Cancel subscription
+						unsubscribe();
+
+					} else {
+						if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) {
+							// Already active; unsubscribe first
+							unsubscribe();
+						}
+
+						// Subscribe on the new port ID
+						curSubj->_canard_sub.port_id = (CanardPortID)new_id;
+						PX4_INFO("Subscribing %s%s.%d on port %d", _prefix_name, curSubj->_subject_name, _instance,
+							 curSubj->_canard_sub.port_id);
+						subscribe();
+					}
+				}
+
+			} else if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) { // No valid sub id unsubscribe when neccesary
+				// Already active; unsubscribe first
+				unsubscribe();
+			}
+
+			curSubj = curSubj->next;
+		}
+	};
+
+	UavcanDynamicPortSubscriber *next()
+	{
+		return _next_sub;
+	}
+
+	void setNext(UavcanDynamicPortSubscriber *next)
+	{
+		_next_sub = next;
+	}
+
+protected:
+	UavcanParamManager &_param_manager;
+	UavcanDynamicPortSubscriber *_next_sub {nullptr};
+};

src/drivers/cyphal/Subscribers/Heartbeat.hpp

@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Heartbeat.hpp
+ *
+ * Defines basic functionality of UAVCAN Heartbeat.1.0 subscription
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include "../NodeManager.hpp"
+
+#include <uavcan/node/Heartbeat_1_0.h>
+
+#include "BaseSubscriber.hpp"
+
+class UavcanHeartbeatSubscriber : public UavcanBaseSubscriber
+{
+public:
+	UavcanHeartbeatSubscriber(CanardHandle &handle) :
+		UavcanBaseSubscriber(handle, "", "Heartbeat", 0) { };
+
+	void subscribe() override
+	{
+		// Subscribe to heartbeat messages
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,  // The fixed Subject-ID
+					   uavcan_node_Heartbeat_1_0_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		//TODO heartbeat management
+	};
+
+};

src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp

@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Battery.hpp
+ *
+ * Defines basic functionality of UAVCAN legacy Battery subscription
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include <uORB/topics/battery_status.h>
+#include <uORB/PublicationMulti.hpp>
+
+// Legacy message from UAVCANv0
+#include <legacy/equipment/power/BatteryInfo_1_0.h>
+
+#include "../DynamicPortSubscriber.hpp"
+
+class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber
+{
+public:
+	UavcanLegacyBatteryInfoSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanDynamicPortSubscriber(handle, pmgr, "legacy.", "legacy_bms", instance) { };
+
+	void subscribe() override
+	{
+		// Subscribe to messages reg.drone.service.battery.Status.0.1
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _subj_sub._canard_sub.port_id,
+					   legacy_equipment_power_BatteryInfo_1_0_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 100, //FIXME timeout caused by scheduler
+					   &_subj_sub._canard_sub);
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		PX4_INFO("Legacy BmsCallback");
+
+		legacy_equipment_power_BatteryInfo_1_0 bat_info {};
+		size_t bat_info_size_in_bytes = receive.payload_size;
+		legacy_equipment_power_BatteryInfo_1_0_deserialize_(&bat_info, (const uint8_t *)receive.payload,
+				&bat_info_size_in_bytes);
+
+		battery_status_s bat_status {0};
+		bat_status.timestamp = hrt_absolute_time();
+		bat_status.voltage_filtered_v = bat_info.voltage;
+		bat_status.current_filtered_a = bat_info.current;
+		bat_status.current_average_a = bat_info.average_power_10sec;
+		bat_status.remaining = bat_info.state_of_charge_pct / 100.0f;
+		bat_status.scale = -1;
+
+		if (bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_TEMP_HOT) {
+			bat_status.temperature = 100;
+
+		} else if (bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_TEMP_COLD) {
+			bat_status.temperature = -30;
+
+		} else {
+			bat_status.temperature = 20; // Temp okay ?
+		}
+
+		bat_status.cell_count = 0; // Unknown
+		bat_status.connected = bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_IN_USE;
+		bat_status.source = 1; // External
+		bat_status.capacity = bat_info.full_charge_capacity_wh;
+		bat_status.serial_number = bat_info.model_instance_id & 0xFFFF; // Take first 16 bits
+		bat_status.state_of_health = bat_info.state_of_health_pct; // External
+		bat_status.id = bat_info.battery_id;
+
+		/* Missing fields in UAVCANv0 legacy message
+		 * temperature (partly)
+		 * cell_count
+		 * connected (partly)
+		 * priority
+		 * cycle_count
+		 * time_remaining_s
+		 * average_time_to_empty
+		 * manufacture_date
+		 * max_error
+		 * interface_error
+		 * voltage_cell_v
+		 * max_cell_voltage_delta
+		 * is_powering_off
+		 * warning
+		 */
+
+
+		_battery_status_pub.publish(bat_status);
+		print_message(ORB_ID(battery_status), bat_status);
+	};
+
+private:
+	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
+
+};

src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.cpp

@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file uorb_template.cpp
+ *
+* Defines generic, templatized uORB over UAVCANv1 subscriber
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#include "uorb_subscriber.hpp"
+
+/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */
+
+/* ---- Specializations of convert() to convert incompatbile data, instance no. timestamp ---- */
+
+template<>
+void uORB_over_UAVCAN_Subscriber<sensor_gps_s>::convert(sensor_gps_s *data)
+{
+	/* HOTFIX as long as we don't have UAVCAN timesyncronization we update the timestamp on arrival */
+	data->timestamp = hrt_absolute_time();
+}

src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file uorb_template.hpp
+ *
+* Defines generic, templatized uORB over UAVCANv1 publisher
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include "../DynamicPortSubscriber.hpp"
+
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/actuator_outputs.h>
+
+#include <uORB/topics/sensor_gps.h>
+
+template <class T>
+class uORB_over_UAVCAN_Subscriber : public UavcanDynamicPortSubscriber
+{
+public:
+	uORB_over_UAVCAN_Subscriber(CanardHandle &handle, UavcanParamManager &pmgr, const orb_metadata *meta,
+				    uint8_t instance = 0) :
+		UavcanDynamicPortSubscriber(handle, pmgr, "uorb.", meta->o_name, instance),
+		_uorb_meta{meta},
+		_uorb_pub(meta)
+	{};
+
+	~uORB_over_UAVCAN_Subscriber() override = default;
+
+	void subscribe() override
+	{
+		T *data = NULL;
+
+		// Subscribe to messages uORB sensor_gps payload over UAVCAN
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _subj_sub._canard_sub.port_id,
+					   get_payload_size(data),
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000,
+					   &_subj_sub._canard_sub);
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		T *data = (T *)receive.payload;
+
+		if (receive.payload_size == get_payload_size(data)) {
+
+			/* Data type specific conversion if necceary  */
+			convert(data);
+
+			_uorb_pub.publish(*data);
+
+		} else {
+			PX4_ERR("uORB over UAVCAN %s payload size mismatch got %d expected %d",
+				_subj_sub._subject_name, receive.payload_size, get_payload_size(data));
+		}
+	};
+
+protected:
+	// Default payload-size function -- can specialize in derived class
+	size_t get_payload_size(const T *msg)
+	{
+		(void)msg;
+		return sizeof(T);
+	};
+
+	void convert(T *data) {};
+
+private:
+	const orb_metadata *_uorb_meta;
+	uORB::PublicationMulti<T> _uorb_pub;
+};
+
+/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */
+
+
+/* ---- Specializations of convert() to convert incompatbile data, instance no. timestamp ---- */
+
+template<>
+void uORB_over_UAVCAN_Subscriber<sensor_gps_s>::convert(sensor_gps_s *data);

src/drivers/cyphal/Subscribers/udral/Battery.hpp

@@ -0,0 +1,186 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Battery.hpp
+ *
+ * Defines basic functionality of Cyphal Battery subscription
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+#include <uORB/topics/battery_status.h>
+#include <uORB/PublicationMulti.hpp>
+
+// UDRAL Specification Messages
+#include <reg/udral/physics/electricity/SourceTs_0_1.h>
+#include <reg/udral/service/battery/Parameters_0_3.h>
+#include <reg/udral/service/battery/Status_0_2.h>
+
+#include "../DynamicPortSubscriber.hpp"
+
+#define KELVIN_OFFSET 273.15f
+#define WH_TO_JOULE   3600
+
+class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber
+{
+public:
+	UavcanBmsSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanDynamicPortSubscriber(handle, pmgr, "udral.", "energy_source", instance)
+	{
+		_subj_sub.next = &_status_sub;
+
+		_status_sub._subject_name = _status_name;
+		_status_sub._canard_sub.user_reference = this;
+		_status_sub.next = &_parameters_sub;
+
+		_parameters_sub._subject_name = _parameters_name;
+		_parameters_sub._canard_sub.user_reference = this;
+		_parameters_sub.next = nullptr;
+	}
+
+	void subscribe() override
+	{
+		// Subscribe to messages reg.drone.physics.electricity.SourceTs.0.1
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _subj_sub._canard_sub.port_id,
+					   reg_udral_physics_electricity_SourceTs_0_1_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+
+		// Subscribe to messages reg.drone.service.battery.Status.0.2
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _status_sub._canard_sub.port_id,
+					   reg_udral_service_battery_Status_0_2_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_status_sub._canard_sub);
+
+		// Subscribe to messages reg.drone.service.battery.Parameters.0.3
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _parameters_sub._canard_sub.port_id,
+					   reg_udral_service_battery_Parameters_0_3_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_parameters_sub._canard_sub);
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		if (receive.metadata.port_id == _subj_sub._canard_sub.port_id) {
+			reg_udral_physics_electricity_SourceTs_0_1 source_ts {};
+			size_t source_ts_size_in_bytes = receive.payload_size;
+			reg_udral_physics_electricity_SourceTs_0_1_deserialize_(&source_ts,
+					(const uint8_t *)receive.payload,
+					&source_ts_size_in_bytes);
+			bat_status.timestamp = hrt_absolute_time(); //FIXME timesyncronization source_ts.timestamp.microsecond;
+			bat_status.voltage_v = source_ts.value.power.voltage.volt;
+			bat_status.current_a = source_ts.value.power.current.ampere;
+
+			bat_status.connected = true; // Based on some threshold or an error??
+
+			// Estimate discharged mah from Joule
+			if (_nominal_voltage != NAN) {
+				bat_status.discharged_mah = (source_ts.value.full_energy.joule - source_ts.value.energy.joule)
+							    / (_nominal_voltage * WH_TO_JOULE);
+			}
+
+			bat_status.remaining = source_ts.value.energy.joule / source_ts.value.full_energy.joule;
+
+			// TODO uORB publication rate limiting
+			_battery_status_pub.publish(bat_status);
+
+		} else if (receive.metadata.port_id == _status_sub._canard_sub.port_id) {
+			reg_udral_service_battery_Status_0_2 bat {};
+			size_t bat_size_in_bytes = receive.payload_size;
+			reg_udral_service_battery_Status_0_2_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bytes);
+
+			bat_status.scale = -1; // What does the mean?
+
+			bat_status.temperature = bat.temperature_min_max[1].kelvin - KELVIN_OFFSET; // PX4 uses degC we assume
+
+			bat_status.cell_count = bat.cell_voltages.count;
+
+			uint32_t cell_count = bat_status.cell_count;
+
+			if (cell_count > (sizeof(bat_status.voltage_cell_v) / sizeof(bat_status.voltage_cell_v[0]))) {
+				cell_count = sizeof(bat_status.voltage_cell_v) / sizeof(bat_status.voltage_cell_v[0]);
+			}
+
+			float voltage_cell_min = FLT_MAX_EXP;
+			float voltage_cell_max = FLT_MIN_EXP;
+
+			for (uint32_t i = 0; i < cell_count; i++) {
+				bat_status.voltage_cell_v[i] = bat.cell_voltages.elements[i];
+
+				if (bat_status.voltage_cell_v[i] > voltage_cell_max) {
+					voltage_cell_max = bat_status.voltage_cell_v[i];
+				}
+
+				if (bat_status.voltage_cell_v[i] < voltage_cell_min) {
+					voltage_cell_min = bat_status.voltage_cell_v[i];
+				}
+			}
+
+			bat_status.max_cell_voltage_delta = voltage_cell_max - voltage_cell_min; // Current delta or max delta over time?
+
+		} else if (receive.metadata.port_id == _parameters_sub._canard_sub.port_id) {
+			reg_udral_service_battery_Parameters_0_3 parameters {};
+			size_t parameters_size_in_bytes = receive.payload_size;
+			reg_udral_service_battery_Parameters_0_3_deserialize_(&parameters,
+					(const uint8_t *)receive.payload,
+					&parameters_size_in_bytes);
+
+			bat_status.capacity = parameters.design_capacity.coulomb / 3.6f; // Coulomb -> mAh
+			bat_status.cycle_count = parameters.cycle_count;
+			bat_status.serial_number = parameters.unique_id & 0xFFFF;
+			bat_status.state_of_health = parameters.state_of_health_pct;
+			bat_status.max_error = 1; // UAVCAN didn't spec'ed this, but we're optimistic
+			bat_status.id = 0; //TODO instancing
+			_nominal_voltage = parameters.nominal_voltage.volt;
+		}
+	}
+
+private:
+	float _nominal_voltage = NAN;
+
+	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
+
+	SubjectSubscription _status_sub;
+	SubjectSubscription _parameters_sub;
+
+	const char *_status_name = "battery_status";
+	const char *_parameters_name = "battery_parameters";
+
+	battery_status_s bat_status {0};
+};

src/drivers/cyphal/Subscribers/udral/Esc.hpp

@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Esc.hpp
+ *
+ * Defines basic functionality of Cyphal ESC setpoint subscription
+ * (For use on a CAN-->PWM node)
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+/// For use with PR-16808 once merged
+// #include <uORB/topics/output_control.h>
+
+// UDRAL Specification Messages
+#include <reg/udral/service/actuator/common/sp/Vector8_0_1.h>
+#include <reg/udral/service/common/Readiness_0_1.h>
+
+#include "../DynamicPortSubscriber.hpp"
+
+class UavcanEscSubscriber : public UavcanDynamicPortSubscriber
+{
+public:
+	UavcanEscSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanDynamicPortSubscriber(handle, pmgr, "udral.", "esc", instance) { };
+
+	void subscribe() override
+	{
+		// Subscribe to messages reg.drone.service.actuator.common.sp.Vector8.0.1
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _subj_sub._canard_sub.port_id,
+					   reg_udral_service_actuator_common_sp_Vector8_0_1_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+
+		// Subscribe to messages reg.drone.service.common.Readiness.0.1
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   static_cast<CanardPortID>(static_cast<uint32_t>(_subj_sub._canard_sub.port_id) + 1),
+					   reg_udral_service_common_Readiness_0_1_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_canard_sub_readiness);
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		// Test with Yakut:
+		// export YAKUT_TRANSPORT="pyuavcan.transport.can.CANTransport(pyuavcan.transport.can.media.slcan.SLCANMedia('/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_23002B000E514E413431302000000000-if00', 8, 115200), 42)"
+		// yakut pub 22.reg.drone.service.actuator.common.sp.Vector8.0.1 '{value: [1000, 2000, 3000, 4000, 0, 0, 0, 0]}'
+		PX4_INFO("EscCallback");
+
+		reg_udral_service_actuator_common_sp_Vector8_0_1 esc {};
+		size_t esc_size_in_bits = receive.payload_size;
+		reg_udral_service_actuator_common_sp_Vector8_0_1_deserialize_(&esc, (const uint8_t *)receive.payload,
+				&esc_size_in_bits);
+
+		double val1 = static_cast<double>(esc.value[0]);
+		double val2 = static_cast<double>(esc.value[1]);
+		double val3 = static_cast<double>(esc.value[2]);
+		double val4 = static_cast<double>(esc.value[3]);
+		PX4_INFO("values[0-3] = {%f, %f, %f, %f}", val1, val2, val3, val4);
+		/// do something with the data
+
+		/// For use with PR-16808 once merged
+		// output_control_s outputs;
+
+		// for (uint8_t i = 0; i < 8; i++) {
+		// 	outputs.value[i] = 2.f * (esc.value[i] / 8191.f) - 1.f;
+		// }
+
+		// _output_pub.publish(outputs);
+	};
+
+private:
+	/// For use with PR-16808 once merged
+	// uORB::Publication<output_control_s> _output_pub{ORB_ID(output_control_mc)};
+
+	CanardRxSubscription _canard_sub_readiness;
+};

src/drivers/cyphal/Subscribers/udral/Gnss.hpp

@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Gnss.hpp
+ *
+ * Defines basic functionality of Cyphal GNSS subscription
+ *
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+// UDRAL Specification Messages
+#include <reg/udral/physics/kinematics/geodetic/Point_0_1.h>
+
+#include "../DynamicPortSubscriber.hpp"
+
+class UavcanGnssSubscriber : public UavcanDynamicPortSubscriber
+{
+public:
+	UavcanGnssSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanDynamicPortSubscriber(handle, pmgr, "udral.", "gps", instance) { };
+
+	void subscribe() override
+	{
+		// Subscribe to messages reg.drone.physics.kinematics.geodetic.Point.0.1
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _subj_sub._canard_sub.port_id,
+					   reg_udral_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+
+		/** TODO: Add additional GPS-data messages: (reg.drone.service.gnss._.0.1.uavcan):
+		 * # A compliant implementation of this service should publish the following subjects:
+		 * #
+		 * #   PUBLISHED SUBJECT NAME      SUBJECT TYPE                                            TYP. RATE [Hz]
+		 * #   point_kinematics            reg.drone.physics.kinematics.geodetic.PointStateVarTs   1...100
+		 * #   time                        reg.drone.service.gnss.Time                             1...10
+		 * #   heartbeat                   reg.drone.service.gnss.Heartbeat                        ~1
+		 * #   sensor_status               reg.drone.service.sensor.Status                         ~1
+		 *
+		 * Not mentioned, but should also be included: Dilution of Precision
+		 *   (reg.drone.service.gnss.DilutionOfPrecision.0.1.uavcan)
+		 * For PX4, only the PointStateVarTs, DilutionOfPrecision, and perhaps Time would be needed
+		 * to publish 'sensor_gps'
+		 */
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		// Test with Yakut:
+		// export YAKUT_TRANSPORT="pyuavcan.transport.can.CANTransport(pyuavcan.transport.can.media.slcan.SLCANMedia('/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_23002B000E514E413431302000000000-if00', 8, 115200), 42)"
+		// yakut pub 1500.reg.drone.physics.kinematics.geodetic.Point.0.1 '{latitude: 1.234, longitude: 2.34, altitude: {meter: 0.5}}'
+		PX4_INFO("GpsCallback");
+
+		reg_udral_physics_kinematics_geodetic_Point_0_1 geo {};
+		size_t geo_size_in_bits = receive.payload_size;
+		reg_udral_physics_kinematics_geodetic_Point_0_1_deserialize_(&geo, (const uint8_t *)receive.payload, &geo_size_in_bits);
+
+		double lat = geo.latitude;
+		double lon = geo.longitude;
+		double alt = geo.altitude.meter;
+		PX4_INFO("Latitude: %f, Longitude: %f, Altitude: %f", lat, lon, alt);
+		/// do something with the data
+	};
+
+};