implemented checking for late sensor packets
diff --git a/aos/common/common.gyp b/aos/common/common.gyp
index e696c13..7ae3f61 100644
--- a/aos/common/common.gyp
+++ b/aos/common/common.gyp
@@ -139,7 +139,7 @@
'target_name': 'controls',
'type': 'static_library',
'sources': [
- # 'control_loop/ControlLoop-tmpl.h',
+ 'control_loop/ControlLoop.cc',
],
'dependencies': [
'<(AOS)/common/messages/messages.gyp:aos_queues',
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.
diff --git a/aos/common/sensors/sensor_receiver-tmpl.h b/aos/common/sensors/sensor_receiver-tmpl.h
index b5a20a1..834af7c 100644
--- a/aos/common/sensors/sensor_receiver-tmpl.h
+++ b/aos/common/sensors/sensor_receiver-tmpl.h
@@ -6,12 +6,13 @@
template<class Values>
const time::Time SensorReceiver<Values>::kJitterDelay =
- time::Time::InSeconds(0.0015);
+ time::Time::InSeconds(0.002);
template<class Values>
SensorReceiver<Values>::SensorReceiver(
SensorUnpackerInterface<Values> *unpacker)
: unpacker_(unpacker),
+ start_time_(0, 0),
synchronized_(false) {}
template<class Values>
@@ -39,7 +40,14 @@
bool SensorReceiver<Values>::GoodPacket() {
// If it's a multiple of kSensorSendFrequency from start_count_.
if (((data_.count - start_count_) % kSendsPerCycle) == 0) {
- return true;
+ if (((data_.count - start_count_) / kSendsPerCycle) ==
+ ((NextLoopTime() - start_time_).ToNSec() / kLoopFrequency.ToNSec())) {
+ return true;
+ } else {
+ LOG(INFO, "not calling packet %"PRId32" good because it's late\n",
+ data_.count);
+ return false;
+ }
} else {
return false;
}
@@ -53,9 +61,7 @@
time::Time old_received_time(0, 0);
time::Time start_time = time::Time::Now();
// When we want to send out the next set of values.
- time::Time goal_time = (start_time / kLoopFrequency.ToNSec()) *
- kLoopFrequency.ToNSec() +
- kLoopFrequency - kJitterDelay;
+ time::Time goal_time = NextLoopTime(start_time) - kJitterDelay;
while (true) {
if (ReceiveData()) return false;
time::Time received_time = time::Time::Now();
@@ -72,6 +78,7 @@
} else {
start_count_ = data_.count - 1;
}
+ start_time_ = goal_time;
int bad_count = 0;
for (int i = 0; i < kTestCycles;) {
diff --git a/aos/common/sensors/sensor_receiver.h b/aos/common/sensors/sensor_receiver.h
index 1fe3003..aa5a359 100644
--- a/aos/common/sensors/sensor_receiver.h
+++ b/aos/common/sensors/sensor_receiver.h
@@ -67,6 +67,9 @@
// The count that we started out (all other sent packets will be multiples of
// this).
int32_t start_count_;
+ // When start_count_ "should" have been received. Used for checking to make
+ // sure that we don't send out a packet late.
+ time::Time start_time_;
bool synchronized_;
DISALLOW_COPY_AND_ASSIGN(SensorReceiver<Values>);
diff --git a/aos/common/sensors/sensor_receiver_test.cc b/aos/common/sensors/sensor_receiver_test.cc
index 5a66432..c931b58 100644
--- a/aos/common/sensors/sensor_receiver_test.cc
+++ b/aos/common/sensors/sensor_receiver_test.cc
@@ -33,6 +33,7 @@
last_received_count_ = ++data()->count;
data()->values.count = last_received_count_;
Time::IncrementMockTime(kSensorSendFrequency);
+ data()->HostToNetwork();
}
int resets() { return resets_; }
diff --git a/aos/common/sensors/sensors.h b/aos/common/sensors/sensors.h
index 6d533b4..11cf105 100644
--- a/aos/common/sensors/sensors.h
+++ b/aos/common/sensors/sensors.h
@@ -34,6 +34,7 @@
// convenience.
extern const time::Time kSensorSendFrequency;
using ::aos::control_loops::kLoopFrequency;
+using ::aos::control_loops::NextLoopTime;
// This is the struct that actually gets sent over the UDP socket.
template<class Values>
@@ -45,6 +46,9 @@
void NetworkToHost() {
count = ntoh(count);
}
+ void HostToNetwork() {
+ count = hton(count);
+ }
} __attribute__((packed));
} // namespace sensors