From ce909b23b173233b0ade5d80664f60c3b42b2780 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Tue, 30 Nov 2021 15:50:51 +0100 Subject: [PATCH] UAVCANv1 Node implementation work, PNP, Registers and uORB publisher --- boards/nxp/fmuk66-e/socketcan.px4board | 2 + boards/nxp/ucans32k146/default.px4board | 5 +- boards/nxp/ucans32k146/init/rc.board_defaults | 3 + src/drivers/uavcan_v1/Actuators/EscClient.hpp | 2 +- src/drivers/uavcan_v1/CMakeLists.txt | 1 + src/drivers/uavcan_v1/NodeClient.cpp | 5 +- src/drivers/uavcan_v1/NodeManager.hpp | 2 +- src/drivers/uavcan_v1/ParamManager.cpp | 13 ++ src/drivers/uavcan_v1/ParamManager.hpp | 4 +- .../uavcan_v1/Publishers/DS-015/Gnss.hpp | 2 +- .../uavcan_v1/Publishers/DS-015/Readiness.hpp | 2 +- .../uavcan_v1/Publishers/Publisher.hpp | 22 ++-- .../Publishers/uORB/uorb_publisher.hpp | 2 +- .../uavcan_v1/ServiceClients/Access.hpp | 2 +- .../uavcan_v1/ServiceClients/GetInfo.hpp | 2 +- src/drivers/uavcan_v1/ServiceClients/List.hpp | 116 ++++++++++++++++++ .../uavcan_v1/Services/AccessRequest.hpp | 2 +- .../uavcan_v1/Services/ListRequest.hpp | 2 +- .../uavcan_v1/Services/ServiceRequest.hpp | 5 +- .../uavcan_v1/Subscribers/BaseSubscriber.hpp | 5 +- .../uavcan_v1/Subscribers/DS-015/Battery.hpp | 2 +- .../uavcan_v1/Subscribers/DS-015/Esc.hpp | 2 +- .../uavcan_v1/Subscribers/DS-015/Gnss.hpp | 2 +- .../Subscribers/DynamicPortSubscriber.hpp | 11 +- .../uavcan_v1/Subscribers/Heartbeat.hpp | 2 +- .../Subscribers/legacy/LegacyBatteryInfo.hpp | 2 +- .../Subscribers/uORB/uorb_subscriber.cpp | 54 ++++++++ .../{sensor_gps.hpp => uorb_subscriber.hpp} | 66 +++++++--- src/drivers/uavcan_v1/SubscriptionManager.cpp | 1 + src/drivers/uavcan_v1/SubscriptionManager.hpp | 6 +- src/drivers/uavcan_v1/parameters.c | 10 ++ 31 files changed, 303 insertions(+), 54 deletions(-) create mode 100644 src/drivers/uavcan_v1/ServiceClients/List.hpp create mode 100644 src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.cpp rename src/drivers/uavcan_v1/Subscribers/uORB/{sensor_gps.hpp => uorb_subscriber.hpp} (61%) diff --git a/boards/nxp/fmuk66-e/socketcan.px4board b/boards/nxp/fmuk66-e/socketcan.px4board index ff4df8af58..3a4f62ea6a 100644 --- a/boards/nxp/fmuk66-e/socketcan.px4board +++ b/boards/nxp/fmuk66-e/socketcan.px4board @@ -1,2 +1,4 @@ CONFIG_DRIVERS_UAVCAN=n CONFIG_DRIVERS_UAVCAN_V1=y +CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y +CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y diff --git a/boards/nxp/ucans32k146/default.px4board b/boards/nxp/ucans32k146/default.px4board index 424f1c583f..87d91630e7 100644 --- a/boards/nxp/ucans32k146/default.px4board +++ b/boards/nxp/ucans32k146/default.px4board @@ -7,7 +7,10 @@ CONFIG_DRIVERS_BOOTLOADERS=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y CONFIG_DRIVERS_PWM_OUT=y -CONFIG_DRIVERS_UAVCANNODE_GPS_DEMO=y +CONFIG_DRIVERS_UAVCAN_V1=y +CONFIG_UAVCAN_V1_CLIENT=y +CONFIG_UAVCAN_V1_APP_DESCRIPTOR=y +CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_SYSTEMCMDS_I2CDETECT=y CONFIG_SYSTEMCMDS_LED_CONTROL=y diff --git a/boards/nxp/ucans32k146/init/rc.board_defaults b/boards/nxp/ucans32k146/init/rc.board_defaults index 5bccf6aa10..fadf85f18a 100644 --- a/boards/nxp/ucans32k146/init/rc.board_defaults +++ b/boards/nxp/ucans32k146/init/rc.board_defaults @@ -4,3 +4,6 @@ #------------------------------------------------------------------------------ pwm_out mode_pwm1 start + +ifup can0 +uavcan_v1 start diff --git a/src/drivers/uavcan_v1/Actuators/EscClient.hpp b/src/drivers/uavcan_v1/Actuators/EscClient.hpp index f4b2f31b48..f22e0c983a 100644 --- a/src/drivers/uavcan_v1/Actuators/EscClient.hpp +++ b/src/drivers/uavcan_v1/Actuators/EscClient.hpp @@ -68,7 +68,7 @@ public: static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; UavcanEscController(CanardInstance &ins, UavcanParamManager &pmgr) : - UavcanPublisher(ins, pmgr, "esc") { }; + UavcanPublisher(ins, pmgr, "ds_015", "esc") { }; ~UavcanEscController() {}; diff --git a/src/drivers/uavcan_v1/CMakeLists.txt b/src/drivers/uavcan_v1/CMakeLists.txt index 66cde46ad0..b07540af96 100644 --- a/src/drivers/uavcan_v1/CMakeLists.txt +++ b/src/drivers/uavcan_v1/CMakeLists.txt @@ -122,6 +122,7 @@ px4_add_module( Uavcan.cpp Uavcan.hpp Publishers/uORB/uorb_publisher.cpp + Subscribers/uORB/uorb_subscriber.cpp ${SRCS} o1heap/o1heap.c o1heap/o1heap.h diff --git a/src/drivers/uavcan_v1/NodeClient.cpp b/src/drivers/uavcan_v1/NodeClient.cpp index db8cee63da..ca42646486 100644 --- a/src/drivers/uavcan_v1/NodeClient.cpp +++ b/src/drivers/uavcan_v1/NodeClient.cpp @@ -46,7 +46,7 @@ void NodeClient::callback(const CanardTransfer &receive) { - if (receive.remote_node_id != CANARD_NODE_ID_UNSET && _canard_instance.node_id != CANARD_NODE_ID_UNSET) { + 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; @@ -88,6 +88,9 @@ void NodeClient::callback(const CanardTransfer &receive) } _canard_instance.node_id = allocated; + + PX4_INFO("Allocated Node ID %d", _canard_instance.node_id); + } } diff --git a/src/drivers/uavcan_v1/NodeManager.hpp b/src/drivers/uavcan_v1/NodeManager.hpp index 0b32b55913..c44678a234 100644 --- a/src/drivers/uavcan_v1/NodeManager.hpp +++ b/src/drivers/uavcan_v1/NodeManager.hpp @@ -70,7 +70,7 @@ typedef struct { class NodeManager : public UavcanBaseSubscriber, public UavcanServiceRequestInterface { public: - NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : UavcanBaseSubscriber(ins, "NodeIDAllocationData", 0), + NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : UavcanBaseSubscriber(ins, "", "NodeIDAllocationData", 0), _canard_instance(ins), _access_request(ins, pmgr), _list_request(ins) { }; void subscribe() override diff --git a/src/drivers/uavcan_v1/ParamManager.cpp b/src/drivers/uavcan_v1/ParamManager.cpp index 8674f76a29..15a57f3744 100644 --- a/src/drivers/uavcan_v1/ParamManager.cpp +++ b/src/drivers/uavcan_v1/ParamManager.cpp @@ -76,6 +76,19 @@ bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, ua return false; } +bool UavcanParamManager::GetParamName(uint32_t id, uavcan_register_Name_1_0 &name) +{ + if (id >= sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) { + return false; + } + + strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + + name.name.count = strlen(_uavcan_params[id].uavcan_name); + + return true; +} + bool UavcanParamManager::SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value) { for (auto ¶m : _uavcan_params) { diff --git a/src/drivers/uavcan_v1/ParamManager.hpp b/src/drivers/uavcan_v1/ParamManager.hpp index 7d30f693fb..c9c0cd7b62 100644 --- a/src/drivers/uavcan_v1/ParamManager.hpp +++ b/src/drivers/uavcan_v1/ParamManager.hpp @@ -110,12 +110,13 @@ public: bool GetParamByName(const char *param_name, uavcan_register_Value_1_0 &value); bool GetParamByName(const uavcan_register_Name_1_0 &name, uavcan_register_Value_1_0 &value); + bool GetParamName(uint32_t id, uavcan_register_Name_1_0 &name); bool SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value); private: - const UavcanParamBinder _uavcan_params[12] { + const UavcanParamBinder _uavcan_params[13] { {"uavcan.pub.esc.0.id", "UCAN1_ESC_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.servo.0.id", "UCAN1_SERVO_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.gps.0.id", "UCAN1_GPS_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, @@ -128,6 +129,7 @@ private: {"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.sub.legacy_bms.0.id", "UCAN1_LG_BMS_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.sub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.pub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS_P", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, //{"uavcan.sub.bms.0.id", "UCAN1_BMS0_SUB"}, //FIXME instancing //{"uavcan.sub.bms.1.id", "UCAN1_BMS1_SUB"}, }; diff --git a/src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp b/src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp index 2f3097d908..1447e42ae2 100644 --- a/src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp +++ b/src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp @@ -51,7 +51,7 @@ class UavcanGnssPublisher : public UavcanPublisher { public: UavcanGnssPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanPublisher(ins, pmgr, "gps", instance) + UavcanPublisher(ins, pmgr, "ds_015", "gps", instance) { }; diff --git a/src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp b/src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp index 6ae3775d7b..98b5cd4837 100644 --- a/src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp +++ b/src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp @@ -51,7 +51,7 @@ class UavcanReadinessPublisher : public UavcanPublisher { public: UavcanReadinessPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanPublisher(ins, pmgr, "readiness", instance) + UavcanPublisher(ins, pmgr, "ds_015", "readiness", instance) { }; diff --git a/src/drivers/uavcan_v1/Publishers/Publisher.hpp b/src/drivers/uavcan_v1/Publishers/Publisher.hpp index 590381110e..f13c176b68 100644 --- a/src/drivers/uavcan_v1/Publishers/Publisher.hpp +++ b/src/drivers/uavcan_v1/Publishers/Publisher.hpp @@ -60,8 +60,10 @@ class UavcanPublisher : public ListNode { public: - UavcanPublisher(CanardInstance &ins, UavcanParamManager &pmgr, const char *subject_name, uint8_t instance = 0) : - _canard_instance(ins), _param_manager(pmgr), _subject_name(subject_name), _instance(instance) { }; + UavcanPublisher(CanardInstance &ins, UavcanParamManager &pmgr, const char *prefix_name, const char *subject_name, + uint8_t instance = 0) : + _canard_instance(ins), _param_manager(pmgr), _prefix_name(prefix_name), _subject_name(subject_name), + _instance(instance) { }; virtual ~UavcanPublisher() = default; @@ -73,7 +75,7 @@ public: void updateParam() { char uavcan_param[256]; - snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.pub.%s.%d.id", _subject_name, _instance); + snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.pub.%s%s.%d.id", _prefix_name, _subject_name, _instance); // Set _port_id from _uavcan_param uavcan_register_Value_1_0 value; @@ -83,12 +85,12 @@ public: if (_port_id != new_id) { if (new_id == CANARD_PORT_ID_UNSET) { - PX4_INFO("Disabling publication of subject %s.%d", _subject_name, _instance); + PX4_INFO("Disabling publication of subject %s%s.%d", _prefix_name, _subject_name, _instance); _port_id = CANARD_PORT_ID_UNSET; } else { _port_id = (CanardPortID)new_id; - PX4_INFO("Enabling subject %s.%d on port %d", _subject_name, _instance, _port_id); + PX4_INFO("Enabling subject %s%s.%d on port %d", _prefix_name, _subject_name, _instance, _port_id); } } } @@ -97,10 +99,10 @@ public: void printInfo() { if (_port_id != CANARD_PORT_ID_UNSET) { - PX4_INFO("Enabled subject %s.%d on port %d", _subject_name, _instance, _port_id); + PX4_INFO("Enabled subject %s%s.%d on port %d", _prefix_name, _subject_name, _instance, _port_id); } else { - PX4_INFO("Subject %s.%d disabled", _subject_name, _instance); + PX4_INFO("Subject %s%s.%d disabled", _prefix_name, _subject_name, _instance); } } @@ -109,6 +111,11 @@ public: return _subject_name; } + const char *getPrefixName() + { + return _prefix_name; + } + uint8_t getInstance() { return _instance; @@ -127,6 +134,7 @@ public: protected: CanardInstance &_canard_instance; UavcanParamManager &_param_manager; + const char *_prefix_name; const char *_subject_name; uint8_t _instance {0}; diff --git a/src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp b/src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp index b913181df7..518915da12 100644 --- a/src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp +++ b/src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp @@ -52,7 +52,7 @@ class uORB_over_UAVCAN_Publisher : public UavcanPublisher public: uORB_over_UAVCAN_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, const orb_metadata *meta, uint8_t instance = 0) : - UavcanPublisher(ins, pmgr, meta->o_name, instance), + UavcanPublisher(ins, pmgr, "uorb.", meta->o_name, instance), _uorb_meta{meta}, _uorb_sub(meta) {}; diff --git a/src/drivers/uavcan_v1/ServiceClients/Access.hpp b/src/drivers/uavcan_v1/ServiceClients/Access.hpp index 5655cfa7fc..128912b067 100644 --- a/src/drivers/uavcan_v1/ServiceClients/Access.hpp +++ b/src/drivers/uavcan_v1/ServiceClients/Access.hpp @@ -56,7 +56,7 @@ class UavcanAccessResponse : public UavcanBaseSubscriber { public: UavcanAccessResponse(CanardInstance &ins, UavcanParamManager &pmgr) : - UavcanBaseSubscriber(ins, "Access", 0), _param_manager(pmgr) { }; + UavcanBaseSubscriber(ins, "", "Access", 0), _param_manager(pmgr) { }; void subscribe() override { diff --git a/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp b/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp index 4012fcbffa..4b752c205a 100644 --- a/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp +++ b/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp @@ -55,7 +55,7 @@ class UavcanGetInfoResponse : public UavcanBaseSubscriber { public: UavcanGetInfoResponse(CanardInstance &ins) : - UavcanBaseSubscriber(ins, "GetInfo", 0) { }; + UavcanBaseSubscriber(ins, "", "GetInfo", 0) { }; void subscribe() override { diff --git a/src/drivers/uavcan_v1/ServiceClients/List.hpp b/src/drivers/uavcan_v1/ServiceClients/List.hpp new file mode 100644 index 0000000000..8259fbdaf3 --- /dev/null +++ b/src/drivers/uavcan_v1/ServiceClients/List.hpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * 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 response to a List request + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include +#include + +#include "../ParamManager.hpp" + +#include + +#include "../Subscribers/BaseSubscriber.hpp" + +class UavcanListResponse : public UavcanBaseSubscriber +{ +public: + UavcanListResponse(CanardInstance &ins, UavcanParamManager &pmgr) : + UavcanBaseSubscriber(ins, "", "List", 0), _param_manager(pmgr) { }; + + void subscribe() override + { + // Subscribe to requests uavcan.pnp.NodeIDAllocationData + canardRxSubscribe(&_canard_instance, + CanardTransferKindRequest, + uavcan_register_List_1_0_FIXED_PORT_ID_, + uavcan_register_List_Response_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); + + }; + + void callback(const CanardTransfer &receive) override + { + PX4_INFO("List request"); + + uavcan_register_List_Request_1_0 msg; + uavcan_register_List_Response_1_0 response; + + uavcan_register_List_Request_1_0_initialize_(&msg); + uavcan_register_List_Response_1_0_initialize_(&response); + + size_t register_in_size_bits = receive.payload_size; + uavcan_register_List_Request_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, ®ister_in_size_bits); + + int result {0}; + + if (_param_manager.GetParamName(msg.index, response.name) == 0) { + response.name.name.count = 0; + } + + uint8_t response_payload_buffer[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + CanardTransfer transfer = { + .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindResponse, + .port_id = uavcan_register_List_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 = receive.transfer_id, + .payload_size = uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &response_payload_buffer, + }; + + result = uavcan_register_List_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 + result = canardTxPush(&_canard_instance, &transfer); + } + + }; + +private: + UavcanParamManager &_param_manager; + +}; diff --git a/src/drivers/uavcan_v1/Services/AccessRequest.hpp b/src/drivers/uavcan_v1/Services/AccessRequest.hpp index 8f43ce9186..7d5941c951 100644 --- a/src/drivers/uavcan_v1/Services/AccessRequest.hpp +++ b/src/drivers/uavcan_v1/Services/AccessRequest.hpp @@ -55,7 +55,7 @@ class UavcanAccessServiceRequest : public UavcanServiceRequest { public: UavcanAccessServiceRequest(CanardInstance &ins, UavcanParamManager &pmgr) : - UavcanServiceRequest(ins, "Access", uavcan_register_Access_1_0_FIXED_PORT_ID_, + UavcanServiceRequest(ins, "", "Access", uavcan_register_Access_1_0_FIXED_PORT_ID_, uavcan_register_Access_Response_1_0_EXTENT_BYTES_), _param_manager(pmgr) { }; bool setPortId(CanardNodeID node_id, uavcan_register_Name_1_0 &name, UavcanServiceRequestInterface *handler) diff --git a/src/drivers/uavcan_v1/Services/ListRequest.hpp b/src/drivers/uavcan_v1/Services/ListRequest.hpp index 7e60b4e763..c6f8f6a4fd 100644 --- a/src/drivers/uavcan_v1/Services/ListRequest.hpp +++ b/src/drivers/uavcan_v1/Services/ListRequest.hpp @@ -55,7 +55,7 @@ class UavcanListServiceRequest : public UavcanServiceRequest { public: UavcanListServiceRequest(CanardInstance &ins) : - UavcanServiceRequest(ins, "List", uavcan_register_List_1_0_FIXED_PORT_ID_, + UavcanServiceRequest(ins, "", "List", uavcan_register_List_1_0_FIXED_PORT_ID_, uavcan_register_List_Response_1_0_EXTENT_BYTES_) { }; diff --git a/src/drivers/uavcan_v1/Services/ServiceRequest.hpp b/src/drivers/uavcan_v1/Services/ServiceRequest.hpp index 1424e9dfae..0dabb65fe2 100644 --- a/src/drivers/uavcan_v1/Services/ServiceRequest.hpp +++ b/src/drivers/uavcan_v1/Services/ServiceRequest.hpp @@ -59,8 +59,9 @@ public: class UavcanServiceRequest : public UavcanBaseSubscriber { public: - UavcanServiceRequest(CanardInstance &ins, const char *subject_name, CanardPortID portID, size_t extent) : - UavcanBaseSubscriber(ins, subject_name, 0), _portID(portID), _extent(extent) { }; + UavcanServiceRequest(CanardInstance &ins, const char *prefix_name, const char *subject_name, CanardPortID portID, + size_t extent) : + UavcanBaseSubscriber(ins, prefix_name, subject_name, 0), _portID(portID), _extent(extent) { }; void subscribe() override diff --git a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp index c0a32cd5e7..06160b3667 100644 --- a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp @@ -51,8 +51,8 @@ class UavcanBaseSubscriber { public: - UavcanBaseSubscriber(CanardInstance &ins, const char *subject_name, uint8_t instance = 0) : - _canard_instance(ins), _instance(instance) + UavcanBaseSubscriber(CanardInstance &ins, const char *prefix_name, const char *subject_name, uint8_t instance = 0) : + _canard_instance(ins), _prefix_name(prefix_name), _instance(instance) { _subj_sub._subject_name = subject_name; _subj_sub._canard_sub.user_reference = this; @@ -146,6 +146,7 @@ protected: }; CanardInstance &_canard_instance; + const char *_prefix_name; SubjectSubscription _subj_sub; uint8_t _instance {0}; /// TODO: 'type' parameter? uavcan.pub.PORT_NAME.type (see 384.Access.1.0.uavcan) diff --git a/src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp b/src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp index d6b5b49c89..767ec10dfc 100644 --- a/src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp +++ b/src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp @@ -58,7 +58,7 @@ class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber { public: UavcanBmsSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanDynamicPortSubscriber(ins, pmgr, "energy_source", instance) + UavcanDynamicPortSubscriber(ins, pmgr, "ds_015.", "energy_source", instance) { _subj_sub.next = &_status_sub; diff --git a/src/drivers/uavcan_v1/Subscribers/DS-015/Esc.hpp b/src/drivers/uavcan_v1/Subscribers/DS-015/Esc.hpp index 1bbc3bea54..90f26e5dbe 100644 --- a/src/drivers/uavcan_v1/Subscribers/DS-015/Esc.hpp +++ b/src/drivers/uavcan_v1/Subscribers/DS-015/Esc.hpp @@ -55,7 +55,7 @@ class UavcanEscSubscriber : public UavcanDynamicPortSubscriber { public: UavcanEscSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanDynamicPortSubscriber(ins, pmgr, "esc", instance) { }; + UavcanDynamicPortSubscriber(ins, pmgr, "ds_015.", "esc", instance) { }; void subscribe() override { diff --git a/src/drivers/uavcan_v1/Subscribers/DS-015/Gnss.hpp b/src/drivers/uavcan_v1/Subscribers/DS-015/Gnss.hpp index 43e38ef9c4..d768f01ea8 100644 --- a/src/drivers/uavcan_v1/Subscribers/DS-015/Gnss.hpp +++ b/src/drivers/uavcan_v1/Subscribers/DS-015/Gnss.hpp @@ -50,7 +50,7 @@ class UavcanGnssSubscriber : public UavcanDynamicPortSubscriber { public: UavcanGnssSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanDynamicPortSubscriber(ins, pmgr, "gps", instance) { }; + UavcanDynamicPortSubscriber(ins, pmgr, "ds_015.", "gps", instance) { }; void subscribe() override { diff --git a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp index fabbd5c028..7a18f19184 100644 --- a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp @@ -54,9 +54,9 @@ class UavcanDynamicPortSubscriber : public UavcanBaseSubscriber { public: - UavcanDynamicPortSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, const char *subject_name, - uint8_t instance = 0) : - UavcanBaseSubscriber(ins, subject_name, instance), _param_manager(pmgr) { }; + UavcanDynamicPortSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, const char *prefix_name, + const char *subject_name, uint8_t instance = 0) : + UavcanBaseSubscriber(ins, prefix_name, subject_name, instance), _param_manager(pmgr) { }; void updateParam() { @@ -64,7 +64,7 @@ public: while (curSubj != nullptr) { char uavcan_param[90]; - snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.sub.%s.%d.id", curSubj->_subject_name, _instance); + snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.sub.%s%s.%d.id", _prefix_name, curSubj->_subject_name, _instance); // Set _port_id from _uavcan_param uavcan_register_Value_1_0 value; @@ -86,7 +86,8 @@ public: // Subscribe on the new port ID curSubj->_canard_sub.port_id = (CanardPortID)new_id; - PX4_INFO("Subscribing %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub.port_id); + PX4_INFO("Subscribing %s%s.%d on port %d", _prefix_name, curSubj->_subject_name, _instance, + curSubj->_canard_sub.port_id); subscribe(); } } diff --git a/src/drivers/uavcan_v1/Subscribers/Heartbeat.hpp b/src/drivers/uavcan_v1/Subscribers/Heartbeat.hpp index d863488497..acd3667344 100644 --- a/src/drivers/uavcan_v1/Subscribers/Heartbeat.hpp +++ b/src/drivers/uavcan_v1/Subscribers/Heartbeat.hpp @@ -51,7 +51,7 @@ class UavcanHeartbeatSubscriber : public UavcanBaseSubscriber { public: UavcanHeartbeatSubscriber(CanardInstance &ins) : - UavcanBaseSubscriber(ins, "Heartbeat", 0) { }; + UavcanBaseSubscriber(ins, "", "Heartbeat", 0) { }; void subscribe() override { diff --git a/src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp b/src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp index ef26c6520b..3273c25ba8 100644 --- a/src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp +++ b/src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp @@ -53,7 +53,7 @@ class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber { public: UavcanLegacyBatteryInfoSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanDynamicPortSubscriber(ins, pmgr, "legacy_bms", instance) { }; + UavcanDynamicPortSubscriber(ins, pmgr, "legacy.", "legacy_bms", instance) { }; void subscribe() override { diff --git a/src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.cpp b/src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.cpp new file mode 100644 index 0000000000..87a3c2d39f --- /dev/null +++ b/src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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 uorb_template.cpp + * +* Defines generic, templatized uORB over UAVCANv1 subscriber + * + * @author Peter van der Perk + * @author Jacob Crabill + */ + +#include "uorb_subscriber.hpp" + +/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */ + +/* ---- Specializations of convert() to convert incompatbile data, instance no. timestamp ---- */ + +template<> +void uORB_over_UAVCAN_Subscriber::convert(sensor_gps_s *data) +{ + /* HOTFIX as long as we don't have UAVCAN timesyncronization we update the timestamp on arrival */ + data->timestamp = hrt_absolute_time(); +} diff --git a/src/drivers/uavcan_v1/Subscribers/uORB/sensor_gps.hpp b/src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.hpp similarity index 61% rename from src/drivers/uavcan_v1/Subscribers/uORB/sensor_gps.hpp rename to src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.hpp index 8fdf4de1fb..98a46524f4 100644 --- a/src/drivers/uavcan_v1/Subscribers/uORB/sensor_gps.hpp +++ b/src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.hpp @@ -32,56 +32,84 @@ ****************************************************************************/ /** - * @file sensor_gps.hpp + * @file uorb_template.hpp * - * Defines uORB over UAVCANv1 sensor_gps subscriber +* Defines generic, templatized uORB over UAVCANv1 publisher * * @author Peter van der Perk */ #pragma once -#include -#include - #include "../DynamicPortSubscriber.hpp" -class UORB_over_UAVCAN_sensor_gps_Subscriber : public UavcanDynamicPortSubscriber +#include +#include + +#include + +template +class uORB_over_UAVCAN_Subscriber : public UavcanDynamicPortSubscriber { public: - UORB_over_UAVCAN_sensor_gps_Subscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanDynamicPortSubscriber(ins, pmgr, "uorb.sensor_gps", instance) { }; + uORB_over_UAVCAN_Subscriber(CanardInstance &ins, UavcanParamManager &pmgr, const orb_metadata *meta, + uint8_t instance = 0) : + UavcanDynamicPortSubscriber(ins, pmgr, "uorb.", meta->o_name, instance), + _uorb_meta{meta}, + _uorb_pub(meta) + {}; + + ~uORB_over_UAVCAN_Subscriber() override = default; void subscribe() override { + T *data = NULL; + // Subscribe to messages uORB sensor_gps payload over UAVCAN canardRxSubscribe(&_canard_instance, CanardTransferKindMessage, _subj_sub._canard_sub.port_id, - sizeof(struct sensor_gps_s), + get_payload_size(data), CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000, &_subj_sub._canard_sub); }; void callback(const CanardTransfer &receive) override { - //PX4_INFO("uORB sensor_gps Callback"); + T *data = (T *)receive.payload; - if (receive.payload_size == sizeof(struct sensor_gps_s)) { - sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload; - gps_msg->timestamp = hrt_absolute_time(); + if (receive.payload_size == get_payload_size(data)) { - /* As long as we don't have timesync between nodes we set the timestamp to the current time */ + /* Data type specific conversion if necceary */ + convert(data); - _sensor_gps_pub.publish(*gps_msg); + _uorb_pub.publish(*data); } else { - PX4_ERR("uORB over UAVCAN %s playload size mismatch got %d expected %d", - _subj_sub._subject_name, receive.payload_size, sizeof(struct sensor_gps_s)); + PX4_ERR("uORB over UAVCAN %s payload size mismatch got %d expected %d", + _subj_sub._subject_name, receive.payload_size, get_payload_size(data)); } }; -private: - uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; +protected: + // Default payload-size function -- can specialize in derived class + size_t get_payload_size(const T *msg) + { + (void)msg; + return sizeof(T); + }; + void convert(T *data) {}; + +private: + const orb_metadata *_uorb_meta; + uORB::PublicationMulti _uorb_pub; }; + +/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */ + + +/* ---- Specializations of convert() to convert incompatbile data, instance no. timestamp ---- */ + +template<> +void uORB_over_UAVCAN_Subscriber::convert(sensor_gps_s *data); diff --git a/src/drivers/uavcan_v1/SubscriptionManager.cpp b/src/drivers/uavcan_v1/SubscriptionManager.cpp index 98eb24283d..2801383a80 100644 --- a/src/drivers/uavcan_v1/SubscriptionManager.cpp +++ b/src/drivers/uavcan_v1/SubscriptionManager.cpp @@ -64,6 +64,7 @@ void SubscriptionManager::subscribe() #endif _access_rsp.subscribe(); + _list_rsp.subscribe(); updateDynamicSubscriptions(); } diff --git a/src/drivers/uavcan_v1/SubscriptionManager.hpp b/src/drivers/uavcan_v1/SubscriptionManager.hpp index 8520c3e317..519d5d12be 100644 --- a/src/drivers/uavcan_v1/SubscriptionManager.hpp +++ b/src/drivers/uavcan_v1/SubscriptionManager.hpp @@ -76,13 +76,14 @@ #include "ServiceClients/GetInfo.hpp" #include "ServiceClients/Access.hpp" +#include "ServiceClients/List.hpp" #include "Subscribers/BaseSubscriber.hpp" #include "Subscribers/Heartbeat.hpp" #include "Subscribers/DS-015/Battery.hpp" #include "Subscribers/DS-015/Esc.hpp" #include "Subscribers/DS-015/Gnss.hpp" #include "Subscribers/legacy/LegacyBatteryInfo.hpp" -#include "Subscribers/uORB/sensor_gps.hpp" +#include "Subscribers/uORB/uorb_subscriber.hpp" typedef struct { UavcanDynamicPortSubscriber *(*create_sub)(CanardInstance &ins, UavcanParamManager &pmgr) {}; @@ -116,6 +117,7 @@ private: // Process register requests UavcanAccessResponse _access_rsp {_canard_instance, _param_manager}; + UavcanListResponse _list_rsp {_canard_instance, _param_manager}; const UavcanDynSubBinder _uavcan_subs[UAVCAN_SUB_COUNT] { #if CONFIG_UAVCAN_V1_ESC_SUBSCRIBER @@ -172,7 +174,7 @@ private: { [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { - return new UORB_over_UAVCAN_sensor_gps_Subscriber(ins, pmgr, 0); + return new uORB_over_UAVCAN_Subscriber(ins, pmgr, ORB_ID(sensor_gps)); }, "uorb.sensor_gps", 0 diff --git a/src/drivers/uavcan_v1/parameters.c b/src/drivers/uavcan_v1/parameters.c index a650ca27b2..67b5741b0a 100644 --- a/src/drivers/uavcan_v1/parameters.c +++ b/src/drivers/uavcan_v1/parameters.c @@ -141,6 +141,16 @@ PARAM_DEFINE_INT32(UCAN1_LG_BMS_SUB, -1); */ PARAM_DEFINE_INT32(UCAN1_UORB_GPS, -1); + +/** + * sensor_gps uORB over UAVCAN v1 publication port ID. + * + * @min -1 + * @max 6143 + * @group UAVCAN v1 + */ +PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1); + // Publication Port IDs /**