From 45fc9520216013007c2d4f3bd69d778af9051aa3 Mon Sep 17 00:00:00 2001 From: Thiemo Kohrt Date: Thu, 12 Oct 2023 21:45:26 +0200 Subject: [PATCH] next_call_time will always be greater than now after calling rcl_timer_call. (#1089) If next_call_time equals now, consecutive callback calls with the same timestamp can occur when using sim time. Signed-off-by: Thiemo Kohrt --- rcl/src/rcl/timer.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index 23b768d25..8030a653e 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -294,7 +294,7 @@ rcl_timer_call(rcl_timer_t * timer) // between the timer being ready and the callback being triggered next_call_time += period; // in case the timer has missed at least once cycle - if (next_call_time < now) { + if (next_call_time <= now) { if (0 == period) { // a timer with a period of zero is considered always ready next_call_time = now; @@ -302,7 +302,7 @@ rcl_timer_call(rcl_timer_t * timer) // move the next call time forward by as many periods as necessary int64_t now_ahead = now - next_call_time; // rounding up without overflow - int64_t periods_ahead = 1 + (now_ahead - 1) / period; + int64_t periods_ahead = 1 + now_ahead / period; next_call_time += periods_ahead * period; } }