uavcan_v1: Add PublicationManager and uORB Publisher (#17863)

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

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 <peter.vanderperk@nxp.com>
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+
+#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();
+	}
+}

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 <peter.vanderperk@nxp.com>
+ * @author Jacob Crabill <jacob@flyvoly.com>
+ */
+
+#pragma once
+
+
+#include <px4_platform_common/defines.h>
+#include <drivers/drv_hrt.h>
+#include "Publishers/Publisher.hpp"
+#include "CanardInterface.hpp"
+
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/sensor_gps.h>
+
+#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<UavcanPublisher *> _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<actuator_outputs_s>(ins, pmgr, ORB_ID(actuator_outputs));
+			},
+			"uorb.actuator_outputs",
+			0
+		},
+
+		{
+			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
+			{
+				return new uORB_over_UAVCAN_Publisher<sensor_gps_s>(ins, pmgr, ORB_ID(sensor_gps));
+			},
+			"uorb.sensor_gps",
+			0
+		},
+	};
+};

src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp

@@ -45,7 +45,7 @@
 #include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
 #include <reg/drone/service/gnss/DilutionOfPrecision_0_1.h>
 
-#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
 	{

src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp

@@ -45,7 +45,7 @@
 // DS-15 Specification Messages
 #include <reg/drone/service/common/Readiness_0_1.h>
 
-#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
 	{

src/drivers/uavcan_v1/Publishers/Publisher.hpp

@@ -44,6 +44,7 @@
 #include <px4_platform_common/px4_config.h>
 
 #include <lib/parameters/param.h>
+#include <containers/List.hpp>
 
 #include <uavcan/_register/Access_1_0.h>
 
@@ -56,12 +57,14 @@
  */
 #define PUBLISHER_DEFAULT_TIMEOUT_USEC 100000UL
 
-class UavcanPublisher
+class UavcanPublisher : public ListNode<UavcanPublisher *>
 {
 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};
 };

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 <peter.vanderperk@nxp.com>
+ * @author Jacob Crabill <jacob@flyvoly.com>
  */
 
-#pragma once
+#include "uorb_publisher.hpp"
 
-#include <uORB/topics/sensor_gps.h>
+/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */
 
-#include "../Publisher.hpp"
+template<>
-
+size_t uORB_over_UAVCAN_Publisher<actuator_outputs_s>::get_payload_size(actuator_outputs_s *msg)
-class UORB_over_UAVCAN_sensor_gps_Publisher : public UavcanPublisher
 {
-public:
+	// Remove unvalid output & padding from payload_size to save bandwidth
-	UORB_over_UAVCAN_sensor_gps_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
+	return sizeof(struct actuator_outputs_s) - sizeof(msg->_padding0) -
-		UavcanPublisher(ins, pmgr, "sensor_gps", instance)
+	       ((sizeof(msg->output) / sizeof(msg->output[0]) - msg->noutputs) * sizeof(msg->output[0]));
-	{};
+}
-
-	// 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)};
-};

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 <peter.vanderperk@nxp.com>
  */
 
 #pragma once
 
-#include <uORB/topics/actuator_outputs.h>
-
 #include "../Publisher.hpp"
 
-class UORB_over_UAVCAN_actuator_outputs_Publisher : public UavcanPublisher
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/actuator_outputs.h>
+
+template <class T>
+class uORB_over_UAVCAN_Publisher : public UavcanPublisher
 {
 public:
-	UORB_over_UAVCAN_actuator_outputs_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
+	uORB_over_UAVCAN_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, const orb_metadata *meta,
-		UavcanPublisher(ins, pmgr, "actuator_outputs", instance)
+				   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) {
+		if (_uorb_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
-			actuator_outputs_s actuator_msg {};
+			T data {};
-			_actuator_outputs_sub.update(&actuator_msg);
+			_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_size   = get_payload_size(&data),
-				.payload        = &actuator_msg,
+				.payload        = &data,
 			};
 
 			// set the data ready in the buffer and chop if needed
@@ -78,14 +84,20 @@ public:
 		}
 	};
 
-private:
+protected:
-
+	// Default payload-size function -- can specialize in derived class
-	// Remove unvalid output & padding from payload_size to save bandwidth
+	size_t get_payload_size(T *msg)
-	size_t actuator_payload_size(actuator_outputs_s *msg)
 	{
-		return sizeof(struct actuator_outputs_s) - sizeof(msg->_padding0) -
+		(void)msg;
-		       ((sizeof(msg->output) / sizeof(msg->output[0]) - msg->noutputs) * sizeof(msg->output[0]));
+		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<actuator_outputs_s>::get_payload_size(actuator_outputs_s *msg);

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

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;

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

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

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

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

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) {
+	_pub_manager.updateParams();
-		publisher->updateParam();
-	}
 
 	_sub_manager.subscribe();
 
@@ -185,12 +183,8 @@ void UavcanNode::Run()
 		// update parameters from storage
 		updateParams();
 
-		for (auto &publisher : _publishers) {
+		// Update dynamic pub/sub objects based on Port ID params
-			// Have the publisher update its associated port-id parameter
+		_pub_manager.updateParams();
-			// Setting to 0 disable publication
-			publisher->updateParam();
-		}
-
 		_sub_manager.updateParams();
 
 		_mixing_output.updateParams();
@@ -205,9 +199,7 @@ void UavcanNode::Run()
 	sendHeartbeat();
 
 	// Check all publishers
-	for (auto &publisher : _publishers) {
+	_pub_manager.update();
-		publisher->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) {
+	_pub_manager.printInfo();
-		publisher->printInfo();
-	}
 
 	CanardRxSubscription *rxs = _canard_instance.rx_subscriptions[CanardTransferKindMessage];
 
-	while (rxs != NULL) {
+	while (rxs != nullptr) {
-		if (rxs->user_reference == NULL) {
+		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) {
+	while (rxs != nullptr) {
-		if (rxs->user_reference == NULL) {
+		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) {
+	while (rxs != nullptr) {
-		if (rxs->user_reference == NULL) {
+		if (rxs->user_reference == nullptr) {
 			PX4_INFO("Service request port id %d", rxs->port_id);
 
 		} else {

src/drivers/uavcan_v1/Uavcan.hpp

@@ -46,6 +46,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_gps.h>
@@ -58,11 +59,11 @@
 #include "CanardInterface.hpp"
 
 #include "Publishers/Publisher.hpp"
-#include "Publishers/Gnss.hpp"
+#include "Publishers/uORB/uorb_publisher.hpp"
-#include "Publishers/uORB/actuator_outputs.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};
+	/// TODO: Integrate with PublicationManager
-
-	UORB_over_UAVCAN_actuator_outputs_Publisher _actuator_pub {_canard_instance, _param_manager}; // uORB
-
 	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};
 
 };