uavcan_v1: Fix SubscriptionManager dynamic update

src/drivers/uavcan_v1/ParamManager.hpp

@@ -47,23 +47,31 @@
 #include <uavcan/_register/Name_1_0.h>
 #include <uavcan/_register/Value_1_0.h>
 
+static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U;
 static constexpr uint16_t CANARD_PORT_ID_MAX   = 32767U;
 
 static bool px4_param_to_uavcan_port_id(param_t &in, uavcan_register_Value_1_0 &out)
 {
 	if (param_type(in) == PARAM_TYPE_INT32) {
 		int32_t out_val {};
-		param_get(in, &out_val);
+		const int res = param_get(in, &out_val);
+
+		if (res != PX4_OK) {
+			// Parameter not found / internal error
+			return false;
+		}
 
 		if (out_val >= 0 && out_val <= CANARD_PORT_ID_MAX) {
 			out.natural16.value.elements[0] = (uint16_t)out_val;
-			out.natural16.value.count = 1;
-			uavcan_register_Value_1_0_select_natural16_(&out);
-			return true;
 
 		} else {
-			return false;
+			// "Invalid" value -- set to "UNSET"
+			out.natural16.value.elements[0] = CANARD_PORT_ID_UNSET;
 		}
+
+		out.natural16.value.count = 1;
+		uavcan_register_Value_1_0_select_natural16_(&out);
+		return true;
 	}
 
 	return false;

src/drivers/uavcan_v1/Publishers/Gnss.hpp

@@ -59,8 +59,7 @@ 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 (_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
-		    && _port_id != 0) { //FIXME either make default param UNSET or handle 0 in base class
 			sensor_gps_s gps {};
 			_gps_sub.update(&gps);
 

src/drivers/uavcan_v1/Publishers/Readiness.hpp

@@ -60,7 +60,7 @@ 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 && _port_id != 0) {
+		if (_actuator_armed_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
 			actuator_armed_s armed {};
 			_actuator_armed_sub.update(&armed);
 

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

@@ -56,7 +56,7 @@ public:
 	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 && _port_id != 0) {
+		if (_sensor_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) {
 			sensor_gps_s gps_msg {};
 			_sensor_gps_sub.update(&gps_msg);
 

src/drivers/uavcan_v1/SubscriptionManager.cpp

@@ -61,26 +61,44 @@ void SubscriptionManager::subscribe()
 	_access_rsp.subscribe();
 
 	for (auto &sub : _uavcan_subs) {
-		param_t param_handle = param_find(sub.px4_name);
+		if (sub.instance == NULL) {
+			param_t param_handle = param_find(sub.px4_name);
 
-		if (param_handle == PARAM_INVALID) {
+			if (param_handle == PARAM_INVALID) {
-			PX4_ERR("Param %s not found", sub.px4_name);
+				PX4_ERR("Param %s not found", sub.px4_name);
-			break;
+				break;
-		}
+			}
 
-		if ((param_type(param_handle) == PARAM_TYPE_INT32)) {
+			if ((param_type(param_handle) == PARAM_TYPE_INT32)) {
-			int32_t port_id {};
+				int32_t port_id {};
-			param_get(param_handle, &port_id);
+				param_get(param_handle, &port_id);
 
-			if (port_id >= 0) { // PortID is set create a subscriber
+				if (port_id >= 0) { // PortID is set create a subscriber
-				UavcanDynamicPortSubscriber *dynsub = sub.create_sub(_canard_instance, _param_manager);
+					UavcanDynamicPortSubscriber *dynsub = sub.create_sub(_canard_instance, _param_manager);
 
-				if (_dynsubscribers != NULL) {
+					if (dynsub == NULL) {
-					_dynsubscribers->setNext(dynsub);
+						PX4_ERR("Out of memory");
+						return;
+					}
+
+					if (_dynsubscribers == NULL) {
+						// Set the head of our linked list
+						_dynsubscribers = dynsub;
+
+					} else {
+						// Append the new subscriber to our linked list
+						UavcanDynamicPortSubscriber *tmp = _dynsubscribers;
+
+						while (tmp->next() != NULL) {
+							tmp = tmp->next();
+						}
+
+						tmp->setNext(dynsub);
+					}
+
+					sub.instance = dynsub;
+					dynsub->updateParam();
 				}
-
-				_dynsubscribers = dynsub;
-				dynsub->updateParam();
 			}
 		}
 	}
@@ -104,4 +122,7 @@ void SubscriptionManager::updateParams()
 		dynsub->updateParam();
 		dynsub = dynsub->next();
 	}
+
+	// Check for any newly-enabled subscriptions
+	subscribe();
 }

src/drivers/uavcan_v1/SubscriptionManager.hpp

@@ -60,6 +60,7 @@
 typedef struct {
 	const char *px4_name;
 	UavcanDynamicPortSubscriber *(*create_sub)(CanardInstance &ins, UavcanParamManager &pmgr) {};
+	UavcanDynamicPortSubscriber *instance {nullptr};
 } UavcanDynSubBinder;
 
 class SubscriptionManager
@@ -85,48 +86,54 @@ private:
 	// Process register requests
 	UavcanAccessResponse  _access_rsp {_canard_instance, _param_manager};
 
-	const UavcanDynSubBinder _uavcan_subs[6] {
+	UavcanDynSubBinder _uavcan_subs[6] {
 		{
 			"UCAN1_ESC0_PID",
 			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
 			{
 				return new UavcanEscSubscriber(ins, pmgr, 0);
-			}
+			},
+			nullptr
 		},
 		{
 			"UCAN1_GPS0_PID",
 			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
 			{
-				return new UavcanGnssSubscriber(ins, pmgr, 1);
+				return new UavcanGnssSubscriber(ins, pmgr, 0);
-			}
+			},
+			nullptr
 		},
 		{
 			"UCAN1_GPS1_PID",
 			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
 			{
-				return new UavcanGnssSubscriber(ins, pmgr, 0);
+				return new UavcanGnssSubscriber(ins, pmgr, 1);
-			}
+			},
+			nullptr
 		},
 		{
 			"UCAN1_BMS_ES_PID",
 			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
 			{
 				return new UavcanBmsSubscriber(ins, pmgr, 0);
-			}
+			},
+			nullptr
 		},
 		{
 			"UCAN1_LG_BMS_PID",
 			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
 			{
 				return new UavcanLegacyBatteryInfoSubscriber(ins, pmgr, 0);
-			}
+			},
+			nullptr
 		},
 		{
 			"UCAN1_UORB_GPS",
 			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
 			{
 				return new UORB_over_UAVCAN_sensor_gps_Subscriber(ins, pmgr, 0);
-			}
+			},
+			nullptr
 		},
 	};
 };

src/drivers/uavcan_v1/Uavcan.cpp

@@ -364,7 +364,6 @@ void UavcanNode::print_info()
 	}
 
 	_mixing_output.printInfo();
-	_esc_controller.printInfo();
 
 	pthread_mutex_unlock(&_node_mutex);
 }