diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index 087d40f31..eea18e8e4 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -36,6 +36,8 @@ typedef struct rcl_timer_impl_t atomic_uint_least64_t period; // This is a time in nanoseconds since an unspecified time. atomic_uint_least64_t last_call_time; + // This is a time in nanoseconds since an unspecified time. + atomic_uint_least64_t next_call_time; // A flag which indicates if the timer is canceled. atomic_bool canceled; // The user supplied allocator. @@ -75,6 +77,7 @@ rcl_timer_init( atomic_init(&impl.callback, (uintptr_t)callback); atomic_init(&impl.period, period); atomic_init(&impl.last_call_time, now); + atomic_init(&impl.next_call_time, now + period); atomic_init(&impl.canceled, false); impl.allocator = allocator; timer->impl = (rcl_timer_impl_t *)allocator.allocate(sizeof(rcl_timer_impl_t), allocator.state); @@ -127,11 +130,32 @@ rcl_timer_call(rcl_timer_t * timer) if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } + if (now < 0) { + RCL_SET_ERROR_MSG("clock now returned negative time point value", *allocator); + return RCL_RET_ERROR; + } + uint64_t unow = (uint64_t)now; rcl_time_point_value_t previous_ns = - rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now); + rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, unow); rcl_timer_callback_t typed_callback = (rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback); + uint64_t next_call_time = rcl_atomic_load_uint64_t(&timer->impl->next_call_time); + uint64_t period = rcl_atomic_load_uint64_t(&timer->impl->period); + // always move the next call time by exactly period forward + // don't use now as the base to avoid extending each cycle by the time + // 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 < unow) { + // move the next call time forward by as many periods as necessary + uint64_t now_ahead = unow - next_call_time; + // rounding up without overflow + uint64_t periods_ahead = 1 + (now_ahead - 1) / period; + next_call_time += periods_ahead * period; + } + rcl_atomic_store(&timer->impl->next_call_time, next_call_time); + if (typed_callback != NULL) { int64_t since_last_call = now - previous_ns; typed_callback(timer, since_last_call); @@ -171,9 +195,8 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt if (ret != RCL_RET_OK) { return ret; // rcl error state should already be set. } - int64_t period = rcl_atomic_load_uint64_t(&timer->impl->period); *time_until_next_call = - (rcl_atomic_load_uint64_t(&timer->impl->last_call_time) + period) - now; + rcl_atomic_load_uint64_t(&timer->impl->next_call_time) - now; return RCL_RET_OK; } @@ -282,7 +305,8 @@ rcl_timer_reset(rcl_timer_t * timer) if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } - rcl_atomic_store(&timer->impl->last_call_time, now); + int64_t period = rcl_atomic_load_uint64_t(&timer->impl->period); + rcl_atomic_store(&timer->impl->next_call_time, now + period); rcl_atomic_store(&timer->impl->canceled, false); RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Timer successfully reset") return RCL_RET_OK;