got rid of the timer on the gyro board for timing sensor reads
diff --git a/frc971/input/gyro_board_data.h b/frc971/input/gyro_board_data.h
index fb47642..94200c9 100644
--- a/frc971/input/gyro_board_data.h
+++ b/frc971/input/gyro_board_data.h
@@ -1,6 +1,8 @@
#ifndef FRC971_INPUT_GYRO_BOARD_DATA_H_
#define FRC971_INPUT_GYRO_BOARD_DATA_H_
+#include <stdint.h>
+
namespace frc971 {
#define DATA_STRUCT_NAME GyroBoardData
diff --git a/frc971/input/usb_receiver.cc b/frc971/input/usb_receiver.cc
index d119d0a..8b63ae3 100644
--- a/frc971/input/usb_receiver.cc
+++ b/frc971/input/usb_receiver.cc
@@ -18,12 +18,8 @@
if (ReceiveData()) {
Reset();
} else {
- static const int kCountsPerSecond = 100000;
const ::aos::time::Time timestamp =
- ::aos::time::Time(data()->timestamp / kCountsPerSecond,
- (data()->timestamp * ::aos::time::Time::kNSecInSec /
- kCountsPerSecond) %
- ::aos::time::Time::kNSecInSec);
+ ::aos::time::Time::InMS(data()->frame_number);
if (data()->robot_id != expected_robot_id_) {
LOG(ERROR, "gyro board sent data for robot id %hhd instead of %hhd!"
diff --git a/gyro_board/src/usb/data_struct.h b/gyro_board/src/usb/data_struct.h
index 50eb7a9..975084a 100644
--- a/gyro_board/src/usb/data_struct.h
+++ b/gyro_board/src/usb/data_struct.h
@@ -13,17 +13,24 @@
struct DATA_STRUCT_NAME {
int64_t gyro_angle;
- // In units of 100,000 counts/second.
- uint64_t timestamp;
-
union {
struct {
- // This is the USB frame number for this data. It gets incremented on
- // every packet sent.
+ // This is the USB frame number for this data. It (theoretically) gets
+ // incremented on every packet sent, but the gyro board will deal with it
+ // correctly if it misses a frame or whatever by tracking the frame
+ // numbers sent out by the host.
// Negative numbers mean that the gyro board has no idea what the right
// answer is.
// This value going down at all indicates that the code on the gyro board
// dealing with it reset.
+ //
+ // The USB 2.0 standard says that timing of frames is 1.000ms +- 500ns.
+ // Testing with a fitpc and gyro board on 2013-10-30 by Brian gave 10us
+ // (the resolution of the timer on the gyro board that was used) of drift
+ // every 90-130 frames (~100ns per frame) and no jitter (and the timer on
+ // the gyro board isn't necessarily that good). This is plenty accurate
+ // for what we need for timing, so this number is what the code uses to do
+ // all timing calculations.
int32_t frame_number;
// Checksum of this file calculated with sum(1).
diff --git a/gyro_board/src/usb/encoder.c b/gyro_board/src/usb/encoder.c
index ef5af7f..f01dd47 100644
--- a/gyro_board/src/usb/encoder.c
+++ b/gyro_board/src/usb/encoder.c
@@ -12,15 +12,6 @@
// 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) {
@@ -394,24 +385,7 @@
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);
@@ -513,10 +487,6 @@
packet->bad_gyro = 0;
}
- NVIC_DisableIRQ(TIMER1_IRQn);
- packet->timestamp = ((uint64_t)sensor_timing_wraps << 32) | TIM1->TC;
- NVIC_EnableIRQ(TIMER1_IRQn);
-
packet->checksum = DATA_STRUCT_CHECKSUM;
packet->dip_switch0 = dip_switch(0);
diff --git a/gyro_board/src/usb/usb_device.c b/gyro_board/src/usb/usb_device.c
index 39c3cae..c90bd0d 100644
--- a/gyro_board/src/usb/usb_device.c
+++ b/gyro_board/src/usb/usb_device.c
@@ -260,14 +260,17 @@
}
// Instead of registering an lpcusb handler for this, we do it ourself so that
-// we can get the timing jitter down.
+// we can get the timing jitter down and deal with the frame number right.
static void HandleFrame(void) {
USB->USBDevIntClr = FRAME;
static struct DataStruct sensor_values;
fillSensorPacket(&sensor_values);
+ // What the last good frame number that we got was.
+ // Values <0 are uninitialized.
static int32_t current_frame = -1;
+ // How many extra frames we're guessing happened since we got a good one.
static int guessed_frames = 0;
uint8_t error_status = USBHwCmdRead(CMD_DEV_READ_ERROR_STATUS);
@@ -283,6 +286,11 @@
current_frame = read_frame;
guessed_frames = 0;
} else {
+ // All of the complicated stuff in here tracks the frame number from
+ // hardware (which comes from the SOF tokens sent out by the host) except
+ // deal with it if we miss a couple or get off by a little bit (and reset
+ // completely if we get off by a lot or miss a lot in a row).
+
static const uint32_t kMaxReadFrame = 0x800;
static const uint32_t kReadMask = kMaxReadFrame - 1;
if ((current_frame & kReadMask) == read_frame) {
@@ -290,21 +298,24 @@
++guessed_frames;
} else {
guessed_frames = 0;
+ // The frame number that we think we should have gotten.
+ int32_t expected_frame = current_frame + guessed_frames + 1;
int16_t difference =
- read_frame - (int16_t)((current_frame + 1) & kReadMask);
+ read_frame - (int16_t)(expected_frame & kReadMask);
// If we're off by only a little.
if (difference > -10 && difference < 10) {
- current_frame = ((current_frame + 1) & ~kReadMask) | read_frame;
- // If we're ahead by only a little but we wrapped.
+ current_frame = (expected_frame & ~kReadMask) | read_frame;
+ // If we're ahead by only a little (or dead on) but we wrapped.
} else if (difference > kMaxReadFrame - 10) {
current_frame =
- ((current_frame & ~kReadMask) - kMaxReadFrame) | read_frame;
- // If we're behind by only a little but the packet counter wrapped.
+ ((expected_frame & ~kReadMask) - kMaxReadFrame) | read_frame;
+ // If we're behind by only a little (or dead on) but the number in the
+ // token wrapped.
} else if (difference < -(kMaxReadFrame - 10)) {
current_frame =
- ((current_frame & ~kReadMask) + kMaxReadFrame) | read_frame;
+ ((expected_frame & ~kReadMask) + kMaxReadFrame) | read_frame;
} else {
- // Give up and reset.
+ // We're way off, so give up and reset.
current_frame = -1;
}
}