Fix gyro reading code when it misses cycles
Change-Id: Ife59545495709eda7a9e4c9961a1b78125cd975b
diff --git a/aos/common/time.cc b/aos/common/time.cc
index d1985b5..019cb52 100644
--- a/aos/common/time.cc
+++ b/aos/common/time.cc
@@ -47,6 +47,14 @@
} // namespace
+const int32_t Time::kNSecInSec;
+const int32_t Time::kNSecInMSec;
+const int32_t Time::kNSecInUSec;
+const int32_t Time::kMSecInSec;
+const int32_t Time::kUSecInSec;
+
+const Time Time::kZero{0, 0};
+
void Time::EnableMockTime(const Time &now) {
MutexLocker time_mutex_locker(&time_mutex);
mock_time_enabled = true;
diff --git a/aos/common/time.h b/aos/common/time.h
index 9afe9a0..226cbc2 100644
--- a/aos/common/time.h
+++ b/aos/common/time.h
@@ -44,6 +44,9 @@
static const int32_t kNSecInUSec = 1000;
static const int32_t kMSecInSec = 1000;
static const int32_t kUSecInSec = 1000000;
+
+ static const Time kZero;
+
constexpr Time(int32_t sec = 0, int32_t nsec = 0)
: sec_(sec), nsec_(CheckConstexpr(nsec)) {
}
@@ -96,27 +99,36 @@
// Constructs a time representing microseconds.
static constexpr Time InNS(int64_t nseconds) {
- return (nseconds < 0) ?
- Time(nseconds / static_cast<int64_t>(kNSecInSec) - 1,
- (nseconds % kNSecInSec) + kNSecInSec) :
- Time(nseconds / static_cast<int64_t>(kNSecInSec),
- nseconds % kNSecInSec);
+ return (nseconds < 0)
+ ? Time((nseconds - 1) / static_cast<int64_t>(kNSecInSec) - 1,
+ (((nseconds - 1) % kNSecInSec) + 1) + kNSecInSec)
+ : Time(nseconds / static_cast<int64_t>(kNSecInSec),
+ nseconds % kNSecInSec);
}
// Constructs a time representing microseconds.
static constexpr Time InUS(int useconds) {
- return (useconds < 0) ?
- Time(useconds / kUSecInSec - 1,
- (useconds % kUSecInSec) * kNSecInUSec + kNSecInSec) :
- Time(useconds / kUSecInSec, (useconds % kUSecInSec) * kNSecInUSec);
+ return (useconds < 0)
+ ? Time((useconds + 1) / kUSecInSec - 1,
+ (((useconds + 1) % kUSecInSec) - 1) * kNSecInUSec +
+ kNSecInSec)
+ : Time(useconds / kUSecInSec,
+ (useconds % kUSecInSec) * kNSecInUSec);
}
// Constructs a time representing mseconds.
static constexpr Time InMS(int mseconds) {
- return (mseconds < 0) ?
- Time(mseconds / kMSecInSec - 1,
- (mseconds % kMSecInSec) * kNSecInMSec + kNSecInSec) :
- Time(mseconds / kMSecInSec, (mseconds % kMSecInSec) * kNSecInMSec);
+ return (mseconds < 0)
+ ? Time((mseconds + 1) / kMSecInSec - 1,
+ (((mseconds + 1) % kMSecInSec) - 1) * kNSecInMSec +
+ kNSecInSec)
+ : Time(mseconds / kMSecInSec,
+ (mseconds % kMSecInSec) * kNSecInMSec);
+ }
+
+ // Construct a time representing the period of hertz.
+ static constexpr Time FromRate(int hertz) {
+ return Time(0, kNSecInSec / hertz);
}
// Checks whether or not this time is within amount nanoseconds of other.
diff --git a/aos/common/time_test.cc b/aos/common/time_test.cc
index c6ddd0c..008b3cf 100644
--- a/aos/common/time_test.cc
+++ b/aos/common/time_test.cc
@@ -188,6 +188,30 @@
Time t2 = Time::InMS(-254971);
EXPECT_EQ(-255, t2.sec());
EXPECT_EQ(Time::kNSecInSec - 971000000, t2.nsec());
+
+ Time t3 = Time::InMS(-1000);
+ EXPECT_EQ(-1, t3.sec());
+ EXPECT_EQ(0, t3.nsec());
+
+ Time t4 = Time::InMS(1000);
+ EXPECT_EQ(1, t4.sec());
+ EXPECT_EQ(0, t4.nsec());
+
+ Time t5 = Time::InMS(1001);
+ EXPECT_EQ(1, t5.sec());
+ EXPECT_EQ(Time::kNSecInMSec, t5.nsec());
+
+ Time t6 = Time::InMS(-1001);
+ EXPECT_EQ(-2, t6.sec());
+ EXPECT_EQ(Time::kNSecInSec - Time::kNSecInMSec, t6.nsec());
+
+ Time t7 = Time::InMS(-999);
+ EXPECT_EQ(-1, t7.sec());
+ EXPECT_EQ(Time::kNSecInMSec, t7.nsec());
+
+ Time t8 = Time::InMS(999);
+ EXPECT_EQ(0, t8.sec());
+ EXPECT_EQ(Time::kNSecInSec - Time::kNSecInMSec, t8.nsec());
}
TEST(TimeTest, ToMSec) {
@@ -228,6 +252,10 @@
MACRO_DARG(Time(970, Time::kNSecInSec - 973).ToNSec()));
}
+TEST(TimeTest, FromRate) {
+ EXPECT_EQ(MACRO_DARG(Time(0, Time::kNSecInSec / 100)), Time::FromRate(100));
+}
+
} // namespace testing
} // namespace time
} // namespace aos
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