UAVCANv1 Node implementation work, PNP, Registers and uORB publisher

boards/nxp/fmuk66-e/socketcan.px4board

@@ -1,2 +1,4 @@
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_DRIVERS_UAVCAN_V1=y
+CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y
+CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y

boards/nxp/ucans32k146/default.px4board

@@ -7,7 +7,10 @@ CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
 CONFIG_DRIVERS_PWM_OUT=y
-CONFIG_DRIVERS_UAVCANNODE_GPS_DEMO=y
+CONFIG_DRIVERS_UAVCAN_V1=y
+CONFIG_UAVCAN_V1_CLIENT=y
+CONFIG_UAVCAN_V1_APP_DESCRIPTOR=y
+CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y

boards/nxp/ucans32k146/init/rc.board_defaults

@@ -4,3 +4,6 @@
 #------------------------------------------------------------------------------
 
 pwm_out mode_pwm1 start
+
+ifup can0
+uavcan_v1 start

src/drivers/uavcan_v1/Actuators/EscClient.hpp

@@ -68,7 +68,7 @@ public:
 	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
 
 	UavcanEscController(CanardInstance &ins, UavcanParamManager &pmgr) :
-		UavcanPublisher(ins, pmgr, "esc") { };
+		UavcanPublisher(ins, pmgr, "ds_015", "esc") { };
 
 	~UavcanEscController() {};
 

src/drivers/uavcan_v1/CMakeLists.txt

@@ -122,6 +122,7 @@ px4_add_module(
 		Uavcan.cpp
 		Uavcan.hpp
 		Publishers/uORB/uorb_publisher.cpp
+		Subscribers/uORB/uorb_subscriber.cpp
 		${SRCS}
 		o1heap/o1heap.c
 		o1heap/o1heap.h

src/drivers/uavcan_v1/NodeClient.cpp

@@ -46,7 +46,7 @@
 
 void NodeClient::callback(const CanardTransfer &receive)
 {
-	if (receive.remote_node_id != CANARD_NODE_ID_UNSET && _canard_instance.node_id != CANARD_NODE_ID_UNSET) {
+	if (receive.remote_node_id != CANARD_NODE_ID_UNSET && _canard_instance.node_id == CANARD_NODE_ID_UNSET) {
 
 		int32_t allocated = CANARD_NODE_ID_UNSET;
 		px4_guid_t px4_guid;
@@ -88,6 +88,9 @@ void NodeClient::callback(const CanardTransfer &receive)
 		}
 
 		_canard_instance.node_id = allocated;
+
+		PX4_INFO("Allocated Node ID %d", _canard_instance.node_id);
+
 	}
 }
 

src/drivers/uavcan_v1/NodeManager.hpp

@@ -70,7 +70,7 @@ typedef struct {
 class NodeManager : public UavcanBaseSubscriber, public UavcanServiceRequestInterface
 {
 public:
-	NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : UavcanBaseSubscriber(ins, "NodeIDAllocationData", 0),
+	NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : UavcanBaseSubscriber(ins, "", "NodeIDAllocationData", 0),
 		_canard_instance(ins), _access_request(ins, pmgr), _list_request(ins) { };
 
 	void subscribe() override

src/drivers/uavcan_v1/ParamManager.cpp

@@ -76,6 +76,19 @@ bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, ua
 	return false;
 }
 
+bool UavcanParamManager::GetParamName(uint32_t id, uavcan_register_Name_1_0 &name)
+{
+	if (id >= sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) {
+		return false;
+	}
+
+	strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_);
+
+	name.name.count = strlen(_uavcan_params[id].uavcan_name);
+
+	return true;
+}
+
 bool UavcanParamManager::SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value)
 {
 	for (auto &param : _uavcan_params) {

src/drivers/uavcan_v1/ParamManager.hpp

@@ -110,12 +110,13 @@ public:
 
 	bool GetParamByName(const char *param_name, uavcan_register_Value_1_0 &value);
 	bool GetParamByName(const uavcan_register_Name_1_0 &name, uavcan_register_Value_1_0 &value);
+	bool GetParamName(uint32_t id, uavcan_register_Name_1_0 &name);
 	bool SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value);
 
 private:
 
 
-	const UavcanParamBinder _uavcan_params[12] {
+	const UavcanParamBinder _uavcan_params[13] {
 		{"uavcan.pub.esc.0.id",                "UCAN1_ESC_PUB",		    px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.servo.0.id",              "UCAN1_SERVO_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.gps.0.id",                "UCAN1_GPS_PUB",		    px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
@@ -128,6 +129,7 @@ private:
 		{"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.sub.legacy_bms.0.id",         "UCAN1_LG_BMS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.sub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS_P",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		//{"uavcan.sub.bms.0.id",   "UCAN1_BMS0_SUB"}, //FIXME instancing
 		//{"uavcan.sub.bms.1.id",   "UCAN1_BMS1_SUB"},
 	};

src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp

@@ -51,7 +51,7 @@ class UavcanGnssPublisher : public UavcanPublisher
 {
 public:
 	UavcanGnssPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
-		UavcanPublisher(ins, pmgr, "gps", instance)
+		UavcanPublisher(ins, pmgr, "ds_015", "gps", instance)
 	{
 
 	};

src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp

@@ -51,7 +51,7 @@ class UavcanReadinessPublisher : public UavcanPublisher
 {
 public:
 	UavcanReadinessPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
-		UavcanPublisher(ins, pmgr, "readiness", instance)
+		UavcanPublisher(ins, pmgr, "ds_015", "readiness", instance)
 	{
 
 	};

src/drivers/uavcan_v1/Publishers/Publisher.hpp

@@ -60,8 +60,10 @@
 class UavcanPublisher : public ListNode<UavcanPublisher *>
 {
 public:
-	UavcanPublisher(CanardInstance &ins, UavcanParamManager &pmgr, const char *subject_name, uint8_t instance = 0) :
+	UavcanPublisher(CanardInstance &ins, UavcanParamManager &pmgr, const char *prefix_name, const char *subject_name,
-		_canard_instance(ins), _param_manager(pmgr), _subject_name(subject_name), _instance(instance) { };
+			uint8_t instance = 0) :
+		_canard_instance(ins), _param_manager(pmgr), _prefix_name(prefix_name), _subject_name(subject_name),
+		_instance(instance) { };
 
 	virtual ~UavcanPublisher() = default;
 
@@ -73,7 +75,7 @@ public:
 	void updateParam()
 	{
 		char uavcan_param[256];
-		snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.pub.%s.%d.id", _subject_name, _instance);
+		snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.pub.%s%s.%d.id", _prefix_name, _subject_name, _instance);
 
 		// Set _port_id from _uavcan_param
 		uavcan_register_Value_1_0 value;
@@ -83,12 +85,12 @@ public:
 
 			if (_port_id != new_id) {
 				if (new_id == CANARD_PORT_ID_UNSET) {
-					PX4_INFO("Disabling publication of subject %s.%d", _subject_name, _instance);
+					PX4_INFO("Disabling publication of subject %s%s.%d", _prefix_name, _subject_name, _instance);
 					_port_id = CANARD_PORT_ID_UNSET;
 
 				} else {
 					_port_id = (CanardPortID)new_id;
-					PX4_INFO("Enabling subject %s.%d on port %d", _subject_name, _instance, _port_id);
+					PX4_INFO("Enabling subject %s%s.%d on port %d", _prefix_name, _subject_name, _instance, _port_id);
 				}
 			}
 		}
@@ -97,10 +99,10 @@ public:
 	void printInfo()
 	{
 		if (_port_id != CANARD_PORT_ID_UNSET) {
-			PX4_INFO("Enabled subject %s.%d on port %d", _subject_name, _instance, _port_id);
+			PX4_INFO("Enabled subject %s%s.%d on port %d", _prefix_name, _subject_name, _instance, _port_id);
 
 		} else {
-			PX4_INFO("Subject %s.%d disabled", _subject_name, _instance);
+			PX4_INFO("Subject %s%s.%d disabled", _prefix_name, _subject_name, _instance);
 		}
 	}
 
@@ -109,6 +111,11 @@ public:
 		return _subject_name;
 	}
 
+	const char *getPrefixName()
+	{
+		return _prefix_name;
+	}
+
 	uint8_t getInstance()
 	{
 		return _instance;
@@ -127,6 +134,7 @@ public:
 protected:
 	CanardInstance &_canard_instance;
 	UavcanParamManager &_param_manager;
+	const char *_prefix_name;
 	const char *_subject_name;
 	uint8_t _instance {0};
 

src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp

@@ -52,7 +52,7 @@ class uORB_over_UAVCAN_Publisher : public UavcanPublisher
 public:
 	uORB_over_UAVCAN_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, const orb_metadata *meta,
 				   uint8_t instance = 0) :
-		UavcanPublisher(ins, pmgr, meta->o_name, instance),
+		UavcanPublisher(ins, pmgr, "uorb.", meta->o_name, instance),
 		_uorb_meta{meta},
 		_uorb_sub(meta)
 	{};

src/drivers/uavcan_v1/ServiceClients/Access.hpp

@@ -56,7 +56,7 @@ class UavcanAccessResponse : public UavcanBaseSubscriber
 {
 public:
 	UavcanAccessResponse(CanardInstance &ins, UavcanParamManager &pmgr) :
-		UavcanBaseSubscriber(ins, "Access", 0),  _param_manager(pmgr) { };
+		UavcanBaseSubscriber(ins, "", "Access", 0),  _param_manager(pmgr) { };
 
 	void subscribe() override
 	{

src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp

@@ -55,7 +55,7 @@ class UavcanGetInfoResponse : public UavcanBaseSubscriber
 {
 public:
 	UavcanGetInfoResponse(CanardInstance &ins) :
-		UavcanBaseSubscriber(ins, "GetInfo", 0) { };
+		UavcanBaseSubscriber(ins, "", "GetInfo", 0) { };
 
 	void subscribe() override
 	{

src/drivers/uavcan_v1/ServiceClients/List.hpp

@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   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 List.hpp
+ *
+ * Defines response to a List request
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/module.h>
+#include <version/version.h>
+
+#include "../ParamManager.hpp"
+
+#include <uavcan/_register/List_1_0.h>
+
+#include "../Subscribers/BaseSubscriber.hpp"
+
+class UavcanListResponse : public UavcanBaseSubscriber
+{
+public:
+	UavcanListResponse(CanardInstance &ins, UavcanParamManager &pmgr) :
+		UavcanBaseSubscriber(ins, "", "List", 0),  _param_manager(pmgr) { };
+
+	void subscribe() override
+	{
+		// Subscribe to requests uavcan.pnp.NodeIDAllocationData
+		canardRxSubscribe(&_canard_instance,
+				  CanardTransferKindRequest,
+				  uavcan_register_List_1_0_FIXED_PORT_ID_,
+				  uavcan_register_List_Response_1_0_EXTENT_BYTES_,
+				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+				  &_subj_sub._canard_sub);
+
+	};
+
+	void callback(const CanardTransfer &receive) override
+	{
+		PX4_INFO("List request");
+
+		uavcan_register_List_Request_1_0 msg;
+		uavcan_register_List_Response_1_0 response;
+
+		uavcan_register_List_Request_1_0_initialize_(&msg);
+		uavcan_register_List_Response_1_0_initialize_(&response);
+
+		size_t register_in_size_bits = receive.payload_size;
+		uavcan_register_List_Request_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &register_in_size_bits);
+
+		int result {0};
+
+		if (_param_manager.GetParamName(msg.index, response.name) == 0) {
+			response.name.name.count = 0;
+		}
+
+		uint8_t response_payload_buffer[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+		CanardTransfer transfer = {
+			.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+			.priority       = CanardPriorityNominal,
+			.transfer_kind  = CanardTransferKindResponse,
+			.port_id        = uavcan_register_List_1_0_FIXED_PORT_ID_,                // This is the subject-ID.
+			.remote_node_id = receive.remote_node_id,       // Messages cannot be unicast, so use UNSET.
+			.transfer_id    = receive.transfer_id,
+			.payload_size   = uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
+			.payload        = &response_payload_buffer,
+		};
+
+		result = uavcan_register_List_Response_1_0_serialize_(&response, response_payload_buffer, &transfer.payload_size);
+
+		if (result == 0) {
+			// set the data ready in the buffer and chop if needed
+			result = canardTxPush(&_canard_instance, &transfer);
+		}
+
+	};
+
+private:
+	UavcanParamManager &_param_manager;
+
+};

src/drivers/uavcan_v1/Services/AccessRequest.hpp

@@ -55,7 +55,7 @@ class UavcanAccessServiceRequest : public UavcanServiceRequest
 {
 public:
 	UavcanAccessServiceRequest(CanardInstance &ins, UavcanParamManager &pmgr) :
-		UavcanServiceRequest(ins, "Access", uavcan_register_Access_1_0_FIXED_PORT_ID_,
+		UavcanServiceRequest(ins, "", "Access", uavcan_register_Access_1_0_FIXED_PORT_ID_,
 				     uavcan_register_Access_Response_1_0_EXTENT_BYTES_),  _param_manager(pmgr)  { };
 
 	bool setPortId(CanardNodeID node_id, uavcan_register_Name_1_0 &name, UavcanServiceRequestInterface *handler)

src/drivers/uavcan_v1/Services/ListRequest.hpp

@@ -55,7 +55,7 @@ class UavcanListServiceRequest : public UavcanServiceRequest
 {
 public:
 	UavcanListServiceRequest(CanardInstance &ins) :
-		UavcanServiceRequest(ins, "List", uavcan_register_List_1_0_FIXED_PORT_ID_,
+		UavcanServiceRequest(ins, "", "List", uavcan_register_List_1_0_FIXED_PORT_ID_,
 				     uavcan_register_List_Response_1_0_EXTENT_BYTES_) { };
 
 

src/drivers/uavcan_v1/Services/ServiceRequest.hpp

@@ -59,8 +59,9 @@ public:
 class UavcanServiceRequest : public UavcanBaseSubscriber
 {
 public:
-	UavcanServiceRequest(CanardInstance &ins, const char *subject_name, CanardPortID portID, size_t extent) :
+	UavcanServiceRequest(CanardInstance &ins, const char *prefix_name, const char *subject_name, CanardPortID portID,
-		UavcanBaseSubscriber(ins, subject_name, 0), _portID(portID), _extent(extent) { };
+			     size_t extent) :
+		UavcanBaseSubscriber(ins, prefix_name, subject_name, 0), _portID(portID), _extent(extent) { };
 
 
 	void subscribe() override

src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp

@@ -51,8 +51,8 @@
 class UavcanBaseSubscriber
 {
 public:
-	UavcanBaseSubscriber(CanardInstance &ins, const char *subject_name, uint8_t instance = 0) :
+	UavcanBaseSubscriber(CanardInstance &ins, const char *prefix_name, const char *subject_name, uint8_t instance = 0) :
-		_canard_instance(ins), _instance(instance)
+		_canard_instance(ins), _prefix_name(prefix_name), _instance(instance)
 	{
 		_subj_sub._subject_name = subject_name;
 		_subj_sub._canard_sub.user_reference = this;
@@ -146,6 +146,7 @@ protected:
 	};
 
 	CanardInstance &_canard_instance;
+	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/uavcan_v1/Subscribers/DS-015/Battery.hpp

@@ -58,7 +58,7 @@ class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber
 {
 public:
 	UavcanBmsSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
-		UavcanDynamicPortSubscriber(ins, pmgr, "energy_source", instance)
+		UavcanDynamicPortSubscriber(ins, pmgr, "ds_015.", "energy_source", instance)
 	{
 		_subj_sub.next = &_status_sub;
 

src/drivers/uavcan_v1/Subscribers/DS-015/Esc.hpp

@@ -55,7 +55,7 @@ class UavcanEscSubscriber : public UavcanDynamicPortSubscriber
 {
 public:
 	UavcanEscSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
-		UavcanDynamicPortSubscriber(ins, pmgr, "esc", instance) { };
+		UavcanDynamicPortSubscriber(ins, pmgr, "ds_015.", "esc", instance) { };
 
 	void subscribe() override
 	{

src/drivers/uavcan_v1/Subscribers/DS-015/Gnss.hpp

@@ -50,7 +50,7 @@ class UavcanGnssSubscriber : public UavcanDynamicPortSubscriber
 {
 public:
 	UavcanGnssSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
-		UavcanDynamicPortSubscriber(ins, pmgr, "gps", instance) { };
+		UavcanDynamicPortSubscriber(ins, pmgr, "ds_015.", "gps", instance) { };
 
 	void subscribe() override
 	{

src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp

@@ -54,9 +54,9 @@
 class UavcanDynamicPortSubscriber : public UavcanBaseSubscriber
 {
 public:
-	UavcanDynamicPortSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, const char *subject_name,
+	UavcanDynamicPortSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, const char *prefix_name,
-				    uint8_t instance = 0) :
+				    const char *subject_name,  uint8_t instance = 0) :
-		UavcanBaseSubscriber(ins, subject_name, instance), _param_manager(pmgr) { };
+		UavcanBaseSubscriber(ins, prefix_name, subject_name, instance), _param_manager(pmgr) { };
 
 	void updateParam()
 	{
@@ -64,7 +64,7 @@ public:
 
 		while (curSubj != nullptr) {
 			char uavcan_param[90];
-			snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.sub.%s.%d.id", curSubj->_subject_name, _instance);
+			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;
@@ -86,7 +86,8 @@ public:
 
 						// Subscribe on the new port ID
 						curSubj->_canard_sub.port_id = (CanardPortID)new_id;
-						PX4_INFO("Subscribing %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub.port_id);
+						PX4_INFO("Subscribing %s%s.%d on port %d", _prefix_name, curSubj->_subject_name, _instance,
+							 curSubj->_canard_sub.port_id);
 						subscribe();
 					}
 				}

src/drivers/uavcan_v1/Subscribers/Heartbeat.hpp

@@ -51,7 +51,7 @@ class UavcanHeartbeatSubscriber : public UavcanBaseSubscriber
 {
 public:
 	UavcanHeartbeatSubscriber(CanardInstance &ins) :
-		UavcanBaseSubscriber(ins, "Heartbeat", 0) { };
+		UavcanBaseSubscriber(ins, "", "Heartbeat", 0) { };
 
 	void subscribe() override
 	{

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

@@ -53,7 +53,7 @@ class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber
 {
 public:
 	UavcanLegacyBatteryInfoSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
-		UavcanDynamicPortSubscriber(ins, pmgr, "legacy_bms", instance) { };
+		UavcanDynamicPortSubscriber(ins, pmgr, "legacy.", "legacy_bms", instance) { };
 
 	void subscribe() override
 	{

src/drivers/uavcan_v1/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/uavcan_v1/Subscribers/uORB/uorb_subscriber.hpp

@@ -32,56 +32,84 @@
  ****************************************************************************/
 
 /**
- * @file sensor_gps.hpp
+ * @file uorb_template.hpp
  *
- * Defines uORB over UAVCANv1 sensor_gps subscriber
+* Defines generic, templatized uORB over UAVCANv1 publisher
  *
  * @author Peter van der Perk <peter.vanderperk@nxp.com>
  */
 
 #pragma once
 
-#include <uORB/topics/sensor_gps.h>
-#include <uORB/PublicationMulti.hpp>
-
 #include "../DynamicPortSubscriber.hpp"
 
-class UORB_over_UAVCAN_sensor_gps_Subscriber : public UavcanDynamicPortSubscriber
+#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_sensor_gps_Subscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
+	uORB_over_UAVCAN_Subscriber(CanardInstance &ins, UavcanParamManager &pmgr, const orb_metadata *meta,
-		UavcanDynamicPortSubscriber(ins, pmgr, "uorb.sensor_gps", instance) { };
+				    uint8_t instance = 0) :
+		UavcanDynamicPortSubscriber(ins, 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
 		canardRxSubscribe(&_canard_instance,
 				  CanardTransferKindMessage,
 				  _subj_sub._canard_sub.port_id,
-				  sizeof(struct sensor_gps_s),
+				  get_payload_size(data),
 				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000,
 				  &_subj_sub._canard_sub);
 	};
 
 	void callback(const CanardTransfer &receive) override
 	{
-		//PX4_INFO("uORB sensor_gps Callback");
+		T *data = (T *)receive.payload;
 
-		if (receive.payload_size == sizeof(struct sensor_gps_s)) {
+		if (receive.payload_size == get_payload_size(data)) {
-			sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload;
-			gps_msg->timestamp = hrt_absolute_time();
 
-			/* As long as we don't have timesync between nodes we set the timestamp to the current time */
+			/* Data type specific conversion if necceary  */
+			convert(data);
 
-			_sensor_gps_pub.publish(*gps_msg);
+			_uorb_pub.publish(*data);
 
 		} else {
-			PX4_ERR("uORB over UAVCAN %s playload size mismatch got %d expected %d",
+			PX4_ERR("uORB over UAVCAN %s payload size mismatch got %d expected %d",
-				_subj_sub._subject_name, receive.payload_size, sizeof(struct sensor_gps_s));
+				_subj_sub._subject_name, receive.payload_size, get_payload_size(data));
 		}
 	};
 
-private:
+protected:
-	uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
+	// 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/uavcan_v1/SubscriptionManager.cpp

@@ -64,6 +64,7 @@ void SubscriptionManager::subscribe()
 #endif
 
 	_access_rsp.subscribe();
+	_list_rsp.subscribe();
 
 	updateDynamicSubscriptions();
 }

src/drivers/uavcan_v1/SubscriptionManager.hpp

@@ -76,13 +76,14 @@
 
 #include "ServiceClients/GetInfo.hpp"
 #include "ServiceClients/Access.hpp"
+#include "ServiceClients/List.hpp"
 #include "Subscribers/BaseSubscriber.hpp"
 #include "Subscribers/Heartbeat.hpp"
 #include "Subscribers/DS-015/Battery.hpp"
 #include "Subscribers/DS-015/Esc.hpp"
 #include "Subscribers/DS-015/Gnss.hpp"
 #include "Subscribers/legacy/LegacyBatteryInfo.hpp"
-#include "Subscribers/uORB/sensor_gps.hpp"
+#include "Subscribers/uORB/uorb_subscriber.hpp"
 
 typedef struct {
 	UavcanDynamicPortSubscriber *(*create_sub)(CanardInstance &ins, UavcanParamManager &pmgr) {};
@@ -116,6 +117,7 @@ private:
 
 	// Process register requests
 	UavcanAccessResponse  _access_rsp {_canard_instance, _param_manager};
+	UavcanListResponse  _list_rsp {_canard_instance, _param_manager};
 
 	const UavcanDynSubBinder _uavcan_subs[UAVCAN_SUB_COUNT] {
 #if CONFIG_UAVCAN_V1_ESC_SUBSCRIBER
@@ -172,7 +174,7 @@ private:
 		{
 			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
 			{
-				return new UORB_over_UAVCAN_sensor_gps_Subscriber(ins, pmgr, 0);
+				return new uORB_over_UAVCAN_Subscriber<sensor_gps_s>(ins, pmgr, ORB_ID(sensor_gps));
 			},
 			"uorb.sensor_gps",
 			0

src/drivers/uavcan_v1/parameters.c

@@ -141,6 +141,16 @@ PARAM_DEFINE_INT32(UCAN1_LG_BMS_SUB, -1);
  */
 PARAM_DEFINE_INT32(UCAN1_UORB_GPS, -1);
 
+
+/**
+ * sensor_gps uORB over UAVCAN v1 publication port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1);
+
 // Publication Port IDs
 
 /**