implemented checking for late sensor packets
diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index 018a2f5..a5666a9 100644
--- a/aos/common/control_loop/ControlLoop-tmpl.h
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -115,7 +115,7 @@
template <class T, bool has_position>
void ControlLoop<T, has_position>::Run() {
while (true) {
- ::aos::time::PhasedLoopXMS(kLoopFrequency.ToMSec(), 0);
+ time::SleepUntil(NextLoopTime());
Iterate();
}
}
diff --git a/aos/common/control_loop/ControlLoop.cc b/aos/common/control_loop/ControlLoop.cc
new file mode 100644
index 0000000..f5253d4
--- /dev/null
+++ b/aos/common/control_loop/ControlLoop.cc
@@ -0,0 +1,13 @@
+#include "aos/common/control_loop/ControlLoop.h"
+
+namespace aos {
+namespace control_loops {
+
+time::Time NextLoopTime(time::Time start) {
+ return (start / kLoopFrequency.ToNSec()) *
+ kLoopFrequency.ToNSec() +
+ kLoopFrequency;
+}
+
+} // namespace control_loops
+} // namespace aos
diff --git a/aos/common/control_loop/ControlLoop.h b/aos/common/control_loop/ControlLoop.h
index bb76ffa..e47e0ad 100644
--- a/aos/common/control_loop/ControlLoop.h
+++ b/aos/common/control_loop/ControlLoop.h
@@ -40,6 +40,9 @@
// Control loops run this often, "starting" at time 0.
const time::Time kLoopFrequency = time::Time::InSeconds(0.01);
+// Calculates the next time to run control loops after start.
+time::Time NextLoopTime(time::Time start = time::Time::Now());
+
// Provides helper methods to assist in writing control loops.
// This template expects to be constructed with a queue group as an argument
// that has a goal, position, status, and output queue.