uORB: Initialize the _last_update properly for the new SubscriptionIn…

…tervals Set the last_update to now-interval, instead of 0. Signed-off-by: Jukka Laitinen <[email protected]>
tiiuae · Feb 23, 2024 · 415da09 · 415da09
1 parent 513c554
commit 415da09
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 4 deletions.
diff --git a/platforms/common/uORB/SubscriptionCallback.hpp b/platforms/common/uORB/SubscriptionCallback.hpp
@@ -199,10 +199,11 @@ class SubscriptionPollable : public SubscriptionInterval
 	 * Constructor
 	 *
 	 * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
+	 * @param interval The requested maximum update interval in microseconds.
 	 * @param instance The instance for multi sub.
 	 */
-	SubscriptionPollable(const orb_metadata *meta, uint8_t instance = 0) :
-		SubscriptionInterval(meta, 0, instance)
+	SubscriptionPollable(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
+		SubscriptionInterval(meta, interval_us, instance)
 	{
 	}
 

diff --git a/platforms/common/uORB/SubscriptionInterval.hpp b/platforms/common/uORB/SubscriptionInterval.hpp
@@ -62,6 +62,7 @@ class SubscriptionInterval
 	 */
 	SubscriptionInterval(ORB_ID id, uint32_t interval_us = 0, uint8_t instance = 0) :
 		_subscription{id, instance},
+		_last_update(hrt_absolute_time() - interval_us),
 		_interval_us(interval_us)
 	{}
 
@@ -74,6 +75,7 @@ class SubscriptionInterval
 	 */
 	SubscriptionInterval(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
 		_subscription{meta, instance},
+		_last_update(hrt_absolute_time() - interval_us),
 		_interval_us(interval_us)
 	{}
 
@@ -140,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; }
+	void		set_interval_us(uint32_t interval) { _interval_us = interval; _last_update = hrt_absolute_time() - interval; }
 
 	/**
 	 * Set the interval in milliseconds

diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp
@@ -333,7 +333,7 @@ orb_sub_t uORB::Manager::orb_subscribe(const struct orb_metadata *meta)
 // Should only be called from old interface
 orb_sub_t uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
 {
-	uORB::SubscriptionInterval *sub = new uORB::SubscriptionPollable(meta, instance);
+	uORB::SubscriptionInterval *sub = new uORB::SubscriptionPollable(meta, 0, instance);
 
 	if (sub && !sub->valid()) {
 		// subscribe and advertise the topic