From c4ab2797ebdc612b5e3f60be76d36a119e6050a5 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Sun, 7 Mar 2021 22:04:11 +0100 Subject: [PATCH] Added the notion of BaseSubscriber which allows to subsscription for services responses and request and helps the usage of fixed port subscribers Furthermore move register autconfigure logic from Uavcan.cpp to NodeManager --- src/drivers/uavcan_v1/NodeManager.cpp | 55 +++- src/drivers/uavcan_v1/NodeManager.hpp | 41 ++- .../uavcan_v1/ServiceClients/Access.hpp | 134 ++++++++++ .../uavcan_v1/ServiceClients/GetInfo.hpp | 147 +++++++++++ .../uavcan_v1/Services/AccessReply.hpp | 82 ++++++ .../uavcan_v1/Services/AccessRequest.hpp | 94 +++++++ src/drivers/uavcan_v1/Services/ListReply.hpp | 90 +++++++ .../uavcan_v1/Services/ListRequest.hpp | 88 +++++++ .../uavcan_v1/Subscribers/BaseSubscriber.hpp | 80 ++++++ src/drivers/uavcan_v1/Subscribers/Battery.hpp | 6 +- ...bscriber.hpp => DynamicPortSubscriber.hpp} | 35 +-- src/drivers/uavcan_v1/Subscribers/Esc.hpp | 6 +- src/drivers/uavcan_v1/Subscribers/Gnss.hpp | 6 +- .../Subscribers/NodeIDAllocationData.hpp | 8 +- src/drivers/uavcan_v1/Uavcan.cpp | 241 +----------------- src/drivers/uavcan_v1/Uavcan.hpp | 19 +- 16 files changed, 843 insertions(+), 289 deletions(-) create mode 100644 src/drivers/uavcan_v1/ServiceClients/Access.hpp create mode 100644 src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp create mode 100644 src/drivers/uavcan_v1/Services/AccessReply.hpp create mode 100644 src/drivers/uavcan_v1/Services/AccessRequest.hpp create mode 100644 src/drivers/uavcan_v1/Services/ListReply.hpp create mode 100644 src/drivers/uavcan_v1/Services/ListRequest.hpp create mode 100644 src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp rename src/drivers/uavcan_v1/Subscribers/{Subscriber.hpp => DynamicPortSubscriber.hpp} (73%) diff --git a/src/drivers/uavcan_v1/NodeManager.cpp b/src/drivers/uavcan_v1/NodeManager.cpp index 8f11d7ed90..f337935aed 100644 --- a/src/drivers/uavcan_v1/NodeManager.cpp +++ b/src/drivers/uavcan_v1/NodeManager.cpp @@ -48,7 +48,7 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg) msg.allocated_node_id.elements[0].value = CANARD_NODE_ID_UNSET; /* Search for an available NodeID to assign */ - for (uint32_t i = 1; i < 16; i++) { + for (uint32_t i = 1; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) { if (i == _canard_instance.node_id) { continue; // Don't give our NodeID to a node @@ -65,11 +65,6 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg) 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_]; @@ -93,6 +88,8 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg) result = canardTxPush(&_canard_instance, &transfer); } + _register_request_last = hrt_absolute_time(); + return result >= 0; } } @@ -106,3 +103,49 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg) //TODO V2 CAN FD implementation return false; } + + +void NodeManager::HandleListResponse(CanardNodeID node_id, uavcan_register_List_Response_1_0 &msg) +{ + 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) { + 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) { + nodeid_registry[i].register_index++; // Increment index counter for next update() + nodeid_registry[i].register_setup = false; + } + } + + + if (strncmp((char *)msg.name.name.elements, gps_uorb_register_name, + msg.name.name.count) == 0) { + _access_request.setPortId(node_id, gps_uorb_register_name, 1235); //TODO configurable and combine with ParamManager. + + } else if (strncmp((char *)msg.name.name.elements, bms_status_register_name, + msg.name.name.count) == 0) { //Battery status publisher + _access_request.setPortId(node_id, bms_status_register_name, 1234); //TODO configurable and combine with ParamManager. + } + } +} +void NodeManager::update() +{ + if (hrt_elapsed_time(&_register_request_last) >= hrt_abstime(2 * + 1000000ULL)) { // Compiler hates me here, some 1_s doesn't work + 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); + nodeid_registry[i].register_setup = true; + } + } + + _register_request_last = hrt_absolute_time(); + } +} diff --git a/src/drivers/uavcan_v1/NodeManager.hpp b/src/drivers/uavcan_v1/NodeManager.hpp index bcd3fcf898..37ae76b0cb 100644 --- a/src/drivers/uavcan_v1/NodeManager.hpp +++ b/src/drivers/uavcan_v1/NodeManager.hpp @@ -43,6 +43,7 @@ #include +#include #include "CanardInterface.hpp" @@ -50,30 +51,46 @@ #include #include + +class NodeManager; + +#include "Services/AccessRequest.hpp" +#include "Services/ListRequest.hpp" + +//TODO make this an object instead? typedef struct { - uint8_t node_id; - uint8_t unique_id[16]; -} UavcanNodeUniqueID; + uint8_t node_id; + uint8_t unique_id[16]; + bool register_setup; + uint16_t register_index; +} UavcanNodeEntry; class NodeManager { public: - NodeManager(CanardInstance &ins) : _canard_instance(ins) { }; + NodeManager(CanardInstance &ins) : _canard_instance(ins), _access_request(ins), _list_request(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}; + void HandleListResponse(CanardNodeID node_id, uavcan_register_List_Response_1_0 &msg); + + void update(); private: CanardInstance &_canard_instance; CanardTransferID _uavcan_pnp_nodeidallocation_v1_transfer_id{0}; - UavcanNodeUniqueID nodeid_registry[16] {0}; //TODO configurable or just rewrite + UavcanNodeEntry nodeid_registry[16] {0}; //TODO configurable or just rewrite + + UavcanAccessServiceRequest _access_request; + UavcanListServiceRequest _list_request; + + bool nodeRegisterSetup = 0; + + hrt_abstime _register_request_last{0}; + + //TODO work this out + const char *gps_uorb_register_name = "uavcan.pub.gnss_uorb.id"; + const char *bms_status_register_name = "uavcan.pub.battery_status.id"; }; diff --git a/src/drivers/uavcan_v1/ServiceClients/Access.hpp b/src/drivers/uavcan_v1/ServiceClients/Access.hpp new file mode 100644 index 0000000000..9c64cc4847 --- /dev/null +++ b/src/drivers/uavcan_v1/ServiceClients/Access.hpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * 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 Access.hpp + * + * Defines response to a Access request + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include +#include + +#include "../ParamManager.hpp" + +#include +#include + +#include "../Subscribers/BaseSubscriber.hpp" + +class UavcanAccessResponse : public UavcanBaseSubscriber +{ +public: + UavcanAccessResponse(CanardInstance &ins, UavcanParamManager &pmgr) : + UavcanBaseSubscriber(ins, "Access", 0), _param_manager(pmgr) { }; + + void subscribe() override + { + // Subscribe to requests uavcan.pnp.NodeIDAllocationData + canardRxSubscribe(&_canard_instance, + CanardTransferKindRequest, + uavcan_register_Access_1_0_FIXED_PORT_ID_, + uavcan_register_Access_Response_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_canard_sub); + + _port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_; + + }; + + void callback(const CanardTransfer &receive) override + { + PX4_INFO("Access request"); + + uavcan_register_Access_Request_1_0 msg; + uavcan_register_Access_Request_1_0_initialize_(&msg); + + size_t register_in_size_bits = receive.payload_size; + uavcan_register_Access_Request_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, ®ister_in_size_bits); + + int result {0}; + + uavcan_register_Value_1_0 value = msg.value; + uavcan_register_Name_1_0 name = msg.name; + + /// TODO: get/set parameter based on whether empty or not + if (uavcan_register_Value_1_0_is_empty_(&value)) { // Tag Type: uavcan_primitive_Empty_1_0 + // Value is empty -- 'Get' only + result = _param_manager.GetParamByName(name, value) ? 0 : -1; + + } else { + // Set value + result = _param_manager.SetParamByName(name, value) ? 0 : -1; + + } + + /// TODO: Access_Response + uavcan_register_Access_Response_1_0 response {}; + response.value = value; + + uint8_t response_payload_buffer[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + CanardTransfer transfer = { + .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindResponse, + .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET. + .transfer_id = access_response_transfer_id, /// TODO: track register Access _response_ separately? + .payload_size = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &response_payload_buffer, + }; + + result = uavcan_register_Access_Response_1_0_serialize_(&response, response_payload_buffer, &transfer.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++access_response_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = canardTxPush(&_canard_instance, &transfer); + } + + //return result; + + }; + +private: + UavcanParamManager &_param_manager; + CanardTransferID access_response_transfer_id = 0; + +}; diff --git a/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp b/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp new file mode 100644 index 0000000000..bc24004e2e --- /dev/null +++ b/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * 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 GetInfo.hpp + * + * Defines response to a GetInfo request + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include +#include + +#include "../NodeManager.hpp" + +#include +#include + +#include "../Subscribers/BaseSubscriber.hpp" + +class UavcanGetInfoResponse : public UavcanBaseSubscriber +{ +public: + UavcanGetInfoResponse(CanardInstance &ins) : + UavcanBaseSubscriber(ins, "GetInfo", 0) { }; + + void subscribe() override + { + // Subscribe to requests uavcan.pnp.NodeIDAllocationData + canardRxSubscribe(&_canard_instance, + CanardTransferKindRequest, + uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, + uavcan_node_GetInfo_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_canard_sub); + + _port_id = uavcan_node_GetInfo_1_0_FIXED_PORT_ID_; + + }; + + void callback(const CanardTransfer &receive) override + { + PX4_INFO("GetInfo request"); + + // Setup node.GetInfo response + + uavcan_node_GetInfo_Response_1_0 node_info; + + uavcan_node_GetInfo_Response_1_0_initialize_(&node_info); + + node_info.protocol_version.major = 1; + node_info.protocol_version.minor = 0; + +#if defined(BOARD_HAS_VERSIONING) + node_info.hardware_version.major = (uint8_t)px4_board_hw_version(); + node_info.hardware_version.minor = (uint8_t)px4_board_hw_revision(); +#endif + + unsigned fwver = px4_firmware_version(); + node_info.software_version.major = (fwver >> (8 * 3)) & 0xFF; + node_info.software_version.minor = (fwver >> (8 * 2)) & 0xFF; + + node_info.software_vcs_revision_id = px4_firmware_version_binary(); + + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + memcpy(node_info.unique_id, px4_guid, sizeof(node_info.unique_id)); + + //TODO proper name + strncpy((char *)node_info.name.elements, + px4_board_name(), + uavcan_node_GetInfo_Response_1_0_name_ARRAY_CAPACITY_); + + node_info.name.count = strlen(px4_board_name()); + + uint8_t response_payload_buffer[uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + CanardMicrosecond transmission_deadline = hrt_absolute_time() + 1000 * 100; + + CanardTransfer response = { + .timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindResponse, + .port_id = uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = receive.remote_node_id, // Send back to request Node + .transfer_id = getinfo_response_transfer_id, + .payload_size = uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &response_payload_buffer, + }; + + int32_t result = uavcan_node_GetInfo_Response_1_0_serialize_(&node_info, (uint8_t *)&response_payload_buffer, + &response.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++getinfo_response_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = canardTxPush(&_canard_instance, &response); + } + + //TODO proper error handling + if (result < 0) { + // An error has occurred: either an argument is invalid or we've ran out of memory. + // It is possible to statically prove that an out-of-memory will never occur for a given application if the + // heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. + // return -UAVCAN_REGISTER_ERROR_SERIALIZATION; + } + + }; + +private: + CanardTransferID getinfo_response_transfer_id = 0; + +}; diff --git a/src/drivers/uavcan_v1/Services/AccessReply.hpp b/src/drivers/uavcan_v1/Services/AccessReply.hpp new file mode 100644 index 0000000000..38a4d83e1d --- /dev/null +++ b/src/drivers/uavcan_v1/Services/AccessReply.hpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * 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 Access.hpp + * + * Defines a Access Service invoker and process Access responses + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include +#include + +#include "../NodeManager.hpp" + +#include + +#include "../Subscribers/BaseSubscriber.hpp" + +class UavcanAccessServiceReply : public UavcanBaseSubscriber +{ +public: + UavcanAccessServiceReply(CanardInstance &ins, NodeManager &nmgr) : + UavcanBaseSubscriber(ins, "Access", 0), _nmgr(nmgr) { }; + + void subscribe() override + { + // Subscribe to requests uavcan.pnp.NodeIDAllocationData + canardRxSubscribe(&_canard_instance, + CanardTransferKindResponse, + uavcan_register_Access_1_0_FIXED_PORT_ID_, + uavcan_register_Access_Response_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_canard_sub); + + _port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_; + + }; + + void callback(const CanardTransfer &receive) override + { + PX4_INFO("Access response"); + }; + +private: + NodeManager &_nmgr; + +}; diff --git a/src/drivers/uavcan_v1/Services/AccessRequest.hpp b/src/drivers/uavcan_v1/Services/AccessRequest.hpp new file mode 100644 index 0000000000..15b1789c31 --- /dev/null +++ b/src/drivers/uavcan_v1/Services/AccessRequest.hpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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 Access.hpp + * + * Defines a Access Service invoker and process Access responses + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include +#include + +#include + +class UavcanAccessServiceRequest +{ +public: + UavcanAccessServiceRequest(CanardInstance &ins) : + _canard_instance(ins) { }; + + void setPortId(CanardNodeID node_id, const char *register_name, uint16_t port_id) + { + int result {0}; + + uavcan_register_Access_Request_1_0 request_msg; + strncpy((char *)&request_msg.name.name.elements[0], register_name, sizeof(uavcan_register_Name_1_0)); + request_msg.name.name.count = strlen(register_name); + + uavcan_register_Value_1_0_select_natural16_(&request_msg.value); + request_msg.value.natural16.value.count = 1; + request_msg.value.natural16.value.elements[0] = port_id; + + uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + CanardTransfer transfer = { + .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindRequest, + .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = node_id, // Messages cannot be unicast, so use UNSET. + .transfer_id = access_request_transfer_id, + .payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &request_payload_buffer, + }; + + 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); + } + }; + +private: + CanardInstance &_canard_instance; + CanardTransferID access_request_transfer_id = 0; + +}; diff --git a/src/drivers/uavcan_v1/Services/ListReply.hpp b/src/drivers/uavcan_v1/Services/ListReply.hpp new file mode 100644 index 0000000000..1b747da316 --- /dev/null +++ b/src/drivers/uavcan_v1/Services/ListReply.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file List.hpp + * + * Defines a List Service invoker and process List responses + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include +#include + +#include "../NodeManager.hpp" + +#include + +#include "../Subscribers/BaseSubscriber.hpp" + +class UavcanListServiceReply : public UavcanBaseSubscriber +{ +public: + UavcanListServiceReply(CanardInstance &ins, NodeManager &nmgr) : + UavcanBaseSubscriber(ins, "List", 0), _nmgr(nmgr) { }; + + void subscribe() override + { + // Subscribe to requests uavcan.pnp.NodeIDAllocationData + canardRxSubscribe(&_canard_instance, + CanardTransferKindResponse, + uavcan_register_List_1_0_FIXED_PORT_ID_, + uavcan_register_List_Response_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_canard_sub); + + _port_id = uavcan_register_List_1_0_FIXED_PORT_ID_; + + }; + + void callback(const CanardTransfer &receive) override + { + PX4_INFO("List 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, ®ister_in_size_bits); + + // Pass msg.name.name.elements to NodeManager + _nmgr.HandleListResponse(receive.remote_node_id, msg); + }; + +private: + NodeManager &_nmgr; + +}; diff --git a/src/drivers/uavcan_v1/Services/ListRequest.hpp b/src/drivers/uavcan_v1/Services/ListRequest.hpp new file mode 100644 index 0000000000..8a529cdc1c --- /dev/null +++ b/src/drivers/uavcan_v1/Services/ListRequest.hpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * 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 ListRequest.hpp + * + * Defines a List Service invoker and process List responses + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include +#include + +#include + +class UavcanListServiceRequest +{ +public: + UavcanListServiceRequest(CanardInstance &ins) : + _canard_instance(ins) { }; + + + void request(CanardNodeID node_id, uint16_t index) + { + 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 = { + .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .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, + .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 (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); + } + }; + +private: + CanardInstance &_canard_instance; + CanardTransferID list_request_transfer_id = 0; + +}; diff --git a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp new file mode 100644 index 0000000000..1f963197f2 --- /dev/null +++ b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file BaseSubscriber.hpp + * + * Defines basic functionality of UAVCAN v1 subscriber class + * + * @author Jacob Crabill + */ + +#pragma once + +#include + +#include + +#include "../CanardInterface.hpp" + +class UavcanBaseSubscriber +{ +public: + static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U; + + UavcanBaseSubscriber(CanardInstance &ins, const char *subject_name, uint8_t instance = 0) : + _canard_instance(ins), _subject_name(subject_name), _instance(instance) { }; + + virtual void subscribe() = 0; + virtual void unsubscribe() { canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, _port_id); }; + + virtual void callback(const CanardTransfer &msg) = 0; + + CanardPortID id() { return _port_id; }; + + void printInfo() + { + if (_port_id != CANARD_PORT_ID_UNSET) { + PX4_INFO("Subscribed %s.%d on port %d", _subject_name, _instance, _port_id); + } + } + +protected: + CanardInstance &_canard_instance; + CanardRxSubscription _canard_sub; + const char *_subject_name; + uint8_t _instance {0}; + /// TODO: 'type' parameter? uavcan.pub.PORT_NAME.type (see 384.Access.1.0.uavcan) + + CanardPortID _port_id {CANARD_PORT_ID_UNSET}; +}; diff --git a/src/drivers/uavcan_v1/Subscribers/Battery.hpp b/src/drivers/uavcan_v1/Subscribers/Battery.hpp index 3736fef275..fb41ab95e2 100644 --- a/src/drivers/uavcan_v1/Subscribers/Battery.hpp +++ b/src/drivers/uavcan_v1/Subscribers/Battery.hpp @@ -45,13 +45,13 @@ #include #include -#include "Subscriber.hpp" +#include "DynamicPortSubscriber.hpp" -class UavcanBmsSubscriber : public UavcanSubscriber +class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber { public: UavcanBmsSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanSubscriber(ins, pmgr, "bms", instance) { }; + UavcanDynamicPortSubscriber(ins, pmgr, "bms", instance) { }; void subscribe() override { diff --git a/src/drivers/uavcan_v1/Subscribers/Subscriber.hpp b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp similarity index 73% rename from src/drivers/uavcan_v1/Subscribers/Subscriber.hpp rename to src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp index 062348d785..93140fb811 100644 --- a/src/drivers/uavcan_v1/Subscribers/Subscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp @@ -32,9 +32,9 @@ ****************************************************************************/ /** - * @file Subscriber.hpp + * @file DynamicPortSubscriber.hpp * - * Defines basic functionality of UAVCAN v1 subscriber class + * Defines basic functionality of UAVCAN v1 subscriber class with Non-fixed unregulated port identifier * * @author Jacob Crabill */ @@ -47,23 +47,16 @@ #include +#include "DynamicPortSubscriber.hpp" #include "../CanardInterface.hpp" #include "../ParamManager.hpp" -class UavcanSubscriber +class UavcanDynamicPortSubscriber : public UavcanBaseSubscriber { public: - static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U; - - UavcanSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, const char *subject_name, uint8_t instance = 0) : - _canard_instance(ins), _param_manager(pmgr), _subject_name(subject_name), _instance(instance) { }; - - virtual void subscribe() = 0; - virtual void unsubscribe() { canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, _port_id); }; - - virtual void callback(const CanardTransfer &msg) = 0; - - CanardPortID id() { return _port_id; }; + UavcanDynamicPortSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, const char *subject_name, + uint8_t instance = 0) : + UavcanBaseSubscriber(ins, subject_name, instance), _param_manager(pmgr) { }; void updateParam() { @@ -94,20 +87,6 @@ public: } }; - void printInfo() - { - if (_port_id != CANARD_PORT_ID_UNSET) { - PX4_INFO("Subscribed %s.%d on port %d", _subject_name, _instance, _port_id); - } - } - protected: - CanardInstance &_canard_instance; UavcanParamManager &_param_manager; - CanardRxSubscription _canard_sub; - const char *_subject_name; - uint8_t _instance {0}; - /// TODO: 'type' parameter? uavcan.pub.PORT_NAME.type (see 384.Access.1.0.uavcan) - - CanardPortID _port_id {CANARD_PORT_ID_UNSET}; }; diff --git a/src/drivers/uavcan_v1/Subscribers/Esc.hpp b/src/drivers/uavcan_v1/Subscribers/Esc.hpp index 2d8edf13bb..e323a058e0 100644 --- a/src/drivers/uavcan_v1/Subscribers/Esc.hpp +++ b/src/drivers/uavcan_v1/Subscribers/Esc.hpp @@ -49,13 +49,13 @@ #include #include -#include "Subscriber.hpp" +#include "DynamicPortSubscriber.hpp" -class UavcanEscSubscriber : public UavcanSubscriber +class UavcanEscSubscriber : public UavcanDynamicPortSubscriber { public: UavcanEscSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanSubscriber(ins, pmgr, "esc", instance) { }; + UavcanDynamicPortSubscriber(ins, pmgr, "esc", instance) { }; void subscribe() override { diff --git a/src/drivers/uavcan_v1/Subscribers/Gnss.hpp b/src/drivers/uavcan_v1/Subscribers/Gnss.hpp index 183feabb45..83c22cffdf 100644 --- a/src/drivers/uavcan_v1/Subscribers/Gnss.hpp +++ b/src/drivers/uavcan_v1/Subscribers/Gnss.hpp @@ -44,13 +44,13 @@ // DS-15 Specification Messages #include -#include "Subscriber.hpp" +#include "DynamicPortSubscriber.hpp" -class UavcanGnssSubscriber : public UavcanSubscriber +class UavcanGnssSubscriber : public UavcanDynamicPortSubscriber { public: UavcanGnssSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanSubscriber(ins, pmgr, "gps", instance) { }; + UavcanDynamicPortSubscriber(ins, pmgr, "gps", instance) { }; void subscribe() override { diff --git a/src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp b/src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp index 6d35d9b3f7..0505eb5cc9 100644 --- a/src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp +++ b/src/drivers/uavcan_v1/Subscribers/NodeIDAllocationData.hpp @@ -53,13 +53,13 @@ #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" +#include "BaseSubscriber.hpp" -class UavcanNodeIDAllocationDataSubscriber : public UavcanSubscriber +class UavcanNodeIDAllocationDataSubscriber : public UavcanBaseSubscriber { public: - UavcanNodeIDAllocationDataSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, NodeManager &nmgr) : - UavcanSubscriber(ins, pmgr, "NodeIDAllocationData", 0), _nmgr(nmgr) { }; + UavcanNodeIDAllocationDataSubscriber(CanardInstance &ins, NodeManager &nmgr) : + UavcanBaseSubscriber(ins, "NodeIDAllocationData", 0), _nmgr(nmgr) { }; void subscribe() override { diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index 4abf99f7cf..6437312a7d 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -87,7 +87,7 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) : publisher->updateParam(); } - for (auto &subscriber : _subscribers) { + for (auto &subscriber : _dynsubscribers) { subscriber->updateParam(); } @@ -168,20 +168,6 @@ void UavcanNode::init() CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &_heartbeat_subscription); - canardRxSubscribe(&_canard_instance, - CanardTransferKindResponse, - uavcan_register_Access_1_0_FIXED_PORT_ID_, - uavcan_register_Access_Response_1_0_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_register_access_subscription); - - canardRxSubscribe(&_canard_instance, - CanardTransferKindResponse, - uavcan_register_List_1_0_FIXED_PORT_ID_, - uavcan_register_List_Response_1_0_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_register_list_subscription); - canardRxSubscribe(&_canard_instance, CanardTransferKindMessage, bms_port_id, @@ -196,6 +182,9 @@ void UavcanNode::init() CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &_drone_srv_gps_subscription); + for (auto &subscriber : _subscribers) { + subscriber->subscribe(); + } _initialized = true; } @@ -231,7 +220,7 @@ void UavcanNode::Run() publisher->updateParam(); } - for (auto &subscriber : _subscribers) { + 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 @@ -246,40 +235,6 @@ void UavcanNode::Run() perf_begin(_cycle_perf); perf_count(_interval_perf); - 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_manager._node_register_setup, - _node_manager._node_register_request_index); - - uavcan_register_List_Request_1_0 msg; - msg.index = _node_manager._node_register_request_index; - - uint8_t request_payload_buffer[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - - CanardTransfer request = { - .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindRequest, - .port_id = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID. - .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, - }; - - int32_t result = uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &request.payload_size); - - if (result == 0) { - // set the data ready in the buffer and chop if needed - ++_uavcan_register_list_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &request); - - ++_node_manager._node_register_request_index; - } - } - // send uavcan::node::Heartbeat_1_0 @ 1 Hz sendHeartbeat(); @@ -288,6 +243,8 @@ void UavcanNode::Run() publisher->update(); } + _node_manager.update(); + // Transmitting // Look at the top of the TX queue. for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) { @@ -340,13 +297,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_register_List_1_0_FIXED_PORT_ID_) { - result = handleRegisterList(receive); - - } else if (receive.port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) { - result = handleRegisterAccess(receive); - - } else if (receive.port_id == bms_port_id) { + if (receive.port_id == bms_port_id) { result = handleBMSStatus(receive); } else if (receive.port_id == gps_port_id) { @@ -359,6 +310,12 @@ void UavcanNode::Run() subscriber->callback(receive); } } + + for (auto &subscriber : _dynsubscribers) { + if (receive.port_id == subscriber->id()) { + subscriber->callback(receive); + } + } } // Deallocate the dynamic memory afterwards. @@ -492,176 +449,6 @@ void UavcanNode::sendHeartbeat() } } -int UavcanNode::handleRegisterList(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, ®ister_in_size_bits); - - int result; - - if (strncmp((char *)msg.name.name.elements, "uavcan.pub.gnss_uorb.id", - msg.name.name.count) == 0) { //Demo GPS status publisher - _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_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)); - - uavcan_register_Value_1_0_select_natural16_(&request_msg.value); - request_msg.value.natural16.value.count = 1; - request_msg.value.natural16.value.elements[0] = gps_port_id; - - - uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindRequest, - .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID. - .remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET. - .transfer_id = _uavcan_register_access_request_transfer_id, - .payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &request_payload_buffer, - }; - - 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 - ++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &transfer); - } - - } else if (strncmp((char *)msg.name.name.elements, "uavcan.pub.battery_status.id", - msg.name.name.count) == 0) { //Battery status publisher - _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_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)); - - uavcan_register_Value_1_0_select_natural16_(&request_msg.value); - request_msg.value.natural16.value.count = 1; - request_msg.value.natural16.value.elements[0] = bms_port_id; - - uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindRequest, - .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID. - .remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET. - .transfer_id = _uavcan_register_access_request_transfer_id, - .payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &request_payload_buffer, - }; - - 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 - ++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &transfer); - } - - } else { - uavcan_register_Value_1_0 out_value; - _node_manager._node_register_last_received_index++; - _node_manager._uavcan_pnp_nodeidallocation_last = hrt_absolute_time(); // Reset timer for next request - - if (_param_manager.GetParamByName(msg.name, out_value)) { - _node_manager._node_register_setup = CANARD_NODE_ID_UNSET; - - uavcan_register_Access_Request_1_0 request_msg; - memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0)); - - uavcan_register_Value_1_0_select_natural16_(&request_msg.value); /// TODO don't select; see which it is - request_msg.value.natural16.value.count = 1; - request_msg.value.natural16.value.elements[0] = out_value.natural16.value.elements[0]; /// TODO - - uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindRequest, - .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID. - .remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET. - .transfer_id = _uavcan_register_access_request_transfer_id, - .payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &request_payload_buffer, - }; - - 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 - ++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &transfer); - } - } - } - - return result; -} - -int UavcanNode::handleRegisterAccess(const CanardTransfer &receive) -{ - uavcan_register_Access_Request_1_0 msg {}; - - size_t register_in_size_bits = receive.payload_size; - uavcan_register_Access_Request_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, ®ister_in_size_bits); - - int result {0}; - - uavcan_register_Value_1_0 value = msg.value; - uavcan_register_Name_1_0 name = msg.name; - - /// TODO: get/set parameter based on whether empty or not - if (uavcan_register_Value_1_0_is_empty_(&value)) { // Tag Type: uavcan_primitive_Empty_1_0 - // Value is empty -- 'Get' only - result = _param_manager.GetParamByName(name, value) ? 0 : -1; - - } else { - // Set value - result = _param_manager.SetParamByName(name, value) ? 0 : -1; - - } - - /// TODO: Access_Response - uavcan_register_Access_Response_1_0 response {}; - response.value = value; - - uint8_t response_payload_buffer[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindResponse, - .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID. - .remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET. - .transfer_id = _uavcan_register_access_request_transfer_id, /// TODO: track register Access _response_ separately? - .payload_size = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &response_payload_buffer, - }; - - result = uavcan_register_Access_Response_1_0_serialize_(&response, response_payload_buffer, &transfer.payload_size); - - if (result == 0) { - // set the data ready in the buffer and chop if needed - ++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &transfer); - } - - return result; -} - int UavcanNode::handleBMSStatus(const CanardTransfer &receive) { PX4_INFO("NodeID %i Battery Status msg", receive.remote_node_id); diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index e2d794403e..6ca66b4d2e 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -71,12 +71,18 @@ #include "Publishers/Publisher.hpp" #include "Publishers/Gnss.hpp" -#include "Subscribers/Subscriber.hpp" +#include "Subscribers/BaseSubscriber.hpp" #include "Subscribers/Battery.hpp" #include "Subscribers/Esc.hpp" #include "Subscribers/Gnss.hpp" #include "Subscribers/NodeIDAllocationData.hpp" +#include "ServiceClients/GetInfo.hpp" +#include "ServiceClients/Access.hpp" + +#include "Services/AccessReply.hpp" +#include "Services/ListReply.hpp" + #include "NodeManager.hpp" #include "Actuators/EscClient.hpp" /// TODO: Add EscServer.hpp for node-side service @@ -239,10 +245,17 @@ 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}; + UavcanNodeIDAllocationDataSubscriber _nodeid_sub {_canard_instance, _node_manager}; + + 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 - UavcanSubscriber *_subscribers[6] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub, &_nodeid_sub}; /// TODO: turn into List + UavcanDynamicPortSubscriber *_dynsubscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub}; /// TODO: turn into List + UavcanBaseSubscriber *_subscribers[5] {&_nodeid_sub, &_getinfo_rsp, &_access_rsp, &_access_service, &_list_service}; /// TODO: turn into List UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};