Fix gyro reading code when it misses cycles
Change-Id: Ife59545495709eda7a9e4c9961a1b78125cd975b
diff --git a/aos/common/util/BUILD b/aos/common/util/BUILD
index 7f045bd..d176733 100644
--- a/aos/common/util/BUILD
+++ b/aos/common/util/BUILD
@@ -61,6 +61,7 @@
deps = [
'//aos/common/logging',
'//aos/common:time',
+ '//aos/common:queue_testutils',
],
)
@@ -201,3 +202,14 @@
'//aos/common/logging',
],
)
+
+cc_test(
+ name = 'phased_loop_test',
+ srcs = [
+ 'phased_loop_test.cc',
+ ],
+ deps = [
+ ':phased_loop',
+ '//aos/testing:googletest',
+ ],
+)
diff --git a/aos/common/util/phased_loop.cc b/aos/common/util/phased_loop.cc
index 4b9dcdb..b5804c8 100644
--- a/aos/common/util/phased_loop.cc
+++ b/aos/common/util/phased_loop.cc
@@ -1,20 +1,29 @@
#include "aos/common/util/phased_loop.h"
-#include <string.h>
-
-#include "aos/common/logging/logging.h"
-#include "aos/common/time.h"
-
namespace aos {
namespace time {
void PhasedLoopXMS(int ms, int offset) {
- // TODO(brians): Tests!
const Time frequency = Time::InMS(ms);
SleepUntil((Time::Now() / static_cast<int32_t>(frequency.ToNSec())) *
static_cast<int32_t>(frequency.ToNSec()) +
frequency + Time::InUS(offset));
}
+int PhasedLoop::Iterate(const Time &now) {
+ const Time next_time = Time::InNS(((now - offset_).ToNSec() + 1) /
+ interval_.ToNSec() * interval_.ToNSec()) +
+ ((now < offset_) ? Time::kZero : interval_) + offset_;
+
+ const Time difference = next_time - last_time_;
+ const int result = difference.ToNSec() / interval_.ToNSec();
+ CHECK_EQ(difference, interval_ * result);
+ CHECK_EQ(0, (next_time - offset_).ToNSec() % interval_.ToNSec());
+ CHECK_GE(next_time, now);
+ CHECK_LE(next_time - now, interval_);
+ last_time_ = next_time;
+ return result;
+}
+
} // namespace timing
} // namespace aos
diff --git a/aos/common/util/phased_loop.h b/aos/common/util/phased_loop.h
index f3c417a..fc0f247 100644
--- a/aos/common/util/phased_loop.h
+++ b/aos/common/util/phased_loop.h
@@ -1,16 +1,63 @@
#ifndef AOS_COMMON_UTIL_PHASED_LOOP_H_
#define AOS_COMMON_UTIL_PHASED_LOOP_H_
-#include <time.h>
-#include <string>
+#include "aos/common/time.h"
+
+#include "aos/common/logging/logging.h"
namespace aos {
namespace time {
// Will not be accurate if ms isn't a factor of 1000.
// offset is in us.
+// DEPRECATED(Brian): Use PhasedLoop instead.
void PhasedLoopXMS(int ms, int offset);
+// Handles sleeping until a fixed offset from some time interval.
+class PhasedLoop {
+ public:
+ // For example, with interval = 1s and offset = 0.1s this will fire at:
+ // 0.1s
+ // 1.1s
+ // ...
+ // 10000.1s
+ // offset must be >= Time::kZero and < interval.
+ PhasedLoop(const Time &interval, const Time &offset = Time::kZero)
+ : interval_(interval), offset_(offset), last_time_(offset) {
+ CHECK_GE(offset, Time::kZero);
+ CHECK_GT(interval, Time::kZero);
+ CHECK_LT(offset, interval);
+ Reset();
+ }
+
+ // Resets the count of skipped iterations.
+ // Iterate(now) will return 1 and set sleep_time() to something within
+ // interval of now.
+ void Reset(const Time &now = Time::Now()) { Iterate(now - interval_); }
+
+ // Calculates the next time to run after now.
+ // The result can be retrieved with sleep_time().
+ // Returns the number of iterations which have passed (1 if this is called
+ // often enough). This can be < 1 iff now goes backwards between calls.
+ int Iterate(const Time &now = Time::Now());
+
+ // Sleeps until the next time and returns the number of iterations which have
+ // passed.
+ int SleepUntilNext() {
+ const int r = Iterate(Time::Now());
+ SleepUntil(sleep_time());
+ return r;
+ }
+
+ const Time &sleep_time() const { return last_time_; }
+
+ private:
+ const Time interval_, offset_;
+
+ // The time we most recently slept until.
+ Time last_time_ = Time::kZero;
+};
+
} // namespace time
} // namespace aos
diff --git a/aos/common/util/phased_loop_test.cc b/aos/common/util/phased_loop_test.cc
new file mode 100644
index 0000000..5846f25
--- /dev/null
+++ b/aos/common/util/phased_loop_test.cc
@@ -0,0 +1,161 @@
+#include "aos/common/util/phased_loop.h"
+
+#include "gtest/gtest.h"
+
+#include "aos/common/queue_testutils.h"
+
+namespace aos {
+namespace time {
+namespace testing {
+
+class PhasedLoopTest : public ::testing::Test {
+ protected:
+ PhasedLoopTest() {
+ ::aos::common::testing::EnableTestLogging();
+ }
+};
+
+typedef PhasedLoopTest PhasedLoopDeathTest;
+
+TEST_F(PhasedLoopTest, Reset) {
+ {
+ PhasedLoop loop(Time::InMS(100), Time::kZero);
+
+ loop.Reset(Time::kZero);
+ EXPECT_EQ(Time::InMS(0), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::kZero));
+ EXPECT_EQ(Time::InMS(100), loop.sleep_time());
+
+ loop.Reset(Time::InMS(99));
+ EXPECT_EQ(Time::InMS(0), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(99)));
+ EXPECT_EQ(Time::InMS(100), loop.sleep_time());
+
+ loop.Reset(Time::InMS(100));
+ EXPECT_EQ(Time::InMS(100), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(199)));
+ EXPECT_EQ(Time::InMS(200), loop.sleep_time());
+
+ loop.Reset(Time::InMS(101));
+ EXPECT_EQ(Time::InMS(100), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(101)));
+ EXPECT_EQ(Time::InMS(200), loop.sleep_time());
+ }
+ {
+ PhasedLoop loop(Time::InMS(100), Time::InMS(1));
+ loop.Reset(Time::kZero);
+ EXPECT_EQ(Time::InMS(-99), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::kZero));
+ EXPECT_EQ(Time::InMS(1), loop.sleep_time());
+ }
+ {
+ PhasedLoop loop(Time::InMS(100), Time::InMS(99));
+
+ loop.Reset(Time::kZero);
+ EXPECT_EQ(Time::InMS(-1), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::kZero));
+ EXPECT_EQ(Time::InMS(99), loop.sleep_time());
+
+ loop.Reset(Time::InMS(98));
+ EXPECT_EQ(Time::InMS(-1), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(98)));
+ EXPECT_EQ(Time::InMS(99), loop.sleep_time());
+
+ loop.Reset(Time::InMS(99));
+ EXPECT_EQ(Time::InMS(99), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(99)));
+ EXPECT_EQ(Time::InMS(199), loop.sleep_time());
+
+ loop.Reset(Time::InMS(100));
+ EXPECT_EQ(Time::InMS(99), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(100)));
+ EXPECT_EQ(Time::InMS(199), loop.sleep_time());
+ }
+}
+
+TEST_F(PhasedLoopTest, Iterate) {
+ {
+ PhasedLoop loop(Time::InMS(100), Time::InMS(99));
+ loop.Reset(Time::kZero);
+ EXPECT_EQ(1, loop.Iterate(Time::kZero));
+ EXPECT_EQ(Time::InMS(99), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(100)));
+ EXPECT_EQ(Time::InMS(199), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(Time::InMS(100)));
+ EXPECT_EQ(Time::InMS(199), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(Time::InMS(101)));
+ EXPECT_EQ(Time::InMS(199), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(Time::InMS(198)));
+ EXPECT_EQ(Time::InMS(199), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(199)));
+ EXPECT_EQ(Time::InMS(299), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(300)));
+ EXPECT_EQ(Time::InMS(399), loop.sleep_time());
+ EXPECT_EQ(3, loop.Iterate(Time::InMS(600)));
+ EXPECT_EQ(Time::InMS(699), loop.sleep_time());
+ }
+ {
+ PhasedLoop loop(Time::InMS(100), Time::InMS(1));
+ loop.Reset(Time::kZero);
+ EXPECT_EQ(1, loop.Iterate(Time::kZero));
+ EXPECT_EQ(Time::InMS(1), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(100)));
+ EXPECT_EQ(Time::InMS(101), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(Time::InMS(100)));
+ EXPECT_EQ(Time::InMS(101), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(103)));
+ EXPECT_EQ(Time::InMS(201), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(Time::InMS(198)));
+ EXPECT_EQ(Time::InMS(201), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(Time::InMS(200)));
+ EXPECT_EQ(Time::InMS(201), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(201)));
+ EXPECT_EQ(Time::InMS(301), loop.sleep_time());
+ EXPECT_EQ(3, loop.Iterate(Time::InMS(600)));
+ EXPECT_EQ(Time::InMS(601), loop.sleep_time());
+ }
+}
+
+// Makes sure that everything works correctly when crossing zero.
+// This seems like a rare case at first, but starting from zero needs to
+// work, which means negatives should too.
+TEST_F(PhasedLoopTest, CrossingZero) {
+ PhasedLoop loop(Time::InMS(100), Time::InMS(1));
+ loop.Reset(Time::InMS(-1000));
+ EXPECT_EQ(Time::InMS(-1099), loop.sleep_time());
+ EXPECT_EQ(9, loop.Iterate(Time::InMS(-250)));
+ EXPECT_EQ(Time::InMS(-199), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(-199)));
+ EXPECT_EQ(Time::InMS(-99), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(-90)));
+ EXPECT_EQ(Time::InMS(1), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(Time::InMS(0)));
+ EXPECT_EQ(Time::InMS(1), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(1)));
+ EXPECT_EQ(Time::InMS(101), loop.sleep_time());
+
+ EXPECT_EQ(0, loop.Iterate(Time::InMS(2)));
+ EXPECT_EQ(Time::InMS(101), loop.sleep_time());
+
+ EXPECT_EQ(-2, loop.Iterate(Time::InMS(-101)));
+ EXPECT_EQ(Time::InMS(-99), loop.sleep_time());
+ EXPECT_EQ(1, loop.Iterate(Time::InMS(-99)));
+ EXPECT_EQ(Time::InMS(1), loop.sleep_time());
+
+ EXPECT_EQ(0, loop.Iterate(Time::InMS(-99)));
+ EXPECT_EQ(Time::InMS(1), loop.sleep_time());
+}
+
+// Tests that passing invalid values to the constructor dies correctly.
+TEST_F(PhasedLoopDeathTest, InvalidValues) {
+ EXPECT_DEATH(PhasedLoop(Time::InMS(1), Time::InMS(2)), ".*offset<interval.*");
+ EXPECT_DEATH(PhasedLoop(Time::InMS(1), Time::InMS(1)), ".*offset<interval.*");
+ EXPECT_DEATH(PhasedLoop(Time::InMS(1), Time::InMS(-1)),
+ ".*offset>=Time::kZero.*");
+ EXPECT_DEATH(PhasedLoop(Time::InMS(0), Time::InMS(0)),
+ ".*interval>Time::kZero.*");
+}
+
+} // namespace testing
+} // namespace time
+} // namespace aos