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