From ed394027b1d8fcabbe87a40f99f2b16a2ba396f6 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Mon, 11 Oct 2021 15:00:53 +0200 Subject: [PATCH] UAVCANv1 Include Kconfig & Fix #18396 --- boards/nxp/fmuk66-v3/socketcan.px4board | 7 + src/drivers/uavcan_v1/CMakeLists.txt | 40 ++++- src/drivers/uavcan_v1/Kconfig | 92 +++++++++- src/drivers/uavcan_v1/NodeClient.cpp | 170 ++++++++++++++++++ src/drivers/uavcan_v1/PublicationManager.hpp | 43 ++++- .../uavcan_v1/ServiceClients/Access.hpp | 4 +- .../uavcan_v1/ServiceClients/GetInfo.hpp | 6 +- .../uavcan_v1/Services/ServiceRequest.hpp | 12 +- src/drivers/uavcan_v1/SubscriptionManager.cpp | 5 +- src/drivers/uavcan_v1/SubscriptionManager.hpp | 43 ++++- src/drivers/uavcan_v1/Uavcan.cpp | 61 ++++++- src/drivers/uavcan_v1/Uavcan.hpp | 12 ++ src/drivers/uavcan_v1/parameters.c | 2 +- 13 files changed, 464 insertions(+), 33 deletions(-) create mode 100644 src/drivers/uavcan_v1/NodeClient.cpp diff --git a/boards/nxp/fmuk66-v3/socketcan.px4board b/boards/nxp/fmuk66-v3/socketcan.px4board index ff4df8af58..abb97157ed 100644 --- a/boards/nxp/fmuk66-v3/socketcan.px4board +++ b/boards/nxp/fmuk66-v3/socketcan.px4board @@ -1,2 +1,9 @@ CONFIG_DRIVERS_UAVCAN=n CONFIG_DRIVERS_UAVCAN_V1=y +CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y +CONFIG_UAVCAN_V1_ESC_SUBSCRIBER=y +CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0=y +CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1=y +CONFIG_UAVCAN_V1_READINESS_PUBLISHER=y +CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER=y +CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y diff --git a/src/drivers/uavcan_v1/CMakeLists.txt b/src/drivers/uavcan_v1/CMakeLists.txt index 12ba82d19a..45b7ee4711 100644 --- a/src/drivers/uavcan_v1/CMakeLists.txt +++ b/src/drivers/uavcan_v1/CMakeLists.txt @@ -50,6 +50,8 @@ else() endif() set(SRCS) +set(DPNDS) + if(${PX4_PLATFORM} MATCHES "nuttx") if(CONFIG_NET_CAN) list(APPEND SRCS @@ -64,6 +66,40 @@ if(${PX4_PLATFORM} MATCHES "nuttx") endif() endif() +if(CONFIG_UAVCAN_V1_NODE_MANAGER) + list(APPEND SRCS + NodeManager.hpp + NodeManager.cpp + ) +endif() + +if(CONFIG_UAVCAN_V1_NODE_CLIENT) + list(APPEND SRCS + NodeClient.hpp + NodeClient.cpp + ) + list(APPEND DPNDS + drivers_bootloaders + ) +endif() + +# Temporary measure to get Kconfig option as defines into this application +# Ideally we want nicely decouple and define this in kconfig.cmake +# But then we need to overhaul the src modules naming convention +set(DRIVERS_UAVCAN_V1_OPTIONS) +get_cmake_property(_variableNames VARIABLES) +foreach (_variableName ${_variableNames}) + string(REGEX MATCH "^CONFIG_UAVCAN_V1_" UAVCAN_V1_OPTION ${_variableName}) + + if(UAVCAN_V1_OPTION) + if(${${_variableName}} STREQUAL "y") + list(APPEND DRIVERS_UAVCAN_V1_OPTIONS "-D${_variableName}=1") + else() + list(APPEND DRIVERS_UAVCAN_V1_OPTIONS "-D${_variableName}=${${_variableName}}") + endif() + endif() +endforeach() + px4_add_module( MODULE drivers__uavcan_v1 MAIN uavcan_v1 @@ -72,6 +108,7 @@ px4_add_module( #-DCANARD_ASSERT -DUINT32_C\(x\)=__UINT32_C\(x\) -O0 + ${DRIVERS_UAVCAN_V1_OPTIONS} INCLUDES ${LIBCANARD_DIR}/libcanard/ ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated @@ -82,8 +119,6 @@ px4_add_module( SubscriptionManager.hpp ParamManager.hpp ParamManager.cpp - NodeManager.hpp - NodeManager.cpp Uavcan.cpp Uavcan.hpp Publishers/uORB/uorb_publisher.cpp @@ -97,4 +132,5 @@ px4_add_module( git_public_regulated_data_types git_legacy_data_types version + ${DPNDS} ) diff --git a/src/drivers/uavcan_v1/Kconfig b/src/drivers/uavcan_v1/Kconfig index 46c7f15199..5db92ea02f 100644 --- a/src/drivers/uavcan_v1/Kconfig +++ b/src/drivers/uavcan_v1/Kconfig @@ -20,15 +20,91 @@ if DRIVERS_UAVCAN_V1 endchoice - config UAVCAN_V1_GETINFO_RESPONDER - bool "GetInfo1.0 responder" - default n + config UAVCAN_V1_NODE_MANAGER + bool "Node manager" + default y + depends on UAVCAN_V1_FMU + help + Implement UAVCAN v1 PNP server functionality and manages discovered nodes - config UAVCAN_V1_EXECUTECOMMAND_RESPONDER - bool "ExecuteCommand1.0 responder" - default n + config UAVCAN_V1_NODE_CLIENT + bool "Node client" + default y + depends on UAVCAN_V1_CLIENT + help + Implement UAVCAN v1 PNP client functionality - config UAVCAN_V1_GNSS_PUBLISHER - bool "GNSS Publisher" + config UAVCAN_V1_APP_DESCRIPTOR + bool "UAVCAN v0 bootloader app descriptor" default n + depends on UAVCAN_V1_CLIENT && DRIVERS_BOOTLOADERS + help + When the board uses the UAVCANv0 bootloader functionality you need a AppImageDescriptor defined + + + menu "Publisher support" + + config UAVCAN_V1_GNSS_PUBLISHER + bool "GNSS Publisher" + default n + + config UAVCAN_V1_ESC_CONTROLLER + bool "ESC Controller" + default n + + config UAVCAN_V1_READINESS_PUBLISHER + bool "Readiness Publisher" + default n + + config UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER + bool "uORB actuator_outputs publisher" + default n + + config UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER + bool "uORB sensor_gps publisher" + default n + + endmenu + + menu "Subscriber support" + + config UAVCAN_V1_ESC_SUBSCRIBER + bool "ESC Subscriber" + default n + + config UAVCAN_V1_GNSS_SUBSCRIBER_0 + bool "GNSS Subscriber 0 " + default n + + config UAVCAN_V1_GNSS_SUBSCRIBER_1 + bool "GNSS Subscriber 1" + default n + + config UAVCAN_V1_BMS_SUBSCRIBER + bool "BMS Subscriber" + default n + + config UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER + bool "uORB sensor_gps Subscriber" + default n + endmenu + + menu "Advertised Services" + config UAVCAN_V1_GETINFO_RESPONDER + bool "GetInfo1.0 responder" + default y + help + Responds to uavcan.node.GetInfo.1.0 request sending over node information + See https://github.com/UAVCAN/public_regulated_data_types/blob/master/uavcan/node/430.GetInfo.1.0.uavcan for full response + + config UAVCAN_V1_EXECUTECOMMAND_RESPONDER + bool "ExecuteCommand1.0 responder" + default n + help + To be implemented + endmenu + + menu "Service invokers" + endmenu + endif #DRIVERS_UAVCAN_V1 diff --git a/src/drivers/uavcan_v1/NodeClient.cpp b/src/drivers/uavcan_v1/NodeClient.cpp new file mode 100644 index 0000000000..db8cee63da --- /dev/null +++ b/src/drivers/uavcan_v1/NodeClient.cpp @@ -0,0 +1,170 @@ +/**************************************************************************** + * + * 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 NodeClient.cpp + * + * Defines basic implementation of client UAVCAN PNP for requesting a Node ID + * + * @author Peter van der Perk + */ + +#define PNP_UNIQUE_ID_SIZE 16 // 128 bit unique id + +#include +#include "NodeClient.hpp" + +void NodeClient::callback(const CanardTransfer &receive) +{ + if (receive.remote_node_id != CANARD_NODE_ID_UNSET && _canard_instance.node_id != CANARD_NODE_ID_UNSET) { + + int32_t allocated = CANARD_NODE_ID_UNSET; + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + + if (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD) { + uavcan_pnp_NodeIDAllocationData_2_0 msg; + + size_t msg_size_in_bytes = receive.payload_size; + uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&msg, (const uint8_t *)receive.payload, + &msg_size_in_bytes); + + if (memcmp(msg.unique_id, px4_guid, sizeof(msg.unique_id)) == 0) { + allocated = msg.node_id.value; + } + + } else { + uavcan_pnp_NodeIDAllocationData_1_0 msg; + + size_t msg_size_in_bytes = receive.payload_size; + uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, + &msg_size_in_bytes); + + if (msg.allocated_node_id.count > 0) { + if (msg.unique_id_hash == (crc64(px4_guid, PNP_UNIQUE_ID_SIZE) & 0xFFFFFFFFFFFF)) { + allocated = msg.allocated_node_id.elements[0].value; + } + } + } + + if (allocated == CANARD_NODE_ID_UNSET) { + return; // UID mismatch. + } + + if (allocated <= 0 || allocated >= (int32_t)CANARD_NODE_ID_MAX) + // Allocated node-ID ignored because it exceeds max_node_id + { + return; + } + + _canard_instance.node_id = allocated; + } +} + + +void NodeClient::update() +{ + if (hrt_elapsed_time(&_nodealloc_request_last) >= hrt_abstime(2 * + 1000000ULL)) { // Compiler hates me here, some 1_s doesn't work + + int32_t result; + + // Allocation already done, nothing to do + if (_canard_instance.node_id != CANARD_NODE_ID_UNSET) { + return; + } + + if (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD) { + // NodeIDAllocationData message + uavcan_pnp_NodeIDAllocationData_2_0 node_id_alloc_msg; + uint8_t node_id_alloc_payload_buffer[PNP2_PAYLOAD_SIZE]; + + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + memcpy(node_id_alloc_msg.unique_id, px4_guid, sizeof(node_id_alloc_msg.unique_id)); + //node_id_alloc_msg.node_id.value = preffered_node_id; //FIXME preffered ID PX4 Param + + CanardTransfer transfer = { + .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = PNP2_PORT_ID, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = _node_id_alloc_transfer_id, + .payload_size = PNP2_PAYLOAD_SIZE, + .payload = &node_id_alloc_payload_buffer, + }; + + result = uavcan_pnp_NodeIDAllocationData_2_0_serialize_(&node_id_alloc_msg, (uint8_t *)&node_id_alloc_payload_buffer, + &transfer.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_node_id_alloc_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + canardTxPush(&_canard_instance, &transfer); + } + + } else { + // NodeIDAllocationData message + uavcan_pnp_NodeIDAllocationData_1_0 node_id_alloc_msg; + uavcan_pnp_NodeIDAllocationData_1_0_initialize_(&node_id_alloc_msg); + uint8_t node_id_alloc_payload_buffer[PNP1_PAYLOAD_SIZE]; + + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + node_id_alloc_msg.unique_id_hash = (crc64(px4_guid, PNP_UNIQUE_ID_SIZE) & 0xFFFFFFFFFFFF); + + CanardTransfer transfer = { + .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = PNP1_PORT_ID, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = _node_id_alloc_transfer_id, + .payload_size = PNP1_PAYLOAD_SIZE, + .payload = &node_id_alloc_payload_buffer, + }; + + result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&node_id_alloc_msg, (uint8_t *)&node_id_alloc_payload_buffer, + &transfer.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_node_id_alloc_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + canardTxPush(&_canard_instance, &transfer); + } + } + + _nodealloc_request_last = hrt_absolute_time(); + } +} diff --git a/src/drivers/uavcan_v1/PublicationManager.hpp b/src/drivers/uavcan_v1/PublicationManager.hpp index 82397f931e..1be1b433bc 100644 --- a/src/drivers/uavcan_v1/PublicationManager.hpp +++ b/src/drivers/uavcan_v1/PublicationManager.hpp @@ -42,6 +42,35 @@ #pragma once +#include + +#ifndef CONFIG_UAVCAN_V1_GNSS_PUBLISHER +#define CONFIG_UAVCAN_V1_GNSS_PUBLISHER 0 +#endif + +#ifndef CONFIG_UAVCAN_V1_ESC_CONTROLLER +#define CONFIG_UAVCAN_V1_ESC_CONTROLLER 0 +#endif + +#ifndef CONFIG_UAVCAN_V1_READINESS_PUBLISHER +#define CONFIG_UAVCAN_V1_READINESS_PUBLISHER 0 +#endif + +#ifndef CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER +#define CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER 0 +#endif + +#ifndef CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER +#define CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER 0 +#endif + +/* Preprocessor calculation of publisher count */ + +#define UAVCAN_PUB_COUNT CONFIG_UAVCAN_V1_GNSS_PUBLISHER + \ + CONFIG_UAVCAN_V1_ESC_CONTROLLER + \ + CONFIG_UAVCAN_V1_READINESS_PUBLISHER + \ + CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \ + CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER #include #include @@ -79,7 +108,9 @@ private: UavcanParamManager &_param_manager; List _dynpublishers; - const UavcanDynPubBinder _uavcan_pubs[5] { + + const UavcanDynPubBinder _uavcan_pubs[UAVCAN_PUB_COUNT] { +#if CONFIG_UAVCAN_V1_GNSS_PUBLISHER { [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * { @@ -88,6 +119,8 @@ private: "gps", 0 }, +#endif +#if CONFIG_UAVCAN_V1_ESC_CONTROLLER { [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * { @@ -96,6 +129,8 @@ private: "esc", 0 }, +#endif +#if CONFIG_UAVCAN_V1_READINESS_PUBLISHER { [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * { @@ -104,6 +139,8 @@ private: "readiness", 0 }, +#endif +#if CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER { [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * { @@ -112,7 +149,8 @@ private: "uorb.actuator_outputs", 0 }, - +#endif +#if CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER { [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * { @@ -121,5 +159,6 @@ private: "uorb.sensor_gps", 0 }, +#endif }; }; diff --git a/src/drivers/uavcan_v1/ServiceClients/Access.hpp b/src/drivers/uavcan_v1/ServiceClients/Access.hpp index a2505967c1..5655cfa7fc 100644 --- a/src/drivers/uavcan_v1/ServiceClients/Access.hpp +++ b/src/drivers/uavcan_v1/ServiceClients/Access.hpp @@ -108,7 +108,7 @@ public: .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? + .transfer_id = receive.transfer_id, .payload_size = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, .payload = &response_payload_buffer, }; @@ -117,7 +117,6 @@ public: 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); } @@ -127,6 +126,5 @@ public: 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 index fd38c29a95..4012fcbffa 100644 --- a/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp +++ b/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp @@ -112,7 +112,7 @@ public: .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, + .transfer_id = receive.transfer_id, .payload_size = uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, .payload = &response_payload_buffer, }; @@ -122,7 +122,6 @@ public: 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); } @@ -136,7 +135,4 @@ public: }; -private: - CanardTransferID getinfo_response_transfer_id = 0; - }; diff --git a/src/drivers/uavcan_v1/Services/ServiceRequest.hpp b/src/drivers/uavcan_v1/Services/ServiceRequest.hpp index 81956de0b4..1424e9dfae 100644 --- a/src/drivers/uavcan_v1/Services/ServiceRequest.hpp +++ b/src/drivers/uavcan_v1/Services/ServiceRequest.hpp @@ -32,9 +32,9 @@ ****************************************************************************/ /** - * @file List.hpp + * @file ServiceRequest.hpp * - * Defines a List Service invoker and process List responses + * Defines a Service invoker base class and process responses * * @author Peter van der Perk */ @@ -45,8 +45,6 @@ #include #include -#include "../NodeManager.hpp" - #include #include "../Subscribers/BaseSubscriber.hpp" @@ -79,6 +77,7 @@ public: bool request(CanardTransfer *transfer, UavcanServiceRequestInterface *handler) { _response_callback = handler; + remote_node_id = transfer->remote_node_id; ++request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. return canardTxPush(&_canard_instance, transfer) > 0; } @@ -87,7 +86,9 @@ public: { PX4_INFO("Response"); - if (_response_callback != nullptr) { + if (_response_callback != nullptr && + receive.transfer_id == (request_transfer_id - 1) && + receive.remote_node_id == remote_node_id) { _response_callback->response_callback(receive); } }; @@ -96,6 +97,7 @@ public: protected: CanardTransferID request_transfer_id = 0; + CanardNodeID remote_node_id = CANARD_NODE_ID_UNSET; const CanardPortID _portID; const size_t _extent; diff --git a/src/drivers/uavcan_v1/SubscriptionManager.cpp b/src/drivers/uavcan_v1/SubscriptionManager.cpp index 2e8bb3c682..98eb24283d 100644 --- a/src/drivers/uavcan_v1/SubscriptionManager.cpp +++ b/src/drivers/uavcan_v1/SubscriptionManager.cpp @@ -55,11 +55,14 @@ SubscriptionManager::~SubscriptionManager() } } - void SubscriptionManager::subscribe() { _heartbeat_sub.subscribe(); + +#if CONFIG_UAVCAN_V1_GETINFO_RESPONDER _getinfo_rsp.subscribe(); +#endif + _access_rsp.subscribe(); updateDynamicSubscriptions(); diff --git a/src/drivers/uavcan_v1/SubscriptionManager.hpp b/src/drivers/uavcan_v1/SubscriptionManager.hpp index df721635ad..b61dda8cf8 100644 --- a/src/drivers/uavcan_v1/SubscriptionManager.hpp +++ b/src/drivers/uavcan_v1/SubscriptionManager.hpp @@ -41,6 +41,33 @@ #pragma once +#ifndef CONFIG_UAVCAN_V1_ESC_SUBSCRIBER +#define CONFIG_UAVCAN_V1_ESC_SUBSCRIBER 0 +#endif + +#ifndef CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0 +#define CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0 0 +#endif + +#ifndef CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1 +#define CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1 0 +#endif + +#ifndef CONFIG_UAVCAN_V1_BMS_SUBSCRIBER +#define CONFIG_UAVCAN_V1_BMS_SUBSCRIBER 0 +#endif + +#ifndef CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER +#define CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER 0 +#endif + +/* Preprocessor calculation of Subscribers count */ + +#define UAVCAN_SUB_COUNT CONFIG_UAVCAN_V1_ESC_SUBSCRIBER + \ + CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0 + \ + CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1 + \ + CONFIG_UAVCAN_V1_BMS_SUBSCRIBER + \ + CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER #include #include @@ -82,13 +109,16 @@ private: UavcanHeartbeatSubscriber _heartbeat_sub {_canard_instance}; +#if UAVCAN_V1_GETINFO_RESPONDER // GetInfo response UavcanGetInfoResponse _getinfo_rsp {_canard_instance}; +#endif // Process register requests UavcanAccessResponse _access_rsp {_canard_instance, _param_manager}; - const UavcanDynSubBinder _uavcan_subs[6] { + const UavcanDynSubBinder _uavcan_subs[UAVCAN_SUB_COUNT] { +#if CONFIG_UAVCAN_V1_ESC_SUBSCRIBER { [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { @@ -97,6 +127,8 @@ private: "esc", 0 }, +#endif +#if CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0 { [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { @@ -105,6 +137,8 @@ private: "gps", 0 }, +#endif +#if CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1 //FIXME decouple instanceing { [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { @@ -113,6 +147,8 @@ private: "gps", 1 }, +#endif +#if CONFIG_UAVCAN_V1_BMS_SUBSCRIBER { [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { @@ -121,6 +157,8 @@ private: "energy_source", 0 }, +#endif +#if 0 //Obsolete to be removed { [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { @@ -129,6 +167,8 @@ private: "legacy_bms", 0 }, +#endif +#if CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER { [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { @@ -137,5 +177,6 @@ private: "uorb.sensor_gps", 0 }, +#endif }; }; diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index 1dd6a6742d..2001edc373 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -36,6 +36,28 @@ #include #include + +#ifdef CONFIG_UAVCAN_V1_APP_DESCRIPTOR +#include "boot_app_shared.h" +/* + * This is the AppImageDescriptor used + * by the make_can_boot_descriptor.py tool to set + * the application image's descriptor so that the + * uavcan bootloader has the ability to validate the + * image crc, size etc of this application +*/ +boot_app_shared_section app_descriptor_t AppDescriptor = { + .signature = APP_DESCRIPTOR_SIGNATURE, + .image_crc = 0, + .image_size = 0, + .git_hash = 0, + .major_version = APP_VERSION_MAJOR, + .minor_version = APP_VERSION_MINOR, + .board_id = HW_VERSION_MAJOR << 8 | HW_VERSION_MINOR, + .reserved = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff } +}; +#endif + using namespace time_literals; UavcanNode *UavcanNode::_instance; @@ -80,7 +102,15 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) : _canard_instance.mtu_bytes = CANARD_MTU_CAN_CLASSIC; } +#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER _node_manager.subscribe(); +#endif + +#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT + _node_client = new NodeClient(_canard_instance, _param_manager); + + _node_client->subscribe(); +#endif _pub_manager.updateParams(); @@ -195,13 +225,30 @@ void UavcanNode::Run() perf_begin(_cycle_perf); perf_count(_interval_perf); - // send uavcan::node::Heartbeat_1_0 @ 1 Hz - sendHeartbeat(); + if (_canard_instance.node_id != CANARD_NODE_ID_UNSET) { + // send uavcan::node::Heartbeat_1_0 @ 1 Hz + sendHeartbeat(); + + // Check all publishers + _pub_manager.update(); + +#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER + _node_manager.update(); +#endif + } + +#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT - // Check all publishers - _pub_manager.update(); + else if (_node_client != nullptr) { + if (_canard_instance.node_id == CANARD_NODE_ID_UNSET) { + _node_client->update(); - _node_manager.update(); + } else { + delete _node_client; + } + } + +#endif transmit(); @@ -390,6 +437,10 @@ extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[]) int32_t node_id = 0; param_get(param_find("UAVCAN_V1_ID"), &node_id); + if (node_id == -1) { + node_id = CANARD_NODE_ID_UNSET; + } + // Start PX4_INFO("Node ID %" PRIu32 ", bitrate %" PRIu32, node_id, bitrate); return UavcanNode::start(node_id, bitrate); diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index 318e2eec83..9c2ab4c42c 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -61,7 +61,13 @@ #include "Publishers/Publisher.hpp" #include "Publishers/uORB/uorb_publisher.hpp" +#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER #include "NodeManager.hpp" +#endif + +#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT +#include "NodeClient.hpp" +#endif #include "PublicationManager.hpp" #include "SubscriptionManager.hpp" @@ -179,7 +185,13 @@ private: UavcanParamManager _param_manager; +#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER NodeManager _node_manager {_canard_instance, _param_manager}; +#endif + +#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT + NodeClient *_node_client {nullptr}; +#endif PublicationManager _pub_manager {_canard_instance, _param_manager}; SubscriptionManager _sub_manager {_canard_instance, _param_manager}; diff --git a/src/drivers/uavcan_v1/parameters.c b/src/drivers/uavcan_v1/parameters.c index e3274bb49e..a650ca27b2 100644 --- a/src/drivers/uavcan_v1/parameters.c +++ b/src/drivers/uavcan_v1/parameters.c @@ -48,7 +48,7 @@ PARAM_DEFINE_INT32(UAVCAN_V1_ENABLE, 0); * * Read the specs at http://uavcan.org to learn more about Node ID. * - * @min 1 + * @min -1 * @max 125 * @reboot_required true * @group UAVCANv1