diff --git a/platforms/common/uORB/Publication.hpp b/platforms/common/uORB/Publication.hpp index 7547e3b7fa..d92310a5e1 100644 --- a/platforms/common/uORB/Publication.hpp +++ b/platforms/common/uORB/Publication.hpp @@ -84,7 +84,7 @@ protected: { if (_handle != nullptr) { // don't automatically unadvertise queued publications (eg vehicle_command) - if (Manager::orb_get_queue_size(_handle) == 1) { + if ((Manager::orb_get_queue_size(_handle) == 1) && (Manager::orb_get_subscriber_count(_handle) <= 0)) { unadvertise(); } } diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index 5b5530694b..d1d608633e 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -455,6 +455,11 @@ void uORB::Manager::orb_remove_internal_subscriber(void *node_handle) uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) { return static_cast(node_handle)->get_queue_size(); } +int8_t uORB::Manager::orb_get_subscriber_count(const void *node_handle) +{ + return static_cast(node_handle)->subscriber_count(); +} + bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated) { if (!is_advertised(node_handle)) { diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp index 1c2870b1ee..ae235a896d 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -445,6 +445,8 @@ public: static uint8_t orb_get_queue_size(const void *node_handle); + static int8_t orb_get_subscriber_count(const void *node_handle); + static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated); static bool register_callback(void *node_handle, SubscriptionCallback *callback_sub);