commit 332a254c5b42e20d6a7e2cb9456aa35fae4e4ca7
author Brian Silverman <brians> Thu Oct 31 17:50:41 2013 -0700
committer Brian Silverman <brians> Thu Oct 31 17:50:41 2013 -0700
tree ba070a05df9dc81897f2ed9fb8d1785c5b4ae1ac
parent 28d9778ebf852090c3a06e88eb2f04429a3a2ec6

added a place to check gyro status every packet

frc971/input/gyro_sensor_receiver.cc

diff --git a/frc971/input/gyro_sensor_receiver.cc b/frc971/input/gyro_sensor_receiver.cc
index 411fb5e..1d3546d 100644
--- a/frc971/input/gyro_sensor_receiver.cc
+++ b/frc971/input/gyro_sensor_receiver.cc
@@ -79,10 +79,25 @@
  public:
   GyroSensorReceiver() : USBReceiver(2) {}
 
+  virtual void PacketReceived(const ::aos::time::Time &/*timestamp*/) override {
+    if (data()->bad_gyro) {
+      LOG(ERROR, "bad gyro\n");
+      bad_gyro_ = true;
+      gyro.MakeWithBuilder().angle(0).Send();
+    } else if (data()->old_gyro_reading) {
+      LOG(WARNING, "old gyro reading\n");
+      bad_gyro_ = true;
+    } else {
+      bad_gyro_ = false;
+    }
+  }
+
   virtual void ProcessData(const ::aos::time::Time &/*timestamp*/) override {
-    gyro.MakeWithBuilder()
-        .angle(gyro_translate(data()->gyro_angle))
-        .Send();
+    if (!bad_gyro_) {
+      gyro.MakeWithBuilder()
+          .angle(gyro_translate(data()->gyro_angle))
+          .Send();
+    }
 
     drivetrain.position.MakeWithBuilder()
         .right_encoder(drivetrain_translate(data()->main.right_drive))
@@ -140,6 +155,8 @@
         adc_translate(data()->main.right_drive_hall));
   }
 
+  bool bad_gyro_;
+
   WrappingCounter top_rise_;
   WrappingCounter top_fall_;
   WrappingCounter bottom_rise_;

frc971/input/usb_receiver.cc

diff --git a/frc971/input/usb_receiver.cc b/frc971/input/usb_receiver.cc
index 81b960e..d119d0a 100644
--- a/frc971/input/usb_receiver.cc
+++ b/frc971/input/usb_receiver.cc
@@ -38,6 +38,8 @@
       return;
     }
 
+    PacketReceived(timestamp);
+
     if (phase_locker_.IsCurrentPacketGood(transfer_received_time_, frame_number_)) {
       LOG(DEBUG, "processing dips %hhx frame %" PRId32 " at %f\n",
           data()->dip_switches, data()->frame_number, timestamp.ToSeconds());
@@ -56,6 +58,9 @@
   good_phase_early_ = good_phase_late_ = 0;
 }
 
+void USBReceiver::PacketReceived(const ::aos::time::Time &/*timestamp*/) {}
+void USBReceiver::ProcessData(const ::aos::time::Time &/*timestamp*/) {}
+
 bool USBReceiver::PhaseLocker::IsCurrentPacketGood(
     const ::aos::time::Time &received_time,
     uint32_t sequence) {

frc971/input/usb_receiver.h

diff --git a/frc971/input/usb_receiver.h b/frc971/input/usb_receiver.h
index 509b740..fec847f 100644
--- a/frc971/input/usb_receiver.h
+++ b/frc971/input/usb_receiver.h
@@ -102,7 +102,19 @@
 
   void Reset();
 
-  virtual void ProcessData(const ::aos::time::Time &timestamp) = 0;
+  // These 2 are the functions for subclasses to override and do stuff in.
+  // timestamp for both of them is the time (as best as this code can determine)
+  // that the values in the packet were captured.
+  // They both have empty implementations here for subclasses that don't want to
+  // do anything in one of them.
+
+  // Gets called after each packet is received (possibly before ProcessData for
+  // the same packet).
+  virtual void PacketReceived(const ::aos::time::Time &timestamp);
+  // Gets called every 10th packet (or so) (at the right time for data for
+  // control loops to get read). PacketReceived will always be called right
+  // before this.
+  virtual void ProcessData(const ::aos::time::Time &timestamp);
 
   const uint8_t expected_robot_id_;