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};