got real timestamps on the sensor data
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);