From a1b1f2d48ab5a7f4cfcf019545609cfd6080fede Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 31 May 2024 23:02:11 +0300 Subject: [PATCH] uORB: change last_updated timestamp into device node Signed-off-by: Jukka Laitinen --- platforms/common/uORB/Subscription.hpp | 4 ++-- platforms/common/uORB/SubscriptionInterval.hpp | 6 +++--- platforms/common/uORB/uORBDeviceNode.cpp | 5 +++-- platforms/common/uORB/uORBDeviceNode.hpp | 12 +++++++++++- platforms/common/uORB/uORBManager.hpp | 5 +++-- 5 files changed, 22 insertions(+), 10 deletions(-) diff --git a/platforms/common/uORB/Subscription.hpp b/platforms/common/uORB/Subscription.hpp index eefa7e310413..a7ed0c1f35ac 100644 --- a/platforms/common/uORB/Subscription.hpp +++ b/platforms/common/uORB/Subscription.hpp @@ -121,13 +121,13 @@ class Subscription /** * Check if there is a new update. */ - bool updated() + bool updated(hrt_abstime last_update = 0, uint32_t interval_us = 0) { if (!valid()) { subscribe(); } - return valid() ? Manager::updates_available(_node, _last_generation) : false; + return valid() ? Manager::updates_available(_node, _last_generation, last_update, interval_us) : false; } /** diff --git a/platforms/common/uORB/SubscriptionInterval.hpp b/platforms/common/uORB/SubscriptionInterval.hpp index 045011a00634..9c71e7dda4b3 100644 --- a/platforms/common/uORB/SubscriptionInterval.hpp +++ b/platforms/common/uORB/SubscriptionInterval.hpp @@ -93,8 +93,8 @@ class SubscriptionInterval * */ bool updated() { - if (advertised() && (hrt_elapsed_time(&_last_update) >= _interval_us)) { - return _subscription.updated(); + if (advertised()) { + return _subscription.updated(_last_update, _interval_us); } return false; @@ -153,7 +153,7 @@ class SubscriptionInterval * Set the interval in microseconds * @param interval The interval in microseconds. */ - void set_interval_us(uint32_t interval) { _interval_us = interval; _last_update = hrt_absolute_time() - interval; } + void set_interval_us(uint32_t interval) { _interval_us = interval; } /** * Set the interval in milliseconds diff --git a/platforms/common/uORB/uORBDeviceNode.cpp b/platforms/common/uORB/uORBDeviceNode.cpp index c34e70843cd7..71b635f40925 100644 --- a/platforms/common/uORB/uORBDeviceNode.cpp +++ b/platforms/common/uORB/uORBDeviceNode.cpp @@ -579,6 +579,8 @@ uORB::DeviceNode::write(const char *buffer, const orb_metadata *meta, orb_advert /* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */ unsigned generation = _generation.fetch_add(1); + hrt_abstime now = hrt_absolute_time(); + _last_update = now; memcpy(((uint8_t *)node_data(handle)) + o_size * (generation % _queue_size), buffer, o_size); @@ -592,8 +594,7 @@ uORB::DeviceNode::write(const char *buffer, const orb_metadata *meta, orb_advert while (callbacks.handle_valid(cb)) { EventWaitItem *item = callbacks.peek(cb); - if (item->interval_us == 0 || hrt_elapsed_time(&item->last_update) >= item->interval_us) { - + if (item->interval_us == 0 || now - item->last_update >= item->interval_us) { #ifdef CONFIG_BUILD_FLAT if (item->subscriber != nullptr) { diff --git a/platforms/common/uORB/uORBDeviceNode.hpp b/platforms/common/uORB/uORBDeviceNode.hpp index 83e1d5752e33..8f110790a50b 100644 --- a/platforms/common/uORB/uORBDeviceNode.hpp +++ b/platforms/common/uORB/uORBDeviceNode.hpp @@ -196,7 +196,15 @@ class DeviceNode * We can get the correct value regardless of wrap-around or not. * @param generation The generation of subscriber */ - unsigned updates_available(unsigned generation) const { return _data_valid ? _generation.load() - generation : 0; } + + unsigned updates_available(unsigned generation, hrt_abstime last_update = 0, uint32_t interval_us = 0) const + { + if (_data_valid && (interval_us == 0 || _last_update - last_update >= interval_us)) { + return _generation.load() - generation; + } + + return 0; + } const orb_metadata *get_meta() const { return get_orb_meta(_orb_id); } @@ -359,6 +367,8 @@ class DeviceNode void unlock() { px4_sem_post(&_lock); } px4_sem_t _lock; /**< lock to protect access to all class members */ + hrt_abstime _last_update; + #ifdef CONFIG_BUILD_FLAT char *_devname; void *_data{nullptr}; diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp index 96e95c7b04b5..4c905059c548 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -400,9 +400,10 @@ class Manager return -1; } - static unsigned updates_available(const orb_advert_t &node_handle, unsigned last_generation) + static unsigned updates_available(const orb_advert_t &node_handle, unsigned last_generation, + hrt_abstime last_update = 0, uint32_t interval = 0) { - return node(node_handle)->updates_available(last_generation); + return node(node_handle)->updates_available(last_generation, last_update, interval); } static bool has_publisher(ORB_ID orb_id, uint8_t instance)