From e8adff93c1f43fa1b1fbf2ba62a5ebb04cc8289d Mon Sep 17 00:00:00 2001 From: Gavan Fantom Date: Tue, 6 May 2014 21:03:48 +0000 Subject: [PATCH] Bye bye Wii Motion Plus, hello MPU6050. Also, increase control loop to 200Hz, add SD card logging, and a number of other changes. --- Makefile | 9 +- crt0.s | 21 +- dcm.c | 22 +- event.c | 19 +- event.h | 7 +- fisqrt.c | 12 +- i2c.c | 10 +- i2c.h | 1 + interrupt.h | 1 + log.c | 110 +++++++++ main.c | 324 +++++++++---------------- motor.c | 8 +- mpu6050.c | 263 ++++++++++++++++++++ panic.c | 36 ++- panic.h | 8 + sdcard.c | 678 ++++++++++++++++++++++++++++++++++++++++++++++++++++ sensors.c | 233 ++++++++++++++++++ spi.c | 116 +++++++++ stick.c | 22 +- timer.c | 6 +- timer.h | 13 +- uart.c | 62 ++++- uart.h | 1 + watchdog.c | 2 +- wmp.c | 320 ------------------------- 25 files changed, 1724 insertions(+), 580 deletions(-) create mode 100644 log.c create mode 100644 mpu6050.c create mode 100644 sdcard.c create mode 100644 sensors.c create mode 100644 spi.c delete mode 100644 wmp.c diff --git a/Makefile b/Makefile index 46f732d..ac70acc 100644 --- a/Makefile +++ b/Makefile @@ -3,14 +3,17 @@ NAME=quad SSRCS=crt0.s -CSRCS=main.c i2c.c wmp.c timer.c interrupt.c uart.c event.c matrix.c dcm.c +CSRCS=main.c i2c.c mpu6050.c timer.c interrupt.c uart.c event.c matrix.c dcm.c CSRCS+=fisqrt.c stick.c trig.c motor.c led.c watchdog.c panic.c status.c -CSRCS+=thrust.c +CSRCS+=thrust.c sensors.c spi.c sdcard.c log.c #PROJOPTS=-DUSE_UART -DSEND_DCM -DSTICK_DEBUG_CALIBRATE PROJOPTS=-DTIMER_CPPM +#PROJOPTS=-DTIMER_CPPM -DUSE_UART -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DSEND_DCM +#PROJOPTS=-DTIMER_CPPM -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DSEND_DCM +#PROJOPTS=-DTIMER_CPPM -DPANIC_32BIT -DPANIC_CHECKPOINT -DI2C_FAST -DUSE_UART -DSEND_DCM -COPTIM?=-O1 +COPTIM?=-Os CFLAGS=-march=armv4t -msoft-float $(COPTIM) -Wall -Werror -Wextra $(PROJOPTS) LDSCRIPT=lpc2103_flash.ld diff --git a/crt0.s b/crt0.s index eb33aec..7a5ac03 100644 --- a/crt0.s +++ b/crt0.s @@ -7,7 +7,7 @@ .equ PLL_P, 2 .equ FLASHCLOCKS, 3 /* 40-60MHz clock */ - .equ APB_DIVIDER, 4 /* 1, 2 or 4 */ + .equ APB_DIVIDER, 1 /* 1, 2 or 4 */ .equ UND_STACK_SIZE, 0x0004 .equ SVC_STACK_SIZE, 0x0010 @@ -62,6 +62,8 @@ .equ APBDIV_BASE, 0xE01FC100 .equ APBDIV, 0 + .equ FP0XVAL, 0x3FFFC014 + # True is -1 so we subtract values together. .equ PLL_LOG_P, (0-(PLL_P>1)-(PLL_P>2)-(PLL_P>4)) .equ PLLCFG_VAL, (PLL_M-1) | (PLL_LOG_P << 5) @@ -185,10 +187,27 @@ lzi: b main # Undefined handlers can just spin for now + +# Turn on LED +# ldr r2, =FP0XVAL +# ldr r0, [r2, #0] +# bic r0, r0, #0x04000000 +# str r0, [r2, #0] +# b __back + +# Turn off LED +# ldr r2, =FP0XVAL +# ldr r0, [r2, #0] +# orr r0, r0, #0x04000000 +# str r0, [r2, #0] +# b __back + undefined_handler: prefetch_abort_handler: data_abort_handler: fiq_handler: + mov r0, r14 + bl panic __back: b __back diff --git a/dcm.c b/dcm.c index 30f4148..c3b6afe 100644 --- a/dcm.c +++ b/dcm.c @@ -11,8 +11,13 @@ #include "motor.h" #include "status.h" #include "abs.h" +#include "log.h" +#if 0 #define GRAVITY 9.80665f +#endif + +#define GRAVITY 1.0f #define KP_ROLLPITCH 0.05967 #define KI_ROLLPITCH 0.00001278 @@ -22,6 +27,9 @@ /* Maximum allowed error for arming */ #define ERROR_THRESHOLD 0.20f +#define LOG_MAGIC_DCM_UPDATE 0x00DC111A +#define LOG_MAGIC_DCM_DRIFT 0x00DC111B + /* Implementation of the DCM IMU concept as described by Premerlani * and Bizard @@ -36,7 +44,15 @@ float omega_i[3] = {0.0, 0.0, 0.0}; float omega_x, omega_y, omega_z; -float delta_t = 0.01; +float delta_t = 0.005; + +void dcm_log(unsigned int magic) +{ + int i; + log_put_uint(magic); + for (i = 0; i < 9; i++) + log_put_float(dcm[i]); +} void dcm_update(float x, float y, float z) { @@ -57,6 +73,8 @@ void dcm_update(float x, float y, float z) matrix_add(dcm, dcm, temp_matrix, 3, 3); dcm_normalise(); + +/* dcm_log(LOG_MAGIC_DCM_UPDATE); */ } void dcm_setvector(float x, float y, float z) @@ -200,6 +218,8 @@ void dcm_drift_correction(float x, float y, float z) omega_i[i] += error[i] * (KI_ROLLPITCH * weight); } + dcm_log(LOG_MAGIC_DCM_DRIFT); + #if 0 putstr("w: "); putint_s((int)(weight * 100000.0f)); diff --git a/event.c b/event.c index 2aa6ff6..dc9c3ae 100644 --- a/event.c +++ b/event.c @@ -1,6 +1,7 @@ #include "event.h" #include "interrupt.h" #include "types.h" +#include "log.h" event_handler *event_handler_table[EVENT_MAX+1]; @@ -13,14 +14,18 @@ unsigned int event_pending[EVENT_WORDS]; #define EVENT_BIT(x) (x%EVENT_WORDLEN) #define EVENT_MASK(x) (1< EVENT_MAX) return; - interrupt_block(); event_pending[EVENT_WORD(event)] |= EVENT_MASK(event); - interrupt_unblock(); } static int bitset(unsigned int x) @@ -56,15 +61,21 @@ void event_clear(unsigned int event) interrupt_unblock(); } -void event_dispatch(void) +bool event_dispatch(void) { unsigned int event; if (event_get(&event)) { event_clear(event); - if (event_handler_table[event] != NULL) + if (event_handler_table[event] != NULL) { + log_mark_busy(); (event_handler_table[event])(); + log_mark_idle(); + } + return TRUE; } + + return FALSE; } void event_register(unsigned int event, event_handler *handler) diff --git a/event.h b/event.h index ef9eb77..f18cc24 100644 --- a/event.h +++ b/event.h @@ -4,17 +4,18 @@ #include "types.h" #define EVENT_TIMER 0 -#define EVENT_I2C_COMPLETE 1 +#define EVENT_MPU6050_I2C_COMPLETE 1 #define EVENT_UART_INPUT 2 +#define EVENT_SDCARD 3 -#define EVENT_MAX 2 +#define EVENT_MAX 3 typedef void event_handler(void); void event_set(unsigned int event); bool event_get(unsigned int *event); void event_clear(unsigned int event); -void event_dispatch(void); +bool event_dispatch(void); void event_register(unsigned int event, event_handler *handler); #endif /* __EVENT_H */ diff --git a/fisqrt.c b/fisqrt.c index d0aa8ab..c4139bf 100644 --- a/fisqrt.c +++ b/fisqrt.c @@ -6,12 +6,20 @@ float fisqrt(float n) { long i; float x2, y; + union { + float f; + long l; + } u; x2 = n * 0.5f; y = n; - i = *(long *)&y; +/* i = *(long *)&y; */ + u.f = y; + i = u.l; i = 0x5f3759df - (i>>1); - y = *(float *)&i; +/* y = *(float *)&i; */ + u.l = i; + y = u.f; y = y * (1.5f - (x2*y*y)); return y; diff --git a/i2c.c b/i2c.c index 1a7adfc..243e3d9 100644 --- a/i2c.c +++ b/i2c.c @@ -33,8 +33,12 @@ void init_i2c(void) IREG(I2CONSET) = 0x40; /* Enable I2C ready for Master Tx */ /* Set up for just under 400kHz */ #ifdef I2C_FAST +#if 0 IWREG(I2SCLL) = (25 * 100); IWREG(I2SCLH) = (12 * 100); +#endif + IWREG(I2SCLL) = 25 * 4; /* ~400kHz */ + IWREG(I2SCLH) = 12 * 4; #else IWREG(I2SCLL) = 73; /* ~100kHz */ IWREG(I2SCLH) = 73; @@ -106,10 +110,10 @@ void __attribute__((interrupt("IRQ"))) i2c_interrupt_handler(void) IREG(I2CONSET) = STAFLAG; IREG(I2CONCLR) = STOFLAG | AAFLAG | SIFLAG; } else { + event_set(i2c_transaction->event); i2c_transaction = NULL; IREG(I2CONSET) = STOFLAG; IREG(I2CONCLR) = STAFLAG | AAFLAG | SIFLAG; - event_set(EVENT_I2C_COMPLETE); } } break; @@ -137,10 +141,10 @@ void __attribute__((interrupt("IRQ"))) i2c_interrupt_handler(void) IREG(I2CONSET) = STAFLAG; IREG(I2CONCLR) = STOFLAG | AAFLAG | SIFLAG; } else { + event_set(i2c_transaction->event); i2c_transaction = NULL; IREG(I2CONSET) = STOFLAG; IREG(I2CONCLR) = STAFLAG | AAFLAG | SIFLAG; - event_set(EVENT_I2C_COMPLETE); } break; @@ -150,10 +154,10 @@ void __attribute__((interrupt("IRQ"))) i2c_interrupt_handler(void) case 0x38: /* arbitration lost during SA+W or data */ case 0x00: /* bus error */ *(i2c_transaction->result) = I2C_FAIL; + event_set(i2c_transaction->event); i2c_transaction = NULL; IREG(I2CONSET) = STOFLAG; IREG(I2CONCLR) = STAFLAG | AAFLAG | SIFLAG; - event_set(EVENT_I2C_COMPLETE); break; /* We don't handle slave mode */ diff --git a/i2c.h b/i2c.h index ad49e5d..c21698a 100644 --- a/i2c.h +++ b/i2c.h @@ -14,6 +14,7 @@ struct i2c_transaction { int bytes; /* number of bytes to read or write */ unsigned char *data; /* pointer to the data */ i2c_result *result; /* pointer to store the result */ + unsigned int event; /* Which event to set when complete */ struct i2c_transaction *next; /* NULL or next transaction */ }; diff --git a/interrupt.h b/interrupt.h index 25ee6bd..b8ce099 100644 --- a/interrupt.h +++ b/interrupt.h @@ -36,6 +36,7 @@ #define I_PRIORITY_UART0 1 #define I_PRIORITY_TIMER3 2 #define I_PRIORITY_TIMER0 3 +#define I_PRIORITY_SPI1 4 #define interrupt_clear() do { VICVectAddr = 0xff; } while (0) diff --git a/log.c b/log.c new file mode 100644 index 0000000..3a33fb6 --- /dev/null +++ b/log.c @@ -0,0 +1,110 @@ +/* log.c */ + +#include "types.h" +#include "sdcard.h" +#include "uart.h" +#include "timer.h" +#include "log.h" + +/* This is shared with sdcard.c */ +bool log_enabled; + +char log_buffer[LOG_BUFFERSIZE]; +unsigned int log_bufferstart; +unsigned int log_bufferend; + +unsigned int log_generation; + +/* DO NOT call when the buffer is empty */ +/* This should be safe against writes to the buffer, as the writes only + * affect log_bufferend. So no blocking of interrupts is necessary. + */ +char log_get_byte(void) +{ + char i; + i = log_buffer[log_bufferstart++]; + log_bufferstart = log_bufferstart % LOG_BUFFERSIZE; + + return i; +} + +void log_put_byte(char c) +{ + if (!log_enabled) + return; + + /* If the buffer is full, we just discard data. + * Better than overrunning. + */ + if (((log_bufferend + 1) % LOG_BUFFERSIZE) == log_bufferstart) + return; + log_buffer[log_bufferend++] = c; + log_bufferend = log_bufferend % LOG_BUFFERSIZE; +#if 0 + putint(c); + putch(' '); +#endif +} + +void log_put_uint16(unsigned int i) +{ + log_put_byte(i & 0xff); + log_put_byte((i >> 8) & 0xff); +} + +void log_put_uint(unsigned int i) +{ + log_put_byte(i & 0xff); + log_put_byte((i >> 8) & 0xff); + log_put_byte((i >> 16) & 0xff); + log_put_byte((i >> 24) & 0xff); +} + +void log_put_header(unsigned int timestamp) +{ + log_put_uint(LOG_MAGIC); + log_put_uint(log_generation); + log_put_uint(timestamp); + log_put_uint(log_read_busytime()); +} + +void log_put_array(char *data, int length) +{ + int i; + + for (i = 0; i < length; i++) + log_put_byte(data[i]); +} + +void log_put_float(float f) +{ + union { + float f; + unsigned int i; + } data; + data.f = f; + log_put_uint(data.i); +} + +unsigned int log_busystamp; +unsigned int log_busytime; + +void log_mark_busy(void) +{ + unsigned int time = timer_read(); + log_busystamp = time; +} + +void log_mark_idle(void) +{ + unsigned int time = timer_read(); + unsigned int diff = time - log_busystamp; + log_busytime += diff; +} + +unsigned int log_read_busytime(void) +{ + unsigned int time = log_busytime; + log_busytime = 0; + return time; +} diff --git a/main.c b/main.c index 5313a04..88febd8 100644 --- a/main.c +++ b/main.c @@ -1,6 +1,6 @@ /* main.c */ -#include "wmp.h" +#include "sensors.h" #include "i2c.h" #include "timer.h" #include "uart.h" @@ -10,6 +10,10 @@ #include "status.h" #include "watchdog.h" #include "thrust.h" +#include "panic.h" +#include "sdcard.h" +#include "log.h" +#include "spi.h" #define PINSEL0 (*((volatile unsigned int *) 0xE002C000)) #define PINSEL1 (*((volatile unsigned int *) 0xE002C004)) @@ -20,6 +24,8 @@ #define BUTTON_PRESSED (!((FP0XVAL) & 0x00010000)) +char buffer[512]; + void init_pins(void) { PINSEL0 = 0x2a09a255; /* P0.0 and P0.1 assigned to UART */ @@ -30,10 +36,10 @@ void init_pins(void) /* P0.12 and P0.13 assigned to MAT1.[01] */ /* P0.14 assigned to SPI1 */ - PINSEL1 = 0x00000540; /* P0.19 to P0.21 assigned to SPI1 */ + PINSEL1 = 0x00000140; /* P0.19 and P0.20 assigned to SPI1 */ SCS = 1; - FP0XDIR = 0x04000000; /* P0.26 is an output */ + FP0XDIR = 0x04200000; /* P0.26 and P0.21 are outputs */ FP0XVAL = 0x0; } @@ -47,140 +53,12 @@ void reply(char *str) #define reply(x) ((void)0) #endif -unsigned int count = 0; - -void minmax_sample(void) -{ - int count; - unsigned int fast_roll_min, fast_roll_max; - unsigned int fast_pitch_min, fast_pitch_max; - unsigned int fast_yaw_min, fast_yaw_max; - unsigned int slow_roll_min, slow_roll_max; - unsigned int slow_pitch_min, slow_pitch_max; - unsigned int slow_yaw_min, slow_yaw_max; - - putstr("Sampling min/max values\r\n"); - if (!wmp_sample()) { - putstr("\r\nRead error\r\n"); - return; - } - - fast_roll_min = fast_roll_max = wmp_roll; - fast_pitch_min = fast_pitch_max = wmp_pitch; - fast_yaw_min = fast_yaw_max = wmp_yaw; - - slow_roll_min = slow_roll_max = wmp_roll; - slow_pitch_min = slow_pitch_max = wmp_pitch; - slow_yaw_min = slow_yaw_max = wmp_yaw; - - count = 0; - - while (1) { - if (!wmp_sample()) { - putstr("\r\nRead error\r\n"); - return; - } - if (wmp_roll_fast) { - if (wmp_roll < fast_roll_min) - fast_roll_min = wmp_roll; - if (wmp_roll > fast_roll_max) - fast_roll_max = wmp_roll; - } else { - if (wmp_roll < slow_roll_min) - slow_roll_min = wmp_roll; - if (wmp_roll > slow_roll_max) - slow_roll_max = wmp_roll; - } - if (wmp_pitch_fast) { - if (wmp_pitch < fast_pitch_min) - fast_pitch_min = wmp_pitch; - if (wmp_pitch > fast_pitch_max) - fast_pitch_max = wmp_pitch; - } else { - if (wmp_pitch < slow_pitch_min) - slow_pitch_min = wmp_pitch; - if (wmp_pitch > slow_pitch_max) - slow_pitch_max = wmp_pitch; - } - if (wmp_yaw_fast) { - if (wmp_yaw < fast_yaw_min) - fast_yaw_min = wmp_yaw; - if (wmp_yaw > fast_yaw_max) - fast_yaw_max = wmp_yaw; - } else { - if (wmp_yaw < slow_yaw_min) - slow_yaw_min = wmp_yaw; - if (wmp_yaw > slow_yaw_max) - slow_yaw_max = wmp_yaw; - } - count++; - if (count > 1000) { - putstr("("); - puthex(slow_roll_min); - putstr(", "); - puthex(slow_pitch_min); - putstr(", "); - puthex(slow_yaw_min); - putstr(") ("); - puthex(slow_roll_max); - putstr(", "); - puthex(slow_pitch_max); - putstr(", "); - puthex(slow_yaw_max); - putstr(") ("); - puthex(fast_roll_min); - putstr(", "); - puthex(fast_pitch_min); - putstr(", "); - puthex(fast_yaw_min); - putstr(") ("); - puthex(fast_roll_max); - putstr(", "); - puthex(fast_pitch_max); - putstr(", "); - puthex(fast_yaw_max); - putstr(") \r"); - count = 0; - } - timer_delay_ms(2); - } -} - -void average_sample(void) -{ - int i; - int roll_total; - int pitch_total; - int yaw_total; - - putstr("Sampling average values\r\n"); - - roll_total = 0; - pitch_total = 0; - yaw_total = 0; - - for (i = 0; i < 0x1000; i++) { - if (!wmp_sample()) { - putstr("\r\nRead error\r\n"); - return; - } - roll_total += wmp_roll; - pitch_total += wmp_pitch; - yaw_total += wmp_yaw; - timer_delay_ms(2); - } - putstr("("); - puthex(roll_total); - putstr(", "); - puthex(pitch_total); - putstr(", "); - puthex(yaw_total); - putstr(")\r\n"); -} - void timer_event_handler(void) { - wmp_start_sample(); + unsigned int timestamp = timer_read(); + + log_put_header(timestamp); + sensors_start_sample(); } void menu_handler(void); @@ -232,7 +110,33 @@ void calibrate_escs() putstr("Exit calibration mode\r\n"); } +#ifdef USE_UART +void dump_buffer(char *buffer, unsigned int length, unsigned int addr); +void dump_buffer(char *buffer, unsigned int length, unsigned int addr) +{ + unsigned int i; + + for (i = 0; i < length; i++) { + if ((i % 16) == 0) { + puthex(addr+i); + putstr(":"); + } + putstr(" "); + puthex(buffer[i]); + if ((i % 16) == 15) { + putstr("\r\n"); + } + } + if ((i % 16) != 0) + putstr("\r\n"); +} +#else +#define dump_buffer(a, b, c) ((void)0) +#endif + int main(void) { + int i; + init_interrupt(); init_uart(); init_i2c(); @@ -242,10 +146,13 @@ int main(void) { event_register(EVENT_UART_INPUT, menu_handler); - event_register(EVENT_I2C_COMPLETE, wmp_event_handler); - event_register(EVENT_TIMER, timer_event_handler); + for (i = 0; i < 10; i++) { + if (init_sdcard()) + break; + } + putstr("Your entire life has been a mathematical error... a mathematical error I'm about to correct!\r\n"); if (BUTTON_PRESSED) @@ -254,12 +161,14 @@ int main(void) { putstr("prompt> "); timer_delay_ms(1000); - if (!wmp_init()) - putstr("WMP initialisation failed\r\n"); + if (!sensors_init()) + putstr("Sensor initialisation failed\r\n"); /* Flight is potentially live after this. */ - timer_set_period(5*TIMER_MS); - wmp_start_zero(); + timer_set_period(TIMER_MS(5)); +#if 1 + sensors_start_zero(); +#endif led_init(); @@ -267,8 +176,12 @@ int main(void) { /* Good luck! */ while (1) { + CHECKPOINT(0); led_update(); - event_dispatch(); + CHECKPOINT(1); + if (!event_dispatch()) + sdcard_poll(); + CHECKPOINT(2); watchdog_check(); } @@ -276,6 +189,40 @@ int main(void) { } static int power = 0; +static unsigned int sd_address = 0; + +unsigned int read_number(void) +{ + unsigned int number; + unsigned int base; + int digit; + char c; + + number = 0; + base = 10; + + while (1) { + if (getch(&c)) { + digit = -1; + if ((c == 0x0a) || (c == 0x0d)) + break; + putch(c); + if (c == 'x') + base = 16; + if ((c >= '0') && (c <= '9')) + digit = c - '0'; + if ((c >= 'A') && (c <= 'F')) + digit = c - 'A' + 10; + + if ((digit >= 0) && (digit < (int)base)) { + number = number * base; + number += digit; + } + } + } + putstr("\r\n"); + return number; +} void menu_handler(void) { @@ -283,7 +230,7 @@ void menu_handler(void) char c; while (getch(&c)) { -#if 1 +#if 0 continue; /* Yes, let's just ignore UART input now. */ #endif if (c == 0x0a) @@ -294,84 +241,37 @@ void menu_handler(void) case 0x0a: case 0x0d: break; - case 'A': - reply("apple"); - break; - case 'C': - count++; - putstr("The current count is "); - putint(count); - reply("."); - break; case 'H': case '?': reply("Help is not available. Try a psychiatrist."); break; - case 'T': - putstr(" I2C status is: "); - puthex(i2c_statreg()); - reply("."); - putstr("I2C register is: "); - puthex(i2c_conreg()); + case 'D': + sensors_dump(); + break; + case 'N': + putstr("The time is "); + puthex(timer_read()); reply("."); break; case 'I': - putstr("Initialising WMP... "); - if (wmp_init()) - reply("done"); - else - reply("FAIL"); - break; - case 'M': - putstr("Reading from WMP... "); - if (wmp_sample()) { - putstr("("); - puthex(wmp_roll); - putstr(", "); - puthex(wmp_pitch); - putstr(", "); - puthex(wmp_yaw); - reply(")."); - } else - reply("FAIL"); - break; - case 'L': - minmax_sample(); + init_sdcard(); break; - case 'V': - average_sample(); - break; - case 'D': - putstr("Reading calibration data... "); - if (wmp_read_calibration_data()) { - putstr("\r\n"); - for (i = 0; i < 0x10 ; i++) { - puthex(wmp_calibration_data[i]); - putstr(" "); - } - putstr("\r\n"); - for (i = 0x10; i < 0x20 ; i++) { - puthex(wmp_calibration_data[i]); - putstr(" "); - } - putstr("\r\n"); + case 'R': + sd_address = 0; + case 'S': + if (sdcard_read(sd_address++, buffer, 512)) { + dump_buffer(buffer, 512, (sd_address-1) * 512); + reply ("SD card read success"); } else { - reply("FAIL"); + reply("SD card read failed"); } break; - case 'N': - putstr("The time is "); - puthex(timer_read()); + case 'A': + sd_address = read_number(); + putstr("SD read address set to 0x"); + puthex(sd_address); reply("."); break; - case 'P': - putstr("Initialising timer... "); - /* We want a 100Hz loop but two samples per iteration. - * So, we go for 200Hz. */ - timer_set_period(5*TIMER_MS); - reply("done"); - wmp_start_zero(); - break; case 'W': for (i = 0; i < 4; i++) { putstr("Width "); @@ -386,6 +286,14 @@ void menu_handler(void) putstr("ALL INVALID!\r\n"); } break; +#if 0 + case 'T': + sdcard_start_write(); + break; +#endif + case 'L': + spi_drain(); + break; case '0' & 0xdf: set_thrust(0, 0.0); set_thrust(1, 0.0); diff --git a/motor.c b/motor.c index 4e119c9..0f9c714 100644 --- a/motor.c +++ b/motor.c @@ -5,6 +5,7 @@ #include "dcm.h" #include "uart.h" #include "status.h" +#include "log.h" float integral[3] = {0.0f, 0.0f, 0.0f}; float last[3]; @@ -33,7 +34,7 @@ void motor_pid_update(float troll, float mroll, { float derivative[3]; float out[3]; - float motor[3]; + float motor[4]; float roll, pitch, yaw; float error, max_error; float min_motor; @@ -145,6 +146,11 @@ void motor_pid_update(float troll, float mroll, set_thrust(1, motor[1]); set_thrust(2, motor[2]); set_thrust(3, motor[3]); + + log_put_uint16((unsigned int) (motor[0] * 65535)); + log_put_uint16((unsigned int) (motor[1] * 65535)); + log_put_uint16((unsigned int) (motor[2] * 65535)); + log_put_uint16((unsigned int) (motor[3] * 65535)); } void motor_kill(void) { diff --git a/mpu6050.c b/mpu6050.c new file mode 100644 index 0000000..c9c3b3e --- /dev/null +++ b/mpu6050.c @@ -0,0 +1,263 @@ +/* mpu6050.c */ + +#include "sensors.h" +#include "mpu6050.h" +#include "i2c.h" +#include "uart.h" +#include "dcm.h" +#include "fisqrt.h" +#include "stick.h" +#include "watchdog.h" +#include "status.h" +#include "abs.h" +#include "event.h" +#include "timer.h" +#include "panic.h" +#include "log.h" + +i2c_result mpu6050_result; +unsigned int mpu6050_generation; + +signed int gyro_zero_roll; +signed int gyro_zero_pitch; +signed int gyro_zero_yaw; + +#define GYRO_ZERO_COUNT 1000 + +unsigned int mpu6050_gyro_zero_count; + +unsigned char mpu6050_sample_data[14]; + +/*unsigned char mpu6050_whoami_command[1] = {0x75}; */ +unsigned char mpu6050_whoami_command[1] = {0x6B}; + +struct i2c_transaction mpu6050_whoami_transaction2; + +struct i2c_transaction mpu6050_whoami_transaction = { + (0x68 << 1) + 0, /* write */ + 1, + mpu6050_whoami_command, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + &mpu6050_whoami_transaction2 +}; + +struct i2c_transaction mpu6050_whoami_transaction2 = { + (0x68 << 1) + 1, /* read */ + 1, + mpu6050_sample_data, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + NULL +}; + +/* Accelerometer scaling */ +#define AFS_SEL 2 + + +unsigned char mpu6050_init_command[] = {0x6B, 0x01}; +unsigned char mpu6050_accel_init_command[] = {0x1c, (AFS_SEL<<3)}; + +struct i2c_transaction mpu6050_accel_init_transaction = { + (0x68 << 1) + 0, /* write */ + 2, + mpu6050_accel_init_command, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + NULL +}; + +struct i2c_transaction mpu6050_init_transaction = { + (0x68 << 1) + 0, /* write */ + 2, + mpu6050_init_command, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + &mpu6050_accel_init_transaction +}; + +unsigned char mpu6050_sample_command[] = {0x3B}; + +struct i2c_transaction mpu6050_sample_transaction2; + +struct i2c_transaction mpu6050_sample_transaction = { + (0x68 << 1) + 0, /* write */ + 1, + mpu6050_sample_command, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + &mpu6050_sample_transaction2 +}; + +struct i2c_transaction mpu6050_sample_transaction2 = { + (0x68 << 1) + 1, /* read */ + 14, + mpu6050_sample_data, + &mpu6050_result, + EVENT_MPU6050_I2C_COMPLETE, + NULL +}; + +void mpu6050_event_handler(void); + +bool mpu6050_init(void) +{ + event_register(EVENT_MPU6050_I2C_COMPLETE, mpu6050_event_handler); + + if (!i2c_start_transaction(&mpu6050_init_transaction)) + return FALSE; + while (i2c_busy()) ; + + return (mpu6050_result == I2C_SUCCESS); +} + +/* LSB / g */ +/* 1 for +- 2g */ +/* 2 for +- 4g */ +/* 4 for +- 8g */ +/* 8 for +- 16g */ +#define ACCEL_STEP (16384.0 / (float)(1<> 6) { + case 0: + /* CSD Version 1.0 */ + read_bl_len = response[5] & 0x0f; + c_size_mult = ((response[9] & 0x03) << 1) | (response[10] >> 7); + c_size = ((response[6] & 0x03) << 10) | (response[7] << 2) | + (response[8] >> 6); + + block_len = 1< 0) + response[0] = byte; + + /* We need to store the response, plus read one extra byte for luck. */ + /* XXX not an extra byte for luck any more */ + for (i = 1; i < response_length; i++) { + byte = spi_read_byte(); + response[i] = byte; + } + + if (wait_busy) { + do { + byte = spi_read_byte(); + } while (byte == 0); + + spi_write_byte(0xff); + } + + return TRUE; +} + +static bool sdcard_check_data_response(void) +{ + char byte; + unsigned int i; + + i = 0; + + do + { + byte = spi_read_byte(); + i++; + } while (((byte & 0x11) != 0x01) && (i < SDCARD_COMMAND_TIMEOUT)); + + if ((byte & 0x11) != 0x01) + return FALSE; + + if ((byte & 0x0f) != 0x05) /* Data accepted */ + return FALSE; + + /* Read one more byte for luck */ + byte = spi_read_byte(); + + return TRUE; +} + +static bool sdcard_command(char *command, unsigned int command_length, + char *response, unsigned int response_length, bool wait_busy) +{ + bool result; + + spi_transaction_start(); + + result = sdcard_command_innards(command, command_length, + response, response_length, wait_busy); + + spi_transaction_stop(); + + return result; +} + +static bool sdcard_read_block(char *buffer, unsigned int length) +{ + unsigned int i; + unsigned int crc_hi; + unsigned int crc_lo; + unsigned int crc; + + while (1) { + char byte = spi_read_byte(); + if (byte == 0xff) + continue; + if (byte == 0xfe) + break; + if ((byte & 0xf0) == 0) + if (byte != 0) + return FALSE; + } + + /* We need to store the response, plus read one extra byte for luck. */ + for (i = 0; i < length; i++) { + buffer[i] = spi_read_byte(); + } + + crc_hi = spi_read_byte(); + crc_lo = spi_read_byte(); + + crc = (crc_hi << 8) + crc_lo; + + /* XXX check CRC and return FALSE if doesn't match */ + + return TRUE; +} + +bool sdcard_read(unsigned int address, char *buffer, unsigned int length) +{ + bool valid; + char response; + + char cmd[6]; + + if (!high_capacity) + address = address * 512; + + cmd[0] = 0x51; /* CMD17 */ + cmd[1] = (address >> 24) & 0xff; + cmd[2] = (address >> 16) & 0xff; + cmd[3] = (address >> 8) & 0xff; + cmd[4] = (address >> 0) & 0xff; + cmd[5] = 0xff; /* dummy CRC */ + + spi_transaction_start(); + + if (!sdcard_command_innards(cmd, sizeof(cmd), + &response, 1, FALSE)) { + spi_transaction_stop(); + return FALSE; + } + + if (response != 0) { + spi_transaction_stop(); + return FALSE; + } + + valid = sdcard_read_block(buffer, length); + + spi_transaction_stop(); + + return valid; +} + +bool sdcard_read_csd(char *buffer) +{ + bool valid; + char response; + + spi_transaction_start(); + + if (!sdcard_command_innards(sdcard_cmd9, sizeof(sdcard_cmd9), + &response, 1, FALSE)) { + spi_transaction_stop(); + return FALSE; + } + + if (response != 0) { + spi_transaction_stop(); + return FALSE; + } + + valid = sdcard_read_block(buffer, 16); + + spi_transaction_stop(); + + return valid; +} + +bool sdcard_send_write_cmd(unsigned int address) +{ + char response; + + char cmd[6]; + + if (!high_capacity) + address = address * 512; + + cmd[0] = 0x59; /* CMD25 */ + cmd[1] = (address >> 24) & 0xff; + cmd[2] = (address >> 16) & 0xff; + cmd[3] = (address >> 8) & 0xff; + cmd[4] = (address >> 0) & 0xff; + cmd[5] = 0xff; /* dummy CRC */ + + spi_transaction_start(); + + if (!sdcard_command_innards(cmd, sizeof(cmd), + &response, 1, FALSE)) { + spi_transaction_stop(); + return FALSE; + } + + if (response != 0) { + spi_transaction_stop(); + return FALSE; + } + + return TRUE; +} + +static void sdcard_send_data_token(void) +{ + spi_write_byte(0xfc); +} + +static void sdcard_send_stop_token(void) +{ + spi_write_byte(0xfd); +} + +#define READ_UINT(b, i) ((b)[(i)] + ((b)[(i)+1] << 8) + \ + ((b)[(i)+2] << 16) + ((b)[(i)+3] << 24)) + +#define WRITE_UINT(b, i, d) \ + do { \ + (b)[(i)] = (d) & 0xff; \ + (b)[(i)+1] = ((d) >> 8) & 0xff; \ + (b)[(i)+2] = ((d) >> 16) & 0xff; \ + (b)[(i)+3] = ((d) >> 24) & 0xff; \ + } while (0) + + +/* We assume that the magic is to be found within this area. If not, + * we will need to read a bigger area. If the typical record size grows + * to more than a sector, for example, then we will need to read in multiple + * sectors where this function is called. + */ +bool sdcard_scan_magic(char *buffer, unsigned int size, unsigned int generation) +{ + unsigned int i; + + for (i = 0; i < size - 8; i++) { + if ((buffer[i] == (LOG_MAGIC & 0xff)) && + (buffer[i+1] == ((LOG_MAGIC >> 8) & 0xff)) && + (buffer[i+2] == ((LOG_MAGIC >> 16) & 0xff)) && + (buffer[i+3] == ((LOG_MAGIC >> 24) & 0xff)) && + (buffer[i+4] == ((generation >> 0) & 0xff)) && + (buffer[i+5] == ((generation >> 8) & 0xff)) && + (buffer[i+6] == ((generation >> 16) & 0xff)) && + (buffer[i+7] == ((generation >> 24) & 0xff))) + return TRUE; + } + + return FALSE; +} + +void sdcard_prepare(void) +{ + unsigned int magic; + unsigned int start_sector; + unsigned int count; + + if (!sdcard_read(0, log_buffer, 512)) + return; + + magic = READ_UINT(log_buffer, 0); + + if (magic != LOG_MAGIC) { + unsigned int i; + for (i = 0; i < 512; i++) + log_buffer[i] = 0; + WRITE_UINT(log_buffer, 0, LOG_MAGIC); + start_sector = SDCARD_BOUNDARY_SIZE; + log_generation = 0; + putstr("Did not find header. Formatting.\r\n"); + } else { + start_sector = READ_UINT(log_buffer, 4); + log_generation = READ_UINT(log_buffer, 8); + count = 0; + putstr("Found header.\r\n"); + putstr("Last started at sector "); + putint(start_sector); + putstr(" with generation "); + putint(log_generation); + putstr("\r\n"); + while (1) { + if (!sdcard_read(start_sector, log_buffer+512, 512)) + return; + /* This needs to change if record length exceeds 512 */ + if (sdcard_scan_magic(log_buffer+512, 512, + log_generation)) { + start_sector += SDCARD_BOUNDARY_SIZE; + if (start_sector >= sdcard_size) + start_sector = SDCARD_BOUNDARY_SIZE; + } else { + break; + } + if (count++ > (sdcard_size / SDCARD_BOUNDARY_SIZE)) { + start_sector = SDCARD_BOUNDARY_SIZE; + break; + } + } + log_generation++; + } + + WRITE_UINT(log_buffer, 4, start_sector); + WRITE_UINT(log_buffer, 8, log_generation); + + putstr("Starting at sector "); + putint(start_sector); + putstr(" with generation "); + putint(log_generation); + putstr("\r\n"); + + if (!sdcard_write(0, log_buffer, 512)) + return; + + sdcard_sector = start_sector; + sdcard_offset = 0; + log_enabled = TRUE; +} + + +static bool sdcard_busy(void) +{ + return (spi_read_byte() != 0xff); +} + +static void sdcard_send_dummy_crc(void) +{ + spi_write_byte(0xff); + spi_write_byte(0xff); +} + +void sdcard_poll(void) +{ + if (!log_enabled) + return; + if (LOG_BUFFER_EMPTY) + return; + log_mark_busy(); + if (sdcard_active == SDCARD_IDLE) { + spi_transaction_start(); + if (sdcard_busy()) { + spi_transaction_stop(); + log_mark_idle(); + return; + } + putch('C'); + if (sdcard_send_write_cmd(sdcard_sector)) + sdcard_active = SDCARD_WRITE_GAP; + else { + spi_transaction_stop(); + sdcard_active = SDCARD_ERROR; + } + } + if (sdcard_active == SDCARD_WRITE_GAP) { + if (sdcard_busy()) { + log_mark_idle(); + return; + } + sdcard_send_data_token(); + sdcard_active = SDCARD_WRITING_BLOCK; + } + if (sdcard_active == SDCARD_WRITING_BLOCK) { + unsigned int bytes_to_end_of_sector; + unsigned int i; + + i = LOG_BUFFER_BYTES; + bytes_to_end_of_sector = 512 - sdcard_offset; + if (i > bytes_to_end_of_sector) + i = bytes_to_end_of_sector; + if (i > 32) + i = 32; + sdcard_offset += i; + while (i--) { + spi_write_byte(log_get_byte()); + } + if (sdcard_offset >= 512) { + sdcard_offset = 0; + sdcard_sector++; + sdcard_send_dummy_crc(); + putch('.'); + if (!sdcard_check_data_response()) { + /* Set state to STOPPING instead? */ + /* How do we test this? */ + spi_transaction_stop(); + sdcard_active = SDCARD_ERROR; + log_mark_idle(); + return; + } + sdcard_active = SDCARD_WRITE_GAP; + if ((sdcard_sector & SDCARD_BOUNDARY_MASK) == 0) { + putch('S'); + sdcard_active = SDCARD_STOPPING; + } + } + } + if (sdcard_active == SDCARD_STOPPING) { + if (sdcard_busy()) { + log_mark_idle(); + return; + } + sdcard_send_stop_token(); + spi_transaction_stop(); + sdcard_active = SDCARD_IDLE; + } + log_mark_idle(); +} + +bool sdcard_write(unsigned int address, char *buffer, unsigned int length) +{ + unsigned int i; + + spi_transaction_start(); + + if (!sdcard_send_write_cmd(address)) { + spi_transaction_stop(); + return FALSE; + } + + sdcard_send_data_token(); + + for (i = 0; i < length; i++) { + spi_write_byte(buffer[i]); + } + + sdcard_send_dummy_crc(); + if (!sdcard_check_data_response()) { + spi_transaction_stop(); + return FALSE; + } + + while (sdcard_busy()) ; + + sdcard_send_stop_token(); + + while (sdcard_busy()) ; + + spi_transaction_stop(); + + return TRUE; +} + diff --git a/sensors.c b/sensors.c new file mode 100644 index 0000000..d516d6b --- /dev/null +++ b/sensors.c @@ -0,0 +1,233 @@ +/* sensors.c */ + +#include "mpu6050.h" +#include "dcm.h" +#include "fisqrt.h" +#include "watchdog.h" +#include "status.h" +#include "abs.h" +#include "panic.h" +#include "uart.h" +#include "log.h" +#include "stick.h" + +bool (*sensor_start_fns[])(void) = { + mpu6050_start_sample, +}; + +#define SENSOR_START_FNS (sizeof(sensor_start_fns)/sizeof(sensor_start_fns[0])) + +static unsigned int next_sensor; + +static bool sensors_zero; +static bool sensors_update; +static unsigned int sensors_discard; +static unsigned int sensors_generation; + +float sensors_gyro_roll; +float sensors_gyro_pitch; +float sensors_gyro_yaw; + +float sensors_temp; + +float sensors_accel_x; +float sensors_accel_y; +float sensors_accel_z; + +float gyro_yaw_zero; +float gyro_pitch_zero; +float gyro_roll_zero; + +void sensors_write_log(void); +void sensors_process(void); + +#define TWO_PI 6.28318531f +#define DEG_TO_RAD (TWO_PI/360.0f) + +/* The gyro has to stay within this limit in each axis in order to arm */ +#define GYRO_RATE_THRESHOLD (0.01 / DEG_TO_RAD) + +#define GYRO_ZERO_COUNT 1000 + +void sensors_dump(void); + +bool sensors_init(void) +{ + next_sensor = 0; + + if (!mpu6050_init()) + return FALSE; + + return TRUE; +} + +bool sensors_next_sample(void) +{ + bool result; + + result = (sensor_start_fns[next_sensor])(); + if (next_sensor >= SENSOR_START_FNS) + next_sensor = 0; + + return result; +} + +void sensors_sample_done(void) +{ + if (next_sensor == 0) { + sensors_write_log(); + return; + } + + if (!sensors_next_sample()) + panic(PANIC_SENSOR_FAIL); +} + +bool sensors_start_sample(void) +{ + next_sensor = 0; + return sensors_next_sample(); +} + +void sensors_write_gyro_data(float roll, float pitch, float yaw) +{ +#if 0 + sensors_gyro_roll = roll - gyro_roll_zero; + sensors_gyro_pitch = pitch - gyro_pitch_zero; + sensors_gyro_yaw = yaw - gyro_yaw_zero; +#else + sensors_gyro_roll = roll; + sensors_gyro_pitch = pitch; + sensors_gyro_yaw = yaw; +#endif +} + +void sensors_write_accel_data(float x, float y, float z) +{ + sensors_accel_x = x; + sensors_accel_y = y; + sensors_accel_z = z; +} + +void sensors_write_temp_data(float temp) +{ + sensors_temp = temp; + /* XXX HACK find a better place for this call */ + sensors_process(); +} + +#define LOG_SIGNATURE_SENSORS 0xDA7ADA7A +#define LOG_SIGNATURE_SENSORS2 0xDA7AF173 +void sensors_write_log(void) +{ +#if 0 + log_put_uint(LOG_SIGNATURE_SENSORS); + log_put_float(sensors_accel_x); + log_put_float(sensors_accel_y); + log_put_float(sensors_accel_z); + log_put_float(sensors_gyro_roll); + log_put_float(sensors_gyro_pitch); + log_put_float(sensors_gyro_yaw); + log_put_float(sensors_temp); +#else + /* XXX this just about comes out in the right place, but by luck */ + log_put_uint(LOG_SIGNATURE_SENSORS2); +#endif +} + +void sensors_start_zero(void) +{ + sensors_zero = TRUE; + sensors_update = FALSE; + sensors_discard = 100; + sensors_generation = 0; + putstr("Starting zero\r\n"); + mpu6050_start_zero(); +} + +void sensors_process(void) +{ + if (sensors_update) { +#if 1 + dcm_update(-sensors_gyro_pitch, -sensors_gyro_roll, + -sensors_gyro_yaw); +#else + dcm_update(0.0, 0.0, 0.0); +#endif + if (!status_armed()) { + if ( (abs(sensors_gyro_roll) < GYRO_RATE_THRESHOLD) && + (abs(sensors_gyro_pitch) < GYRO_RATE_THRESHOLD) && + (abs(sensors_gyro_yaw) < GYRO_RATE_THRESHOLD)) { + status_set_ready(STATUS_MODULE_GYRO_RATE, TRUE); + } else { + status_set_ready(STATUS_MODULE_GYRO_RATE, FALSE); + } + + } + + sensors_generation++; + +#if SEND_DCM + if ((sensors_generation % 40) == 0) { + dcm_send_packet(); + sensors_dump(); + } +#endif + + } else if (sensors_zero) { + if (sensors_discard) { + sensors_discard--; + } else { + gyro_yaw_zero += sensors_gyro_yaw; + gyro_pitch_zero += sensors_gyro_pitch; + gyro_roll_zero += sensors_gyro_roll; + sensors_generation++; + if (sensors_generation >= GYRO_ZERO_COUNT) { + sensors_zero = FALSE; + sensors_update = TRUE; + sensors_generation = 0; + gyro_yaw_zero /= GYRO_ZERO_COUNT; + gyro_pitch_zero /= GYRO_ZERO_COUNT; + gyro_roll_zero /= GYRO_ZERO_COUNT; + putstr("Zero finished\r\n"); + status_set_ready(STATUS_MODULE_GYRO_ZERO, TRUE); + } + } + } + watchdog_kick(WATCHDOG_GYRO); + +#if 1 + dcm_drift_correction(-sensors_accel_y, -sensors_accel_x, + -sensors_accel_z); +#endif +#if 0 + dcm_drift_correction(sensors_accel_x, sensors_accel_y, + sensors_accel_z); +#endif + + watchdog_kick(WATCHDOG_ACCEL); + stick_input(); +} + +void sensors_dump(void) +{ + putstr("("); + putint_s((int)(sensors_accel_x * 1000.0)); + putstr(","); + putint_s((int)(sensors_accel_y * 1000.0)); + putstr(","); + putint_s((int)(sensors_accel_z * 1000.0)); + putstr(")"); + + putstr("("); + putint_s((int)(sensors_gyro_roll * 1000.0)); + putstr(","); + putint_s((int)(sensors_gyro_pitch * 1000.0)); + putstr(","); + putint_s((int)(sensors_gyro_yaw * 1000.0)); + putstr(")"); + + putstr("("); + putint_s((int)(sensors_temp * 1000.0)); + putstr(")\r\n"); +} diff --git a/spi.c b/spi.c new file mode 100644 index 0000000..7a2a544 --- /dev/null +++ b/spi.c @@ -0,0 +1,116 @@ +/* spi.c */ + +#include "spi.h" +#include "interrupt.h" +#include "event.h" +#include "uart.h" + +#define SSPBASE 0xE0068000 + +#define SSPCR0 0x00 +#define SSPCR1 0x04 +#define SSPDR 0x08 +#define SSPSR 0x0c +#define SSPCPSR 0x10 +#define SSPIMSC 0x14 +#define SSPRIS 0x18 +#define SSPMIS 0x1c +#define SSPICR 0x20 + +#define REG(x) (((volatile unsigned char *)SSPBASE)[x]) +#define WREG(x) (((volatile unsigned int *)SSPBASE)[(x)/sizeof(unsigned int)]) + +#define TNF (REG(SSPSR) & (1<<1)) +#define RNE (REG(SSPSR) & (1<<2)) + +#define FP0XVAL (*((volatile unsigned int *) 0x3FFFC014)) +#define FP0XSET (*((volatile unsigned int *) 0x3FFFC018)) +#define FP0XCLR (*((volatile unsigned int *) 0x3FFFC01C)) + +void init_spi(void) +{ + WREG(SSPCR0) = 0x1f07; /* SPI clock = PCLK/64, mode 0, 8 bits */ +// WREG(SSPCR0) = 0xff07; + /* Set to 0x0007 later */ + REG(SSPCPSR) = 2; /* Divide PCLK by 2 */ + REG(SSPCR1) = 0x0002; /* Enable SSP, Master mode */ +} + +void spi_speedup(void) +{ +#if 1 + WREG(SSPCR0) = 0x0107; /* SPI clock = PCLK/4, mode 0, 8 bits */ +#endif +} + +void spi_write_byte(char byte) +{ + unsigned int dummy; + + while (!TNF) ; + WREG(SSPDR) = byte; + + while (!RNE) ; + dummy = REG(SSPDR); + +#ifdef SPIDEBUG + putstr(">"); + puthex(byte); + putstr("("); + puthex(dummy); + putstr(") "); +#endif +} + +char spi_read_byte(void) +{ + char byte; + + while (!TNF) ; + WREG(SSPDR) = 0xff; + + while (!RNE) ; + byte = (char) REG(SSPDR); + +#ifdef SPIDEBUG + putstr("<"); + puthex(byte); + putstr(" "); +#endif + + return byte; +} + +void spi_write_bytes(char *data, int len) +{ + while (len--) + spi_write_byte(*data++); +} + +void spi_read_bytes(char *data, int len) +{ + while (len--) + *data++ = spi_read_byte(); +} + +void spi_transaction_start(void) +{ + FP0XCLR = 0x00200000; +} + +void spi_transaction_stop(void) +{ + FP0XSET = 0x00200000; +} + +void spi_drain(void) +{ + char byte; + putstr("Draining:"); + while (RNE) { + byte = (char) REG(SSPDR); + putstr(" "); + puthex(byte); + } + putstr("\r\n"); +} diff --git a/stick.c b/stick.c index 524f7f9..60e53e2 100644 --- a/stick.c +++ b/stick.c @@ -12,9 +12,9 @@ #include "timer.h" #include "trig.h" #include "motor.h" -#include "wmp.h" #include "status.h" #include "watchdog.h" +#include "log.h" #define TWO_PI 6.28318531f #define PI 3.14159265f @@ -105,11 +105,23 @@ void stick_debug_calibrate() void stick_input(void) { float x, y, z, throttle; + unsigned int xi, yi, zi, throttlei; + if (timer_allvalid()) { - x = timer_input(0); - y = timer_input(1); - throttle = timer_input(2); - z = timer_input(3); + xi = timer_input(0); + yi = timer_input(1); + throttlei = timer_input(2); + zi = timer_input(3); + + log_put_uint16(xi); + log_put_uint16(yi); + log_put_uint16(throttlei); + log_put_uint16(zi); + + x = xi; + y = yi; + throttle = throttlei; + z = zi; #ifdef STICK_DEBUG_CALIBRATE if ((stick_counter % 100) == 0) diff --git a/timer.c b/timer.c index 3f04427..924e154 100644 --- a/timer.c +++ b/timer.c @@ -105,7 +105,7 @@ void init_timer(void) T2REG(TCR) = TCR_ENABLE | TCR_RESET; T2REG(CTCR) = 0; /* Use PCLK */ - T2WREG(PR) = 0; // Prescaling + T2WREG(PR) = 3; // Prescaling T2WREG(PC) = 0; // Reset the prescale counter T2WREG(TC) = 0; // Reset the counter @@ -120,7 +120,7 @@ void init_timer(void) T1REG(TCR) = TCR_ENABLE | TCR_RESET; T1REG(CTCR) = 0; /* Use PCLK */ - T1WREG(PR) = 0; // Prescaling + T1WREG(PR) = 3; // Prescaling T1WREG(PC) = 0; // Reset the prescale counter T1WREG(TC) = 0; // Reset the counter @@ -151,7 +151,7 @@ void timer_delay_clocks(unsigned int clocks) void timer_set_period(unsigned int period) { interrupt_register(TIMER3, timer_interrupt_handler); - T3WREG(MR0) = period; + T3WREG(MR0) = period-1; T3WREG(MCR) = MR0I | MR0R; T3WREG(TC) = 0; } diff --git a/timer.h b/timer.h index 2427a9f..415bffe 100644 --- a/timer.h +++ b/timer.h @@ -3,13 +3,14 @@ #include "types.h" -#define TIMER_PCLK 14745600 -#define TIMER_PRESCALE 9215 -#define TIMER0_PRESCALE 0 +#define TIMER_PCLK 58982400 +#define TIMER_PRESCALE 36863 +#define TIMER0_PRESCALE 3 #define TIMER_SECOND (TIMER_PCLK/(TIMER_PRESCALE+1)) -#define TIMER_MS (TIMER_SECOND/1000) -#define TIMER_US (TIMER_SECOND/1000000) +/* Since we're using awkward numbers, this gives better accuracy */ +#define TIMER_MS(x) ((x) * TIMER_SECOND / 1000) +#define TIMER_US(x) ((x) * TIMER_SECOND / 1000000) #define TIMER0_SECOND (TIMER_PCLK/(TIMER0_PRESCALE+1)) #define TIMER0_MS (TIMER0_SECOND/1000) @@ -21,7 +22,7 @@ #endif #define PWM_PERIOD ((4*PWM_MAX)+1) -#define TIMER_INPUT_TIMEOUT (TIMER_PCLK/10) +#define TIMER_INPUT_TIMEOUT (TIMER0_SECOND/10) #define TIMER_CPPM_SYNC 40000 #define TIMER_CH(x) (timer_map[x]) diff --git a/uart.c b/uart.c index 774f043..d262550 100644 --- a/uart.c +++ b/uart.c @@ -2,6 +2,8 @@ #include "types.h" #include "interrupt.h" #include "event.h" +#include "led.h" +#include "panic.h" #define UARTBASE 0xE000C000 @@ -30,10 +32,10 @@ char uart_txbuf[UART_TXBUFSIZE]; char uart_rxbuf[UART_RXBUFSIZE]; -volatile int uart_txread; -volatile int uart_txwrite; -volatile int uart_rxread; -volatile int uart_rxwrite; +volatile unsigned int uart_txread; +volatile unsigned int uart_txwrite; +volatile unsigned int uart_rxread; +volatile unsigned int uart_rxwrite; volatile bool tx_running; void __attribute__((interrupt("IRQ"))) uart_interrupt_handler(void); @@ -46,7 +48,7 @@ void init_uart(void) UREG(LCR) = 0x80; UREG(DLM) = 0x00; - UREG(DLL) = 0x08; /* 14745600 / (16*115200) */ + UREG(DLL) = 0x20; /* 58982400 / (16*115200) */ UREG(LCR) = 0x13; UREG(FCR) = 0x07; @@ -61,13 +63,18 @@ void init_uart(void) } void putch(char c) { + CHECKPOINT(4); /* Wait for space in the buffer */ - while (uart_txread == ((uart_txwrite+1) % UART_TXBUFSIZE)); + while (uart_txread == ((uart_txwrite+1) % UART_TXBUFSIZE)) ; + + interrupt_block(); if (uart_txread == uart_txwrite) { if (U0THRE) { tx_running = TRUE; UREG(THR) = c; + interrupt_unblock(); + CHECKPOINT(5); return; } } @@ -82,6 +89,37 @@ void putch(char c) { UREG(THR) = c; } } + interrupt_unblock(); + CHECKPOINT(5); +} + +void putch_irq(char c) { + /* Hope for space in the buffer */ +// if (uart_txread == ((uart_txwrite+1) % UART_TXBUFSIZE)) +// return; + +#if 1 + if (uart_txread == uart_txwrite) { + if (U0THRE) { + tx_running = TRUE; + UREG(THR) = c; + return; + } + } + + uart_txbuf[uart_txwrite] = c; + uart_txwrite = (uart_txwrite + 1) % UART_TXBUFSIZE; + + if (!tx_running) { + if (uart_txread != uart_txwrite) { + tx_running = TRUE; + uart_txread = (uart_txread + 1) % UART_TXBUFSIZE; + UREG(THR) = c; + } + } +#else + UREG(THR) = c; +#endif } void __attribute__((interrupt("IRQ"))) uart_interrupt_handler(void) @@ -93,10 +131,12 @@ void __attribute__((interrupt("IRQ"))) uart_interrupt_handler(void) * to treat them as such in this handler, so let the compiler * have an easier time. */ - int local_txwrite; - int local_txread; - int local_rxwrite; - int local_rxread; + unsigned int local_txwrite; + unsigned int local_txread; + unsigned int local_rxwrite; + unsigned int local_rxread; + + CHECKPOINT(((checkpoint & 0x00ff) | 0x0100)); source = UREG(IIR); @@ -136,6 +176,8 @@ void __attribute__((interrupt("IRQ"))) uart_interrupt_handler(void) break; } + CHECKPOINT((checkpoint & 0x00ff) | 0x0200); + interrupt_clear(); } diff --git a/uart.h b/uart.h index 94debdb..8ea2c2a 100644 --- a/uart.h +++ b/uart.h @@ -6,6 +6,7 @@ #ifdef USE_UART void init_uart(void); void putch(char c); +void putch_irq(char c); void putstr(char *s); void putint(unsigned int n); void putint_s(int n); diff --git a/watchdog.c b/watchdog.c index 95661ad..3189da2 100644 --- a/watchdog.c +++ b/watchdog.c @@ -30,7 +30,7 @@ void watchdog_check(void) int i; /* XXX not yet */ -/* return; */ + return; for (i = 0; i < WATCHDOG_MODULES; i++) { if ((signed int)(watchdog_last_seen[i] + WATCHDOG_TIMEOUT - time) < 0) { diff --git a/wmp.c b/wmp.c deleted file mode 100644 index 5a2a750..0000000 --- a/wmp.c +++ /dev/null @@ -1,320 +0,0 @@ -/* wmp.c */ - -#include "wmp.h" -#include "i2c.h" -#include "uart.h" -#include "dcm.h" -#include "fisqrt.h" -#include "stick.h" -#include "watchdog.h" -#include "status.h" -#include "abs.h" - -#define WMP_ZERO_COUNT 100 - - -#define ACCEL_ZERO_X 520 -#define ACCEL_ZERO_Y 516 -#define ACCEL_ZERO_Z 514 - -/* -516, 510, 710 --4, -6, 196 -16, 36, 38416 = 38468 -sqrt(38468) = 196.1326... -... somehow once we scale by gravity we get almost exactly 0.05. -*/ - -#define ACCEL_SCALE 0.05 - -/* Nunchuck pass-through mode */ -unsigned char wmp_init_command[2] = {0xfe, 0x05}; - -i2c_result wmp_result; -unsigned int wmp_generation; - -struct i2c_transaction wmp_init_transaction = { - (0x53 << 1) + 0, /* write */ - 2, - wmp_init_command, - &wmp_result, - NULL -}; - -unsigned char wmp_read_cal_command[1] = {0x20}; - -struct i2c_transaction wmp_read_cal_transaction2; - -struct i2c_transaction wmp_read_cal_transaction = { - (0x53 << 1) + 0, /* write */ - 1, - wmp_read_cal_command, - &wmp_result, - &wmp_read_cal_transaction2 -}; - -struct i2c_transaction wmp_read_cal_transaction2 = { - (0x53 << 1) + 1, /* read */ - 0x20, - wmp_calibration_data, - &wmp_result, - NULL -}; - -unsigned char wmp_sample_command[1] = {0x00}; - -unsigned char wmp_sample_data[6]; - -struct i2c_transaction wmp_sample_transaction2; - -struct i2c_transaction wmp_sample_transaction = { - (0x52 << 1) + 0, /* write */ - 1, - wmp_sample_command, - &wmp_result, - &wmp_sample_transaction2 -}; - -struct i2c_transaction wmp_sample_transaction2 = { - (0x52 << 1) + 1, /* read */ - 6, - wmp_sample_data, - &wmp_result, - NULL -}; - - -bool wmp_init(void) -{ - if (!i2c_start_transaction(&wmp_init_transaction)) - return FALSE; - while (i2c_busy()) ; - return (wmp_result == I2C_SUCCESS); -} - -unsigned char wmp_calibration_data[0x20]; - -bool wmp_read_calibration_data(void) -{ - if (!i2c_start_transaction(&wmp_read_cal_transaction)) - return FALSE; - while (i2c_busy()); - return (wmp_result == I2C_SUCCESS); -} - -unsigned int wmp_yaw; -unsigned int wmp_pitch; -unsigned int wmp_roll; - -unsigned int wmp_yaw_zero; -unsigned int wmp_pitch_zero; -unsigned int wmp_roll_zero; - -bool wmp_yaw_fast; -bool wmp_pitch_fast; -bool wmp_roll_fast; - -int accel_x; -int accel_y; -int accel_z; - -bool wmp_update; -bool wmp_zero; -unsigned int wmp_discard; - -#define TWO_PI 6.28318531f -#define DEG_TO_RAD (TWO_PI/360.0f) - -/* There's considerable debate about these values, and they may vary - * between different models of the Wii Motion Plus. It would be nice - * to be able to use the calibration data stored on the device itself - * but we don't know the format yet. - */ -#define SLOW_YAW_STEP (20 / DEG_TO_RAD) -#define SLOW_PITCH_STEP (20 / DEG_TO_RAD) -#define SLOW_ROLL_STEP (20 / DEG_TO_RAD) -#define FAST_YAW_STEP (4 / DEG_TO_RAD) -#define FAST_PITCH_STEP (4 / DEG_TO_RAD) -#define FAST_ROLL_STEP (4 / DEG_TO_RAD) - -/* The gyro has to stay within this limit in each axis in order to arm */ -#define GYRO_RATE_THRESHOLD (0.01 / DEG_TO_RAD) - -bool wmp_sample(void) -{ - if (!i2c_start_transaction(&wmp_sample_transaction)) - return FALSE; - - while (i2c_busy()); - - if (wmp_result != I2C_SUCCESS) - return FALSE; - - wmp_result = I2C_IN_PROGRESS; - - wmp_yaw = ((wmp_sample_data[3]>>2)<<8) + wmp_sample_data[0]; - wmp_pitch = ((wmp_sample_data[4]>>2)<<8) + wmp_sample_data[1]; - wmp_roll = ((wmp_sample_data[5]>>2)<<8) + wmp_sample_data[2]; - - /* XXX We don't take into account the fast/slow mode flag here */ - wmp_yaw_fast = !(wmp_sample_data[3] & 0x2); - wmp_pitch_fast = !(wmp_sample_data[3] & 0x1); - wmp_roll_fast = !(wmp_sample_data[4] & 0x2); - - return TRUE; -} - -bool wmp_start_sample(void) -{ - return i2c_start_transaction(&wmp_sample_transaction); -} - -void wmp_process_gyro_sample(void) -{ - float yaw, pitch, roll; - - wmp_yaw = ((wmp_sample_data[3]>>2)<<8) + wmp_sample_data[0]; - wmp_pitch = ((wmp_sample_data[4]>>2)<<8) + wmp_sample_data[1]; - wmp_roll = ((wmp_sample_data[5]>>2)<<8) + wmp_sample_data[2]; - - /* XXX We don't take into account the fast/slow mode flag here */ - wmp_yaw_fast = !(wmp_sample_data[3] & 0x2); - wmp_pitch_fast = !(wmp_sample_data[3] & 0x1); - wmp_roll_fast = !(wmp_sample_data[4] & 0x2); - - if (wmp_update) { - int tmp_yaw = wmp_yaw; - int tmp_pitch = wmp_pitch; - int tmp_roll = wmp_roll; - - tmp_yaw -= wmp_yaw_zero; - tmp_pitch -= wmp_pitch_zero; - tmp_roll -= wmp_roll_zero; - - if (wmp_yaw_fast) - yaw = ((float)tmp_yaw) / FAST_YAW_STEP; - else - yaw = ((float)tmp_yaw) / SLOW_YAW_STEP; - - if (wmp_pitch_fast) - pitch = ((float)tmp_pitch) / FAST_PITCH_STEP; - else - pitch = ((float)tmp_pitch) / SLOW_PITCH_STEP; - - if (wmp_roll_fast) - roll = ((float)tmp_roll) / FAST_ROLL_STEP; - else - roll = ((float)tmp_roll) / SLOW_ROLL_STEP; - - dcm_update(roll, pitch, yaw); - - if (!status_armed()) { - if ( (abs(roll) < GYRO_RATE_THRESHOLD) && - (abs(pitch) < GYRO_RATE_THRESHOLD) && - (abs(yaw) < GYRO_RATE_THRESHOLD) ) { - status_set_ready(STATUS_MODULE_GYRO_RATE, TRUE); - } else { - status_set_ready(STATUS_MODULE_GYRO_RATE, FALSE); - } - } - - wmp_generation++; - -#if SEND_DCM - if ((wmp_generation % 20) == 0) - dcm_send_packet(); -#endif - - } else if (wmp_zero) { - if (wmp_discard) { - wmp_discard--; - } else { - wmp_yaw_zero += wmp_yaw; - wmp_pitch_zero += wmp_pitch; - wmp_roll_zero += wmp_roll; - wmp_generation++; - if (wmp_generation >= WMP_ZERO_COUNT) { - wmp_zero = FALSE; - wmp_update = TRUE; - wmp_generation = 0; - wmp_yaw_zero /= WMP_ZERO_COUNT; - wmp_pitch_zero /= WMP_ZERO_COUNT; - wmp_roll_zero /= WMP_ZERO_COUNT; - putstr("Zero finished\r\n"); - status_set_ready(STATUS_MODULE_GYRO_ZERO, TRUE); - } - } - } - watchdog_kick(WATCHDOG_GYRO); -} - -void wmp_process_accel_sample(void) -{ - float x, y, z; -#if 0 - float invmag; -#endif - - accel_x = (wmp_sample_data[2]<<2) + ((wmp_sample_data[5]>>3) & 0x02); - accel_y = (wmp_sample_data[3]<<2) + ((wmp_sample_data[5]>>4) & 0x02); - accel_z = ((wmp_sample_data[4]<<2) & 0x3f8) + - ((wmp_sample_data[5]>>5) & 0x06); - - x = (accel_x - ACCEL_ZERO_X) * ACCEL_SCALE; - y = (accel_y - ACCEL_ZERO_Y) * ACCEL_SCALE; - z = (accel_z - ACCEL_ZERO_Z) * ACCEL_SCALE; - -#if 0 - invmag = fisqrt(x*x + y*y + z*z); - - x = x * invmag; - y = y * invmag; - z = z * invmag; -#endif - -#if 0 - accel_x = (x * 512.0 + 1000.0); - accel_y = (y * 512.0 + 1000.0); - accel_z = (z * 512.0 + 1000.0); -#endif - -#if 0 - putstr("("); - putint(accel_x); - putstr(", "); - putint(accel_y); - putstr(", "); - putint(accel_z); - putstr(")\r\n"); -#endif - - /* The minus signs are needed because something is upside down. - * It might actually be the WMP, but we're defining coordinates based - * on that so we'll just fudge it here. - */ - dcm_drift_correction(x, -y, -z); - watchdog_kick(WATCHDOG_ACCEL); - stick_input(); -} - -void wmp_event_handler(void) -{ - if (wmp_result != I2C_SUCCESS) - return; - - wmp_result = I2C_IN_PROGRESS; - - if (wmp_sample_data[5] & 0x02) - wmp_process_gyro_sample(); - else - wmp_process_accel_sample(); -} - -void wmp_start_zero(void) -{ - wmp_zero = TRUE; - wmp_update = FALSE; - wmp_discard = 100; - wmp_generation = 0; - putstr("Starting zero\r\n"); -}