got real timestamps on the sensor data
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()
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);
}
}
}
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 ×tamp) = 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