commit 5d4b098036dfb81aa6ac5d66c121ddc1dc6bcf4a
author Austin Schuh <austin.linux@gmail.com> Sat Apr 08 14:36:08 2017 -0700
committer Austin Schuh <austin.linux@gmail.com> Sun Apr 09 17:14:22 2017 -0700
tree c61b977b0c7f00c2df7af2c22f719058b49d804e
parent 25db126343102eebf65caaf8a5790f80293a2760

Added utility to compute phased loop offsets.

Change-Id: If9dfe61e20a8e93af39c1cfb2459eb5fbb1cbd3b

aos/common/util/phased_loop.h

diff --git a/aos/common/util/phased_loop.h b/aos/common/util/phased_loop.h
index fbfe954..958bd01 100644
--- a/aos/common/util/phased_loop.h
+++ b/aos/common/util/phased_loop.h
@@ -27,6 +27,28 @@
     Reset();
   }
 
+  // Updates the offset and interval.
+  void set_interval_and_offset(const monotonic_clock::duration interval,
+                               const monotonic_clock::duration offset) {
+    interval_ = interval;
+    offset_ = offset;
+    CHECK_GE(offset_, monotonic_clock::duration(0));
+    CHECK_GT(interval_, monotonic_clock::duration(0));
+    CHECK_LT(offset_, interval_);
+  }
+
+  // Computes the offset given an interval and a time that we should trigger.
+  static monotonic_clock::duration OffsetFromIntervalAndTime(
+      const monotonic_clock::duration interval,
+      const monotonic_clock::time_point monotonic_trigger) {
+    CHECK_GT(interval, monotonic_clock::duration(0));
+    return monotonic_trigger.time_since_epoch() -
+           (monotonic_trigger.time_since_epoch() / interval) * interval +
+           ((monotonic_trigger.time_since_epoch() >= monotonic_clock::zero())
+                ? monotonic_clock::zero()
+                : interval);
+  }
+
   // Resets the count of skipped iterations.
   // Iterate(monotonic_now) will return 1 and set sleep_time() to something
   // within interval of monotonic_now.
@@ -54,7 +76,7 @@
   monotonic_clock::time_point sleep_time() const { return last_time_; }
 
  private:
-  const monotonic_clock::duration interval_, offset_;
+  monotonic_clock::duration interval_, offset_;
 
   // The time we most recently slept until.
   monotonic_clock::time_point last_time_ = monotonic_clock::epoch();

aos/common/util/phased_loop_test.cc

diff --git a/aos/common/util/phased_loop_test.cc b/aos/common/util/phased_loop_test.cc
index b013c2e..b9d6e52 100644
--- a/aos/common/util/phased_loop_test.cc
+++ b/aos/common/util/phased_loop_test.cc
@@ -150,6 +150,29 @@
   EXPECT_EQ(InMs(1), loop.sleep_time());
 }
 
+// Tests OffsetFromIntervalAndTime for various edge conditions.
+TEST_F(PhasedLoopTest, OffsetFromIntervalAndTimeTest) {
+  PhasedLoop loop(milliseconds(1000), milliseconds(300));
+
+  EXPECT_EQ(milliseconds(1),
+            loop.OffsetFromIntervalAndTime(milliseconds(1000), InMs(1001)));
+
+  EXPECT_EQ(milliseconds(0),
+            loop.OffsetFromIntervalAndTime(milliseconds(1000), InMs(1000)));
+
+  EXPECT_EQ(milliseconds(0),
+            loop.OffsetFromIntervalAndTime(milliseconds(1000), InMs(0)));
+
+  EXPECT_EQ(milliseconds(999),
+            loop.OffsetFromIntervalAndTime(milliseconds(1000), InMs(-1)));
+
+  EXPECT_EQ(milliseconds(7),
+            loop.OffsetFromIntervalAndTime(milliseconds(1000), InMs(19115007)));
+
+  EXPECT_EQ(milliseconds(7), loop.OffsetFromIntervalAndTime(milliseconds(1000),
+                                                            InMs(-19115993)));
+}
+
 // Tests that passing invalid values to the constructor dies correctly.
 TEST_F(PhasedLoopDeathTest, InvalidValues) {
   EXPECT_DEATH(PhasedLoop(milliseconds(1), milliseconds(2)),