worked on making sensor receiver handle crio oddness better
diff --git a/aos/common/sensors/sensor_receiver-tmpl.h b/aos/common/sensors/sensor_receiver-tmpl.h
index 834af7c..ade9734 100644
--- a/aos/common/sensors/sensor_receiver-tmpl.h
+++ b/aos/common/sensors/sensor_receiver-tmpl.h
@@ -7,24 +7,37 @@
template<class Values>
const time::Time SensorReceiver<Values>::kJitterDelay =
time::Time::InSeconds(0.002);
+template<class Values>
+const time::Time SensorReceiver<Values>::kGiveupTime =
+ time::Time::InSeconds(1.5);
template<class Values>
SensorReceiver<Values>::SensorReceiver(
SensorUnpackerInterface<Values> *unpacker)
: unpacker_(unpacker),
start_time_(0, 0),
- synchronized_(false) {}
+ synchronized_(false),
+ last_good_time_(0, 0) {}
template<class Values>
void SensorReceiver<Values>::RunIteration() {
if (synchronized_) {
if (ReceiveData()) {
LOG(DEBUG, "receive said to try a reset\n");
- synchronized_ = false;
- return;
- }
- if (GoodPacket()) {
- unpacker_->UnpackFrom(&data_.values);
+ Unsynchronize();
+ } else {
+ if (GoodPacket()) {
+ unpacker_->UnpackFrom(&data_.values);
+ last_good_time_ = time::Time::Now();
+ LOG(DEBUG, "set to now\n");
+ } else {
+ if ((time::Time::Now() - last_good_time_) > kGiveupTime) {
+ LOG(INFO, "resetting because didn't get a good one in too long\n");
+ Unsynchronize();
+ } else {
+ // We got a packet, but it wasn't an interesting one.
+ }
+ }
}
} else {
LOG(INFO, "resetting to try receiving data\n");
@@ -32,25 +45,77 @@
if (Synchronize()) {
LOG(INFO, "synchronized successfully\n");
synchronized_ = true;
+ before_better_cycles_ = after_better_cycles_ = 0;
+ last_good_time_ = time::Time::Now();
}
}
}
template<class Values>
bool SensorReceiver<Values>::GoodPacket() {
+ bool good;
// If it's a multiple of kSensorSendFrequency from start_count_.
if (((data_.count - start_count_) % kSendsPerCycle) == 0) {
if (((data_.count - start_count_) / kSendsPerCycle) ==
((NextLoopTime() - start_time_).ToNSec() / kLoopFrequency.ToNSec())) {
- return true;
+ good = true;
} else {
LOG(INFO, "not calling packet %"PRId32" good because it's late\n",
data_.count);
- return false;
+ good = false;
}
} else {
- return false;
+ good = false;
}
+
+ static time::Time last_time(0, 0);
+ time::Time now = time::Time::Now();
+ time::Time next_goal_time = NextLoopTime() - kJitterDelay;
+ // If this is the packet after the right one.
+ if (((data_.count - start_count_ - 1) % kSendsPerCycle) == 0) {
+ // If this one is closer than the last one (aka the one that we used).
+ if ((now - next_goal_time).abs() < (last_time - next_goal_time).abs()) {
+ LOG(DEBUG, "next one better than one being used %d\n",
+ after_better_cycles_);
+ if (after_better_cycles_ > kBadCyclesToSwitch) {
+ LOG(INFO, "switching to the packet after\n");
+ UpdateStartTime(data_.count);
+ before_better_cycles_ = after_better_cycles_ = 0;
+ } else {
+ ++after_better_cycles_;
+ }
+ } else {
+ after_better_cycles_ = 0;
+ }
+ }
+ // If this is the right packet.
+ if (((data_.count - start_count_) % kSendsPerCycle) == 0) {
+ // If the last one was closer than this one (aka the one that we used).
+ if ((last_time - next_goal_time).abs() < (now - next_goal_time).abs()) {
+ LOG(DEBUG, "previous better than one being used %d\n",
+ before_better_cycles_);
+ if (before_better_cycles_ > kBadCyclesToSwitch) {
+ LOG(INFO, "switching to the packet before\n");
+ UpdateStartTime(data_.count - 1);
+ start_count_ = data_.count - 1;
+ start_time_ = last_time;
+ before_better_cycles_ = after_better_cycles_ = 0;
+ } else {
+ ++before_better_cycles_;
+ }
+ } else {
+ before_better_cycles_ = 0;
+ }
+ }
+ last_time = now;
+
+ return good;
+}
+
+template<class Values>
+void SensorReceiver<Values>::UpdateStartTime(int new_start_count) {
+ start_time_ += kSensorSendFrequency * (new_start_count - start_count_);
+ start_count_ = new_start_count;
}
// Looks for when the timestamps transition from before where we want to after
@@ -129,6 +194,12 @@
}
template<class Values>
+void SensorReceiver<Values>::Unsynchronize() {
+ synchronized_ = false;
+ before_better_cycles_ = after_better_cycles_ = 0;
+}
+
+template<class Values>
const time::Time NetworkSensorReceiver<Values>::kWarmupTime =
time::Time::InSeconds(0.125);
diff --git a/aos/common/sensors/sensor_receiver.h b/aos/common/sensors/sensor_receiver.h
index aa5a359..eb4fc06 100644
--- a/aos/common/sensors/sensor_receiver.h
+++ b/aos/common/sensors/sensor_receiver.h
@@ -39,6 +39,11 @@
// (during this time, the code verifies that <= 1 cycle is not within 1
// cycle's time of kJitterDelay).
static const int kTestCycles = 10;
+ // How many cycles that we need (consecutively) of another packet being closer
+ // to the right time than the ones we're reading before we switch.
+ static const int kBadCyclesToSwitch = 10;
+ // If we don't get a good packet in this long, then we Synchronize() again.
+ static const time::Time kGiveupTime;
FRIEND_TEST_NAMESPACE(SensorReceiverTest, Simple, testing);
@@ -54,6 +59,11 @@
// Returns whether the current packet looks like a good one to use.
bool GoodPacket();
+ // Updates start_count_ to new_start_count and changes start_time_
+ // accordingly. Does it relative to avoid resetting start_time_ based off of 1
+ // bad packet.
+ void UpdateStartTime(int new_start_count);
+
// Synchronizes with incoming packets and sets start_count_ to where we
// started reading.
// Returns whether it succeeded in locking on.
@@ -61,6 +71,7 @@
// Receives a set of values and makes sure that it's sane.
// Returns whether to start over again with timing.
bool ReceiveData();
+ void Unsynchronize();
SensorUnpackerInterface<Values> *const unpacker_;
SensorData<Values> data_;
@@ -71,6 +82,9 @@
// sure that we don't send out a packet late.
time::Time start_time_;
bool synchronized_;
+ int before_better_cycles_, after_better_cycles_;
+ // The time of the last packet that we sent out.
+ time::Time last_good_time_;
DISALLOW_COPY_AND_ASSIGN(SensorReceiver<Values>);
};