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; }