niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

fix calculation of next timer call (#291)

Browse source 
* fix calculation of next timer call

* fix logic for period zero
This commit is contained in:
Dirk Thomas 2018-09-06 17:41:51 -07:00 • committed by GitHub
parent e78e400018
commit 16adb1e4f6
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 33 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

37
rcl/src/rcl/timer.c
View file

@ -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,37 @@ 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) {
if (0 == period) {
// a timer with a period of zero is considered always ready
next_call_time = unow;
} else {
// 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 +200,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 +310,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;