Fix rounding in phased_loop
For 1 nanosecond every period, it would CHECK-fail.
Change-Id: I5b9d73eab87f84169697996923edb9e56c85ac2d
diff --git a/aos/util/BUILD b/aos/util/BUILD
index f487892..b23b720 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -267,7 +267,7 @@
deps = [
":phased_loop",
"//aos/testing:googletest",
- "//aos/testing:test_logging",
+ "//aos/time",
],
)
diff --git a/aos/util/phased_loop.cc b/aos/util/phased_loop.cc
index 7369a93..1e520b1 100644
--- a/aos/util/phased_loop.cc
+++ b/aos/util/phased_loop.cc
@@ -37,20 +37,28 @@
}
int PhasedLoop::Iterate(const monotonic_clock::time_point now) {
- const monotonic_clock::time_point next_time =
- monotonic_clock::time_point(
- (((now - offset_).time_since_epoch() + monotonic_clock::duration(1)) /
- interval_) *
- interval_) +
- ((now.time_since_epoch() < offset_) ? monotonic_clock::zero()
- : interval_) +
- offset_;
+ auto next_time = monotonic_clock::epoch();
+ // Round up to the next whole interval, ignoring offset_.
+ {
+ const auto offset_now = (now - offset_).time_since_epoch();
+ monotonic_clock::duration prerounding;
+ if (now.time_since_epoch() >= offset_) {
+ // We're above 0, so rounding up means away from 0.
+ prerounding = offset_now + interval_;
+ } else {
+ // We're below 0, so rounding up means towards 0.
+ prerounding = offset_now + monotonic_clock::duration(1);
+ }
+ next_time += (prerounding / interval_) * interval_;
+ }
+ // Add offset_ back in.
+ next_time += offset_;
const monotonic_clock::duration difference = next_time - last_time_;
const int result = difference / interval_;
CHECK_EQ(
0, (next_time - offset_).time_since_epoch().count() % interval_.count());
- CHECK(next_time >= now);
+ CHECK(next_time > now);
CHECK(next_time - now <= interval_);
last_time_ = next_time;
return result;
diff --git a/aos/util/phased_loop_test.cc b/aos/util/phased_loop_test.cc
index 4a80522..1c766c4 100644
--- a/aos/util/phased_loop_test.cc
+++ b/aos/util/phased_loop_test.cc
@@ -2,13 +2,14 @@
#include "gtest/gtest.h"
-#include "aos/testing/test_logging.h"
+#include "aos/time/time.h"
namespace aos {
namespace time {
namespace testing {
using ::std::chrono::milliseconds;
+using ::std::chrono::nanoseconds;
typedef ::testing::Test PhasedLoopTest;
typedef PhasedLoopTest PhasedLoopDeathTest;
@@ -191,6 +192,31 @@
".*interval > monotonic_clock::duration\\(0\\).*");
}
+// Tests that every single value within two intervals of 0 works.
+// This is good at finding edge cases in the rounding.
+TEST_F(PhasedLoopTest, SweepingZero) {
+ for (int i = -30; i < -20; ++i) {
+ PhasedLoop loop(nanoseconds(20),
+ monotonic_clock::epoch() - nanoseconds(30));
+ EXPECT_EQ(1, loop.Iterate(monotonic_clock::epoch() + nanoseconds(i)));
+ }
+ for (int i = -20; i < 0; ++i) {
+ PhasedLoop loop(nanoseconds(20),
+ monotonic_clock::epoch() - nanoseconds(30));
+ EXPECT_EQ(2, loop.Iterate(monotonic_clock::epoch() + nanoseconds(i)));
+ }
+ for (int i = 0; i < 20; ++i) {
+ PhasedLoop loop(nanoseconds(20),
+ monotonic_clock::epoch() - nanoseconds(30));
+ EXPECT_EQ(3, loop.Iterate(monotonic_clock::epoch() + nanoseconds(i)));
+ }
+ for (int i = 20; i < 30; ++i) {
+ PhasedLoop loop(nanoseconds(20),
+ monotonic_clock::epoch() - nanoseconds(30));
+ EXPECT_EQ(4, loop.Iterate(monotonic_clock::epoch() + nanoseconds(i)));
+ }
+}
+
} // namespace testing
} // namespace time
} // namespace aos