commit 3d6e3dfc426ad78271175f61ee9fcdff023cbcc8
author Austin Schuh <austin@peloton-tech.com> Mon Feb 17 01:51:03 2014 -0800
committer Austin Schuh <austin@peloton-tech.com> Mon Feb 17 01:51:03 2014 -0800
tree 4c231193c75ef7e0402d370a32a85e2969fc1522
parent 0c733420591d9a4c72bd85b4f799168c992169d6

Now detecting reset using the sensor_generation queue.

aos/common/control_loop/ControlLoop-tmpl.h

diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index b5714d5..4e1999c 100644
--- a/aos/common/control_loop/ControlLoop-tmpl.h
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -5,6 +5,8 @@
 #include "aos/common/messages/RobotState.q.h"
 #include "aos/common/logging/queue_logging.h"
 
+#include "bbb/sensor_generation.q.h"
+
 namespace aos {
 namespace control_loops {
 
@@ -37,6 +39,21 @@
     ZeroOutputs();
     return;
   }
+
+  ::bbb::sensor_generation.FetchLatest();
+  if (::bbb::sensor_generation.get() == nullptr) {
+    ZeroOutputs();
+    return;
+  }
+  if (!has_sensor_reset_counters_ ||
+      ::bbb::sensor_generation->reader_pid != reader_pid_ ||
+      ::bbb::sensor_generation->cape_resets != cape_resets_) {
+    reader_pid_ = ::bbb::sensor_generation->reader_pid;
+    cape_resets_ = ::bbb::sensor_generation->cape_resets;
+    has_sensor_reset_counters_ = true;
+    reset_ = true;
+  }
+    
   LOG_STRUCT(DEBUG, "goal", *goal);
 
   // Only pass in a position if we got one this cycle.
@@ -75,12 +92,12 @@
   bool outputs_enabled = false;
 
   // Check to see if we got a driver station packet recently.
-  if (aos::robot_state.FetchLatest()) {
+  if (::aos::robot_state.FetchLatest()) {
     outputs_enabled = true;
-  } else if (aos::robot_state.IsNewerThanMS(kDSPacketTimeoutMs)) {
+  } else if (::aos::robot_state.IsNewerThanMS(kDSPacketTimeoutMs)) {
     outputs_enabled = true;
   } else {
-    if (aos::robot_state.get()) {
+    if (::aos::robot_state.get()) {
       LOG_INTERVAL(driver_station_old_);
     } else {
       LOG_INTERVAL(no_driver_station_);