diff --git a/platforms/common/uORB/Subscription.hpp b/platforms/common/uORB/Subscription.hpp index 9fcad62179..496b8cf261 100644 --- a/platforms/common/uORB/Subscription.hpp +++ b/platforms/common/uORB/Subscription.hpp @@ -136,19 +136,40 @@ public: /** * Check if there is a new update. */ - bool updated() { return advertised() && Manager::updates_available(_node, _last_generation); } + bool updated() + { + if (!valid()) { + subscribe(); + } + + return valid() ? Manager::updates_available(_node, _last_generation) : false; + } /** * Update the struct * @param dst The uORB message struct we are updating. */ - bool update(void *dst) { return updated() && Manager::orb_data_copy(_node, dst, _last_generation); } + bool update(void *dst) + { + if (!valid()) { + subscribe(); + } + + return valid() ? Manager::orb_data_copy(_node, dst, _last_generation, true) : false; + } /** * Copy the struct * @param dst The uORB message struct we are updating. */ - bool copy(void *dst) { return advertised() && Manager::orb_data_copy(_node, dst, _last_generation); } + bool copy(void *dst) + { + if (!valid()) { + subscribe(); + } + + return valid() ? Manager::orb_data_copy(_node, dst, _last_generation, false) : false; + } /** * Change subscription instance diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index c0b45d950e..0634c4e253 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -172,7 +172,7 @@ int uORB::Manager::orb_ioctl(unsigned int cmd, unsigned long arg) case ORBIOCDEVDATACOPY: { orbiocdevdatacopy_t *data = (orbiocdevdatacopy_t *)arg; - data->ret = uORB::Manager::orb_data_copy(data->handle, data->dst, data->generation); + data->ret = uORB::Manager::orb_data_copy(data->handle, data->dst, data->generation, data->only_if_updated); } break; @@ -479,8 +479,16 @@ 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(); } -bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation) +bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated) { + if (!is_advertised(node_handle)) { + return false; + } + + if (only_if_updated && !static_cast(node_handle)->updates_available(generation)) { + return false; + } + return static_cast(node_handle)->copy(dst, generation); } @@ -509,7 +517,8 @@ uint8_t uORB::Manager::orb_get_instance(const void *node_handle) #if !defined(CONFIG_BUILD_FLAT) unsigned uORB::Manager::updates_available(const void *node_handle, unsigned last_generation) { - return static_cast(node_handle)->updates_available(last_generation); + return is_advertised(node_handle) ? static_cast(node_handle)->updates_available( + last_generation) : 0; } bool uORB::Manager::is_advertised(const void *node_handle) diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp index 7418a6aae2..d5df89605d 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -117,6 +117,7 @@ typedef struct { void *handle; void *dst; unsigned generation; + bool only_if_updated; bool ret; } orbiocdevdatacopy_t; @@ -450,7 +451,7 @@ public: static uint8_t orb_get_queue_size(const void *node_handle); - static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation); + 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); @@ -460,9 +461,10 @@ public: #if defined(CONFIG_BUILD_FLAT) /* These are optimized by inlining in NuttX Flat build */ - static unsigned updates_available(const void *node_handle, unsigned last_generation) { return static_cast(node_handle)->updates_available(last_generation); } + static unsigned updates_available(const void *node_handle, unsigned last_generation) { return is_advertised(node_handle) ? static_cast(node_handle)->updates_available(last_generation) : 0; } static bool is_advertised(const void *node_handle) { return static_cast(node_handle)->is_advertised(); } + #else static unsigned updates_available(const void *node_handle, unsigned last_generation); diff --git a/platforms/common/uORB/uORBManagerUsr.cpp b/platforms/common/uORB/uORBManagerUsr.cpp index 368e98119f..af00dbb9ba 100644 --- a/platforms/common/uORB/uORBManagerUsr.cpp +++ b/platforms/common/uORB/uORBManagerUsr.cpp @@ -303,9 +303,9 @@ uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) return data.size; } -bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation) +bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated) { - orbiocdevdatacopy_t data = {node_handle, dst, generation, false}; + orbiocdevdatacopy_t data = {node_handle, dst, generation, only_if_updated, false}; boardctl(ORBIOCDEVDATACOPY, reinterpret_cast(&data)); generation = data.generation;