From ab46b8026ce792beb351fbf3ec9abab2b51436ee Mon Sep 17 00:00:00 2001
From: JacobCrabill <jacob@flyvoly.com>
Date: Wed, 30 Jun 2021 10:31:23 -0700
Subject: [PATCH] uavcan_v1: Don't re-subscribe to fixed port ID subscribers

---
 src/drivers/uavcan_v1/SubscriptionManager.cpp | 7 ++++++-
 src/drivers/uavcan_v1/SubscriptionManager.hpp | 2 ++
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/drivers/uavcan_v1/SubscriptionManager.cpp b/src/drivers/uavcan_v1/SubscriptionManager.cpp
index 4a93683281..9718992b80 100644
--- a/src/drivers/uavcan_v1/SubscriptionManager.cpp
+++ b/src/drivers/uavcan_v1/SubscriptionManager.cpp
@@ -60,6 +60,11 @@ void SubscriptionManager::subscribe()
 	_getinfo_rsp.subscribe();
 	_access_rsp.subscribe();
 
+	updateDynamicSubscriptions();
+}
+
+void SubscriptionManager::updateDynamicSubscriptions()
+{
 	for (auto &sub : _uavcan_subs) {
 		if (sub.instance == NULL) {
 			param_t param_handle = param_find(sub.px4_name);
@@ -124,5 +129,5 @@ void SubscriptionManager::updateParams()
 	}
 
 	// Check for any newly-enabled subscriptions
-	subscribe();
+	updateDynamicSubscriptions();
 }
diff --git a/src/drivers/uavcan_v1/SubscriptionManager.hpp b/src/drivers/uavcan_v1/SubscriptionManager.hpp
index c340899b82..2256bb0052 100644
--- a/src/drivers/uavcan_v1/SubscriptionManager.hpp
+++ b/src/drivers/uavcan_v1/SubscriptionManager.hpp
@@ -74,6 +74,8 @@ public:
 	void updateParams();
 
 private:
+	void updateDynamicSubscriptions();
+
 	CanardInstance &_canard_instance;
 	UavcanParamManager &_param_manager;
 	UavcanDynamicPortSubscriber *_dynsubscribers {NULL};