uORB: Store the last publish timestamp in the device nodes

Signed-off-by: Jukka Laitinen <[email protected]>
tiiuae · Jun 6, 2024 · 8febdbc · 8febdbc
1 parent 9dc654d
commit 8febdbc
Show file tree

Hide file tree

Showing 5 changed files with 22 additions and 10 deletions.
diff --git 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
@@ -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;
@@ -142,7 +142,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
@@ -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
@@ -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); }
 
@@ -357,6 +365,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
@@ -392,9 +392,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)