reorganized the gyro code + implemented actual error checking
diff --git a/gyro_board/src/usb/main.c b/gyro_board/src/usb/main.c
index 0df1c4c..4a5eca4 100644
--- a/gyro_board/src/usb/main.c
+++ b/gyro_board/src/usb/main.c
@@ -31,179 +31,19 @@
/* Demo app includes. */
#include "flash.h"
#include "partest.h"
-#include "spi.h"
#include "LPCUSB/usbapi.h"
#include "analog.h"
#include "digital.h"
#include "encoder.h"
#include "CAN.h"
-
-int64_t gyro_angle = 0;
-
-/*
- * Configure the hardware.
- */
-static void prvSetupHardware(void);
+#include "gyro.h"
/*
* The task that handles the USB stack.
*/
extern void vUSBTask(void *pvParameters);
-extern int VCOM_getchar(void);
-
-int VCOM_putchar(int c);
-
-inline int32_t encoder()
-{
- return (int32_t)QEI->QEIPOS;
-}
-
-static portTASK_FUNCTION(vPrintPeriodic, pvParameters)
-{
- portTickType xLastFlashTime;
-
- /* We need to initialise xLastFlashTime prior to the first call to
- vTaskDelayUntil(). */
- xLastFlashTime = xTaskGetTickCount();
-
- digital_init();
-
- analog_init();
-
- encoder_init();
-
- // Wait 100 ms for it to boot.
- vTaskDelayUntil(&xLastFlashTime, 100 / portTICK_RATE_MS);
- spi_init();
-
- // Enable USB. The PC has probably disconnected it now.
- USBHwAllowConnect();
-
- // TODO(aschuh): Write this into a gyro calibration function, and check all the outputs.
- vTaskDelayUntil(&xLastFlashTime, 50 / portTICK_RATE_MS);
- enable_gyro_csel();
- printf("SPI Gyro Second Response 0x%x %x\n", transfer_spi_bytes(0x2000), transfer_spi_bytes(0x0000));
- disable_gyro_csel();
-
- vTaskDelayUntil(&xLastFlashTime, 50 / portTICK_RATE_MS);
- enable_gyro_csel();
- printf("SPI Gyro Third Response 0x%x %x\n", transfer_spi_bytes(0x2000), transfer_spi_bytes(0x0000));
- disable_gyro_csel();
-
- vTaskDelayUntil(&xLastFlashTime, 10 / portTICK_RATE_MS);
- enable_gyro_csel();
- printf("SPI Gyro Fourth Response 0x%x %x\n", transfer_spi_bytes(0x2000), transfer_spi_bytes(0x0000));
- disable_gyro_csel();
- const int hz = 200;
- const int flash_hz = 10;
- const int startup_cycles = hz * 2;
- const int zeroing_cycles = hz * 6;
- int32_t zero_bias = 0;
- int32_t startup_cycles_left = startup_cycles;
- int32_t zeroing_cycles_left = zeroing_cycles;
- int32_t full_units_offset = 0;
- int32_t remainder_offset = 0;
- int32_t remainder_sum = 0;
- int32_t led_flash = 0;
- vParTestSetLED(0, 0);
-
- for (;;) {
- led_flash ++;
- if (led_flash < hz / flash_hz / 2) {
- vParTestSetLED(1, 0);
- } else {
- vParTestSetLED(1, 1);
- }
- if (led_flash >= hz / flash_hz) {
- led_flash = 0;
- }
- /* Delay for half the flash period then turn the LED on. */
- vTaskDelayUntil(&xLastFlashTime, 1000 / hz / portTICK_RATE_MS);
- enable_gyro_csel();
- uint16_t high_value = transfer_spi_bytes(0x2000);
- uint16_t low_value = transfer_spi_bytes(0x0000);
- disable_gyro_csel();
- int16_t gyro_value = -((int16_t)((((uint32_t)high_value << 16) | (uint32_t)low_value) >> 10));
-
- if (startup_cycles_left) {
- vParTestSetLED(2, 0);
- --startup_cycles_left;
- } else if (zeroing_cycles_left) {
- vParTestSetLED(2, 1);
- //printf("Zeroing ");
- --zeroing_cycles_left;
- zero_bias -= gyro_value;
- if (zeroing_cycles_left == 0) {
- // Do all the nice math
- full_units_offset = zero_bias / zeroing_cycles;
- remainder_offset = zero_bias % zeroing_cycles;
- if (remainder_offset < 0) {
- remainder_offset += zeroing_cycles;
- --full_units_offset;
- }
- }
- } else {
- vParTestSetLED(2, 0);
- int64_t new_angle = gyro_angle + gyro_value + full_units_offset;
- if (remainder_sum >= zeroing_cycles) {
- remainder_sum -= zeroing_cycles;
- new_angle += 1;
- }
- gyro_angle = new_angle;
- remainder_sum += remainder_offset;
- }
- //printf("Angle %d Rate %d\n", (int)(gyro_angle / 16),
- // (int)(gyro_value + full_units_offset));
-
- //printf("time: %d analog %d encoder %d goal %d\n", (int)i, (int)analog(5),
- // (int)encoder(), (int)goal);
-
- /*
- for(i = 0; i < 4; i++){
- printf("analog(%d) => %d\n",i,analog(i));
- }
- for(i = 1; i < 13; i++){
- printf("digital(%d) => %d\n",i,digital(i));
- }
- for(i = 0; i < 4; i++){
- printf("dip(%d) => %d\n",i,dip(i));
- }
- for(i = 0; i < 4; i++){
- printf("encoder(%d) => %d\n",i,encoder_bits(i));
- }
- for(i = 0; i < 4; i++){
- printf("encoder_val(%d) => %d\n",i,(int)encoder_val(i));
- }*/
- }
-}
-
-int main(void) {
- // Configure the hardware
- prvSetupHardware();
-
- /* Create the USB task. */
- xTaskCreate(vUSBTask, (signed char *) "USB",
- configMINIMAL_STACK_SIZE + 1020, (void *) NULL,
- tskIDLE_PRIORITY + 3, NULL);
-
- xTaskCreate(vPrintPeriodic, (signed char *) "PRINTx",
- configMINIMAL_STACK_SIZE + 100, NULL,
- tskIDLE_PRIORITY + 2, NULL);
-
-
- initCAN();
-
- // Start the scheduler.
- vTaskStartScheduler();
-
- /* Will only get here if there was insufficient memory to create the idle
- task. The idle task is created within vTaskStartScheduler(). */
- for (;;) {}
-}
-/*-----------------------------------------------------------*/
-
// Sets up (and connects) PLL0.
// The CPU will be running at 100 MHz with a 12 MHz clock input when this is
// done.
@@ -300,10 +140,8 @@
while (((SC->PLL1STAT & (1 << 9)) == 0));
}
-void prvSetupHardware(void)
-{
- // Setup the peripherals.
-
+// Setup the peripherals.
+static void setup_hardware(void) {
// Setup GPIO power.
SC->PCONP = PCONP_PCGPIO;
@@ -314,14 +152,41 @@
setup_PLL1();
- // Setup the peripheral bus to be the same as the CCLK, 100 MHz.
// Set CAN to run at CCLK/6, which should have it running about 1 Mbit (1.042)
SC->PCLKSEL0 = 0xff555555;
/* Configure the LEDs. */
vParTestInitialise();
}
-/*-----------------------------------------------------------*/
+
+int main(void) {
+ setup_hardware();
+
+ /* Create the USB task. */
+ xTaskCreate(vUSBTask, (signed char *) "USB",
+ configMINIMAL_STACK_SIZE + 1020, (void *) NULL,
+ tskIDLE_PRIORITY + 3, NULL);
+
+ digital_init();
+
+ analog_init();
+
+ encoder_init();
+
+ gyro_init();
+
+ // Enable USB. The PC has probably disconnected it now.
+ USBHwAllowConnect();
+
+ initCAN();
+
+ // Start the scheduler.
+ vTaskStartScheduler();
+
+ /* Will only get here if there was insufficient memory to create the idle
+ task. The idle task is created within vTaskStartScheduler(). */
+ for (;;) {}
+}
void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed char *pcTaskName)
{
@@ -332,12 +197,11 @@
for (;;);
}
-/*-----------------------------------------------------------*/
// This is what portCONFIGURE_TIMER_FOR_RUN_TIME_STATS in FreeRTOSConfig.h
// actually calls.
-void vConfigureTimerForRunTimeStats(void)
-{
+// It sets up timer 0 to use for timing.
+void vConfigureTimerForRunTimeStats(void) {
const unsigned long TCR_COUNT_RESET = 2, CTCR_CTM_TIMER = 0x00, TCR_COUNT_ENABLE = 0x01;
/* This function configures a timer that is used as the time base when