Fix gyro reading code when it misses cycles
Change-Id: Ife59545495709eda7a9e4c9961a1b78125cd975b
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