uavcan_v1: Publishers now SubscriptionCallbackWorkItems

Publishers now register the UavcanNode WorkItem for callback upon update
to uORB topics, triggering Publisher updates and immediate transmission

This should resolve transmission latency issues

src/drivers/uavcan_v1/ParamManager.hpp

@@ -128,7 +128,7 @@ private:
 		{"uavcan.sub.battery_status.0.id",     "UCAN1_BMS_BS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"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.sub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS_S",		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"},

src/drivers/uavcan_v1/PublicationManager.cpp

@@ -77,7 +77,7 @@ void PublicationManager::updateDynamicPublications()
 			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);
+				UavcanPublisher *dynpub = sub.create_pub(_canard_instance, _param_manager, _work_item);
 
 				if (dynpub == nullptr) {
 					PX4_ERR("Out of memory");

src/drivers/uavcan_v1/PublicationManager.hpp

@@ -48,10 +48,6 @@
 #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
@@ -67,7 +63,6 @@
 /* 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
@@ -86,7 +81,7 @@
 #include "Publishers/uORB/uorb_publisher.hpp"
 
 typedef struct {
-	UavcanPublisher *(*create_pub)(CanardInstance &ins, UavcanParamManager &pmgr) {};
+	UavcanPublisher *(*create_pub)(CanardInstance &ins, UavcanParamManager &pmgr, px4::WorkItem *wrk) {};
 	const char *subject_name;
 	const uint8_t instance;
 } UavcanDynPubBinder;
@@ -94,7 +89,8 @@ typedef struct {
 class PublicationManager
 {
 public:
-	PublicationManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _param_manager(pmgr) {}
+	PublicationManager(CanardInstance &ins, UavcanParamManager &pmgr, px4::WorkItem *wrk) :
+		_canard_instance(ins), _param_manager(pmgr), _work_item(wrk) {}
 	~PublicationManager();
 
 	void update();
@@ -106,35 +102,26 @@ private:
 
 	CanardInstance &_canard_instance;
 	UavcanParamManager &_param_manager;
+	px4::WorkItem *_work_item;
 	List<UavcanPublisher *> _dynpublishers;
 
 
 	const UavcanDynPubBinder _uavcan_pubs[UAVCAN_PUB_COUNT] {
 #if CONFIG_UAVCAN_V1_GNSS_PUBLISHER
 		{
-			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
+			[](CanardInstance & ins, UavcanParamManager & pmgr, px4::WorkItem * wrk) -> UavcanPublisher *
 			{
-				return new UavcanGnssPublisher(ins, pmgr, 0);
+				return new UavcanGnssPublisher(ins, pmgr, wrk, 0);
 			},
 			"gps",
 			0
 		},
 #endif
-#if CONFIG_UAVCAN_V1_ESC_CONTROLLER
-		{
-			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
-			{
-				return new UavcanEscController(ins, pmgr);
-			},
-			"esc",
-			0
-		},
-#endif
 #if CONFIG_UAVCAN_V1_READINESS_PUBLISHER
 		{
-			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
+			[](CanardInstance & ins, UavcanParamManager & pmgr, px4::WorkItem * wrk) -> UavcanPublisher *
 			{
-				return new UavcanReadinessPublisher(ins, pmgr, 0);
+				return new UavcanReadinessPublisher(ins, pmgr, wrk, 0);
 			},
 			"readiness",
 			0
@@ -142,9 +129,9 @@ private:
 #endif
 #if CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER
 		{
-			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
+			[](CanardInstance & ins, UavcanParamManager & pmgr, px4::WorkItem * wrk) -> UavcanPublisher *
 			{
-				return new uORB_over_UAVCAN_Publisher<actuator_outputs_s>(ins, pmgr, ORB_ID(actuator_outputs));
+				return new uORB_over_UAVCAN_Publisher<actuator_outputs_s>(ins, pmgr, wrk, ORB_ID(actuator_outputs));
 			},
 			"uorb.actuator_outputs",
 			0
@@ -152,9 +139,9 @@ private:
 #endif
 #if CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER
 		{
-			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher *
+			[](CanardInstance & ins, UavcanParamManager & pmgr, px4::WorkItem * wrk) -> UavcanPublisher *
 			{
-				return new uORB_over_UAVCAN_Publisher<sensor_gps_s>(ins, pmgr, ORB_ID(sensor_gps));
+				return new uORB_over_UAVCAN_Publisher<sensor_gps_s>(ins, pmgr, wrk, ORB_ID(sensor_gps));
 			},
 			"uorb.sensor_gps",
 			0

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

@@ -41,6 +41,8 @@
 
 #pragma once
 
+#include <uORB/SubscriptionCallback.hpp>
+
 // DS-15 Specification Messages
 #include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
 #include <reg/drone/service/gnss/DilutionOfPrecision_0_1.h>
@@ -49,11 +51,12 @@ class UavcanNode;
 
 #include "Publisher.hpp"
 
-class UavcanGnssPublisher : public UavcanPublisher
+class UavcanGnssPublisher : public UavcanPublisher, public uORB::SubscriptionCallbackWorkItem
 {
 public:
-	UavcanGnssPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
-		UavcanPublisher(ins, pmgr, "ds_015", "gps", instance)
+	UavcanGnssPublisher(CanardInstance &ins, UavcanParamManager &pmgr, px4::WorkItem *work_item, uint8_t instance = 0) :
+		UavcanPublisher(ins, pmgr, "ds_015", "gps", instance),
+		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_gps), instance)
 	{
 
 	};
@@ -63,9 +66,9 @@ public:
 	// Update the uORB Subscription and broadcast a UAVCAN message
 	virtual void update() override
 	{
-		if (_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
+		if (uORB::SubscriptionCallbackWorkItem::updated() && _port_id != CANARD_PORT_ID_UNSET) {
 			sensor_gps_s gps {};
-			_gps_sub.update(&gps);
+			uORB::SubscriptionCallbackWorkItem::update(&gps);
 
 			reg_drone_physics_kinematics_geodetic_Point_0_1 geo {};
 			geo.latitude = gps.lat;
@@ -134,7 +137,5 @@ public:
 
 private:
 
-	/// TODO: Allow >1 instance
-	uORB::Subscription _gps_sub{ORB_ID(sensor_gps)};
 	CanardTransferID _transfer_id_2 {0};
 };

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

@@ -42,16 +42,21 @@
 
 #pragma once
 
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/actuator_armed.h>
+
 // DS-15 Specification Messages
 #include <reg/drone/service/common/Readiness_0_1.h>
 
 #include "../Publisher.hpp"
 
-class UavcanReadinessPublisher : public UavcanPublisher
+class UavcanReadinessPublisher : public UavcanPublisher, public uORB::SubscriptionCallbackWorkItem
 {
 public:
-	UavcanReadinessPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
-		UavcanPublisher(ins, pmgr, "ds_015", "readiness", instance)
+	UavcanReadinessPublisher(CanardInstance &ins, UavcanParamManager &pmgr, px4::WorkItem *work_item,
+				 uint8_t instance = 0) :
+		UavcanPublisher(ins, pmgr, "ds_015", "readiness", instance),
+		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(actuator_armed), instance)
 	{
 
 	};
@@ -62,10 +67,11 @@ public:
 	virtual void update() override
 	{
 		// Not sure if actuator_armed is a good indication of readiness but seems close to it
-		if (_actuator_armed_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
+		if (uORB::SubscriptionCallbackWorkItem::updated() && _port_id != CANARD_PORT_ID_UNSET) {
 			actuator_armed_s armed {};
-			_actuator_armed_sub.update(&armed);
+			uORB::SubscriptionCallbackWorkItem::update(&armed);
 
+			/// TODO: Replace this with Readiness publisher...
 			reg_drone_service_common_Readiness_0_1 readiness {};
 
 			if (armed.armed) {

src/drivers/uavcan_v1/Publishers/Publisher.hpp

@@ -43,6 +43,7 @@
 
 #include <px4_platform_common/px4_config.h>
 
+#include <uORB/SubscriptionCallback.hpp>
 #include <lib/parameters/param.h>
 #include <containers/List.hpp>
 

src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp

@@ -43,18 +43,17 @@
 
 #include "../Publisher.hpp"
 
-#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/actuator_outputs.h>
 
 template <class T>
-class uORB_over_UAVCAN_Publisher : public UavcanPublisher
+class uORB_over_UAVCAN_Publisher : public UavcanPublisher, public uORB::SubscriptionCallbackWorkItem
 {
 public:
-	uORB_over_UAVCAN_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, const orb_metadata *meta,
-				   uint8_t instance = 0) :
+	uORB_over_UAVCAN_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, px4::WorkItem *work_item,
+				   const orb_metadata *meta, uint8_t instance = 0) :
 		UavcanPublisher(ins, pmgr, "uorb.", meta->o_name, instance),
-		_uorb_meta{meta},
-		_uorb_sub(meta)
+		uORB::SubscriptionCallbackWorkItem(work_item, meta, instance)
 	{};
 
 	~uORB_over_UAVCAN_Publisher() override = default;
@@ -63,9 +62,9 @@ public:
 	virtual void update() override
 	{
 		// Not sure if actuator_armed is a good indication of readiness but seems close to it
-		if (_uorb_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
+		if (uORB::SubscriptionCallbackWorkItem::updated() && _port_id != CANARD_PORT_ID_UNSET) {
 			T data {};
-			_uorb_sub.update(&data);
+			uORB::SubscriptionCallbackWorkItem::update(&data);
 
 			CanardTransfer transfer = {
 				.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
@@ -82,6 +81,9 @@ public:
 			++_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
 			canardTxPush(&_canard_instance, &transfer);
 			transmit();
+
+			// ensure callback is registered
+			uORB::SubscriptionCallbackWorkItem::registerCallback();
 		}
 	};
 
@@ -94,8 +96,8 @@ protected:
 	}
 
 private:
-	const orb_metadata *_uorb_meta;
-	uORB::Subscription _uorb_sub;
+	// const orb_metadata *_uorb_meta;
+	// uORB::Subscription _uorb_sub;
 };
 
 /* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */

src/drivers/uavcan_v1/Uavcan.cpp

@@ -78,7 +78,8 @@ static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree
 UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
-	_can_interface(interface)
+	_can_interface(interface),
+	_pub_manager(_canard_instance, _param_manager, this)
 {
 	pthread_mutex_init(&_node_mutex, nullptr);
 
@@ -248,8 +249,30 @@ void UavcanNode::Run()
 
 	transmit();
 
-	/* Process received messages */
+	// Process received messages
+	receive();
 
+	// Pop canardTx queue to send out responses to requets
+	transmit();
+
+	perf_end(_cycle_perf);
+
+	if (_instance && _task_should_exit.load()) {
+		ScheduleClear();
+
+		if (_initialized &&  _can_interface != nullptr) {
+			_can_interface->close();
+			_initialized = false;
+		}
+
+		_instance = nullptr;
+	}
+
+	pthread_mutex_unlock(&_node_mutex);
+}
+
+void UavcanNode::receive()
+{
 	uint8_t data[64] {};
 	CanardFrame received_frame{};
 	received_frame.payload = &data;
@@ -285,24 +308,6 @@ void UavcanNode::Run()
 			//PX4_INFO("RX canard %d", result);
 		}
 	}
-
-	// Pop canardTx queue to send out responses to requets
-	transmit();
-
-	perf_end(_cycle_perf);
-
-	if (_instance && _task_should_exit.load()) {
-		ScheduleClear();
-
-		if (_initialized &&  _can_interface != nullptr) {
-			_can_interface->close();
-			_initialized = false;
-		}
-
-		_instance = nullptr;
-	}
-
-	pthread_mutex_unlock(&_node_mutex);
 }
 
 void UavcanNode::transmit()

src/drivers/uavcan_v1/Uavcan.hpp

@@ -140,6 +140,7 @@ public:
 
 	static UavcanNode *instance() { return _instance; }
 
+	static void do_receive() { if (_instance) _instance->receive(); };
 	static void do_transmit() { if (_instance) _instance->transmit(); };
 
 	/* The bit rate that can be passed back to the bootloader */
@@ -150,6 +151,7 @@ private:
 	void Run() override;
 	void fill_node_info();
 
+	void receive();
 	void transmit();
 
 	// Sends a heartbeat at 1s intervals
@@ -195,12 +197,15 @@ private:
 	NodeClient *_node_client {nullptr};
 #endif
 
-	PublicationManager _pub_manager {_canard_instance, _param_manager};
+	PublicationManager _pub_manager;// {_canard_instance, _param_manager, this};
 	SubscriptionManager _sub_manager {_canard_instance, _param_manager};
 
-	/// TODO: Integrate with PublicationManager
+#if CONFIG_UAVCAN_V1_ESC_CONTROLLER
+	/// NOTE: This needs to be part of the OutputModuleInterface to run as a ScheduledWorkItem,
+	///       so **DON'T** use it within the PublicationManager!!
 	UavcanEscController _esc_controller {_canard_instance, _param_manager};
 
 	UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
+#endif
 
 };

src/drivers/uavcan_v1/parameters.c

@@ -139,7 +139,7 @@ PARAM_DEFINE_INT32(UCAN1_LG_BMS_SUB, -1);
  * @max 6143
  * @group UAVCAN v1
  */
-PARAM_DEFINE_INT32(UCAN1_UORB_GPS, -1);
+PARAM_DEFINE_INT32(UCAN1_UORB_GPS_S, -1);
 
 
 /**