commit a280ae0342d090bbf69214f2d5bad6367910eccf
author Brian Silverman <brians> Mon Oct 28 18:21:15 2013 -0700
committer Brian Silverman <brians> Mon Oct 28 18:21:15 2013 -0700
tree d621a615f47d3cd4165293515d8f37ebbf81746d
parent 7a3fbd1b83be14ebf996bb252c404927fa1ad473

got real timestamps on the sensor data

frc971/input/gyro_sensor_receiver.cc

diff --git a/frc971/input/gyro_sensor_receiver.cc b/frc971/input/gyro_sensor_receiver.cc
index 374268e..2a522c5 100644
--- a/frc971/input/gyro_sensor_receiver.cc
+++ b/frc971/input/gyro_sensor_receiver.cc
@@ -63,22 +63,19 @@
        (kVRefP - kVRefN));
 }
 
+inline double gyro_translate(int64_t in) {
+  return in / 16.0 / 1000.0 / (180.0 / M_PI);
+}
+
 }  // namespace
 
 class GyroSensorReceiver : public USBReceiver {
-  virtual void ProcessData() override {
-    if (data()->robot_id != 2) {
-      LOG(ERROR, "gyro board sent data for robot id %hhd!"
-          " dip switches are %x\n",
-          data()->robot_id, data()->base_status & 0xF);
-      return;
-    } else {
-      LOG(DEBUG, "processing a packet dip switches %x\n",
-          data()->base_status & 0xF);
-    }
+ public:
+  GyroSensorReceiver() : USBReceiver(2) {}
 
+  virtual void ProcessData(const ::aos::time::Time &/*timestamp*/) override {
     gyro.MakeWithBuilder()
-        .angle(data()->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
+        .angle(gyro_translate(data()->gyro_angle))
         .Send();
 
     drivetrain.position.MakeWithBuilder()

frc971/input/usb_receiver.cc

diff --git a/frc971/input/usb_receiver.cc b/frc971/input/usb_receiver.cc
index 0addd66..7892e0e 100644
--- a/frc971/input/usb_receiver.cc
+++ b/frc971/input/usb_receiver.cc
@@ -9,7 +9,8 @@
 
 namespace frc971 {
 
-USBReceiver::USBReceiver() {
+USBReceiver::USBReceiver(uint8_t expected_robot_id)
+    : expected_robot_id_(expected_robot_id) {
   Reset();
 }
 
@@ -17,19 +18,24 @@
   if (ReceiveData()) {
     Reset();
   } else {
-    // TODO(brians): Remove this temporary debug stuff.
-    static ::aos::time::Time temp(0, 0);
-    ::aos::time::Time delta = transfer_received_time_ - temp;
-    if (delta < ::aos::time::Time::InSeconds(0.0008)) {
-      LOG(INFO, "short delta %f\n", delta.ToSeconds());
-    } else if (delta > ::aos::time::Time::InSeconds(0.0012)) {
-      LOG(INFO, "long delta %f\n", delta.ToSeconds());
-    }
-    temp = transfer_received_time_;
-
     if (phase_locker_.IsCurrentPacketGood(transfer_received_time_, sequence_)) {
-      LOG(DEBUG, "processing data %" PRIu32 "\n", sequence_);
-      ProcessData();
+      static const int kCountsPerSecond = 100000;
+      const ::aos::time::Time timestamp =
+          ::aos::time::Time(data()->timestamp / kCountsPerSecond,
+                            data()->timestamp *
+                            ::aos::time::Time::kNSecInSec / kCountsPerSecond);
+
+      if (data()->robot_id != expected_robot_id_) {
+        LOG(ERROR, "gyro board sent data for robot id %hhd instead of %hhd!"
+            " dip switches are %hhx\n",
+            data()->robot_id, expected_robot_id_, data()->dip_switches);
+        return;
+      } else {
+        LOG(DEBUG, "processing dips %hhx at %f\n",
+            data()->dip_switches, timestamp.ToSeconds());
+      }
+
+      ProcessData(timestamp);
     }
   }
 }

frc971/input/usb_receiver.h

diff --git a/frc971/input/usb_receiver.h b/frc971/input/usb_receiver.h
index a3b7c5c..dcb7b92 100644
--- a/frc971/input/usb_receiver.h
+++ b/frc971/input/usb_receiver.h
@@ -4,6 +4,7 @@
 #include <memory>
 
 #include "aos/common/time.h"
+#include "aos/common/macros.h"
 
 #include "gyro_board/src/libusb-driver/libusb_wrap.h"
 #include "frc971/input/gyro_board_data.h"
@@ -14,7 +15,7 @@
 // us.
 class USBReceiver {
  public:
-  USBReceiver();
+  USBReceiver(uint8_t expected_robot_id);
 
   void RunIteration();
 
@@ -101,7 +102,9 @@
 
   void Reset();
 
-  virtual void ProcessData() = 0;
+  virtual void ProcessData(const ::aos::time::Time &timestamp) = 0;
+
+  const uint8_t expected_robot_id_;
 
   GyroBoardData data_;
 
@@ -115,6 +118,8 @@
   // finished from the callback to the rest of the code.
   libusb::Transfer *completed_transfer_;
   ::aos::time::Time transfer_received_time_{0, 0};
+
+  DISALLOW_COPY_AND_ASSIGN(USBReceiver);
 };
 
 }  // namespace frc971