diff --git a/src/drivers/uavcan_v1/CMakeLists.txt b/src/drivers/uavcan_v1/CMakeLists.txt index 362d23bf53..12ba82d19a 100644 --- a/src/drivers/uavcan_v1/CMakeLists.txt +++ b/src/drivers/uavcan_v1/CMakeLists.txt @@ -76,6 +76,8 @@ px4_add_module( ${LIBCANARD_DIR}/libcanard/ ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated SRCS + PublicationManager.cpp + PublicationManager.hpp SubscriptionManager.cpp SubscriptionManager.hpp ParamManager.hpp @@ -84,6 +86,7 @@ px4_add_module( NodeManager.cpp Uavcan.cpp Uavcan.hpp + Publishers/uORB/uorb_publisher.cpp ${SRCS} o1heap/o1heap.c o1heap/o1heap.h diff --git a/src/drivers/uavcan_v1/PublicationManager.cpp b/src/drivers/uavcan_v1/PublicationManager.cpp new file mode 100644 index 0000000000..ff69f687e7 --- /dev/null +++ b/src/drivers/uavcan_v1/PublicationManager.cpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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 PublicationManager.cpp + * + * Manages the dynamic (run-time configurable) UAVCAN publications + * + * @author Peter van der Perk + * @author Jacob Crabill + */ + + +#include "PublicationManager.hpp" + +PublicationManager::~PublicationManager() +{ + _dynpublishers.clear(); +} + +void PublicationManager::updateDynamicPublications() +{ + for (auto &sub : _uavcan_pubs) { + + bool found_publisher = false; + + for (auto &dynpub : _dynpublishers) { + // Check if subscriber has already been created + const char *subj_name = dynpub->getSubjectName(); + const uint8_t instance = dynpub->getInstance(); + + if (strcmp(subj_name, sub.subject_name) == 0 && instance == sub.instance) { + found_publisher = true; + break; + } + } + + if (found_publisher) { + continue; + } + + char uavcan_param[90]; + snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.pub.%s.%d.id", sub.subject_name, sub.instance); + uavcan_register_Value_1_0 value; + + if (_param_manager.GetParamByName(uavcan_param, value)) { + uint16_t port_id = value.natural16.value.elements[0]; + + if (port_id <= CANARD_PORT_ID_MAX) { // PortID is set, create a subscriber + UavcanPublisher *dynpub = sub.create_pub(_canard_instance, _param_manager); + + if (dynpub == nullptr) { + PX4_ERR("Out of memory"); + return; + } + + _dynpublishers.add(dynpub); + + dynpub->updateParam(); + } + + } else { + PX4_ERR("Port ID param for publisher %s.%u not found", sub.subject_name, sub.instance); + return; + } + } +} + +void PublicationManager::printInfo() +{ + for (auto &dynpub : _dynpublishers) { + dynpub->printInfo(); + } +} + +void PublicationManager::updateParams() +{ + for (auto &dynpub : _dynpublishers) { + dynpub->updateParam(); + } + + // Check for any newly-enabled publication + updateDynamicPublications(); +} + +void PublicationManager::update() +{ + for (auto &dynpub : _dynpublishers) { + dynpub->update(); + } +} diff --git a/src/drivers/uavcan_v1/PublicationManager.hpp b/src/drivers/uavcan_v1/PublicationManager.hpp new file mode 100644 index 0000000000..82397f931e --- /dev/null +++ b/src/drivers/uavcan_v1/PublicationManager.hpp @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * 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 PublicationManager.hpp + * + * Manages the dynamic (run-time configurable) UAVCAN publications + * + * @author Peter van der Perk + * @author Jacob Crabill + */ + +#pragma once + + +#include +#include +#include "Publishers/Publisher.hpp" +#include "CanardInterface.hpp" + +#include +#include + +#include "Actuators/EscClient.hpp" +#include "Publishers/DS-015/Readiness.hpp" +#include "Publishers/DS-015/Gnss.hpp" +#include "Publishers/uORB/uorb_publisher.hpp" + +typedef struct { + UavcanPublisher *(*create_pub)(CanardInstance &ins, UavcanParamManager &pmgr) {}; + const char *subject_name; + const uint8_t instance; +} UavcanDynPubBinder; + +class PublicationManager +{ +public: + PublicationManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _param_manager(pmgr) {} + ~PublicationManager(); + + void update(); + void printInfo(); + void updateParams(); + +private: + void updateDynamicPublications(); + + CanardInstance &_canard_instance; + UavcanParamManager &_param_manager; + List _dynpublishers; + + const UavcanDynPubBinder _uavcan_pubs[5] { + { + [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * + { + return new UavcanGnssPublisher(ins, pmgr, 0); + }, + "gps", + 0 + }, + { + [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * + { + return new UavcanEscController(ins, pmgr); + }, + "esc", + 0 + }, + { + [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * + { + return new UavcanReadinessPublisher(ins, pmgr, 0); + }, + "readiness", + 0 + }, + { + [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * + { + return new uORB_over_UAVCAN_Publisher(ins, pmgr, ORB_ID(actuator_outputs)); + }, + "uorb.actuator_outputs", + 0 + }, + + { + [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * + { + return new uORB_over_UAVCAN_Publisher(ins, pmgr, ORB_ID(sensor_gps)); + }, + "uorb.sensor_gps", + 0 + }, + }; +}; diff --git a/src/drivers/uavcan_v1/Publishers/Gnss.hpp b/src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp similarity index 98% rename from src/drivers/uavcan_v1/Publishers/Gnss.hpp rename to src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp index ae8e078c3c..2f3097d908 100644 --- a/src/drivers/uavcan_v1/Publishers/Gnss.hpp +++ b/src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp @@ -45,7 +45,7 @@ #include #include -#include "Publisher.hpp" +#include "../Publisher.hpp" class UavcanGnssPublisher : public UavcanPublisher { @@ -56,6 +56,8 @@ public: }; + ~UavcanGnssPublisher() override = default; + // Update the uORB Subscription and broadcast a UAVCAN message virtual void update() override { diff --git a/src/drivers/uavcan_v1/Publishers/Readiness.hpp b/src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp similarity index 98% rename from src/drivers/uavcan_v1/Publishers/Readiness.hpp rename to src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp index eb2756ba22..6ae3775d7b 100644 --- a/src/drivers/uavcan_v1/Publishers/Readiness.hpp +++ b/src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp @@ -45,7 +45,7 @@ // DS-15 Specification Messages #include -#include "Publisher.hpp" +#include "../Publisher.hpp" class UavcanReadinessPublisher : public UavcanPublisher { @@ -56,6 +56,8 @@ public: }; + ~UavcanReadinessPublisher() override = default; + // Update the uORB Subscription and broadcast a UAVCAN message virtual void update() override { diff --git a/src/drivers/uavcan_v1/Publishers/Publisher.hpp b/src/drivers/uavcan_v1/Publishers/Publisher.hpp index 6c6e3653ac..590381110e 100644 --- a/src/drivers/uavcan_v1/Publishers/Publisher.hpp +++ b/src/drivers/uavcan_v1/Publishers/Publisher.hpp @@ -44,6 +44,7 @@ #include #include +#include #include @@ -56,12 +57,14 @@ */ #define PUBLISHER_DEFAULT_TIMEOUT_USEC 100000UL -class UavcanPublisher +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) { }; + virtual ~UavcanPublisher() = default; + // Update the uORB Subscription and broadcast a UAVCAN message virtual void update() = 0; @@ -101,6 +104,26 @@ public: } } + const char *getSubjectName() + { + return _subject_name; + } + + uint8_t getInstance() + { + return _instance; + } + + UavcanPublisher *next() + { + return _next_pub; + } + + void setNext(UavcanPublisher *next) + { + _next_pub = next; + } + protected: CanardInstance &_canard_instance; UavcanParamManager &_param_manager; @@ -109,4 +132,6 @@ protected: CanardPortID _port_id {CANARD_PORT_ID_UNSET}; CanardTransferID _transfer_id {0}; + + UavcanPublisher *_next_pub {nullptr}; }; diff --git a/src/drivers/uavcan_v1/Publishers/uORB/sensor_gps.hpp b/src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.cpp similarity index 55% rename from src/drivers/uavcan_v1/Publishers/uORB/sensor_gps.hpp rename to src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.cpp index b8ac5a9ce1..590f123b5b 100644 --- a/src/drivers/uavcan_v1/Publishers/uORB/sensor_gps.hpp +++ b/src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.cpp @@ -32,53 +32,22 @@ ****************************************************************************/ /** - * @file sensor_gps.hpp + * @file uorb_template.cpp * -* Defines uORB over UAVCANv1 sensor_gps publisher +* Defines generic, templatized uORB over UAVCANv1 publisher * * @author Peter van der Perk + * @author Jacob Crabill */ -#pragma once +#include "uorb_publisher.hpp" -#include +/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */ -#include "../Publisher.hpp" - -class UORB_over_UAVCAN_sensor_gps_Publisher : public UavcanPublisher +template<> +size_t uORB_over_UAVCAN_Publisher::get_payload_size(actuator_outputs_s *msg) { -public: - UORB_over_UAVCAN_sensor_gps_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanPublisher(ins, pmgr, "sensor_gps", instance) - {}; - - // Update the uORB Subscription and broadcast a UAVCAN message - virtual void update() override - { - // Not sure if actuator_armed is a good indication of readiness but seems close to it - if (_sensor_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) { - sensor_gps_s gps_msg {}; - _sensor_gps_sub.update(&gps_msg); - - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindMessage, - .port_id = _port_id, // This is the subject-ID. - .remote_node_id = CANARD_NODE_ID_UNSET, - .transfer_id = _transfer_id, - .payload_size = sizeof(struct sensor_gps_s), - .payload = &gps_msg, - }; - - if (result == 0) { - // set the data ready in the buffer and chop if needed - ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &transfer); - } - } - }; - -private: - uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)}; -}; + // Remove unvalid output & padding from payload_size to save bandwidth + return sizeof(struct actuator_outputs_s) - sizeof(msg->_padding0) - + ((sizeof(msg->output) / sizeof(msg->output[0]) - msg->noutputs) * sizeof(msg->output[0])); +} diff --git a/src/drivers/uavcan_v1/Publishers/uORB/actuator_outputs.hpp b/src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp similarity index 71% rename from src/drivers/uavcan_v1/Publishers/uORB/actuator_outputs.hpp rename to src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp index 42b80ac72c..b913181df7 100644 --- a/src/drivers/uavcan_v1/Publishers/uORB/actuator_outputs.hpp +++ b/src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp @@ -32,34 +32,40 @@ ****************************************************************************/ /** - * @file actuator_outputs.hpp + * @file uorb_template.hpp * -* Defines uORB over UAVCANv1 actuator_outputs publisher +* Defines generic, templatized uORB over UAVCANv1 publisher * * @author Peter van der Perk */ #pragma once -#include - #include "../Publisher.hpp" -class UORB_over_UAVCAN_actuator_outputs_Publisher : public UavcanPublisher +#include +#include + +template +class uORB_over_UAVCAN_Publisher : public UavcanPublisher { public: - UORB_over_UAVCAN_actuator_outputs_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanPublisher(ins, pmgr, "actuator_outputs", instance) + uORB_over_UAVCAN_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, const orb_metadata *meta, + uint8_t instance = 0) : + UavcanPublisher(ins, pmgr, meta->o_name, instance), + _uorb_meta{meta}, + _uorb_sub(meta) {}; + ~uORB_over_UAVCAN_Publisher() override = default; + // Update the uORB Subscription and broadcast a UAVCAN message - // FIXME think about update and limiting virtual void update() override { // Not sure if actuator_armed is a good indication of readiness but seems close to it - if (_actuator_outputs_sub.updated() && _port_id != CANARD_PORT_ID_UNSET && _port_id != 0) { - actuator_outputs_s actuator_msg {}; - _actuator_outputs_sub.update(&actuator_msg); + if (_uorb_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) { + T data {}; + _uorb_sub.update(&data); CanardTransfer transfer = { .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, @@ -68,8 +74,8 @@ public: .port_id = _port_id, // This is the subject-ID. .remote_node_id = CANARD_NODE_ID_UNSET, .transfer_id = _transfer_id, - .payload_size = actuator_payload_size(&actuator_msg), - .payload = &actuator_msg, + .payload_size = get_payload_size(&data), + .payload = &data, }; // set the data ready in the buffer and chop if needed @@ -78,14 +84,20 @@ public: } }; -private: - - // Remove unvalid output & padding from payload_size to save bandwidth - size_t actuator_payload_size(actuator_outputs_s *msg) +protected: + // Default payload-size function -- can specialize in derived class + size_t get_payload_size(T *msg) { - return sizeof(struct actuator_outputs_s) - sizeof(msg->_padding0) - - ((sizeof(msg->output) / sizeof(msg->output[0]) - msg->noutputs) * sizeof(msg->output[0])); + (void)msg; + return sizeof(T); } - uORB::Subscription _actuator_outputs_sub{ORB_ID(actuator_outputs)}; +private: + const orb_metadata *_uorb_meta; + uORB::Subscription _uorb_sub; }; + +/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */ + +template<> +size_t uORB_over_UAVCAN_Publisher::get_payload_size(actuator_outputs_s *msg); diff --git a/src/drivers/uavcan_v1/Services/ServiceRequest.hpp b/src/drivers/uavcan_v1/Services/ServiceRequest.hpp index 4a302bbbdd..81956de0b4 100644 --- a/src/drivers/uavcan_v1/Services/ServiceRequest.hpp +++ b/src/drivers/uavcan_v1/Services/ServiceRequest.hpp @@ -87,7 +87,7 @@ public: { PX4_INFO("Response"); - if (_response_callback != NULL) { + if (_response_callback != nullptr) { _response_callback->response_callback(receive); } }; @@ -99,6 +99,6 @@ protected: const CanardPortID _portID; const size_t _extent; - UavcanServiceRequestInterface *_response_callback = NULL; + UavcanServiceRequestInterface *_response_callback = nullptr; }; diff --git a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp index debec5e116..c0a32cd5e7 100644 --- a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp @@ -71,7 +71,7 @@ public: { SubjectSubscription *curSubj = &_subj_sub; - while (curSubj != NULL) { + while (curSubj != nullptr) { canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, curSubj->_canard_sub.port_id); curSubj = curSubj->next; } @@ -84,7 +84,7 @@ public: uint32_t i = 0; SubjectSubscription *curSubj = &_subj_sub; - while (curSubj != NULL) { + while (curSubj != nullptr) { if (instance == i) { return curSubj->_canard_sub.port_id; } @@ -104,7 +104,7 @@ public: SubjectSubscription *curSubj = &_subj_sub; - while (curSubj != NULL) { + while (curSubj != nullptr) { if (port_id == curSubj->_canard_sub.port_id) { return true; } @@ -129,7 +129,7 @@ public: { SubjectSubscription *curSubj = &_subj_sub; - while (curSubj != NULL) { + while (curSubj != nullptr) { if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) { PX4_INFO("Subscribed %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub.port_id); } @@ -142,7 +142,7 @@ protected: struct SubjectSubscription { CanardRxSubscription _canard_sub; const char *_subject_name; - struct SubjectSubscription *next {NULL}; + struct SubjectSubscription *next {nullptr}; }; CanardInstance &_canard_instance; diff --git a/src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp b/src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp index 5e255bd220..d6b5b49c89 100644 --- a/src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp +++ b/src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp @@ -68,7 +68,7 @@ public: _parameters_sub._subject_name = _parameters_name; _parameters_sub._canard_sub.user_reference = this; - _parameters_sub.next = NULL; + _parameters_sub.next = nullptr; } void subscribe() override diff --git a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp index 51304151f8..fabbd5c028 100644 --- a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp @@ -62,7 +62,7 @@ public: { SubjectSubscription *curSubj = &_subj_sub; - while (curSubj != NULL) { + while (curSubj != nullptr) { char uavcan_param[90]; snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.sub.%s.%d.id", curSubj->_subject_name, _instance); @@ -112,5 +112,5 @@ public: protected: UavcanParamManager &_param_manager; - UavcanDynamicPortSubscriber *_next_sub {NULL}; + UavcanDynamicPortSubscriber *_next_sub {nullptr}; }; diff --git a/src/drivers/uavcan_v1/SubscriptionManager.cpp b/src/drivers/uavcan_v1/SubscriptionManager.cpp index 1d9a9eb217..2e8bb3c682 100644 --- a/src/drivers/uavcan_v1/SubscriptionManager.cpp +++ b/src/drivers/uavcan_v1/SubscriptionManager.cpp @@ -48,7 +48,7 @@ SubscriptionManager::~SubscriptionManager() { UavcanDynamicPortSubscriber *dynsub; - while (_dynsubscribers != NULL) { + while (_dynsubscribers != nullptr) { dynsub = _dynsubscribers; _dynsubscribers = dynsub->next(); delete dynsub; @@ -72,7 +72,7 @@ void SubscriptionManager::updateDynamicSubscriptions() bool found_subscriber = false; UavcanDynamicPortSubscriber *dynsub = _dynsubscribers; - while (dynsub != NULL) { + while (dynsub != nullptr) { // Check if subscriber has already been created const char *subj_name = dynsub->getSubjectName(); const uint8_t instance = dynsub->getInstance(); @@ -99,12 +99,12 @@ void SubscriptionManager::updateDynamicSubscriptions() if (port_id <= CANARD_PORT_ID_MAX) { // PortID is set, create a subscriber dynsub = sub.create_sub(_canard_instance, _param_manager); - if (dynsub == NULL) { + if (dynsub == nullptr) { PX4_ERR("Out of memory"); return; } - if (_dynsubscribers == NULL) { + if (_dynsubscribers == nullptr) { // Set the head of our linked list _dynsubscribers = dynsub; @@ -112,7 +112,7 @@ void SubscriptionManager::updateDynamicSubscriptions() // Append the new subscriber to our linked list UavcanDynamicPortSubscriber *tmp = _dynsubscribers; - while (tmp->next() != NULL) { + while (tmp->next() != nullptr) { tmp = tmp->next(); } @@ -133,7 +133,7 @@ void SubscriptionManager::printInfo() { UavcanDynamicPortSubscriber *dynsub = _dynsubscribers; - while (dynsub != NULL) { + while (dynsub != nullptr) { dynsub->printInfo(); dynsub = dynsub->next(); } @@ -143,7 +143,7 @@ void SubscriptionManager::updateParams() { UavcanDynamicPortSubscriber *dynsub = _dynsubscribers; - while (dynsub != NULL) { + while (dynsub != nullptr) { dynsub->updateParam(); dynsub = dynsub->next(); } diff --git a/src/drivers/uavcan_v1/SubscriptionManager.hpp b/src/drivers/uavcan_v1/SubscriptionManager.hpp index 4073b71da0..df721635ad 100644 --- a/src/drivers/uavcan_v1/SubscriptionManager.hpp +++ b/src/drivers/uavcan_v1/SubscriptionManager.hpp @@ -78,7 +78,7 @@ private: CanardInstance &_canard_instance; UavcanParamManager &_param_manager; - UavcanDynamicPortSubscriber *_dynsubscribers {NULL}; + UavcanDynamicPortSubscriber *_dynsubscribers {nullptr}; UavcanHeartbeatSubscriber _heartbeat_sub {_canard_instance}; diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index fe9efe46e0..1dd6a6742d 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -82,9 +82,7 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) : _node_manager.subscribe(); - for (auto &publisher : _publishers) { - publisher->updateParam(); - } + _pub_manager.updateParams(); _sub_manager.subscribe(); @@ -185,12 +183,8 @@ void UavcanNode::Run() // update parameters from storage updateParams(); - for (auto &publisher : _publishers) { - // Have the publisher update its associated port-id parameter - // Setting to 0 disable publication - publisher->updateParam(); - } - + // Update dynamic pub/sub objects based on Port ID params + _pub_manager.updateParams(); _sub_manager.updateParams(); _mixing_output.updateParams(); @@ -205,9 +199,7 @@ void UavcanNode::Run() sendHeartbeat(); // Check all publishers - for (auto &publisher : _publishers) { - publisher->update(); - } + _pub_manager.update(); _node_manager.update(); @@ -221,7 +213,7 @@ void UavcanNode::Run() while (!_task_should_exit.load() && _can_interface->receive(&received_frame) > 0) { CanardTransfer receive{}; - CanardRxSubscription *subscription = NULL; + CanardRxSubscription *subscription = nullptr; int32_t result = canardRxAccept2(&_canard_instance, &received_frame, 0, &receive, &subscription); if (result < 0) { @@ -235,7 +227,7 @@ void UavcanNode::Run() // A transfer has been received, process it. // PX4_INFO("received Port ID: %d", receive.port_id); - if (subscription != NULL) { + if (subscription != nullptr) { UavcanBaseSubscriber *sub_instance = (UavcanBaseSubscriber *)subscription->user_reference; sub_instance->callback(receive); @@ -325,14 +317,12 @@ void UavcanNode::print_info() heap_diagnostics.peak_allocated, heap_diagnostics.peak_request_size, heap_diagnostics.oom_count); - for (auto &publisher : _publishers) { - publisher->printInfo(); - } + _pub_manager.printInfo(); CanardRxSubscription *rxs = _canard_instance.rx_subscriptions[CanardTransferKindMessage]; - while (rxs != NULL) { - if (rxs->user_reference == NULL) { + while (rxs != nullptr) { + if (rxs->user_reference == nullptr) { PX4_INFO("Message port id %d", rxs->port_id); } else { @@ -344,8 +334,8 @@ void UavcanNode::print_info() rxs = _canard_instance.rx_subscriptions[CanardTransferKindRequest]; - while (rxs != NULL) { - if (rxs->user_reference == NULL) { + while (rxs != nullptr) { + if (rxs->user_reference == nullptr) { PX4_INFO("Service response port id %d", rxs->port_id); } else { @@ -357,8 +347,8 @@ void UavcanNode::print_info() rxs = _canard_instance.rx_subscriptions[CanardTransferKindResponse]; - while (rxs != NULL) { - if (rxs->user_reference == NULL) { + while (rxs != nullptr) { + if (rxs->user_reference == nullptr) { PX4_INFO("Service request port id %d", rxs->port_id); } else { diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index f703ee0239..318e2eec83 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -58,11 +59,11 @@ #include "CanardInterface.hpp" #include "Publishers/Publisher.hpp" -#include "Publishers/Gnss.hpp" -#include "Publishers/uORB/actuator_outputs.hpp" +#include "Publishers/uORB/uorb_publisher.hpp" #include "NodeManager.hpp" +#include "PublicationManager.hpp" #include "SubscriptionManager.hpp" #include "Actuators/EscClient.hpp" /// TODO: Add EscServer.hpp for node-side service @@ -180,18 +181,12 @@ private: NodeManager _node_manager {_canard_instance, _param_manager}; + PublicationManager _pub_manager {_canard_instance, _param_manager}; SubscriptionManager _sub_manager {_canard_instance, _param_manager}; - UavcanGnssPublisher _gps_pub {_canard_instance, _param_manager}; - - UORB_over_UAVCAN_actuator_outputs_Publisher _actuator_pub {_canard_instance, _param_manager}; // uORB - + /// TODO: Integrate with PublicationManager UavcanEscController _esc_controller {_canard_instance, _param_manager}; - // Publication objects: Any object used to bridge a uORB message to a UAVCAN message - /// TODO: For some service implementations, it makes sense to have them be both Publishers and Subscribers - UavcanPublisher *_publishers[3] {&_gps_pub, &_esc_controller, &_actuator_pub}; - UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller}; };