got it so the gyro should work
I basically split the linear code from the gyro board up into a state
machine that runs in ISRs.
diff --git a/bbb_cape/src/cape/Makefile b/bbb_cape/src/cape/Makefile
index 5dee875..5955c6b 100644
--- a/bbb_cape/src/cape/Makefile
+++ b/bbb_cape/src/cape/Makefile
@@ -37,6 +37,8 @@
cows \
encoder \
crc \
+ gyro \
+ led \
OBJECTS_bootloader := bootloader \
uart_common \
diff --git a/bbb_cape/src/cape/data_struct.h b/bbb_cape/src/cape/data_struct.h
index fdcb064..95992b9 100644
--- a/bbb_cape/src/cape/data_struct.h
+++ b/bbb_cape/src/cape/data_struct.h
@@ -26,6 +26,12 @@
// If the current gyro_angle has been not updated because of a bad
// reading from the sensor.
uint8_t old_gyro_reading : 1;
+ // If the gyro is still initializing.
+ // If this is 1, then all of the other gyro data is invalid.
+ uint8_t uninitialized_gyro : 1;
+ // If the gyro is still zeroing.
+ // If this is 1, then all of the other gyro data is invalid.
+ uint8_t zeroing_gyro : 1;
// If we're not going to get any more good gyro_angles.
uint8_t bad_gyro : 1;
};
diff --git a/bbb_cape/src/cape/fill_packet.c b/bbb_cape/src/cape/fill_packet.c
index 537c527..5779ae3 100644
--- a/bbb_cape/src/cape/fill_packet.c
+++ b/bbb_cape/src/cape/fill_packet.c
@@ -10,6 +10,8 @@
#include "cape/encoder.h"
#include "cape/crc.h"
#include "cape/bootloader_handoff.h"
+#include "cape/gyro.h"
+#include "cape/led.h"
#define TIMESTAMP_TIM TIM6
#define RCC_APB1ENR_TIMESTAMP_TIMEN RCC_APB1ENR_TIM6EN
@@ -29,6 +31,14 @@
packet->flash_checksum = flash_checksum;
+ struct GyroOutput gyro_output;
+ gyro_get_output(&gyro_output);
+ packet->gyro_angle = gyro_output.angle;
+ packet->old_gyro_reading = gyro_output.last_reading_bad;
+ packet->uninitialized_gyro = !gyro_output.initialized;
+ packet->zeroing_gyro = !gyro_output.zeroed;
+ packet->bad_gyro = gyro_output.gyro_bad;
+
packet->main.encoders[0] = encoder_read(0);
packet->main.encoders[1] = encoder_read(1);
packet->main.encoders[2] = encoder_read(2);
@@ -67,7 +77,9 @@
TIMESTAMP_TIM->CR1 |= TIM_CR1_CEN;
crc_init();
+ led_init();
encoder_init();
+ gyro_init();
uint8_t *flash_end = &__etext + (&__data_start__ - &__data_end__) + 8;
flash_checksum = crc_calculate((void *)MAIN_FLASH_START,
diff --git a/bbb_cape/src/cape/gyro.c b/bbb_cape/src/cape/gyro.c
new file mode 100644
index 0000000..a663e16
--- /dev/null
+++ b/bbb_cape/src/cape/gyro.c
@@ -0,0 +1,390 @@
+#include "cape/gyro.h"
+
+#include <inttypes.h>
+
+#include <STM32F2XX.h>
+
+#include "cape/util.h"
+#include "cape/led.h"
+
+#define printf(...)
+
+#define SPI SPI3
+#define SPI_IRQHandler SPI3_IRQHandler
+#define RCC_APB1ENR_SPIEN RCC_APB1ENR_SPI3EN
+#define TIM TIM10
+#define TIM_IRQHandler TIM1_UP_TIM10_IRQHandler
+#define RCC_APB2ENR_TIMEN RCC_APB2ENR_TIM10EN
+#define CSEL_GPIO GPIOA
+#define CSEL_NUM 4
+// The header file also contains references to TIM in gyro_read.
+
+struct GyroOutput gyro_output;
+
+// Set when a parity error is detected and cleared before starting a read.
+static volatile int parity_error;
+// Which byte we're currently waiting to read.
+static volatile int receive_byte;
+// The first byte that we receive (the most significant one).
+static volatile uint16_t high_value;
+
+// 1 if the latest result is potentially bad and 0 if it's good.
+static volatile int bad_reading;
+// 1 if the gyro is bad adn we're not going to get any more readings.
+static volatile int bad_gyro;
+// The new reading waiting for the next timer cycle to be outputted.
+static volatile int16_t new_reading;
+
+struct GyroOutput gyro_output;
+
+// How many times per second to read the gyro value.
+#define kGyroReadFrequency 200
+// How many times per second to flash the LED.
+// Must evenly divide kGyroReadFrequency.
+#define kFlashFrequency 10
+
+#define kStartupCycles (kGyroReadFrequency * 2)
+#define kZeroingCycles (kGyroReadFrequency * 6)
+
+// An accumulator for all of the values read while zeroing.
+int32_t zero_bias = 0;
+
+int startup_cycles_left = kStartupCycles;
+int zeroing_cycles_left = kZeroingCycles;
+
+// These are a pair that hold the offset calculated while zeroing.
+// full_units_ is the base (in ticks) and remainder_ ranges between 0 and
+// kZeroingCycles (like struct timespec). remainder_ is used to calculate which
+// cycles to add an additional unit to the result.
+int32_t full_units_offset = 0;
+int32_t remainder_offset = 0;
+// This keeps track of when to add 1 to the read value (using _offset).
+int32_t remainder_sum = 0;
+
+int32_t led_flash = 0;
+
+enum State {
+ STATE_SETUP0,
+ STATE_SETUP1,
+ STATE_SETUP2,
+ STATE_SETUP3,
+ STATE_READ,
+};
+static volatile enum State state;
+static int setup_counter;
+
+// Switches to new_state in time TIM milliseconds (aka it shows in the TIM ISR).
+static void switch_state(enum State new_state, int time) {
+ TIM->CR1 = TIM_CR1_UDIS;
+ state = new_state;
+ TIM->CCR1 = time;
+ TIM->EGR = TIM_EGR_UG;
+}
+
+static void gyro_setup_failed(void) {
+ printf("gyro setup failed. stopping\n");
+ gyro_output.angle = 0;
+ gyro_output.last_reading_bad = gyro_output.gyro_bad = 1;
+ gyro_output.initialized = 1;
+ gyro_output.zeroed = 0;
+}
+
+static void gyro_enable_csel(void) {
+ // Clear the CSEL pin to select it.
+ // Do it 8 times (9 cycles) to wait for the amount of time the gyro datasheet
+ // says we need to.
+ // (1/2/(7.5MHz)+8ns)*120MHz = 8.96
+ for (int i = 0; i < 8; ++i) CSEL_GPIO->BSRRH = 1 << CSEL_NUM;
+}
+
+// Blocks until there is space to enqueue data.
+static void spi_write(uint16_t data) {
+ while (!(SPI->SR & SPI_SR_TXE)) {}
+ SPI->DR = data;
+}
+
+static void do_gyro_read(uint32_t data) {
+ parity_error = 0;
+ receive_byte = 0;
+
+ gyro_enable_csel();
+ spi_write(data >> 16);
+ if (__builtin_parity(data & ~1) == 0) data |= 1;
+ spi_write(data);
+}
+
+// Returns all of the non-data bits in the "header" except the parity from
+// value.
+static uint8_t gyro_status(uint32_t value) {
+ return (value >> 26) & ~4;
+}
+
+// Returns all of the error bits in the "footer" from value.
+static uint8_t gyro_errors(uint32_t value) {
+ return (value >> 1) & 0x7F;
+}
+
+static void process_reading(int16_t reading) {
+ switch (state) {
+ case STATE_SETUP0:
+ if (parity_error) {
+ switch_state(STATE_SETUP0, 100);
+ } else {
+ if (reading != 1) {
+ printf("gyro unexpected initial response 0x%"PRIx32"\n", reading);
+ // There's a chance that we're retrying because of a parity error
+ // previously, so keep going.
+ }
+ // Wait for it to assert the fault conditions before reading them.
+ switch_state(STATE_SETUP1, 50);
+ }
+ break;
+ case STATE_SETUP1:
+ if (parity_error) {
+ switch_state(STATE_SETUP0, 100);
+ } else {
+ // Wait for it to clear the fault conditions before reading again.
+ switch_state(STATE_SETUP2, 50);
+ }
+ break;
+ case STATE_SETUP2:
+ if (parity_error) {
+ switch_state(STATE_SETUP0, 100);
+ } else {
+ // If it's not reporting self test data.
+ if (gyro_status(reading) != 2) {
+ printf("gyro first value 0x%"PRIx32" not self test data\n", reading);
+ switch_state(STATE_SETUP0, 100);
+ break;
+ }
+ // If we don't see all of the errors.
+ if (gyro_errors(reading) != 0x7F) {
+ printf("gyro self test value 0x%"PRIx32" is bad\n", reading);
+ gyro_setup_failed();
+ break;
+ }
+ // Wait for the sequential transfer delay before reading out the last of
+ // the self test data.
+ switch_state(STATE_SETUP3, 1);
+ }
+ break;
+ case STATE_SETUP3:
+ if (parity_error) {
+ switch_state(STATE_SETUP0, 100);
+ } else {
+ // It should still be reporting self test data.
+ if (gyro_status(reading) != 2) {
+ printf("gyro second value 0x%"PRIx32" not self test data\n", reading);
+ switch_state(STATE_SETUP0, 100);
+ break;
+ }
+
+ gyro_output.initialized = 1;
+ gyro_output.angle = 0;
+ gyro_output.last_reading_bad = 1; // until we're started up
+ gyro_output.gyro_bad = 0;
+ // Start reading values (after the sequential transfer delay).
+ switch_state(STATE_READ, 1);
+ }
+ break;
+ case STATE_READ:
+ new_reading = reading;
+ switch_state(STATE_READ, 1000 / kGyroReadFrequency);
+ break;
+ }
+}
+
+static void reading_received(uint32_t value) {
+ if (parity_error) {
+ bad_reading = 1;
+ } else {
+ // This check assumes that the sequence bits are all 0, but they should be
+ // because that's all we send.
+ if (gyro_status(value) != 1) {
+ uint8_t status = gyro_status(value);
+ if (status == 0) {
+ printf("gyro says sensor data is bad\n");
+ } else {
+ printf("gyro gave weird status 0x%"PRIx8"\n", status);
+ }
+ bad_reading = 1;
+ }
+
+ if (gyro_errors(value) != 0) {
+ uint8_t errors = gyro_errors(value);
+ if (errors & ~(1 << 1)) {
+ bad_reading = 1;
+ // Error 1 (continuous self-test error) will set status to 0 if it's bad
+ // enough by itself.
+ }
+ if (errors & (1 << 6)) {
+ printf("gyro PLL error\n");
+ }
+ if (errors & (1 << 5)) {
+ printf("gyro quadrature error\n");
+ }
+ if (errors & (1 << 4)) {
+ printf("gyro non-volatile memory error\n");
+ bad_gyro = 1;
+ }
+ if (errors & (1 << 3)) {
+ printf("gyro volatile memory error\n");
+ bad_gyro = 1;
+ }
+ if (errors & (1 << 2)) {
+ printf("gyro power error\n");
+ }
+ if (errors & (1 << 1)) {
+ printf("gyro continuous self-test error\n");
+ }
+ if (errors & 1) {
+ printf("gyro unexpected self check mode\n");
+ }
+ }
+ if (bad_gyro) {
+ bad_reading = 1;
+ }
+ }
+ process_reading(-(int16_t)(value >> 10 & 0xFFFF));
+}
+
+void SPI_IRQHandler(void) {
+ uint32_t status = SPI->SR;
+ if (status & SPI_SR_RXNE) {
+ uint16_t value = SPI->DR;
+ if (__builtin_parity(value) != 1) {
+ parity_error = 1;
+ }
+ if (receive_byte == 0) {
+ receive_byte = 1;
+ high_value = value;
+ } else {
+ uint32_t full_value = high_value << 16 | value;
+ // Set the CSEL pin high to deselect it.
+ // The parity calculation etc took long enough that this is safe now.
+ CSEL_GPIO->BSRRL = 1 << CSEL_NUM;
+ reading_received(full_value);
+ }
+ }
+}
+
+void TIM_IRQHandler(void) {
+ TIM->CR1 &= ~TIM_CR1_CEN;
+ TIM->SR = TIM_SR_CC1IF;
+ switch (state) {
+ case STATE_SETUP0:
+ if (setup_counter++ < 100) {
+ // Get it started doing a check.
+ do_gyro_read(0x20000003);
+ } else {
+ gyro_setup_failed();
+ }
+ break;
+ case STATE_SETUP1: // Dummy read to clear the old latched state.
+ case STATE_SETUP2: // Read self-test data.
+ case STATE_SETUP3: // Read the second latched self-test data.
+ do_gyro_read(0x20000000);
+ break;
+ case STATE_READ:
+ ++led_flash;
+ if (led_flash < kGyroReadFrequency / kFlashFrequency / 2) {
+ led_set(LED_HB, 0);
+ } else {
+ led_set(LED_HB, 1);
+ }
+ if (led_flash >= kGyroReadFrequency / kFlashFrequency) {
+ led_flash = 0;
+ }
+
+ if (bad_gyro) {
+ led_set(LED_ERR, 1);
+ printf("gyro reader giving up because of bad gyro\n");
+ gyro_output.gyro_bad = 1;
+ gyro_output.last_reading_bad = 1;
+ gyro_output.angle = 0;
+ break;
+ }
+
+ if (startup_cycles_left) {
+ led_set(LED_Z, 0);
+ --startup_cycles_left;
+ if (bad_reading) {
+ printf("gyro retrying startup wait because of bad reading\n");
+ startup_cycles_left = kStartupCycles;
+ }
+ } else if (zeroing_cycles_left) {
+ led_set(LED_Z, 1);
+ --zeroing_cycles_left;
+ if (bad_reading) {
+ printf("gyro restarting zeroing because of bad reading\n");
+ zeroing_cycles_left = kZeroingCycles;
+ zero_bias = 0;
+ } else {
+ zero_bias -= new_reading;
+ if (zeroing_cycles_left == 0) {
+ // Do all the nice math
+ full_units_offset = zero_bias / kZeroingCycles;
+ remainder_offset = zero_bias % kZeroingCycles;
+ if (remainder_offset < 0) {
+ remainder_offset += kZeroingCycles;
+ --full_units_offset;
+ }
+ gyro_output.zeroed = 1;
+ }
+ }
+ } else {
+ led_set(LED_Z, 0);
+
+ int64_t new_angle = gyro_output.angle;
+ if (!bad_reading) new_angle += new_reading + full_units_offset;
+ if (remainder_sum >= kZeroingCycles) {
+ remainder_sum -= kZeroingCycles;
+ new_angle += 1;
+ }
+ gyro_output.angle = new_angle;
+ gyro_output.last_reading_bad = bad_reading;
+ remainder_sum += remainder_offset;
+ }
+ do_gyro_read(0x20000000);
+ break;
+ }
+}
+
+void gyro_init(void) {
+ gyro_output.initialized = 0;
+ gyro_output.zeroed = 0;
+
+ RCC->APB1ENR |= RCC_APB1ENR_SPIEN;
+ RCC->APB2ENR |= RCC_APB2ENR_TIMEN;
+
+ // Set up CSEL.
+ // It's is just a GPIO pin because we're the master (it would be special if we
+ // were a slave).
+ gpio_setup_out(CSEL_GPIO, CSEL_NUM, 3);
+
+ // Set up SCK, MISO, and MOSI.
+ gpio_setup_alt(GPIOC, 10, 6); // SCK
+ gpio_setup_alt(GPIOC, 11, 6); // MISO
+ gpio_setup_alt(GPIOC, 12, 6); // MOSI
+
+ NVIC_SetPriority(SPI3_IRQn, 4);
+ NVIC_EnableIRQ(SPI3_IRQn);
+ NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 5);
+ NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);
+
+ TIM->CR1 = TIM_CR1_UDIS;
+ TIM->DIER = TIM_DIER_CC1IE;
+ TIM->CCMR1 = 0;
+ TIM->PSC = 60000 - 1;
+
+ SPI->CR1 = 0; // make sure it's disabled
+ SPI->CR1 =
+ SPI_CR1_DFF /* 16 bit frame */ |
+ 1 << 3 /* 30MHz/4 = 7.5MHz */ |
+ SPI_CR1_MSTR /* master mode */;
+ SPI->CR2 = SPI_CR2_RXNEIE;
+ SPI->CR1 |= SPI_CR1_SPE; // enable it
+
+ setup_counter = 0;
+ switch_state(STATE_SETUP0, 100);
+}
diff --git a/bbb_cape/src/cape/gyro.h b/bbb_cape/src/cape/gyro.h
new file mode 100644
index 0000000..1536718
--- /dev/null
+++ b/bbb_cape/src/cape/gyro.h
@@ -0,0 +1,30 @@
+#ifndef GYRO_BOARD_SRC_USB_GYRO_H_
+#define GYRO_BOARD_SRC_USB_GYRO_H_
+
+#include <stdint.h>
+#include <string.h>
+
+#include <STM32F2XX.h>
+
+// Does everything to set up the gyro code, including starting a timer which
+// triggers reads and integrates the gyro values and blinks the LEDs etc.
+void gyro_init(void);
+
+struct GyroOutput {
+ int64_t angle;
+ int last_reading_bad;
+ int gyro_bad;
+ int initialized;
+ int zeroed;
+};
+
+// Reads the most recent output value and avoids race conditions.
+// Must be called from a lower-priority ISR than TIM10's.
+static inline void gyro_get_output(struct GyroOutput *output) {
+ extern struct GyroOutput gyro_output;
+ NVIC_DisableIRQ(TIM1_UP_TIM10_IRQn);
+ memcpy(output, &gyro_output, sizeof(gyro_output));
+ NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);
+}
+
+#endif // GYRO_BOARD_SRC_USB_GYRO_H_
diff --git a/bbb_cape/src/cape/led.c b/bbb_cape/src/cape/led.c
new file mode 100644
index 0000000..1968f96
--- /dev/null
+++ b/bbb_cape/src/cape/led.c
@@ -0,0 +1,48 @@
+#include "cape/led.h"
+
+#include <STM32F2XX.h>
+
+#include "cape/util.h"
+
+#define LED_SPEED 0
+
+// DB = PC3
+// Z = PB1
+// HB = PB4
+// ERR = PB11
+
+static void do_led_set(GPIO_TypeDef *port, int number, int on) {
+ if (on) {
+ port->BSRRL = 1 << number;
+ } else {
+ port->BSRRH = 1 << number;
+ }
+}
+
+void led_set(enum LED led, int on) {
+ switch (led) {
+ case LED_ERR:
+ do_led_set(GPIOB, 11, on);
+ break;
+ case LED_HB:
+ do_led_set(GPIOB, 4, on);
+ break;
+ case LED_Z:
+ do_led_set(GPIOB, 1, on);
+ break;
+ case LED_DB:
+ do_led_set(GPIOC, 3, on);
+ break;
+ }
+}
+
+void led_init(void) {
+ gpio_setup_out(GPIOB, 11, LED_SPEED);
+ led_set(LED_ERR, 0);
+ gpio_setup_out(GPIOB, 4, LED_SPEED);
+ led_set(LED_HB, 0);
+ gpio_setup_out(GPIOB, 1, LED_SPEED);
+ led_set(LED_Z, 0);
+ gpio_setup_out(GPIOC, 3, LED_SPEED);
+ led_set(LED_DB, 0);
+}
diff --git a/bbb_cape/src/cape/led.h b/bbb_cape/src/cape/led.h
new file mode 100644
index 0000000..ee47853
--- /dev/null
+++ b/bbb_cape/src/cape/led.h
@@ -0,0 +1,17 @@
+#ifndef CAPE_LED_H_
+#define CAPE_LED_H_
+
+// The LEDs as referenced by the silkscreen.
+enum LED {
+ LED_ERR,
+ LED_HB,
+ LED_Z,
+ LED_DB,
+};
+
+// Turns the indicated LED on or off.
+void led_set(enum LED led, int on);
+
+void led_init(void);
+
+#endif // CAPE_LED_H_
diff --git a/bbb_cape/src/cape/peripherial_usage.notes b/bbb_cape/src/cape/peripherial_usage.notes
index b0dac40..bf55182 100644
--- a/bbb_cape/src/cape/peripherial_usage.notes
+++ b/bbb_cape/src/cape/peripherial_usage.notes
@@ -16,7 +16,11 @@
TIM7
[gyro communication]
-SPI3
+gyro
+ SPI3
+ SPI3_IRQ:4
+ TIM10
+ TIM10_IRQ:5 (aka TIM1_UP)
[ADC communication]
SPI2
diff --git a/bbb_cape/src/cape/util.h b/bbb_cape/src/cape/util.h
index b27cc77..893c95b 100644
--- a/bbb_cape/src/cape/util.h
+++ b/bbb_cape/src/cape/util.h
@@ -35,4 +35,12 @@
}
}
+// A convenient way to set up a GPIO pin for output (push-pull) without missing
+// part or messing up which bits need setting to what.
+// speed is 0 (slow) to 3 (fast)
+static inline void gpio_setup_out(GPIO_TypeDef *port, int pin, int speed) {
+ SET_BITS(port->MODER, 2, 1 /* output */, pin);
+ SET_BITS(port->OSPEEDR, 2, speed, pin);
+}
+
#endif // CAPE_UTIL_H_