implemented checking for late sensor packets
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