From a644cf19c5ba4a8689e889d959e9206f1931f624 Mon Sep 17 00:00:00 2001
From: JacobCrabill <jacob@flyvoly.com>
Date: Thu, 1 Jul 2021 16:06:34 -0700
Subject: [PATCH] uavcan_v1: Update SubscriptionManager

Make _uavcan_subs struct 'const' again
Refactor updateDynamicSubscriptions()
Add getSubjectName(), getInstance() to BaseSubscriber class
---
 .../uavcan_v1/Subscribers/BaseSubscriber.hpp  | 10 +++
 src/drivers/uavcan_v1/SubscriptionManager.cpp | 75 ++++++++++++-------
 src/drivers/uavcan_v1/SubscriptionManager.hpp | 23 ++++--
 3 files changed, 71 insertions(+), 37 deletions(-)

diff --git a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp
index 6770a8da27..6d9d090698 100644
--- a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp
+++ b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp
@@ -117,6 +117,16 @@ public:
 		return false;
 	}
 
+	const char *getSubjectName()
+	{
+		return _subj_sub._subject_name;
+	}
+
+	uint8_t getInstance()
+	{
+		return _instance;
+	}
+
 	void printInfo()
 	{
 		SubjectSubscription *curSubj = &_subj_sub;
diff --git a/src/drivers/uavcan_v1/SubscriptionManager.cpp b/src/drivers/uavcan_v1/SubscriptionManager.cpp
index 9718992b80..d9f6149449 100644
--- a/src/drivers/uavcan_v1/SubscriptionManager.cpp
+++ b/src/drivers/uavcan_v1/SubscriptionManager.cpp
@@ -66,44 +66,61 @@ void SubscriptionManager::subscribe()
 void SubscriptionManager::updateDynamicSubscriptions()
 {
 	for (auto &sub : _uavcan_subs) {
-		if (sub.instance == NULL) {
-			param_t param_handle = param_find(sub.px4_name);
+		param_t param_handle = param_find(sub.px4_name);
 
-			if (param_handle == PARAM_INVALID) {
-				PX4_ERR("Param %s not found", sub.px4_name);
+		if (param_handle == PARAM_INVALID) {
+			PX4_ERR("Param %s not found", sub.px4_name);
+			break;
+		}
+
+		bool found_subscriber = false;
+		UavcanDynamicPortSubscriber *dynsub = _dynsubscribers;
+
+		while (dynsub != NULL) {
+			// Check if subscriber has already been created
+			const char *subj_name = dynsub->getSubjectName();
+			const uint8_t instance = dynsub->getInstance();
+
+			if (strcmp(subj_name, sub.subject_name) == 0 && instance == sub.instance) {
+				found_subscriber = true;
 				break;
 			}
 
-			if ((param_type(param_handle) == PARAM_TYPE_INT32)) {
-				int32_t port_id {};
-				param_get(param_handle, &port_id);
+			dynsub = dynsub->next();
+		}
 
-				if (port_id >= 0) { // PortID is set create a subscriber
-					UavcanDynamicPortSubscriber *dynsub = sub.create_sub(_canard_instance, _param_manager);
+		if (found_subscriber) {
+			continue;
+		}
 
-					if (dynsub == NULL) {
-						PX4_ERR("Out of memory");
-						return;
-					}
+		if ((param_type(param_handle) == PARAM_TYPE_INT32)) {
+			int32_t port_id {};
+			param_get(param_handle, &port_id);
 
-					if (_dynsubscribers == NULL) {
-						// Set the head of our linked list
-						_dynsubscribers = dynsub;
+			if (port_id >= 0) { // PortID is set create a subscriber
+				dynsub = sub.create_sub(_canard_instance, _param_manager);
 
-					} 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();
+				if (dynsub == NULL) {
+					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);
+				}
+
+				dynsub->updateParam();
 			}
 		}
 	}
diff --git a/src/drivers/uavcan_v1/SubscriptionManager.hpp b/src/drivers/uavcan_v1/SubscriptionManager.hpp
index 2256bb0052..6476c2191c 100644
--- a/src/drivers/uavcan_v1/SubscriptionManager.hpp
+++ b/src/drivers/uavcan_v1/SubscriptionManager.hpp
@@ -60,7 +60,8 @@
 typedef struct {
 	const char *px4_name;
 	UavcanDynamicPortSubscriber *(*create_sub)(CanardInstance &ins, UavcanParamManager &pmgr) {};
-	UavcanDynamicPortSubscriber *instance {nullptr};
+	const char *subject_name;
+	const uint8_t instance;
 } UavcanDynSubBinder;
 
 class SubscriptionManager
@@ -88,14 +89,15 @@ private:
 	// Process register requests
 	UavcanAccessResponse  _access_rsp {_canard_instance, _param_manager};
 
-	UavcanDynSubBinder _uavcan_subs[6] {
+	const UavcanDynSubBinder _uavcan_subs[6] {
 		{
 			"UCAN1_ESC0_PID",
 			[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
 			{
 				return new UavcanEscSubscriber(ins, pmgr, 0);
 			},
-			nullptr
+			"esc",
+			0
 		},
 		{
 			"UCAN1_GPS0_PID",
@@ -103,7 +105,8 @@ private:
 			{
 				return new UavcanGnssSubscriber(ins, pmgr, 0);
 			},
-			nullptr
+			"gps",
+			0
 		},
 		{
 			"UCAN1_GPS1_PID",
@@ -111,7 +114,8 @@ private:
 			{
 				return new UavcanGnssSubscriber(ins, pmgr, 1);
 			},
-			nullptr
+			"gps",
+			1
 		},
 		{
 			"UCAN1_BMS_ES_PID",
@@ -119,7 +123,8 @@ private:
 			{
 				return new UavcanBmsSubscriber(ins, pmgr, 0);
 			},
-			nullptr
+			"energy_source",
+			0
 		},
 		{
 			"UCAN1_LG_BMS_PID",
@@ -127,7 +132,8 @@ private:
 			{
 				return new UavcanLegacyBatteryInfoSubscriber(ins, pmgr, 0);
 			},
-			nullptr
+			"legacy_bms",
+			0
 		},
 		{
 			"UCAN1_UORB_GPS",
@@ -135,7 +141,8 @@ private:
 			{
 				return new UORB_over_UAVCAN_sensor_gps_Subscriber(ins, pmgr, 0);
 			},
-			nullptr
+			"uorb.sensor_gps",
+			0
 		},
 	};
 };