From eb6387cd0422a5f88ee50dcbd637d8b809db0d5c Mon Sep 17 00:00:00 2001
From: Dirk Thomas <dirk-thomas@users.noreply.github.com>
Date: Fri, 31 Aug 2018 16:03:18 -0700
Subject: [PATCH] fix calculation of next timer call

---
 rcl/src/rcl/timer.c | 32 ++++++++++++++++++++++++++++----
 1 file changed, 28 insertions(+), 4 deletions(-)

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;