commit 89e86369868e5bdee2fa3dffa3a31a458c11cb95
author Brian Silverman <brians> Wed Oct 30 17:50:50 2013 -0700
committer Brian Silverman <brians> Wed Oct 30 20:41:19 2013 -0700
tree 5ed0bdbc272fbd9762937152f9fb9b23676f70e0
parent 431500acc413c10a1df14991ad270416d914e279

reworked the gyro board timing stuff

It now goes out of its way to get consistent non-jittery timings for
running the code to read sensor values so it can just keep track of USB
frame numbers to get accurate timestamps for the readings.

frc971/input/usb_receiver.cc

diff --git a/frc971/input/usb_receiver.cc b/frc971/input/usb_receiver.cc
index 7892e0e..b70ff61 100644
--- a/frc971/input/usb_receiver.cc
+++ b/frc971/input/usb_receiver.cc
@@ -18,12 +18,13 @@
   if (ReceiveData()) {
     Reset();
   } else {
-    if (phase_locker_.IsCurrentPacketGood(transfer_received_time_, sequence_)) {
+    if (phase_locker_.IsCurrentPacketGood(transfer_received_time_, frame_number_)) {
       static const int kCountsPerSecond = 100000;
       const ::aos::time::Time timestamp =
           ::aos::time::Time(data()->timestamp / kCountsPerSecond,
-                            data()->timestamp *
-                            ::aos::time::Time::kNSecInSec / kCountsPerSecond);
+                            (data()->timestamp * ::aos::time::Time::kNSecInSec /
+                             kCountsPerSecond) %
+                                ::aos::time::Time::kNSecInSec);
 
       if (data()->robot_id != expected_robot_id_) {
         LOG(ERROR, "gyro board sent data for robot id %hhd instead of %hhd!"
@@ -31,8 +32,8 @@
             data()->robot_id, expected_robot_id_, data()->dip_switches);
         return;
       } else {
-        LOG(DEBUG, "processing dips %hhx at %f\n",
-            data()->dip_switches, timestamp.ToSeconds());
+        LOG(DEBUG, "processing dips %hhx frame %" PRId32 " at %f\n",
+            data()->dip_switches, data()->frame_number, timestamp.ToSeconds());
       }
 
       ProcessData(timestamp);
@@ -63,11 +64,6 @@
     Reset();
     return false;
   }
-  if (sequence < last_good_sequence_) {
-    LOG(WARNING, "sequence went down. gyro board reset?\n");
-    Reset();
-    return false;
-  }
 
   using ::aos::control_loops::kLoopFrequency;
   // How often we (should) receive packets.
@@ -215,14 +211,27 @@
     memcpy(data(), completed_transfer_->data(),
            sizeof(GyroBoardData));
 
-    uint32_t sequence_before = sequence_;
-    sequence_ = data()->sequence;
-    if (sequence_before == 0) {
-      LOG(INFO, "count starting at %" PRIu32 "\n", sequence_);
-    } else if (sequence_ - sequence_before != 1) {
-      LOG(WARNING, "count went from %" PRIu32" to %" PRIu32 "\n",
-          sequence_before, sequence_);
+    if (data()->unknown_frame) {
+      LOG(WARNING, "unknown frame number\n");
+      return true;
     }
+    uint32_t frame_number_before = frame_number_;
+    frame_number_ = data()->frame_number;
+    if (frame_number_ < 0) {
+      LOG(WARNING, "negative frame number %" PRId32 "\n", frame_number_);
+      return true;
+    }
+    if (frame_number_before == 0) {
+      LOG(INFO, "frame number starting at %" PRId32 "\n", frame_number_);
+    } else if (frame_number_ - frame_number_before != 1) {
+      LOG(WARNING, "frame number went from %" PRId32" to %" PRId32 "\n",
+          frame_number_before, frame_number_);
+    }
+    if (frame_number_ < last_frame_number_) {
+      LOG(WARNING, "frame number went down\n");
+      return true;
+    }
+    last_frame_number_ = frame_number_;
 
     return false;
   }
@@ -246,7 +255,7 @@
     c->Submit();
   }
 
-  sequence_ = 0;
+  last_frame_number_ = frame_number_ = 0;
   phase_locker_.Reset();
 }
 

frc971/input/usb_receiver.h

diff --git a/frc971/input/usb_receiver.h b/frc971/input/usb_receiver.h
index dcb7b92..509b740 100644
--- a/frc971/input/usb_receiver.h
+++ b/frc971/input/usb_receiver.h
@@ -108,7 +108,7 @@
 
   GyroBoardData data_;
 
-  uint32_t sequence_;
+  int32_t last_frame_number_, frame_number_;
 
   LibUSB libusb_;
   ::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;