From aa380f351fa01edfce71533c7eaefec4ff6446ee Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 6 May 2020 15:30:55 -0400 Subject: [PATCH] uORB: SubscriptionInterval last update shift forward with interval --- src/modules/uORB/SubscriptionInterval.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/uORB/SubscriptionInterval.hpp b/src/modules/uORB/SubscriptionInterval.hpp index dc392528db..c1ed8077db 100644 --- a/src/modules/uORB/SubscriptionInterval.hpp +++ b/src/modules/uORB/SubscriptionInterval.hpp @@ -47,6 +47,8 @@ #include "Subscription.hpp" +#include + namespace uORB { @@ -121,7 +123,9 @@ public: bool copy(void *dst) { if (_subscription.copy(dst)) { - _last_update = hrt_absolute_time(); + const hrt_abstime now = hrt_absolute_time(); + // shift last update time forward, but don't let it get further behind than the interval + _last_update = math::constrain(_last_update + _interval_us, now - _interval_us, now); return true; }