From 66546c42f5680df665e286071983be184dd6f5a7 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Thu, 22 Feb 2024 11:52:45 +0200 Subject: [PATCH] uORB: Initialize the _last_update properly for the new SubscriptionIntervals Set the last_update to now-interval, instead of 0. Signed-off-by: Jukka Laitinen --- platforms/common/uORB/SubscriptionCallback.hpp | 5 +++-- platforms/common/uORB/SubscriptionInterval.hpp | 4 +++- platforms/common/uORB/uORBManager.cpp | 2 +- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/platforms/common/uORB/SubscriptionCallback.hpp b/platforms/common/uORB/SubscriptionCallback.hpp index 941a3237ff6b..8d365ba02b58 100644 --- 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 index e8261c39bb03..83f27a520dea 100644 --- 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 index 291d84271d7d..eda7ac60922e 100644 --- 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