diff --git a/src/drivers/uavcan_v1/CMakeLists.txt b/src/drivers/uavcan_v1/CMakeLists.txt index 50f8b038bf..f1eed4a12e 100644 --- a/src/drivers/uavcan_v1/CMakeLists.txt +++ b/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} diff --git a/src/drivers/uavcan_v1/NodeManager.cpp b/src/drivers/uavcan_v1/NodeManager.cpp new file mode 100644 index 0000000000..841c09188e --- /dev/null +++ b/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 + */ + +#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; +} diff --git a/src/drivers/uavcan_v1/NodeManager.hpp b/src/drivers/uavcan_v1/NodeManager.hpp new file mode 100644 index 0000000000..bcd3fcf898 --- /dev/null +++ b/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 + */ + +#pragma once + + +#include + +#include "CanardInterface.hpp" + +#include +#include +#include + +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 +}; diff --git a/src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp b/src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp new file mode 100644 index 0000000000..6d35d9b3f7 --- /dev/null +++ b/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 + */ + +#pragma once + +#include "../NodeManager.hpp" + +//Quick and Dirty PNP imlementation only V1 for now as well +#include +#include +#include + +#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; + +}; diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index d22f6b4753..5d0dca0b9d 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/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 && - _node_register_setup != CANARD_NODE_ID_UNSET && - _node_register_request_index == _node_register_last_received_index + 1) { + if (hrt_elapsed_time(&_node_manager._uavcan_pnp_nodeidallocation_last) >= 1_s && + _node_manager._node_register_setup != CANARD_NODE_ID_UNSET && + _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_) { - result = handlePnpNodeIDAllocationData(receive); - - } else if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) { + 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_register_last_received_index++; + _node_manager._node_register_setup = CANARD_NODE_ID_UNSET; + _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)); diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index 8a23589781..e2d794403e 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -61,21 +61,11 @@ #include #include -//Quick and Dirty PNP imlementation only V1 for now as well -#include -#include -#include - // DS-15 Specification Messages #include #include #include -#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 + UavcanSubscriber *_subscribers[6] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub, &_nodeid_sub}; /// TODO: turn into List UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller}; + };