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
diff --git a/gyro_board/src/usb/data_struct.h b/gyro_board/src/usb/data_struct.h
index 8c069b9..156acb6 100644
--- a/gyro_board/src/usb/data_struct.h
+++ b/gyro_board/src/usb/data_struct.h
@@ -3,6 +3,7 @@
// guards.
// This means that it can not #include anything else because it (sometimes) gets
// #included inside a namespace.
+// <stdint.h> must be #included by the containing file.
// In the gyro board code, fill_packet.h #includes this file.
// In the fitpc code, frc971/input/gyro_board_data.h #includes this file.
@@ -12,8 +13,14 @@
struct DATA_STRUCT_NAME {
int64_t gyro_angle;
+ // In units of 100,000 counts/second.
+ uint64_t timestamp;
+
union {
struct {
+ // This is a counter that gets incremented with each packet sent.
+ uint32_t sequence;
+
// Which robot (+version) the gyro board is sending out data for.
// We should keep this in the same place for all gyro board software
// versions so that the fitpc can detect when it's reading from a gyro
@@ -36,23 +43,21 @@
uint8_t dip_switch1 : 1;
uint8_t dip_switch2 : 1;
uint8_t dip_switch3 : 1;
- // If the current gyro_angle has been not updated because of a bad
- // reading from the sensor.
- uint8_t old_gyro_reading : 1;
- // If we're not going to get any more good gyro_angles.
- uint8_t bad_gyro : 1;
};
- uint8_t base_status;
+ uint8_t dip_switches;
+ };
+ struct {
+ // If the current gyro_angle has been not updated because of a bad
+ // reading from the sensor.
+ uint8_t old_gyro_reading : 1;
+ // If we're not going to get any more good gyro_angles.
+ uint8_t bad_gyro : 1;
};
};
- uint32_t header;
+ uint64_t header;
};
- // This is a counter that gets incremented with each packet sent.
- uint32_t sequence;
-
- // We are 64-bit aligned at this point if it matters for anything other than
- // the gyro angle.
+ // We are 64-bit aligned at this point.
union {
struct {
diff --git a/gyro_board/src/usb/encoder.c b/gyro_board/src/usb/encoder.c
index 9277340..7b6d268 100644
--- a/gyro_board/src/usb/encoder.c
+++ b/gyro_board/src/usb/encoder.c
@@ -12,6 +12,15 @@
// before reading the indexer encoder.
static const int kBottomFallDelayTime = 32;
+// The timer to use for timestamping sensor readings.
+// This is a constant to avoid hard-coding it in a lot of places, but there ARE
+// things (PCONP bits, IRQ numbers, etc) that have this value in them
+// implicitly.
+#define SENSOR_TIMING_TIMER TIM1
+// How many counts per second SENSOR_TIMING_TIMER should be.
+// This will wrap the counter about every 1/3 of a second.
+static const int kSensorTimingRate = 100000;
+
#define ENC(gpio, a, b) readGPIO(gpio, a) * 2 + readGPIO(gpio, b)
int encoder_bits(int channel) {
switch (channel) {
@@ -383,7 +392,26 @@
}
}
+static volatile uint32_t sensor_timing_wraps = 0;
+
+void TIMER1_IRQHandler(void) {
+ SENSOR_TIMING_TIMER->IR = 1 << 0; // clear channel 0 match
+ ++sensor_timing_wraps;
+}
+
void encoder_init(void) {
+ // Set up the timer for timestamping sensor readings.
+ SC->PCONP |= 1 << 2;
+ SENSOR_TIMING_TIMER->PR = (configCPU_CLOCK_HZ / kSensorTimingRate) - 1UL;
+ SENSOR_TIMING_TIMER->TC = 1; // don't match the first time around
+ SENSOR_TIMING_TIMER->MR0 = 0; // match every time it wraps
+ SENSOR_TIMING_TIMER->MCR = 1 << 0; // interrupt on match channel 0
+ // Priority 4 is higher than any FreeRTOS-managed stuff (ie USB), but lower
+ // than encoders etc.
+ NVIC_SetPriority(TIMER1_IRQn, 4);
+ NVIC_EnableIRQ(TIMER1_IRQn);
+ SENSOR_TIMING_TIMER->TCR = 1; // enable it
+
// Setup the encoder interface.
SC->PCONP |= PCONP_PCQEI;
PINCON->PINSEL3 = ((PINCON->PINSEL3 & 0xffff3dff) | 0x00004100);
@@ -485,6 +513,10 @@
packet->bad_gyro = 0;
}
+ NVIC_DisableIRQ(TIMER1_IRQn);
+ packet->timestamp = ((uint64_t)sensor_timing_wraps << 32) | TIM1->TC;
+ NVIC_EnableIRQ(TIMER1_IRQn);
+
packet->dip_switch0 = dip_switch(0);
packet->dip_switch1 = dip_switch(1);
packet->dip_switch2 = dip_switch(2);