UAVCANv1 Move PNP from UAVCAN.cpp to NodeManager.cpp

src/drivers/uavcan_v1/CMakeLists.txt

@@ -74,6 +74,8 @@ px4_add_module(
 	SRCS
 		ParamManager.hpp
 		ParamManager.cpp
+		NodeManager.hpp
+		NodeManager.cpp
 		Uavcan.cpp
 		Uavcan.hpp
 		${SRCS}

src/drivers/uavcan_v1/NodeManager.cpp

@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   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 NodeIDManager.cpp
+ *
+ * Defines basic implementation of UAVCAN PNP for dynamic Node ID
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#include "NodeManager.hpp"
+
+bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
+{
+	if (msg.allocated_node_id.count == 0) {
+		msg.allocated_node_id.count = 1;
+		msg.allocated_node_id.elements[0].value = CANARD_NODE_ID_UNSET;
+
+		/* Search for an available NodeID to assign */
+		if (msg.allocated_node_id.elements[0].value == CANARD_NODE_ID_UNSET) {
+			for (uint32_t i = 1; i < 16; i++) { // Note we're node ID 0
+				if (nodeid_registry[i].node_id == 0) { // Unused
+					nodeid_registry[i].node_id = 1;
+					memcpy(&nodeid_registry[i].unique_id, &msg.unique_id_hash, 6);
+					break;
+
+				} else {
+					if (memcmp(&nodeid_registry[i].unique_id[0], &msg.unique_id_hash, 6) == 0) {
+						msg.allocated_node_id.elements[0].value = nodeid_registry[i].node_id; // Existing NodeID
+						break;
+					}
+				}
+			}
+		}
+
+		if (msg.allocated_node_id.elements[0].value != CANARD_NODE_ID_UNSET) {
+
+			_uavcan_pnp_nodeidallocation_last = hrt_absolute_time();
+			_node_register_request_index = 0;
+			_node_register_last_received_index = -1;
+			_node_register_setup = msg.allocated_node_id.elements[0].value; // This nodeID has to be configured
+
+			PX4_INFO("Received NodeID allocation request assigning %i", msg.allocated_node_id.elements[0].value);
+
+			uint8_t node_id_alloc_payload_buffer[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+			CanardTransfer transfer = {
+				.timestamp_usec = hrt_absolute_time(),      // Zero if transmission deadline is not limited.
+				.priority       = CanardPriorityNominal,
+				.transfer_kind  = CanardTransferKindMessage,
+				.port_id        = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_,                // This is the subject-ID.
+				.remote_node_id = CANARD_NODE_ID_UNSET,       // Messages cannot be unicast, so use UNSET.
+				.transfer_id    = _uavcan_pnp_nodeidallocation_v1_transfer_id,
+				.payload_size   = uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
+				.payload        = &node_id_alloc_payload_buffer,
+			};
+
+			int result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &transfer.payload_size);
+
+			if (result == 0) {
+				// set the data ready in the buffer and chop if needed
+				++_uavcan_pnp_nodeidallocation_v1_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+				result = canardTxPush(&_canard_instance, &transfer);
+			}
+
+			return result >= 0;
+		}
+	}
+
+	return false;
+}
+
+
+bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg)
+{
+	//TODO V2 CAN FD implementation
+	return false;
+}

src/drivers/uavcan_v1/NodeManager.hpp

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   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 NodeManager.hpp
+ *
+ * Defines basic implementation of UAVCAN PNP for dynamic Node ID
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+
+#include <px4_platform_common/defines.h>
+
+#include "CanardInterface.hpp"
+
+#include <uavcan/node/ID_1_0.h>
+#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
+#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
+
+typedef struct {
+	uint8_t  node_id;
+	uint8_t  unique_id[16];
+} UavcanNodeUniqueID;
+
+class NodeManager
+{
+public:
+	NodeManager(CanardInstance &ins) : _canard_instance(ins) { };
+
+	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg);
+	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg);
+
+
+	/* TODO temporary store variables here to not break the existing code
+	 * Ideally we implement service/request classes as well and put the logic
+	 * to set registers in here as well */
+	uint8_t _node_register_setup = CANARD_NODE_ID_UNSET;
+	int32_t _node_register_request_index = 0;
+	int32_t _node_register_last_received_index = -1;
+	hrt_abstime _uavcan_pnp_nodeidallocation_last{0};
+
+private:
+	CanardInstance &_canard_instance;
+	CanardTransferID _uavcan_pnp_nodeidallocation_v1_transfer_id{0};
+	UavcanNodeUniqueID nodeid_registry[16] {0}; //TODO configurable or just rewrite
+};

src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   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 "Subscriber.hpp"
+
+class UavcanNodeIDAllocationDataSubscriber : public UavcanSubscriber
+{
+public:
+	UavcanNodeIDAllocationDataSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, NodeManager &nmgr) :
+		UavcanSubscriber(ins, pmgr, "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,
+				  &_canard_sub);
+
+		_port_id = _canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID;
+
+	};
+
+	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;
+
+};

src/drivers/uavcan_v1/Uavcan.cpp

@@ -168,14 +168,6 @@ void UavcanNode::init()
 					  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
 					  &_heartbeat_subscription);
 
-			// Subscribe to messages uavcan.node.NodeIDAllocationData_1_0 for PNP V1
-			canardRxSubscribe(&_canard_instance,
-					  CanardTransferKindMessage,
-					  uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_,
-					  uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_,
-					  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
-					  &_pnp_v1_subscription);
-
 			canardRxSubscribe(&_canard_instance,
 					  CanardTransferKindResponse,
 					  uavcan_register_Access_1_0_FIXED_PORT_ID_,
@@ -190,8 +182,6 @@ void UavcanNode::init()
 					  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
 					  &_register_list_subscription);
 
-
-
 			canardRxSubscribe(&_canard_instance,
 					  CanardTransferKindMessage,
 					  bms_port_id,
@@ -256,14 +246,15 @@ void UavcanNode::Run()
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);
 
-	if (hrt_elapsed_time(&_uavcan_pnp_nodeidallocation_last) >= 1_s &&
+	if (hrt_elapsed_time(&_node_manager._uavcan_pnp_nodeidallocation_last) >= 1_s &&
-	    _node_register_setup != CANARD_NODE_ID_UNSET &&
+	    _node_manager._node_register_setup != CANARD_NODE_ID_UNSET &&
-	    _node_register_request_index == _node_register_last_received_index + 1) {
+	    _node_manager._node_register_request_index == _node_manager._node_register_last_received_index + 1) {
 
-		PX4_INFO("NodeID %i request register %i", _node_register_setup, _node_register_request_index);
+		PX4_INFO("NodeID %i request register %i", _node_manager._node_register_setup,
+			 _node_manager._node_register_request_index);
 
 		uavcan_register_List_Request_1_0 msg;
-		msg.index = _node_register_request_index;
+		msg.index = _node_manager._node_register_request_index;
 
 		uint8_t request_payload_buffer[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
 
@@ -272,7 +263,7 @@ void UavcanNode::Run()
 			.priority       = CanardPriorityNominal,
 			.transfer_kind  = CanardTransferKindRequest,
 			.port_id        = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID.
-			.remote_node_id = _node_register_setup,
+			.remote_node_id = _node_manager._node_register_setup,
 			.transfer_id    = _uavcan_register_list_request_transfer_id,
 			.payload_size   = uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
 			.payload        = &request_payload_buffer,
@@ -285,7 +276,7 @@ void UavcanNode::Run()
 			++_uavcan_register_list_request_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
 			result = canardTxPush(&_canard_instance, &request);
 
-			++_node_register_request_index;
+			++_node_manager._node_register_request_index;
 		}
 	}
 
@@ -349,10 +340,7 @@ void UavcanNode::Run()
 			// A transfer has been received, process it.
 			// PX4_INFO("received Port ID: %d", receive.port_id);
 
-			if (receive.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) {
+			if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
-				result = handlePnpNodeIDAllocationData(receive);
-
-			} else if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
 				result = handleRegisterList(receive);
 
 			} else if (receive.port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) {
@@ -504,48 +492,6 @@ void UavcanNode::sendHeartbeat()
 	}
 }
 
-int UavcanNode::handlePnpNodeIDAllocationData(const CanardTransfer &receive)
-{
-	uavcan_pnp_NodeIDAllocationData_1_0 msg;
-
-	size_t pnp_in_size_bits = receive.payload_size;
-	uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &pnp_in_size_bits);
-
-	//TODO internal database with unique id to node ip mappings now we give an hardcoded ID back
-	msg.allocated_node_id.count = 1;
-	msg.allocated_node_id.elements[0].value = 15; // HACK hardcoded ID
-
-	_uavcan_pnp_nodeidallocation_last = hrt_absolute_time();
-	_node_register_request_index = 0;
-	_node_register_last_received_index = -1;
-	_node_register_setup = msg.allocated_node_id.elements[0].value; // This nodeID has to be configured
-
-	PX4_INFO("Received NodeID allocation request assigning %i", msg.allocated_node_id.elements[0].value);
-
-	uint8_t node_id_alloc_payload_buffer[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
-
-	CanardTransfer transfer = {
-		.timestamp_usec = hrt_absolute_time(),      // Zero if transmission deadline is not limited.
-		.priority       = CanardPriorityNominal,
-		.transfer_kind  = CanardTransferKindMessage,
-		.port_id        = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_,                // This is the subject-ID.
-		.remote_node_id = CANARD_NODE_ID_UNSET,       // Messages cannot be unicast, so use UNSET.
-		.transfer_id    = _uavcan_pnp_nodeidallocation_v1_transfer_id,
-		.payload_size   = uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
-		.payload        = &node_id_alloc_payload_buffer,
-	};
-
-	int result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &transfer.payload_size);
-
-	if (result == 0) {
-		// set the data ready in the buffer and chop if needed
-		++_uavcan_pnp_nodeidallocation_v1_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
-		result = canardTxPush(&_canard_instance, &transfer);
-	}
-
-	return result;
-}
-
 int UavcanNode::handleRegisterList(const CanardTransfer &receive)
 {
 	uavcan_register_List_Response_1_0 msg;
@@ -557,9 +503,9 @@ int UavcanNode::handleRegisterList(const CanardTransfer &receive)
 
 	if (strncmp((char *)msg.name.name.elements, "uavcan.pub.gnss_uorb.id",
 		    msg.name.name.count) == 0) { //Demo GPS status publisher
-		_node_register_setup = CANARD_NODE_ID_UNSET;
+		_node_manager._node_register_setup = CANARD_NODE_ID_UNSET;
 		PX4_INFO("NodeID %i GPS publisher set PortID to %i", receive.remote_node_id, gps_port_id);
-		_node_register_last_received_index++;
+		_node_manager._node_register_last_received_index++;
 
 		uavcan_register_Access_Request_1_0 request_msg;
 		memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0));
@@ -592,9 +538,9 @@ int UavcanNode::handleRegisterList(const CanardTransfer &receive)
 
 	} else if (strncmp((char *)msg.name.name.elements, "uavcan.pub.battery_status.id",
 			   msg.name.name.count) == 0) { //Battery status publisher
-		_node_register_setup = CANARD_NODE_ID_UNSET;
+		_node_manager._node_register_setup = CANARD_NODE_ID_UNSET;
 		PX4_INFO("NodeID %i battery_status publisher set PortID to %i", receive.remote_node_id, bms_port_id);
-		_node_register_last_received_index++;
+		_node_manager._node_register_last_received_index++;
 
 		uavcan_register_Access_Request_1_0 request_msg;
 		memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0));
@@ -628,8 +574,8 @@ int UavcanNode::handleRegisterList(const CanardTransfer &receive)
 		uavcan_register_Value_1_0 out_value;
 
 		if (_param_manager.GetParamByName(msg.name, out_value)) {
-			_node_register_setup = CANARD_NODE_ID_UNSET;
+			_node_manager._node_register_setup = CANARD_NODE_ID_UNSET;
-			_node_register_last_received_index++;
+			_node_manager._node_register_last_received_index++;
 
 			uavcan_register_Access_Request_1_0 request_msg;
 			memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0));

src/drivers/uavcan_v1/Uavcan.hpp

@@ -61,21 +61,11 @@
 #include <uavcan/node/Heartbeat_1_0.h>
 #include <uavcan/primitive/Empty_1_0.h>
 
-//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>
-
 // 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>
 
-#define PNP1_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_
-#define PNP1_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_
-#define PNP2_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_
-#define PNP2_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_2_0_EXTENT_BYTES_
-
 #include "CanardInterface.hpp"
 
 #include "Publishers/Publisher.hpp"
@@ -85,6 +75,9 @@
 #include "Subscribers/Battery.hpp"
 #include "Subscribers/Esc.hpp"
 #include "Subscribers/Gnss.hpp"
+#include "Subscribers/NodeIDAllocationData.hpp"
+
+#include "NodeManager.hpp"
 
 #include "Actuators/EscClient.hpp" /// TODO: Add EscServer.hpp for node-side service
 
@@ -213,15 +206,8 @@ private:
 	const uint16_t bms_port_id = 1234;
 	const uint16_t gps_port_id = 1235;
 
-	CanardTransferID _uavcan_pnp_nodeidallocation_v1_transfer_id{0};
-	hrt_abstime _uavcan_pnp_nodeidallocation_last{0};
-
 	CanardTransferID _uavcan_register_list_request_transfer_id{0};
 	CanardTransferID _uavcan_register_access_request_transfer_id{0};
-	//Register interface NodeID TODO MVP right have to make a queue
-	uint8_t _node_register_setup = CANARD_NODE_ID_UNSET;
-	int32_t _node_register_request_index = 0;
-	int32_t _node_register_last_received_index = -1;
 
 	// regulated::drone::sensor::BMSStatus_1_0
 	uint8_t _regulated_drone_sensor_bmsstatus_buffer[reg_drone_service_battery_Status_0_1_EXTENT_BYTES_];
@@ -238,6 +224,8 @@ private:
 
 	UavcanParamManager _param_manager;
 
+	NodeManager _node_manager {_canard_instance};
+
 	UavcanGnssPublisher _gps_pub {_canard_instance, _param_manager};
 
 	UavcanEscController _esc_controller {_canard_instance, _param_manager};
@@ -251,9 +239,11 @@ private:
 	UavcanBmsSubscriber  _bms0_sub {_canard_instance, _param_manager, 0};
 	UavcanBmsSubscriber  _bms1_sub {_canard_instance, _param_manager, 1};
 	UavcanEscSubscriber  _esc_sub  {_canard_instance, _param_manager, 0};
+	UavcanNodeIDAllocationDataSubscriber _nodeid_sub {_canard_instance, _param_manager, _node_manager};
 
 	// Subscriber objects: Any object used to bridge a UAVCAN message to a uORB message
-	UavcanSubscriber *_subscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub}; /// TODO: turn into List<UavcanSubscription*>
+	UavcanSubscriber *_subscribers[6] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub, &_nodeid_sub}; /// TODO: turn into List<UavcanSubscription*>
 
 	UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
+
 };