From 9756c70491972192d2e0fc56c95a59e59b054259 Mon Sep 17 00:00:00 2001
From: Peter van der Perk <57130844+PetervdPerk-NXP@users.noreply.github.com>
Date: Fri, 4 Jun 2021 15:30:17 +0200
Subject: [PATCH] UAVCANv1 Iteration, dynamic subscribers and decoupling
 (#17624)

* Update to use libcanard callback
Reworked param system with lambda

* Use callback instead of lambda saves 500 bytes of flash

* [UAVCANv1] Added ServiceRequest class, subscribers decoupled

* PX4 UAVCANv1 portID param default CANARD_PORT_ID_UNSET (65535U)

* [UAVCANv1] uORB sensor_gps modify timestamp for now untill we've got timesync working

* UAVCANv1 update parameter definition to use -1 as unset
---
 src/drivers/uavcan_v1/CMakeLists.txt          |   2 +
 src/drivers/uavcan_v1/NodeManager.cpp         |  38 ++++-
 src/drivers/uavcan_v1/NodeManager.hpp         |  35 +++--
 src/drivers/uavcan_v1/ParamManager.cpp        |  74 +---------
 src/drivers/uavcan_v1/ParamManager.hpp        |  58 ++++++--
 .../uavcan_v1/ServiceClients/GetInfo.hpp      |   3 +-
 .../uavcan_v1/Services/AccessRequest.hpp      |  24 ++--
 .../uavcan_v1/Services/ListRequest.hpp        |  25 ++--
 .../{ListReply.hpp => ServiceRequest.hpp}     |  50 ++++---
 .../uavcan_v1/Subscribers/BaseSubscriber.hpp  |  16 ++-
 .../Subscribers/{ => DS-015}/Battery.hpp      |  18 ++-
 .../Subscribers/{ => DS-015}/Esc.hpp          |   6 +-
 .../Subscribers/{ => DS-015}/Gnss.hpp         |   4 +-
 .../Subscribers/DynamicPortSubscriber.hpp     |  21 ++-
 .../Heartbeat.hpp}                            |  31 ++---
 .../Subscribers/NodeIDAllocationData.hpp      | 102 --------------
 .../{ => legacy}/LegacyBatteryInfo.hpp        |   4 +-
 .../uavcan_v1/Subscribers/uORB/sensor_gps.hpp |   6 +-
 src/drivers/uavcan_v1/SubscriptionManager.cpp |  87 ++++++++++++
 src/drivers/uavcan_v1/SubscriptionManager.hpp | 130 ++++++++++++++++++
 src/drivers/uavcan_v1/Uavcan.cpp              |  87 ++++++------
 src/drivers/uavcan_v1/Uavcan.hpp              |  57 +-------
 src/drivers/uavcan_v1/libcanard               |   2 +-
 src/drivers/uavcan_v1/parameters.c            | 121 ++++++++++++----
 24 files changed, 591 insertions(+), 410 deletions(-)
 rename src/drivers/uavcan_v1/Services/{ListReply.hpp => ServiceRequest.hpp} (70%)
 rename src/drivers/uavcan_v1/Subscribers/{ => DS-015}/Battery.hpp (93%)
 rename src/drivers/uavcan_v1/Subscribers/{ => DS-015}/Esc.hpp (97%)
 rename src/drivers/uavcan_v1/Subscribers/{ => DS-015}/Gnss.hpp (98%)
 rename src/drivers/uavcan_v1/{Services/AccessReply.hpp => Subscribers/Heartbeat.hpp} (73%)
 delete mode 100644 src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp
 rename src/drivers/uavcan_v1/Subscribers/{ => legacy}/LegacyBatteryInfo.hpp (98%)
 create mode 100644 src/drivers/uavcan_v1/SubscriptionManager.cpp
 create mode 100644 src/drivers/uavcan_v1/SubscriptionManager.hpp

diff --git a/src/drivers/uavcan_v1/CMakeLists.txt b/src/drivers/uavcan_v1/CMakeLists.txt
index 886bb34f44..362d23bf53 100644
--- a/src/drivers/uavcan_v1/CMakeLists.txt
+++ b/src/drivers/uavcan_v1/CMakeLists.txt
@@ -76,6 +76,8 @@ px4_add_module(
 		${LIBCANARD_DIR}/libcanard/
 		${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated
 	SRCS
+		SubscriptionManager.cpp
+		SubscriptionManager.hpp
 		ParamManager.hpp
 		ParamManager.cpp
 		NodeManager.hpp
diff --git a/src/drivers/uavcan_v1/NodeManager.cpp b/src/drivers/uavcan_v1/NodeManager.cpp
index ec841bbf14..ab584cfce3 100644
--- a/src/drivers/uavcan_v1/NodeManager.cpp
+++ b/src/drivers/uavcan_v1/NodeManager.cpp
@@ -43,6 +43,26 @@
 
 #include "NodeManager.hpp"
 
+void NodeManager::callback(const CanardTransfer &receive)
+{
+	if (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD) {
+		uavcan_pnp_NodeIDAllocationData_2_0 node_id_alloc_msg {};
+		size_t msg_size_in_bytes = receive.payload_size;
+		uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&node_id_alloc_msg, (const uint8_t *)receive.payload,
+				&msg_size_in_bytes);
+		/// do something with the data
+		HandleNodeIDRequest(node_id_alloc_msg);
+
+	} else {
+		uavcan_pnp_NodeIDAllocationData_1_0 node_id_alloc_msg {};
+		size_t msg_size_in_bytes = receive.payload_size;
+		uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&node_id_alloc_msg, (const uint8_t *)receive.payload,
+				&msg_size_in_bytes);
+		/// do something with the data
+		HandleNodeIDRequest(node_id_alloc_msg);
+	}
+}
+
 bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
 {
 	if (msg.allocated_node_id.count == 0) {
@@ -105,34 +125,38 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
 	return false;
 }
 
-
 bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg)
 {
-	//TODO V2 CAN FD implementation
+	//TODO v2 node id
 	return false;
 }
 
 
-void NodeManager::HandleListResponse(CanardNodeID node_id, uavcan_register_List_Response_1_0 &msg)
+void NodeManager::HandleListResponse(const CanardTransfer &receive)
 {
+	uavcan_register_List_Response_1_0 msg;
+
+	size_t register_in_size_bits = receive.payload_size;
+	uavcan_register_List_Response_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &register_in_size_bits);
+
 	if (msg.name.name.count == 0) {
 		// Index doesn't exist, we've parsed through all registers
 		for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
-			if (nodeid_registry[i].node_id == node_id) {
+			if (nodeid_registry[i].node_id == receive.remote_node_id) {
 				nodeid_registry[i].register_setup = true; // Don't update anymore
 			}
 		}
 
 	} else {
 		for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
-			if (nodeid_registry[i].node_id == node_id) {
+			if (nodeid_registry[i].node_id == receive.remote_node_id) {
 				nodeid_registry[i].register_index++; // Increment index counter for next update()
 				nodeid_registry[i].register_setup = false;
 				nodeid_registry[i].retry_count = 0;
 			}
 		}
 
-		if (_access_request.setPortId(node_id, msg.name)) {
+		if (_access_request.setPortId(receive.remote_node_id, msg.name, NULL)) { //FIXME confirm handler
 			PX4_INFO("Set portID succesfull");
 
 		} else {
@@ -147,7 +171,7 @@ void NodeManager::update()
 		for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
 			if (nodeid_registry[i].node_id != 0 && nodeid_registry[i].register_setup == false) {
 				//Setting up registers
-				_list_request.request(nodeid_registry[i].node_id, nodeid_registry[i].register_index);
+				_list_request.getIndex(nodeid_registry[i].node_id, nodeid_registry[i].register_index, this);
 				nodeid_registry[i].retry_count++;
 
 				if (nodeid_registry[i].retry_count > RETRY_COUNT) {
diff --git a/src/drivers/uavcan_v1/NodeManager.hpp b/src/drivers/uavcan_v1/NodeManager.hpp
index 50d31294c3..0b32b55913 100644
--- a/src/drivers/uavcan_v1/NodeManager.hpp
+++ b/src/drivers/uavcan_v1/NodeManager.hpp
@@ -51,13 +51,14 @@
 #include <uavcan/pnp/NodeIDAllocationData_1_0.h>
 #include <uavcan/pnp/NodeIDAllocationData_2_0.h>
 
-
-class NodeManager;
-
 #include "Services/AccessRequest.hpp"
 #include "Services/ListRequest.hpp"
 
-//TODO make this an object instead?
+#define PNP1_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_
+#define PNP1_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_
+#define PNP2_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_
+#define PNP2_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_2_0_SERIALIZATION_BUFFER_SIZE_BYTES_
+
 typedef struct {
 	uint8_t   node_id;
 	uint8_t   unique_id[16];
@@ -66,17 +67,35 @@ typedef struct {
 	uint16_t  retry_count;
 } UavcanNodeEntry;
 
-class NodeManager
+class NodeManager : public UavcanBaseSubscriber, public UavcanServiceRequestInterface
 {
 public:
-	NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _access_request(ins, pmgr),
-		_list_request(ins) { };
+	NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : UavcanBaseSubscriber(ins, "NodeIDAllocationData", 0),
+		_canard_instance(ins), _access_request(ins, pmgr), _list_request(ins) { };
+
+	void subscribe() override
+	{
+		_access_request.subscribe();
+		_list_request.subscribe();
+
+		canardRxSubscribe(&_canard_instance,
+				  CanardTransferKindMessage,
+				  (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID),  // The fixed Subject-ID
+				  (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
+				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+				  &_subj_sub._canard_sub);
+	}
 
 	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg);
 	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg);
 
+	void response_callback(const CanardTransfer &receive) override
+	{
+		HandleListResponse(receive);
+	}
+	void callback(const CanardTransfer &receive); // NodeIDAllocation callback
 
-	void HandleListResponse(CanardNodeID node_id, uavcan_register_List_Response_1_0 &msg);
+	void HandleListResponse(const CanardTransfer &receive);
 
 	void update();
 
diff --git a/src/drivers/uavcan_v1/ParamManager.cpp b/src/drivers/uavcan_v1/ParamManager.cpp
index 35eb42452b..8674f76a29 100644
--- a/src/drivers/uavcan_v1/ParamManager.cpp
+++ b/src/drivers/uavcan_v1/ParamManager.cpp
@@ -52,34 +52,7 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_
 				return false;
 			}
 
-			/// TODO: What will be our approach for handling other UAVCAN data-value types?
-			switch (param_type(param_handle)) {
-			case PARAM_TYPE_INT32: {
-					int32_t out_val {};
-					param_get(param_handle, &out_val);
-
-					if (uavcan_register_Value_1_0_is_natural16_(&value)) { //FIXME param rewrite
-						value.natural16.value.elements[0] = (uint16_t)out_val;
-						uavcan_register_Value_1_0_select_natural16_(&value);
-
-					} else {
-						value.integer32.value.elements[0] = out_val;
-						uavcan_register_Value_1_0_select_integer32_(&value);
-					}
-
-					break;
-				}
-
-			case PARAM_TYPE_FLOAT: {
-					float out_val {};
-					param_get(param_handle, &out_val);
-					value.real32.value.elements[0] = out_val;
-					uavcan_register_Value_1_0_select_real32_(&value);
-					break;
-				}
-			}
-
-			return true;
+			return param.px4_param_to_register_value(param_handle, value);
 		}
 	}
 
@@ -96,34 +69,7 @@ bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, ua
 				return false;
 			}
 
-			/// TODO: What will be our approach for handling other UAVCAN data-value types?
-			switch (param_type(param_handle)) {
-			case PARAM_TYPE_INT32: {
-					int32_t out_val {};
-					param_get(param_handle, &out_val);
-
-					if (uavcan_register_Value_1_0_is_natural16_(&value)) { //FIXME param rewrite
-						value.natural16.value.elements[0] = (uint16_t)out_val;
-						uavcan_register_Value_1_0_select_natural16_(&value);
-
-					} else {
-						value.integer32.value.elements[0] = out_val;
-						uavcan_register_Value_1_0_select_integer32_(&value);
-					}
-
-					break;
-				}
-
-			case PARAM_TYPE_FLOAT: {
-					float out_val {};
-					param_get(param_handle, &out_val);
-					value.real32.value.elements[0] = out_val;
-					uavcan_register_Value_1_0_select_real32_(&value);
-					break;
-				}
-			}
-
-			return true;
+			return param.px4_param_to_register_value(param_handle, value);
 		}
 	}
 
@@ -140,21 +86,7 @@ bool UavcanParamManager::SetParamByName(const uavcan_register_Name_1_0 &name, co
 				return false;
 			}
 
-			switch (param_type(param_handle)) {
-			case PARAM_TYPE_INT32: {
-					int32_t in_val = value.integer32.value.elements[0];
-					param_set(param_handle, &in_val);
-					break;
-				}
-
-			case PARAM_TYPE_FLOAT: {
-					float in_val = value.natural32.value.elements[0];
-					param_set(param_handle, &in_val);
-					break;
-				}
-			}
-
-			return true;
+			return param.register_value_to_px4_param(value, param_handle);
 		}
 	}
 
diff --git a/src/drivers/uavcan_v1/ParamManager.hpp b/src/drivers/uavcan_v1/ParamManager.hpp
index dd6c4745c7..357cb9282f 100644
--- a/src/drivers/uavcan_v1/ParamManager.hpp
+++ b/src/drivers/uavcan_v1/ParamManager.hpp
@@ -42,17 +42,52 @@
 #pragma once
 
 #include <lib/parameters/param.h>
+#include <px4_platform_common/defines.h>
 
 #include <uavcan/_register/Name_1_0.h>
 #include <uavcan/_register/Value_1_0.h>
 
+static bool px4_param_to_uavcan_port_id(param_t &in, uavcan_register_Value_1_0 &out)
+{
+	if (param_type(in) == PARAM_TYPE_INT32) {
+		int32_t out_val {};
+		param_get(in, &out_val);
+		out.natural16.value.elements[0] = (uint16_t)out_val;
+		out.natural16.value.count = 1;
+		uavcan_register_Value_1_0_select_natural16_(&out);
+		return true;
+	}
+
+	return false;
+};
+
+static bool uavcan_port_id_to_px4_param(const uavcan_register_Value_1_0 &in, param_t &out)
+{
+	if (uavcan_register_Value_1_0_is_natural16_(&in) && in.natural16.value.count == 1) {
+		if (param_type(out) == PARAM_TYPE_INT32) {
+			int32_t in_val = in.natural16.value.elements[0];
+			param_set(out, &in_val);
+			return true;
+		}
+	}
+
+	return false;
+};
+
+using param_2_reg_t = bool(*)(param_t &in, uavcan_register_Value_1_0 &out);
+using reg_2_param_t = bool(*)(const uavcan_register_Value_1_0 &in, param_t &out);
+
+
 typedef struct {
 	const char *uavcan_name;
 	const char *px4_name;
+	param_2_reg_t px4_param_to_register_value;
+	reg_2_param_t register_value_to_px4_param;
 	bool is_mutable {true};
 	bool is_persistent {true};
 } UavcanParamBinder;
 
+
 class UavcanParamManager
 {
 public:
@@ -63,18 +98,19 @@ public:
 
 private:
 
+
 	const UavcanParamBinder _uavcan_params[11] {
-		{"uavcan.pub.esc.0.id",              "UCAN1_ESC_PUB"},
-		{"uavcan.pub.servo.0.id",            "UCAN1_SERVO_PUB"},
-		{"uavcan.pub.gps.0.id",              "UCAN1_GPS_PUB"},
-		{"uavcan.sub.esc.0.id",              "UCAN1_ESC0_PID"},
-		{"uavcan.sub.gps.0.id",              "UCAN1_GPS0_PID"},
-		{"uavcan.sub.gps.1.id",              "UCAN1_GPS1_PID"},
-		{"uavcan.sub.energy_source.0.id",      "UCAN1_BMS_ES_PID"},
-		{"uavcan.sub.battery_status.0.id",     "UCAN1_BMS_BS_PID"},
-		{"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_PID"},
-		{"uavcan.sub.legacy_bms.0.id",         "UCAN1_LG_BMS_PID"},
-		{"uavcan.sub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS"},
+		{"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},
+		{"uavcan.sub.esc.0.id",                "UCAN1_ESC0_PID",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.gps.0.id",                "UCAN1_GPS0_PID",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.gps.1.id",                "UCAN1_GPS1_PID",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.energy_source.0.id",      "UCAN1_BMS_ES_PID",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.battery_status.0.id",     "UCAN1_BMS_BS_PID",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_PID",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.legacy_bms.0.id",         "UCAN1_LG_BMS_PID",		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.sub.bms.0.id",   "UCAN1_BMS0_PID"}, //FIXME instancing
 		//{"uavcan.sub.bms.1.id",   "UCAN1_BMS1_PID"},
 	};
diff --git a/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp b/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp
index ce2806dd2f..fd38c29a95 100644
--- a/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp
+++ b/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp
@@ -45,12 +45,11 @@
 #include <px4_platform_common/module.h>
 #include <version/version.h>
 
-#include "../NodeManager.hpp"
-
 #include <uavcan/node/ID_1_0.h>
 #include <uavcan/node/GetInfo_1_0.h>
 
 #include "../Subscribers/BaseSubscriber.hpp"
+#include "../Publishers/Publisher.hpp"
 
 class UavcanGetInfoResponse : public UavcanBaseSubscriber
 {
diff --git a/src/drivers/uavcan_v1/Services/AccessRequest.hpp b/src/drivers/uavcan_v1/Services/AccessRequest.hpp
index 16a62d69f4..8f43ce9186 100644
--- a/src/drivers/uavcan_v1/Services/AccessRequest.hpp
+++ b/src/drivers/uavcan_v1/Services/AccessRequest.hpp
@@ -47,15 +47,18 @@
 
 #include <uavcan/_register/Access_1_0.h>
 
+#include "ServiceRequest.hpp"
+#include "../ParamManager.hpp"
 #include "../Publishers/Publisher.hpp"
 
-class UavcanAccessServiceRequest
+class UavcanAccessServiceRequest : public UavcanServiceRequest
 {
 public:
 	UavcanAccessServiceRequest(CanardInstance &ins, UavcanParamManager &pmgr) :
-		_canard_instance(ins), _param_manager(pmgr) { };
+		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)
+	bool setPortId(CanardNodeID node_id, uavcan_register_Name_1_0 &name, UavcanServiceRequestInterface *handler)
 	{
 		int result {0};
 
@@ -76,9 +79,9 @@ public:
 				.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
 				.priority       = CanardPriorityNominal,
 				.transfer_kind  = CanardTransferKindRequest,
-				.port_id        = uavcan_register_Access_1_0_FIXED_PORT_ID_,                // This is the subject-ID.
+				.port_id        = _portID,                // This is the subject-ID.
 				.remote_node_id = node_id,       // Messages cannot be unicast, so use UNSET.
-				.transfer_id    = access_request_transfer_id,
+				.transfer_id    = request_transfer_id,
 				.payload_size   = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
 				.payload        = &request_payload_buffer,
 			};
@@ -86,12 +89,11 @@ public:
 			result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
 
 			if (result == 0) {
-				// set the data ready in the buffer and chop if needed
-				++access_request_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
-				result = canardTxPush(&_canard_instance, &transfer);
-			}
+				return request(&transfer, handler);
 
-			return result > 0;
+			} else {
+				return false;
+			}
 
 		} else {
 			return false;
@@ -99,8 +101,6 @@ public:
 	};
 
 private:
-	CanardInstance &_canard_instance;
-	CanardTransferID access_request_transfer_id = 0;
 	UavcanParamManager &_param_manager;
 
 };
diff --git a/src/drivers/uavcan_v1/Services/ListRequest.hpp b/src/drivers/uavcan_v1/Services/ListRequest.hpp
index 3cb3fe9a5e..7e60b4e763 100644
--- a/src/drivers/uavcan_v1/Services/ListRequest.hpp
+++ b/src/drivers/uavcan_v1/Services/ListRequest.hpp
@@ -47,44 +47,43 @@
 
 #include <uavcan/_register/List_1_0.h>
 
+#include "ServiceRequest.hpp"
+#include "../ParamManager.hpp"
 #include "../Publishers/Publisher.hpp"
 
-class UavcanListServiceRequest
+class UavcanListServiceRequest : public UavcanServiceRequest
 {
 public:
 	UavcanListServiceRequest(CanardInstance &ins) :
-		_canard_instance(ins) { };
+		UavcanServiceRequest(ins, "List", uavcan_register_List_1_0_FIXED_PORT_ID_,
+				     uavcan_register_List_Response_1_0_EXTENT_BYTES_) { };
 
 
-	void request(CanardNodeID node_id, uint16_t index)
+	bool getIndex(CanardNodeID node_id, uint16_t index, UavcanServiceRequestInterface *handler)
 	{
 		uavcan_register_List_Request_1_0 msg;
 		msg.index = index;
 
 		uint8_t request_payload_buffer[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
 
-		CanardTransfer request = {
+		CanardTransfer transfer = {
 			.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
 			.priority       = CanardPriorityNominal,
 			.transfer_kind  = CanardTransferKindRequest,
 			.port_id        = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID.
 			.remote_node_id = node_id,
-			.transfer_id    = list_request_transfer_id,
+			.transfer_id    = request_transfer_id,
 			.payload_size   = uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
 			.payload        = &request_payload_buffer,
 		};
 
-		int32_t result = uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &request.payload_size);
+		if (uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &transfer.payload_size) == 0) {
+			return request(&transfer, handler);
 
-		if (result == 0) {
-			// set the data ready in the buffer and chop if needed
-			++list_request_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
-			result = canardTxPush(&_canard_instance, &request);
+		} else {
+			return false;
 		}
 	};
 
-private:
-	CanardInstance &_canard_instance;
-	CanardTransferID list_request_transfer_id = 0;
 
 };
diff --git a/src/drivers/uavcan_v1/Services/ListReply.hpp b/src/drivers/uavcan_v1/Services/ServiceRequest.hpp
similarity index 70%
rename from src/drivers/uavcan_v1/Services/ListReply.hpp
rename to src/drivers/uavcan_v1/Services/ServiceRequest.hpp
index 8232190cbf..4a302bbbdd 100644
--- a/src/drivers/uavcan_v1/Services/ListReply.hpp
+++ b/src/drivers/uavcan_v1/Services/ServiceRequest.hpp
@@ -51,38 +51,54 @@
 
 #include "../Subscribers/BaseSubscriber.hpp"
 
-class UavcanListServiceReply : public UavcanBaseSubscriber
+
+class UavcanServiceRequestInterface
 {
 public:
-	UavcanListServiceReply(CanardInstance &ins, NodeManager &nmgr) :
-		UavcanBaseSubscriber(ins, "List", 0), _nmgr(nmgr) { };
+	virtual void response_callback(const CanardTransfer &receive) = 0;
+};
+
+class UavcanServiceRequest : public UavcanBaseSubscriber
+{
+public:
+	UavcanServiceRequest(CanardInstance &ins, const char *subject_name, CanardPortID portID, size_t extent) :
+		UavcanBaseSubscriber(ins, subject_name, 0), _portID(portID), _extent(extent) { };
+
 
 	void subscribe() override
 	{
-		// Subscribe to requests uavcan.pnp.NodeIDAllocationData
+		// Subscribe to requests response
 		canardRxSubscribe(&_canard_instance,
 				  CanardTransferKindResponse,
-				  uavcan_register_List_1_0_FIXED_PORT_ID_,
-				  uavcan_register_List_Response_1_0_EXTENT_BYTES_,
+				  _portID,
+				  _extent,
 				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
 				  &_subj_sub._canard_sub);
-
 	};
 
+	bool request(CanardTransfer *transfer, UavcanServiceRequestInterface *handler)
+	{
+		_response_callback = handler;
+		++request_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+		return canardTxPush(&_canard_instance, transfer) > 0;
+	}
+
 	void callback(const CanardTransfer &receive) override
 	{
-		PX4_INFO("List response");
+		PX4_INFO("Response");
 
-		uavcan_register_List_Response_1_0 msg;
-
-		size_t register_in_size_bits = receive.payload_size;
-		uavcan_register_List_Response_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &register_in_size_bits);
-
-		// Pass msg.name.name.elements to NodeManager
-		_nmgr.HandleListResponse(receive.remote_node_id, msg);
+		if (_response_callback != NULL) {
+			_response_callback->response_callback(receive);
+		}
 	};
 
-private:
-	NodeManager &_nmgr;
+
+
+protected:
+	CanardTransferID request_transfer_id = 0;
+
+	const CanardPortID _portID;
+	const size_t _extent;
+	UavcanServiceRequestInterface *_response_callback = NULL;
 
 };
diff --git a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp
index d58d91040c..3ab7b0dcf3 100644
--- a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp
+++ b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp
@@ -56,6 +56,12 @@ public:
 		_canard_instance(ins), _instance(instance)
 	{
 		_subj_sub._subject_name = subject_name;
+		_subj_sub._canard_sub.user_reference = this;
+	}
+
+	~UavcanBaseSubscriber()
+	{
+		unsubscribe();
 	}
 
 	virtual void subscribe() = 0;
@@ -64,7 +70,7 @@ public:
 		SubjectSubscription *curSubj = &_subj_sub;
 
 		while (curSubj != NULL) {
-			canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, curSubj->_canard_sub._port_id);
+			canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, curSubj->_canard_sub.port_id);
 			curSubj = curSubj->next;
 		}
 	};
@@ -78,7 +84,7 @@ public:
 
 		while (curSubj != NULL) {
 			if (instance == i) {
-				return curSubj->_canard_sub._port_id;
+				return curSubj->_canard_sub.port_id;
 			}
 
 			curSubj = curSubj->next;
@@ -93,7 +99,7 @@ public:
 		SubjectSubscription *curSubj = &_subj_sub;
 
 		while (curSubj != NULL) {
-			if (port_id == curSubj->_canard_sub._port_id) {
+			if (port_id == curSubj->_canard_sub.port_id) {
 				return true;
 			}
 
@@ -108,8 +114,8 @@ public:
 		SubjectSubscription *curSubj = &_subj_sub;
 
 		while (curSubj != NULL) {
-			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);
+			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;
diff --git a/src/drivers/uavcan_v1/Subscribers/Battery.hpp b/src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp
similarity index 93%
rename from src/drivers/uavcan_v1/Subscribers/Battery.hpp
rename to src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp
index e3a6c4ed29..d4cd125c14 100644
--- a/src/drivers/uavcan_v1/Subscribers/Battery.hpp
+++ b/src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp
@@ -49,7 +49,7 @@
 #include <reg/drone/service/battery/Parameters_0_3.h>
 #include <reg/drone/service/battery/Status_0_2.h>
 
-#include "DynamicPortSubscriber.hpp"
+#include "../DynamicPortSubscriber.hpp"
 
 #define KELVIN_OFFSET 273.15f
 #define WH_TO_JOULE   3600
@@ -61,9 +61,13 @@ public:
 		UavcanDynamicPortSubscriber(ins, pmgr, "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 = NULL;
 	}
 
@@ -72,7 +76,7 @@ public:
 		// Subscribe to messages reg.drone.physics.electricity.SourceTs.0.1
 		canardRxSubscribe(&_canard_instance,
 				  CanardTransferKindMessage,
-				  _subj_sub._canard_sub._port_id,
+				  _subj_sub._canard_sub.port_id,
 				  reg_drone_physics_electricity_SourceTs_0_1_EXTENT_BYTES_,
 				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
 				  &_subj_sub._canard_sub);
@@ -80,7 +84,7 @@ public:
 		// Subscribe to messages reg.drone.service.battery.Status.0.2
 		canardRxSubscribe(&_canard_instance,
 				  CanardTransferKindMessage,
-				  _status_sub._canard_sub._port_id,
+				  _status_sub._canard_sub.port_id,
 				  reg_drone_service_battery_Status_0_2_EXTENT_BYTES_,
 				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
 				  &_status_sub._canard_sub);
@@ -88,7 +92,7 @@ public:
 		// Subscribe to messages reg.drone.service.battery.Parameters.0.3
 		canardRxSubscribe(&_canard_instance,
 				  CanardTransferKindMessage,
-				  _parameters_sub._canard_sub._port_id,
+				  _parameters_sub._canard_sub.port_id,
 				  reg_drone_service_battery_Parameters_0_3_EXTENT_BYTES_,
 				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
 				  &_parameters_sub._canard_sub);
@@ -98,7 +102,7 @@ public:
 	{
 		PX4_INFO("BmsCallback");
 
-		if (receive.port_id == _subj_sub._canard_sub._port_id) {
+		if (receive.port_id == _subj_sub._canard_sub.port_id) {
 			reg_drone_physics_electricity_SourceTs_0_1 source_ts {};
 			size_t source_ts_size_in_bytes = receive.payload_size;
 			reg_drone_physics_electricity_SourceTs_0_1_deserialize_(&source_ts,
@@ -123,7 +127,7 @@ public:
 			print_message(bat_status);
 
 
-		} else if (receive.port_id == _status_sub._canard_sub._port_id) {
+		} else if (receive.port_id == _status_sub._canard_sub.port_id) {
 			reg_drone_service_battery_Status_0_2 bat {};
 			size_t bat_size_in_bytes = receive.payload_size;
 			reg_drone_service_battery_Status_0_2_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bytes);
@@ -157,7 +161,7 @@ public:
 
 			bat_status.max_cell_voltage_delta = voltage_cell_max - voltage_cell_min; // Current delta or max delta over time?
 
-		} else if (receive.port_id == _parameters_sub._canard_sub._port_id) {
+		} else if (receive.port_id == _parameters_sub._canard_sub.port_id) {
 			reg_drone_service_battery_Parameters_0_3 parameters {};
 			size_t parameters_size_in_bytes = receive.payload_size;
 			reg_drone_service_battery_Parameters_0_3_deserialize_(&parameters,
diff --git a/src/drivers/uavcan_v1/Subscribers/Esc.hpp b/src/drivers/uavcan_v1/Subscribers/DS-015/Esc.hpp
similarity index 97%
rename from src/drivers/uavcan_v1/Subscribers/Esc.hpp
rename to src/drivers/uavcan_v1/Subscribers/DS-015/Esc.hpp
index 941d059033..1bbc3bea54 100644
--- a/src/drivers/uavcan_v1/Subscribers/Esc.hpp
+++ b/src/drivers/uavcan_v1/Subscribers/DS-015/Esc.hpp
@@ -49,7 +49,7 @@
 #include <reg/drone/service/actuator/common/sp/Vector8_0_1.h>
 #include <reg/drone/service/common/Readiness_0_1.h>
 
-#include "DynamicPortSubscriber.hpp"
+#include "../DynamicPortSubscriber.hpp"
 
 class UavcanEscSubscriber : public UavcanDynamicPortSubscriber
 {
@@ -62,7 +62,7 @@ public:
 		// Subscribe to messages reg.drone.service.actuator.common.sp.Vector8.0.1
 		canardRxSubscribe(&_canard_instance,
 				  CanardTransferKindMessage,
-				  _subj_sub._canard_sub._port_id,
+				  _subj_sub._canard_sub.port_id,
 				  reg_drone_service_actuator_common_sp_Vector8_0_1_EXTENT_BYTES_,
 				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
 				  &_subj_sub._canard_sub);
@@ -70,7 +70,7 @@ public:
 		// Subscribe to messages reg.drone.service.common.Readiness.0.1
 		canardRxSubscribe(&_canard_instance,
 				  CanardTransferKindMessage,
-				  static_cast<CanardPortID>(static_cast<uint32_t>(_subj_sub._canard_sub._port_id) + 1),
+				  static_cast<CanardPortID>(static_cast<uint32_t>(_subj_sub._canard_sub.port_id) + 1),
 				  reg_drone_service_common_Readiness_0_1_EXTENT_BYTES_,
 				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
 				  &_canard_sub_readiness);
diff --git a/src/drivers/uavcan_v1/Subscribers/Gnss.hpp b/src/drivers/uavcan_v1/Subscribers/DS-015/Gnss.hpp
similarity index 98%
rename from src/drivers/uavcan_v1/Subscribers/Gnss.hpp
rename to src/drivers/uavcan_v1/Subscribers/DS-015/Gnss.hpp
index 6c5ee051b3..43e38ef9c4 100644
--- a/src/drivers/uavcan_v1/Subscribers/Gnss.hpp
+++ b/src/drivers/uavcan_v1/Subscribers/DS-015/Gnss.hpp
@@ -44,7 +44,7 @@
 // DS-15 Specification Messages
 #include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
 
-#include "DynamicPortSubscriber.hpp"
+#include "../DynamicPortSubscriber.hpp"
 
 class UavcanGnssSubscriber : public UavcanDynamicPortSubscriber
 {
@@ -57,7 +57,7 @@ public:
 		// Subscribe to messages reg.drone.physics.kinematics.geodetic.Point.0.1
 		canardRxSubscribe(&_canard_instance,
 				  CanardTransferKindMessage,
-				  _subj_sub._canard_sub._port_id,
+				  _subj_sub._canard_sub.port_id,
 				  reg_drone_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_,
 				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
 				  &_subj_sub._canard_sub);
diff --git a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp
index d2e1e2292e..eb8da7e62e 100644
--- a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp
+++ b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp
@@ -47,9 +47,9 @@
 
 #include <uavcan/_register/Access_1_0.h>
 
-#include "DynamicPortSubscriber.hpp"
 #include "../CanardInterface.hpp"
 #include "../ParamManager.hpp"
+#include "BaseSubscriber.hpp"
 
 class UavcanDynamicPortSubscriber : public UavcanBaseSubscriber
 {
@@ -73,20 +73,20 @@ public:
 				int32_t new_id = value.integer32.value.elements[0];
 
 				/* FIXME how about partial subscribing */
-				if (curSubj->_canard_sub._port_id != new_id) {
+				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) {
+						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.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub._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);
 						subscribe();
 					}
 				}
@@ -96,6 +96,17 @@ public:
 		}
 	};
 
+	UavcanDynamicPortSubscriber *next()
+	{
+		return _next_sub;
+	}
+
+	void setNext(UavcanDynamicPortSubscriber *next)
+	{
+		_next_sub = next;
+	}
+
 protected:
 	UavcanParamManager &_param_manager;
+	UavcanDynamicPortSubscriber *_next_sub {NULL};
 };
diff --git a/src/drivers/uavcan_v1/Services/AccessReply.hpp b/src/drivers/uavcan_v1/Subscribers/Heartbeat.hpp
similarity index 73%
rename from src/drivers/uavcan_v1/Services/AccessReply.hpp
rename to src/drivers/uavcan_v1/Subscribers/Heartbeat.hpp
index ed7e5508e8..d863488497 100644
--- a/src/drivers/uavcan_v1/Services/AccessReply.hpp
+++ b/src/drivers/uavcan_v1/Subscribers/Heartbeat.hpp
@@ -32,38 +32,34 @@
  ****************************************************************************/
 
 /**
- * @file Access.hpp
+ * @file Heartbeat.hpp
  *
- * Defines a Access Service invoker and process Access responses
+ * Defines basic functionality of UAVCAN Heartbeat.1.0 subscription
  *
  * @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 "../NodeManager.hpp"
 
-#include <uavcan/_register/Access_1_0.h>
+#include <uavcan/node/Heartbeat_1_0.h>
 
-#include "../Subscribers/BaseSubscriber.hpp"
+#include "BaseSubscriber.hpp"
 
-class UavcanAccessServiceReply : public UavcanBaseSubscriber
+class UavcanHeartbeatSubscriber : public UavcanBaseSubscriber
 {
 public:
-	UavcanAccessServiceReply(CanardInstance &ins, NodeManager &nmgr) :
-		UavcanBaseSubscriber(ins, "Access", 0), _nmgr(nmgr) { };
+	UavcanHeartbeatSubscriber(CanardInstance &ins) :
+		UavcanBaseSubscriber(ins, "Heartbeat", 0) { };
 
 	void subscribe() override
 	{
-		// Subscribe to requests uavcan.pnp.NodeIDAllocationData
+		// Subscribe to heartbeat messages
 		canardRxSubscribe(&_canard_instance,
-				  CanardTransferKindResponse,
-				  uavcan_register_Access_1_0_FIXED_PORT_ID_,
-				  uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
+				  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);
 
@@ -71,10 +67,7 @@ public:
 
 	void callback(const CanardTransfer &receive) override
 	{
-		PX4_INFO("Access response");
+		//TODO heartbeat management
 	};
 
-private:
-	NodeManager &_nmgr;
-
 };
diff --git a/src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp b/src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp
deleted file mode 100644
index 0a6c637781..0000000000
--- a/src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp
+++ /dev/null
@@ -1,102 +0,0 @@
-/****************************************************************************
- *
- *   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 NodeIDAllocationData.hpp
- *
- * Defines basic functionality of UAVCAN NodeIDAllocationData.1.0 subscription
- *
- * @author Peter van der Perk <peter.vanderperk@nxp.com>
- */
-
-#pragma once
-
-#include "../NodeManager.hpp"
-
-//Quick and Dirty PNP imlementation only V1 for now as well
-#include <uavcan/node/ID_1_0.h>
-#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
-#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
-
-#define PNP1_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_
-#define PNP1_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_
-#define PNP2_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_
-#define PNP2_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_2_0_SERIALIZATION_BUFFER_SIZE_BYTES_
-
-#include "BaseSubscriber.hpp"
-
-class UavcanNodeIDAllocationDataSubscriber : public UavcanBaseSubscriber
-{
-public:
-	UavcanNodeIDAllocationDataSubscriber(CanardInstance &ins, NodeManager &nmgr) :
-		UavcanBaseSubscriber(ins, "NodeIDAllocationData", 0), _nmgr(nmgr) { };
-
-	void subscribe() override
-	{
-		// Subscribe to messages uavcan.pnp.NodeIDAllocationData
-		canardRxSubscribe(&_canard_instance,
-				  CanardTransferKindMessage,
-				  (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID),  // The fixed Subject-ID
-				  (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
-				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
-				  &_subj_sub._canard_sub);
-
-	};
-
-	void callback(const CanardTransfer &receive) override
-	{
-		PX4_INFO("NodeIDAllocationData");
-
-		if (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD) {
-			uavcan_pnp_NodeIDAllocationData_2_0 node_id_alloc_msg {};
-			size_t msg_size_in_bytes = receive.payload_size;
-			uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&node_id_alloc_msg, (const uint8_t *)receive.payload,
-					&msg_size_in_bytes);
-			/// do something with the data
-			_nmgr.HandleNodeIDRequest(node_id_alloc_msg);
-
-		} else {
-			uavcan_pnp_NodeIDAllocationData_1_0 node_id_alloc_msg {};
-			size_t msg_size_in_bytes = receive.payload_size;
-			uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&node_id_alloc_msg, (const uint8_t *)receive.payload,
-					&msg_size_in_bytes);
-			/// do something with the data
-			_nmgr.HandleNodeIDRequest(node_id_alloc_msg);
-		}
-
-	};
-
-private:
-	NodeManager &_nmgr;
-
-};
diff --git a/src/drivers/uavcan_v1/Subscribers/LegacyBatteryInfo.hpp b/src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp
similarity index 98%
rename from src/drivers/uavcan_v1/Subscribers/LegacyBatteryInfo.hpp
rename to src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp
index 172ea1bcfc..f1840a12ca 100644
--- a/src/drivers/uavcan_v1/Subscribers/LegacyBatteryInfo.hpp
+++ b/src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp
@@ -47,7 +47,7 @@
 // Legacy message from UAVCANv0
 #include <legacy/equipment/power/BatteryInfo_1_0.h>
 
-#include "DynamicPortSubscriber.hpp"
+#include "../DynamicPortSubscriber.hpp"
 
 class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber
 {
@@ -60,7 +60,7 @@ public:
 		// Subscribe to messages reg.drone.service.battery.Status.0.1
 		canardRxSubscribe(&_canard_instance,
 				  CanardTransferKindMessage,
-				  _subj_sub._canard_sub._port_id,
+				  _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);
diff --git a/src/drivers/uavcan_v1/Subscribers/uORB/sensor_gps.hpp b/src/drivers/uavcan_v1/Subscribers/uORB/sensor_gps.hpp
index d7ddbfa0ae..8fdf4de1fb 100644
--- a/src/drivers/uavcan_v1/Subscribers/uORB/sensor_gps.hpp
+++ b/src/drivers/uavcan_v1/Subscribers/uORB/sensor_gps.hpp
@@ -57,7 +57,7 @@ public:
 		// Subscribe to messages uORB sensor_gps payload over UAVCAN
 		canardRxSubscribe(&_canard_instance,
 				  CanardTransferKindMessage,
-				  _subj_sub._canard_sub._port_id,
+				  _subj_sub._canard_sub.port_id,
 				  sizeof(struct sensor_gps_s),
 				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000,
 				  &_subj_sub._canard_sub);
@@ -69,6 +69,10 @@ public:
 
 		if (receive.payload_size == sizeof(struct sensor_gps_s)) {
 			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 */
+
 			_sensor_gps_pub.publish(*gps_msg);
 
 		} else {
diff --git a/src/drivers/uavcan_v1/SubscriptionManager.cpp b/src/drivers/uavcan_v1/SubscriptionManager.cpp
new file mode 100644
index 0000000000..bd814a477a
--- /dev/null
+++ b/src/drivers/uavcan_v1/SubscriptionManager.cpp
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   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 SubscriptionManager.cpp
+ *
+ * Manages the UAVCAN subscriptions
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+
+#include "SubscriptionManager.hpp"
+
+
+
+void SubscriptionManager::subscribe()
+{
+	_heartbeat_sub.subscribe();
+	_getinfo_rsp.subscribe();
+	_access_rsp.subscribe();
+
+	for (auto &sub : _uavcan_subs) {
+		param_t param_handle = param_find(sub.px4_name);
+
+		if (param_handle == PARAM_INVALID) {
+			PX4_ERR("Param %s not found", sub.px4_name);
+			break;
+		}
+
+		if ((param_type(param_handle) == PARAM_TYPE_INT32)) {
+			int32_t port_id {};
+			param_get(param_handle, &port_id);
+
+			if (port_id >= 0) { // PortID is set create a subscriber
+				UavcanDynamicPortSubscriber *dynsub = sub.create_sub(_canard_instance, _param_manager);
+
+				if (_dynsubscribers != NULL) {
+					_dynsubscribers->setNext(dynsub);
+				}
+
+				_dynsubscribers = dynsub;
+				dynsub->updateParam();
+			}
+		}
+	}
+}
+
+void SubscriptionManager::printInfo()
+{
+
+}
+
+void SubscriptionManager::updateParams()
+{
+	//TODO dynamically update params and unsubscribe
+}
diff --git a/src/drivers/uavcan_v1/SubscriptionManager.hpp b/src/drivers/uavcan_v1/SubscriptionManager.hpp
new file mode 100644
index 0000000000..66c47baa56
--- /dev/null
+++ b/src/drivers/uavcan_v1/SubscriptionManager.hpp
@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   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 SubscriptionManager.hpp
+ *
+ * Manages the UAVCAN subscriptions
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+
+#include <px4_platform_common/defines.h>
+#include <drivers/drv_hrt.h>
+#include "Subscribers/DynamicPortSubscriber.hpp"
+#include "CanardInterface.hpp"
+
+#include "ServiceClients/GetInfo.hpp"
+#include "ServiceClients/Access.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"
+
+typedef struct {
+	const char *px4_name;
+	UavcanDynamicPortSubscriber *(*create_sub)(CanardInstance &ins, UavcanParamManager &pmgr) {};
+} UavcanDynSubBinder;
+
+class SubscriptionManager
+{
+public:
+	SubscriptionManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _param_manager(pmgr) {}
+
+	void subscribe();
+	void printInfo();
+	void updateParams();
+
+private:
+	CanardInstance &_canard_instance;
+	UavcanParamManager &_param_manager;
+	UavcanDynamicPortSubscriber *_dynsubscribers {NULL};
+
+	UavcanHeartbeatSubscriber _heartbeat_sub {_canard_instance};
+	// GetInfo response
+	UavcanGetInfoResponse _getinfo_rsp {_canard_instance};
+
+	// Process register requests
+	UavcanAccessResponse  _access_rsp {_canard_instance, _param_manager};
+
+	const UavcanDynSubBinder _uavcan_subs[6] {
+		{
+			"UCAN1_ESC0_PID",
+			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscSubscriber(ins, pmgr, 0);
+			}
+		},
+		{
+			"UCAN1_GPS0_PID",
+			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanGnssSubscriber(ins, pmgr, 1);
+			}
+		},
+		{
+			"UCAN1_GPS1_PID",
+			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanGnssSubscriber(ins, pmgr, 0);
+			}
+		},
+		{
+			"UCAN1_BMS_ES_PID",
+			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanBmsSubscriber(ins, pmgr, 0);
+			}
+		},
+		{
+			"UCAN1_LG_BMS_PID",
+			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanLegacyBatteryInfoSubscriber(ins, pmgr, 0);
+			}
+		},
+		{
+			"UCAN1_UORB_GPS",
+			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UORB_over_UAVCAN_sensor_gps_Subscriber(ins, pmgr, 0);
+			}
+		},
+	};
+};
diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp
index 5c1d34fd0b..98d32ff1e2 100644
--- a/src/drivers/uavcan_v1/Uavcan.cpp
+++ b/src/drivers/uavcan_v1/Uavcan.cpp
@@ -82,13 +82,13 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
 
 	PX4_INFO("main _canard_instance = %p", &_canard_instance);
 
+	_node_manager.subscribe();
+
 	for (auto &publisher : _publishers) {
 		publisher->updateParam();
 	}
 
-	for (auto &subscriber : _dynsubscribers) {
-		subscriber->updateParam();
-	}
+	_sub_manager.subscribe();
 
 	_mixing_output.mixingOutput().updateSubscriptions(false, false);
 }
@@ -156,21 +156,6 @@ void UavcanNode::init()
 	// interface init
 	if (_can_interface) {
 		if (_can_interface->init() == PX4_OK) {
-
-			// We can't accept just any message; we must fist subscribe to specific IDs
-
-			// Subscribe to messages uavcan.node.Heartbeat.
-			canardRxSubscribe(&_canard_instance,
-					  CanardTransferKindMessage,
-					  uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
-					  uavcan_node_Heartbeat_1_0_EXTENT_BYTES_,
-					  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
-					  &_heartbeat_subscription);
-
-			for (auto &subscriber : _subscribers) {
-				subscriber->subscribe();
-			}
-
 			_initialized = true;
 		}
 	}
@@ -205,12 +190,7 @@ void UavcanNode::Run()
 			publisher->updateParam();
 		}
 
-		for (auto &subscriber : _dynsubscribers) {
-			// Have the subscriber update its associated port-id parameter
-			// If the port-id changes, (re)start the subscription
-			// Setting the port-id to 0 disables the subscription
-			subscriber->updateParam();
-		}
+		_sub_manager.updateParams();
 
 		_mixing_output.updateParams();
 
@@ -240,7 +220,8 @@ void UavcanNode::Run()
 
 	while (_can_interface->receive(&received_frame) > 0) {
 		CanardTransfer receive{};
-		int32_t result = canardRxAccept(&_canard_instance, &received_frame, 0, &receive);
+		CanardRxSubscription *subscription = NULL;
+		int32_t result = canardRxAccept2(&_canard_instance, &received_frame, 0, &receive, &subscription);
 
 		if (result < 0) {
 			// An error has occurred: either an argument is invalid or we've ran out of memory.
@@ -253,19 +234,12 @@ void UavcanNode::Run()
 			// A transfer has been received, process it.
 			// PX4_INFO("received Port ID: %d", receive.port_id);
 
-			if (receive.port_id > 0) {
-				// If not a fixed port ID, check any subscribers which may have registered it
-				for (auto &subscriber : _subscribers) {
-					if (subscriber->hasPortID(receive.port_id)) {
-						subscriber->callback(receive);
-					}
-				}
+			if (subscription != NULL) {
+				UavcanBaseSubscriber *sub_instance = (UavcanBaseSubscriber *)subscription->user_reference;
+				sub_instance->callback(receive);
 
-				for (auto &subscriber : _dynsubscribers) {
-					if (subscriber->hasPortID(receive.port_id)) {
-						subscriber->callback(receive);
-					}
-				}
+			} else {
+				PX4_ERR("No matching sub for %d", receive.port_id);
 			}
 
 			// Deallocate the dynamic memory afterwards.
@@ -342,12 +316,43 @@ void UavcanNode::print_info()
 		publisher->printInfo();
 	}
 
-	for (auto &subscriber : _subscribers) {
-		subscriber->printInfo();
+	CanardRxSubscription *rxs = _canard_instance.rx_subscriptions[CanardTransferKindMessage];
+
+	while (rxs != NULL) {
+		if (rxs->user_reference == NULL) {
+			PX4_INFO("Message port id %d", rxs->port_id);
+
+		} else {
+			((UavcanBaseSubscriber *)rxs->user_reference)->printInfo();
+		}
+
+		rxs = rxs->next;
 	}
 
-	for (auto &subscriber : _dynsubscribers) {
-		subscriber->printInfo();
+	rxs = _canard_instance.rx_subscriptions[CanardTransferKindRequest];
+
+	while (rxs != NULL) {
+		if (rxs->user_reference == NULL) {
+			PX4_INFO("Service response port id %d", rxs->port_id);
+
+		} else {
+			((UavcanBaseSubscriber *)rxs->user_reference)->printInfo();
+		}
+
+		rxs = rxs->next;
+	}
+
+	rxs = _canard_instance.rx_subscriptions[CanardTransferKindResponse];
+
+	while (rxs != NULL) {
+		if (rxs->user_reference == NULL) {
+			PX4_INFO("Service request port id %d", rxs->port_id);
+
+		} else {
+			((UavcanBaseSubscriber *)rxs->user_reference)->printInfo();
+		}
+
+		rxs = rxs->next;
 	}
 
 	_mixing_output.printInfo();
diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp
index 2e98e03bc5..59889cf30d 100644
--- a/src/drivers/uavcan_v1/Uavcan.hpp
+++ b/src/drivers/uavcan_v1/Uavcan.hpp
@@ -55,40 +55,15 @@
 #include <canard.h>
 #include <canard_dsdl.h>
 
-#include <uavcan/_register/Access_1_0.h>
-#include <uavcan/_register/List_1_0.h>
-#include <uavcan/_register/Value_1_0.h>
-#include <uavcan/node/Heartbeat_1_0.h>
-#include <uavcan/primitive/Empty_1_0.h>
-
-// DS-15 Specification Messages
-#include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
-#include <reg/drone/service/battery/Parameters_0_1.h>
-#include <reg/drone/service/battery/Status_0_1.h>
-
 #include "CanardInterface.hpp"
 
 #include "Publishers/Publisher.hpp"
 #include "Publishers/Gnss.hpp"
 
-#include "Subscribers/BaseSubscriber.hpp"
-#include "Subscribers/Battery.hpp"
-#include "Subscribers/Esc.hpp"
-#include "Subscribers/Gnss.hpp"
-#include "Subscribers/NodeIDAllocationData.hpp"
-#include "Subscribers/LegacyBatteryInfo.hpp"
-
-// uORB over UAVCAN subscribers
-#include "Subscribers/uORB/sensor_gps.hpp"
-
-#include "ServiceClients/GetInfo.hpp"
-#include "ServiceClients/Access.hpp"
-
-#include "Services/AccessReply.hpp"
-#include "Services/ListReply.hpp"
-
 #include "NodeManager.hpp"
 
+#include "SubscriptionManager.hpp"
+
 #include "Actuators/EscClient.hpp" /// TODO: Add EscServer.hpp for node-side service
 
 /**
@@ -184,8 +159,6 @@ private:
 
 	pthread_mutex_t _node_mutex;
 
-	CanardRxSubscription _heartbeat_subscription;
-
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
@@ -199,15 +172,15 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,
 		(ParamInt<px4::params::UAVCAN_V1_ID>) _param_uavcan_v1_id,
-		(ParamInt<px4::params::UAVCAN_V1_BAUD>) _param_uavcan_v1_baud,
-		(ParamInt<px4::params::UAVCAN_V1_BAT_MD>) _param_uavcan_v1_bat_md,
-		(ParamInt<px4::params::UAVCAN_V1_BAT_ID>) _param_uavcan_v1_bat_id
+		(ParamInt<px4::params::UAVCAN_V1_BAUD>) _param_uavcan_v1_baud
 	)
 
 	UavcanParamManager _param_manager;
 
 	NodeManager _node_manager {_canard_instance, _param_manager};
 
+	SubscriptionManager _sub_manager {_canard_instance, _param_manager};
+
 	UavcanGnssPublisher _gps_pub {_canard_instance, _param_manager};
 
 	UavcanEscController _esc_controller {_canard_instance, _param_manager};
@@ -216,26 +189,6 @@ private:
 	/// TODO: For some service implementations, it makes sense to have them be both Publishers and Subscribers
 	UavcanPublisher *_publishers[2] {&_gps_pub, &_esc_controller};
 
-	UavcanGnssSubscriber _gps0_sub {_canard_instance, _param_manager, 0};
-	UavcanGnssSubscriber _gps1_sub {_canard_instance, _param_manager, 1};
-	UavcanBmsSubscriber  _bms0_sub {_canard_instance, _param_manager, 0};
-	//UavcanBmsSubscriber  _bms1_sub {_canard_instance, _param_manager, 1};
-	UavcanEscSubscriber  _esc_sub  {_canard_instance, _param_manager, 0};
-	UavcanLegacyBatteryInfoSubscriber  _legacybms_sub {_canard_instance, _param_manager, 0};
-	UavcanNodeIDAllocationDataSubscriber _nodeid_sub {_canard_instance, _node_manager};
-
-	UORB_over_UAVCAN_sensor_gps_Subscriber _sensor_gps_sub {_canard_instance, _param_manager, 0};
-
-	UavcanGetInfoResponse _getinfo_rsp {_canard_instance};
-	UavcanAccessResponse  _access_rsp {_canard_instance, _param_manager};
-
-	UavcanAccessServiceReply _access_service {_canard_instance, _node_manager};
-	UavcanListServiceReply   _list_service {_canard_instance, _node_manager};
-
-	// Subscriber objects: Any object used to bridge a UAVCAN message to a uORB message
-	UavcanDynamicPortSubscriber *_dynsubscribers[6] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_esc_sub, &_legacybms_sub, &_sensor_gps_sub}; /// TODO: turn into List<UavcanSubscription*>
-	UavcanBaseSubscriber *_subscribers[5] {&_nodeid_sub, &_getinfo_rsp, &_access_rsp, &_access_service, &_list_service}; /// TODO: turn into List<UavcanSubscription*>
-
 	UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
 
 };
diff --git a/src/drivers/uavcan_v1/libcanard b/src/drivers/uavcan_v1/libcanard
index 55938c51cf..2a11617028 160000
--- a/src/drivers/uavcan_v1/libcanard
+++ b/src/drivers/uavcan_v1/libcanard
@@ -1 +1 @@
-Subproject commit 55938c51cf7da3096679933302693c9dae7a2b33
+Subproject commit 2a116170285fb47fcaae150ad21c2ccde0756a5f
diff --git a/src/drivers/uavcan_v1/parameters.c b/src/drivers/uavcan_v1/parameters.c
index f371df84d0..f1241fc761 100644
--- a/src/drivers/uavcan_v1/parameters.c
+++ b/src/drivers/uavcan_v1/parameters.c
@@ -66,43 +66,106 @@ PARAM_DEFINE_INT32(UAVCAN_V1_ID, 1);
  */
 PARAM_DEFINE_INT32(UAVCAN_V1_BAUD, 1000000);
 
-/**
- * UAVCAN v1 battery mode.
- *
- * @value 0 Disable
- * @value 1 Receive BMSStatus
- * @value 2 Send BMSStatus
- * @reboot_required true
- * @group UAVCAN v1
- */
-PARAM_DEFINE_INT32(UAVCAN_V1_BAT_MD, 0);
+/* Subscription port ID, -1 will be treated as unset */
 
 /**
- * UAVCAN v1 battery port ID.
+ * ESC0 port ID.
  *
- * @min 1
- * @max 32767
+ * @min -1
+ * @max 6143
  * @group UAVCAN v1
  */
-PARAM_DEFINE_INT32(UAVCAN_V1_BAT_ID, 4242);
+PARAM_DEFINE_INT32(UCAN1_ESC0_PID, -1);
 
-// Subscription Port IDs
-PARAM_DEFINE_INT32(UCAN1_ESC0_PID, 0);
-PARAM_DEFINE_INT32(UCAN1_GPS0_PID, 0);
-PARAM_DEFINE_INT32(UCAN1_GPS1_PID, 0);
-//PARAM_DEFINE_INT32(UCAN1_BMS0_PID, 0);
-//PARAM_DEFINE_INT32(UCAN1_BMS1_PID, 0);
+/**
+ * GPS 0 port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UCAN1_GPS0_PID, -1);
 
-PARAM_DEFINE_INT32(UCAN1_BMS_ES_PID, 0);
-PARAM_DEFINE_INT32(UCAN1_BMS_BS_PID, 0);
-PARAM_DEFINE_INT32(UCAN1_BMS_BP_PID, 0);
+/**
+ * GPS 1 port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UCAN1_GPS1_PID, -1);
 
-PARAM_DEFINE_INT32(UCAN1_LG_BMS_PID, 0);
+/**
+ * DS-015 battery energy source port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UCAN1_BMS_ES_PID, -1);
+
+/**
+ * DS-015 battery status port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UCAN1_BMS_BS_PID, -1);
+
+/**
+ * DS-015 battery parameters port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UCAN1_BMS_BP_PID, -1);
+
+/**
+ * UAVCAN v1 leagcy battery port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UCAN1_LG_BMS_PID, -1);
+
+
+/**
+ * sensor_gps uORB over UAVCAN v1 port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UCAN1_UORB_GPS, -1);
 
 // Publication Port IDs
-PARAM_DEFINE_INT32(UCAN1_ESC_PUB, 0);
-PARAM_DEFINE_INT32(UCAN1_GPS_PUB, 0);
-PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, 0);
 
-// uORB over UAVCAN
-PARAM_DEFINE_INT32(UCAN1_UORB_GPS, 0);
+/**
+ * UAVCAN v1 ESC port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1);
+
+/**
+ * UAVCAN v1 GPS port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UCAN1_GPS_PUB, -1);
+
+/**
+ * UAVCAN v1 Servo port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group UAVCAN v1
+ */
+PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, -1);