reworked the gyro board timing stuff
It now goes out of its way to get consistent non-jittery timings for
running the code to read sensor values so it can just keep track of USB
frame numbers to get accurate timestamps for the readings.
diff --git a/frc971/input/usb_receiver.cc b/frc971/input/usb_receiver.cc
index 7892e0e..b70ff61 100644
--- a/frc971/input/usb_receiver.cc
+++ b/frc971/input/usb_receiver.cc
@@ -18,12 +18,13 @@
if (ReceiveData()) {
Reset();
} else {
- if (phase_locker_.IsCurrentPacketGood(transfer_received_time_, sequence_)) {
+ if (phase_locker_.IsCurrentPacketGood(transfer_received_time_, frame_number_)) {
static const int kCountsPerSecond = 100000;
const ::aos::time::Time timestamp =
::aos::time::Time(data()->timestamp / kCountsPerSecond,
- data()->timestamp *
- ::aos::time::Time::kNSecInSec / kCountsPerSecond);
+ (data()->timestamp * ::aos::time::Time::kNSecInSec /
+ kCountsPerSecond) %
+ ::aos::time::Time::kNSecInSec);
if (data()->robot_id != expected_robot_id_) {
LOG(ERROR, "gyro board sent data for robot id %hhd instead of %hhd!"
@@ -31,8 +32,8 @@
data()->robot_id, expected_robot_id_, data()->dip_switches);
return;
} else {
- LOG(DEBUG, "processing dips %hhx at %f\n",
- data()->dip_switches, timestamp.ToSeconds());
+ LOG(DEBUG, "processing dips %hhx frame %" PRId32 " at %f\n",
+ data()->dip_switches, data()->frame_number, timestamp.ToSeconds());
}
ProcessData(timestamp);
@@ -63,11 +64,6 @@
Reset();
return false;
}
- if (sequence < last_good_sequence_) {
- LOG(WARNING, "sequence went down. gyro board reset?\n");
- Reset();
- return false;
- }
using ::aos::control_loops::kLoopFrequency;
// How often we (should) receive packets.
@@ -215,14 +211,27 @@
memcpy(data(), completed_transfer_->data(),
sizeof(GyroBoardData));
- uint32_t sequence_before = sequence_;
- sequence_ = data()->sequence;
- if (sequence_before == 0) {
- LOG(INFO, "count starting at %" PRIu32 "\n", sequence_);
- } else if (sequence_ - sequence_before != 1) {
- LOG(WARNING, "count went from %" PRIu32" to %" PRIu32 "\n",
- sequence_before, sequence_);
+ if (data()->unknown_frame) {
+ LOG(WARNING, "unknown frame number\n");
+ return true;
}
+ uint32_t frame_number_before = frame_number_;
+ frame_number_ = data()->frame_number;
+ if (frame_number_ < 0) {
+ LOG(WARNING, "negative frame number %" PRId32 "\n", frame_number_);
+ return true;
+ }
+ if (frame_number_before == 0) {
+ LOG(INFO, "frame number starting at %" PRId32 "\n", frame_number_);
+ } else if (frame_number_ - frame_number_before != 1) {
+ LOG(WARNING, "frame number went from %" PRId32" to %" PRId32 "\n",
+ frame_number_before, frame_number_);
+ }
+ if (frame_number_ < last_frame_number_) {
+ LOG(WARNING, "frame number went down\n");
+ return true;
+ }
+ last_frame_number_ = frame_number_;
return false;
}
@@ -246,7 +255,7 @@
c->Submit();
}
- sequence_ = 0;
+ last_frame_number_ = frame_number_ = 0;
phase_locker_.Reset();
}
diff --git a/frc971/input/usb_receiver.h b/frc971/input/usb_receiver.h
index dcb7b92..509b740 100644
--- a/frc971/input/usb_receiver.h
+++ b/frc971/input/usb_receiver.h
@@ -108,7 +108,7 @@
GyroBoardData data_;
- uint32_t sequence_;
+ int32_t last_frame_number_, frame_number_;
LibUSB libusb_;
::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;
diff --git a/gyro_board/src/usb/LPCUSB/usbhw_lpc.c b/gyro_board/src/usb/LPCUSB/usbhw_lpc.c
index 2fa038e..e39a636 100644
--- a/gyro_board/src/usb/LPCUSB/usbhw_lpc.c
+++ b/gyro_board/src/usb/LPCUSB/usbhw_lpc.c
@@ -59,7 +59,7 @@
@param [in] dwIntr Bitmask of interrupts to wait for
*/
-static void Wait4DevInt(unsigned long dwIntr)
+void Wait4DevInt(unsigned long dwIntr)
{
// wait for specific interrupt
while ((USB->USBDevIntSt & dwIntr) != dwIntr);
@@ -107,7 +107,7 @@
@return the data
*/
-static unsigned char USBHwCmdRead(unsigned char bCmd)
+unsigned char USBHwCmdRead(unsigned char bCmd)
{
// write command code
USBHwCmd(bCmd);
diff --git a/gyro_board/src/usb/Makefile b/gyro_board/src/usb/Makefile
index 2dc2286..da8a4c5 100644
--- a/gyro_board/src/usb/Makefile
+++ b/gyro_board/src/usb/Makefile
@@ -44,7 +44,7 @@
CAN.c \
LPCUSB/usbinit.c \
LPCUSB/usbcontrol.c \
- LPCUSB/USB_SENSOR_STREAM.c \
+ usb_device.c \
LPCUSB/usbhw_lpc.c \
gyro.c \
LPCUSB/usbstdreq.c
diff --git a/gyro_board/src/usb/data_struct.h b/gyro_board/src/usb/data_struct.h
index 156acb6..718d19c 100644
--- a/gyro_board/src/usb/data_struct.h
+++ b/gyro_board/src/usb/data_struct.h
@@ -18,8 +18,13 @@
union {
struct {
- // This is a counter that gets incremented with each packet sent.
- uint32_t sequence;
+ // This is the USB frame number for this data. It gets incremented on
+ // every packet sent.
+ // 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.
+ int32_t frame_number;
// Which robot (+version) the gyro board is sending out data for.
// We should keep this in the same place for all gyro board software
@@ -52,6 +57,9 @@
uint8_t old_gyro_reading : 1;
// If we're not going to get any more good gyro_angles.
uint8_t bad_gyro : 1;
+
+ // We're not sure what frame number this packet was sent in.
+ uint8_t unknown_frame : 1;
};
};
uint64_t header;
diff --git a/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c b/gyro_board/src/usb/usb_device.c
similarity index 68%
rename from gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
rename to gyro_board/src/usb/usb_device.c
index 7265393..39c3cae 100644
--- a/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
+++ b/gyro_board/src/usb/usb_device.c
@@ -32,9 +32,19 @@
#include <stdio.h>
#include <string.h>
-#include "usbapi.h"
-#include "usbdebug.h"
-#include "usbstruct.h"
+#include "LPCUSB/usbapi.h"
+#include "LPCUSB/usbdebug.h"
+#include "LPCUSB/usbstruct.h"
+
+// This file is marked private and most of the functions in its associated .c
+// file started out static, but we want to use some of them to do frame handling
+// stuff because we do special stuff with it (handle it ourselves for reduced
+// jitter and actually deal with the frame number correctly), so it's nice to
+// have the utility functions for accessing the hardware available instead of
+// having to rewrite them.
+#include "LPCUSB/usbhw_lpc.h"
+unsigned char USBHwCmdRead(unsigned char bCmd);
+void Wait4DevInt(unsigned long dwIntr);
#include "LPC17xx.h"
@@ -55,8 +65,6 @@
#define LE_WORD(x) ((x)&0xFF),((x)>>8)
-static struct DataStruct usbPacket;
-
static xQueueHandle xRxedChars = NULL, xCharsForTx = NULL;
// This gets cleared each time the ISR is entered and then checked as it's
@@ -144,6 +152,12 @@
0
};
+// Enables interrupts to write data instead of NAKing on the bulk in endpoints.
+// This is in a centralized place so that other NAK interrupts can be enabled
+// all of the time easily in the future.
+static void bulk_in_nak_int(int have_data) {
+ USBHwNakIntEnable(have_data ? INACK_BI : 0);
+}
/**
* Local function to handle incoming bulk data
@@ -179,8 +193,8 @@
(void) bEPStatus;
if (uxQueueMessagesWaitingFromISR(xCharsForTx) == 0) {
- // no more data, disable further NAK interrupts until next USB frame
- USBHwNakIntEnable(INACK_II);
+ // no more data
+ bulk_in_nak_int(0);
return;
}
@@ -245,31 +259,83 @@
return -1;
}
-/**
- * Interrupt handler
- *
- * Simply calls the USB ISR
- */
-void USB_IRQHandler(void) {
- higher_priority_task_woken = pdFALSE;
- USBHwISR();
- portEND_SWITCHING_ISR(higher_priority_task_woken);
-}
+// Instead of registering an lpcusb handler for this, we do it ourself so that
+// we can get the timing jitter down.
+static void HandleFrame(void) {
+ USB->USBDevIntClr = FRAME;
-static void USBFrameHandler(unsigned short wFrame) {
- (void) wFrame;
- if (uxQueueMessagesWaitingFromISR(xCharsForTx) > 0) {
- // Data to send is available so enable interrupt instead of NAK on bulk in
- // too.
- USBHwNakIntEnable(INACK_BI | INACK_II);
+ static struct DataStruct sensor_values;
+ fillSensorPacket(&sensor_values);
+
+ static int32_t current_frame = -1;
+ static int guessed_frames = 0;
+
+ uint8_t error_status = USBHwCmdRead(CMD_DEV_READ_ERROR_STATUS);
+ if (error_status & PID_ERR) {
+ ++guessed_frames;
} else {
- USBHwNakIntEnable(INACK_II);
+ int16_t read_frame = USBHwCmdRead(CMD_DEV_READ_CUR_FRAME_NR);
+ USB->USBCmdCode = 0x00000200 | (CMD_DEV_READ_CUR_FRAME_NR << 16);
+ Wait4DevInt(CDFULL);
+ read_frame |= USB->USBCmdData << 8;
+
+ if (current_frame < 0) {
+ current_frame = read_frame;
+ guessed_frames = 0;
+ } else {
+ static const uint32_t kMaxReadFrame = 0x800;
+ static const uint32_t kReadMask = kMaxReadFrame - 1;
+ if ((current_frame & kReadMask) == read_frame) {
+ // This seems like it must mean that we didn't receive the SOF token.
+ ++guessed_frames;
+ } else {
+ guessed_frames = 0;
+ int16_t difference =
+ read_frame - (int16_t)((current_frame + 1) & 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.
+ } 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.
+ } else if (difference < -(kMaxReadFrame - 10)) {
+ current_frame =
+ ((current_frame & ~kReadMask) + kMaxReadFrame) | read_frame;
+ } else {
+ // Give up and reset.
+ current_frame = -1;
+ }
+ }
+ }
}
- fillSensorPacket(&usbPacket);
- static uint32_t sequence = 0;
- usbPacket.sequence = sequence++;
- USBHwEPWrite(ISOC_IN_EP, (unsigned char *)&usbPacket, DATA_PACKET_SIZE);
+ sensor_values.frame_number = current_frame + guessed_frames;
+ sensor_values.unknown_frame = guessed_frames > 10;
+
+ USBHwEPWrite(ISOC_IN_EP, (unsigned char *)&sensor_values, DATA_PACKET_SIZE);
+
+ if (uxQueueMessagesWaitingFromISR(xCharsForTx) > 0) {
+ // Data to send is available so enable interrupt instead of NAK.
+ bulk_in_nak_int(1);
+ } else {
+ bulk_in_nak_int(0);
+ }
+}
+
+void USB_IRQHandler(void) {
+ higher_priority_task_woken = pdFALSE;
+ uint32_t status = SC->USBIntSt;
+ if (status & USB_INT_REQ_HP) {
+ // We set the frame interrupt to get routed to the high priority line.
+ HandleFrame();
+ }
+ //if (status & USB_INT_REQ_LP) {
+ // Call lpcusb to let it handle all of the other interrupts.
+ USBHwISR();
+ //}
+ portEND_SWITCHING_ISR(higher_priority_task_woken);
}
void usb_init(void) {
@@ -298,8 +364,11 @@
USBHwRegisterEPIntHandler(BULK_IN_EP, DebugIn);
USBHwRegisterEPIntHandler(BULK_OUT_EP, DebugOut);
+ USB->USBDevIntPri = 1; // route frame interrupt to high priority line
+ USB->USBDevIntEn |= FRAME; // enable frame interrupt
+
// register frame handler
- USBHwRegisterFrameHandler(USBFrameHandler);
+ //USBHwRegisterFrameHandler(USBFrameHandler);
DBG("Starting USB communication\n");