The unified diff between revisions [4f22e7ef..] and [9ca449dd..] is displayed below. It can also be downloaded as a raw diff.
# # old_revision [4f22e7ef7d3064e3b51a5b868a4722f3f13c747b] # new_revision [9ca449dd7941ad52e33bdcb5c28b2ba35d54219a] # # delete "wmp.c" # # add_file "log.c" # content [dd8755b026f0047b95fa77ca6906ef13a3bc42a1] # # add_file "mpu6050.c" # content [9459051539738c013ae40e90c65e2936d93fc2bf] # # add_file "sdcard.c" # content [31209e9d85b303159ee1479d140e3f429ca8ec90] # # add_file "sensors.c" # content [129fa873f84fea1677a1d27563afd5402f8d6677] # # add_file "spi.c" # content [82c405280f971eecb205e97ea0b6fc92d23bea85] # # patch "Makefile" # from [33137b98e954e7abaca672f6ad561d1367b25d30] # to [5ffbb5a652dea40a038ffdfb0fab62a0ad843585] # # patch "crt0.s" # from [0dec978fe999503c2346f42cb3588323205db715] # to [87b5cd7607f8ac7300b0343c51a127633c3c6f5c] # # patch "dcm.c" # from [90d94d47a262c72d472ee7aca36bb55e2b328e66] # to [441585448f6476586c2fbcb260b7a6d908cab479] # # patch "event.c" # from [a1e88ceaa05efc054e07ea1958e27a871ea460fe] # to [0931d041137bc29648c8785b0198b3712fda631e] # # patch "event.h" # from [6ae28929ff160f4c112bac5954c1fbfa282b6756] # to [ac8bb369e9ee5d7e0b005aed6811412c323dcb6d] # # patch "fisqrt.c" # from [65553ed1491b6b3d4a266cc821c3f117c623a203] # to [4fb1fdd34c6fc3f9e172991f3149c9c4057eb68e] # # patch "i2c.c" # from [7f5362fc29f8f8f81682424c73930b5e3044c994] # to [2dff1acf82a051d155952429555a65a193ba0fd5] # # patch "i2c.h" # from [096c6ddae7948e9802621e5cda6f7289fd917cab] # to [2712fc89825bdda63fbe762367461fcd3919ce39] # # patch "interrupt.h" # from [bf362242c19f7cf4842e9f571d4e56915037d18d] # to [56b191b60c258a1d20ddf786c75868f6269709f1] # # patch "main.c" # from [dca584ff21fb033c2abff13cfb14db9c8c2c93c2] # to [d72da4d2ad3be9c5db9d6019164152e4eea61e43] # # patch "motor.c" # from [3772a98b596df907c16e491694c952392f804893] # to [743afb567c758bd114cb1dd29f25081f3454108c] # # patch "panic.c" # from [d33cc5af7411880a40d87b7d56a9086705b49112] # to [2f700032fc2a9007e14fd1fa648575a0336ad211] # # patch "panic.h" # from [4ed7a80d4d14e77a557b91a5dcc39e621f765d6e] # to [b263e2768b03feb4970bdeedf75a8ff0de240894] # # patch "stick.c" # from [57244f98d116ac872a524b6de2c760af161fd0a0] # to [50fffafb58abb3d5935c27843ff50c4bd4ec5300] # # patch "timer.c" # from [7124cf1b41c7dfff9aa12b521db3b19870d7c718] # to [bcc39911ffda1137e85438b93c640cfb61c66a65] # # patch "timer.h" # from [35eaca1c3fe810fd864583e55e91ab518ea25bd5] # to [77f91cb86b26421f2d28236d25d384e9a025e939] # # patch "uart.c" # from [7a38c486dc1280695e1e62c6d3a76d6c9f849f67] # to [2ed02bb0e471483e0e77958a43e2e05516d3c127] # # patch "uart.h" # from [86972bb7765797e0b5e660710beed2a9bbd9ec13] # to [0e400c96fd36e3b821c79f86ab43102f9be607c7] # # patch "watchdog.c" # from [97962bde1fc11e8fea8fae642ed8e0bcdd78468a] # to [d26ee7fc5e47c24a05fdd00cb771f966b4a87102] # ============================================================ --- Makefile 33137b98e954e7abaca672f6ad561d1367b25d30 +++ Makefile 5ffbb5a652dea40a038ffdfb0fab62a0ad843585 @@ -3,14 +3,17 @@ SSRCS=crt0.s 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 ============================================================ --- crt0.s 0dec978fe999503c2346f42cb3588323205db715 +++ crt0.s 87b5cd7607f8ac7300b0343c51a127633c3c6f5c @@ -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 ============================================================ --- dcm.c 90d94d47a262c72d472ee7aca36bb55e2b328e66 +++ dcm.c 441585448f6476586c2fbcb260b7a6d908cab479 @@ -11,9 +11,14 @@ #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,7 +27,10 @@ /* 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,8 +44,16 @@ float omega_x, omega_y, omega_z; 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) { omega_x = x + omega_i[0] + omega_p[0]; @@ -57,6 +73,8 @@ void dcm_update(float x, float y, float 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 omega_i[i] += error[i] * (KI_ROLLPITCH * weight); } + dcm_log(LOG_MAGIC_DCM_DRIFT); + #if 0 putstr("w: "); putint_s((int)(weight * 100000.0f)); ============================================================ --- event.c a1e88ceaa05efc054e07ea1958e27a871ea460fe +++ event.c 0931d041137bc29648c8785b0198b3712fda631e @@ -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_BIT(x)) +/* + * This function must be called with interrupts disabled. + * This will normally be the case as it is typically called from within + * an interrupt handler anyway. + */ + void event_set(unsigned int event) { if (event > 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) ============================================================ --- event.h 6ae28929ff160f4c112bac5954c1fbfa282b6756 +++ event.h ac8bb369e9ee5d7e0b005aed6811412c323dcb6d @@ -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 */ ============================================================ --- fisqrt.c 65553ed1491b6b3d4a266cc821c3f117c623a203 +++ fisqrt.c 4fb1fdd34c6fc3f9e172991f3149c9c4057eb68e @@ -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; ============================================================ --- i2c.c 7f5362fc29f8f8f81682424c73930b5e3044c994 +++ i2c.c 2dff1acf82a051d155952429555a65a193ba0fd5 @@ -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"))) i 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"))) i 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"))) i 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 */ ============================================================ --- i2c.h 096c6ddae7948e9802621e5cda6f7289fd917cab +++ i2c.h 2712fc89825bdda63fbe762367461fcd3919ce39 @@ -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 */ }; ============================================================ --- interrupt.h bf362242c19f7cf4842e9f571d4e56915037d18d +++ interrupt.h 56b191b60c258a1d20ddf786c75868f6269709f1 @@ -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) ============================================================ --- /dev/null +++ log.c dd8755b026f0047b95fa77ca6906ef13a3bc42a1 @@ -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; +} ============================================================ --- main.c dca584ff21fb033c2abff13cfb14db9c8c2c93c2 +++ main.c d72da4d2ad3be9c5db9d6019164152e4eea61e43 @@ -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,142 +53,14 @@ void reply(char *str) #define reply(x) ((void)0) #endif -unsigned int count = 0; - -void minmax_sample(void) +void timer_event_handler(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; + unsigned int timestamp = timer_read(); - 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); - } + log_put_header(timestamp); + sensors_start_sample(); } -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(); -} - void menu_handler(void); void wait_for_button_pressed(bool target) @@ -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,9 +146,12 @@ 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); - 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"); @@ -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,14 +189,48 @@ static int power = 0; } 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) { int i; 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()); + case 'D': + sensors_dump(); + break; + case 'N': + putstr("The time is "); + puthex(timer_read()); reply("."); - putstr("I2C register is: "); - puthex(i2c_conreg()); - reply("."); break; case 'I': - putstr("Initialising WMP... "); - if (wmp_init()) - reply("done"); - else - reply("FAIL"); + init_sdcard(); 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(); - 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); ============================================================ --- motor.c 3772a98b596df907c16e491694c952392f804893 +++ motor.c 743afb567c758bd114cb1dd29f25081f3454108c @@ -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 { 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 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) { ============================================================ --- /dev/null +++ mpu6050.c 9459051539738c013ae40e90c65e2936d93fc2bf @@ -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<<AFS_SEL)) + +#define TWO_PI 6.28318531f +#define DEG_TO_RAD (TWO_PI/360.0f) + +/* A step of 131 = 1 degree. */ +/* Overall, this is LSB / rad/s */ +#define GYRO_STEP (131.0 / DEG_TO_RAD) + +/* LSB / degree C */ +#define TEMP_STEP 340.0 +#define TEMP_OFFSET 36.53 + +#define ACCEL_SCALE (1.0 / ACCEL_STEP) +#define GYRO_SCALE (1.0 / GYRO_STEP) +#define TEMP_SCALE (1.0 / TEMP_STEP) + +#if 0 +bool mpu6050_sample(void) +{ + unsigned int x, y, z; + unsigned int temp; + unsigned int roll, pitch, yaw; + + if (!i2c_start_transaction(&mpu6050_sample_transaction)) + return FALSE; + + while (i2c_busy()); + + if (mpu6050_result != I2C_SUCCESS) + return FALSE; + + mpu6050_result = I2C_IN_PROGRESS; + + x = (mpu6050_sample_data[0] << 8) + mpu6050_sample_data[1]; + y = (mpu6050_sample_data[2] << 8) + mpu6050_sample_data[3]; + z = (mpu6050_sample_data[4] << 8) + mpu6050_sample_data[5]; + + temp = (mpu6050_sample_data[6] << 8) + mpu6050_sample_data[7]; + + roll = (mpu6050_sample_data[ 8] << 8) + mpu6050_sample_data[ 9]; + pitch = (mpu6050_sample_data[10] << 8) + mpu6050_sample_data[11]; + yaw = (mpu6050_sample_data[12] << 8) + mpu6050_sample_data[13]; + + putstr("MPU6050 sample data:\r\n"); + putstr("x: "); + puthex(x); + putstr(", y: "); + puthex(y); + putstr(", z: "); + puthex(z); + putstr("\r\ntemperature:"); + puthex(temp); + putstr("\r\nroll:"); + puthex(roll); + putstr(", pitch:"); + puthex(pitch); + putstr(", yaw:"); + puthex(yaw); + putstr("\r\n\r\n"); +#if 0 + sensors_write_gyro_data(roll, pitch, yaw); + sensors_write_accel_data(x, y, z); +#endif + + return TRUE; +} +#endif + +bool mpu6050_start_sample(void) +{ + return i2c_start_transaction(&mpu6050_sample_transaction); +} + +void mpu6050_start_zero(void) +{ + mpu6050_gyro_zero_count = GYRO_ZERO_COUNT; + gyro_zero_roll = 0; + gyro_zero_pitch = 0; + gyro_zero_yaw = 0; +} + +void mpu6050_event_handler(void) +{ + signed short int xi, yi, zi; + signed short int tempi; + signed short int rolli, pitchi, yawi; + + float x, y, z; + float temp; + float roll, pitch, yaw; + + CHECKPOINT(9); + + if (mpu6050_result != I2C_SUCCESS) + return; + + mpu6050_result = I2C_IN_PROGRESS; + + sensors_sample_done(); + + xi = (mpu6050_sample_data[0] << 8) + mpu6050_sample_data[1]; + yi = (mpu6050_sample_data[2] << 8) + mpu6050_sample_data[3]; + zi = (mpu6050_sample_data[4] << 8) + mpu6050_sample_data[5]; + + tempi = (mpu6050_sample_data[6] << 8) + mpu6050_sample_data[7]; + + rolli = (mpu6050_sample_data[ 8] << 8)+mpu6050_sample_data[ 9]; + pitchi = (mpu6050_sample_data[10] << 8)+mpu6050_sample_data[11]; + yawi = (mpu6050_sample_data[12] << 8)+mpu6050_sample_data[13]; + + if (mpu6050_gyro_zero_count) { + gyro_zero_roll += rolli; + gyro_zero_pitch += pitchi; + gyro_zero_yaw += yawi; + if (--mpu6050_gyro_zero_count == 0) { + gyro_zero_roll /= GYRO_ZERO_COUNT; + gyro_zero_pitch /= GYRO_ZERO_COUNT; + gyro_zero_yaw /= GYRO_ZERO_COUNT; + } + } else { + rolli -= gyro_zero_roll; + pitchi -= gyro_zero_pitch; + yawi -= gyro_zero_yaw; + } + + x = ((float)xi) * ACCEL_SCALE; + y = ((float)yi) * ACCEL_SCALE; + z = ((float)zi) * ACCEL_SCALE; + + temp = ((float)tempi) * TEMP_SCALE + TEMP_OFFSET; + + roll = ((float)rolli) * GYRO_SCALE; + pitch = ((float)pitchi) * GYRO_SCALE; + yaw = ((float)yawi) * GYRO_SCALE; + + sensors_write_gyro_data(roll, pitch, yaw); + sensors_write_accel_data(x, y, z); + sensors_write_temp_data(temp); + + log_put_array((char *)mpu6050_sample_data, 14); + + CHECKPOINT(10); +} + ============================================================ --- panic.c d33cc5af7411880a40d87b7d56a9086705b49112 +++ panic.c 2f700032fc2a9007e14fd1fa648575a0336ad211 @@ -13,17 +13,38 @@ #include "motor.h" #include "led.h" +#ifdef PANIC_CHECKPOINT +unsigned int checkpoint; +#endif + +#ifdef PANIC_32BIT +#define PANIC_BITS 32 +#else +#define PANIC_BITS 16 +#endif + +#ifdef PANIC_32BIT led_pattern led_pattern_panic[] = {100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, + 100, 100, 100, 100, 100, 100, 100, 100, + 100, 100, 100, 100, 100, 100, 100, 100, + 100, 100, 100, 100, 100, 100, 100, 100, + 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 3000, 0}; +#else +led_pattern led_pattern_panic[] = {100, 100, 100, 100, 100, 100, 100, 100, + 100, 100, 100, 100, 100, 100, 100, 100, + 100, 100, 100, 100, 100, 100, 100, 100, + 100, 100, 100, 100, 100, 100, 100, 3000, 0}; +#endif /* Take the lower 16 bits and make a pattern of them, MSB first */ static void panic_create_pattern(led_pattern *pattern, unsigned int reason) { int i; - for (i = 0; i < 16; i++) { - if (reason & (1<<(15-i))) { + for (i = 0; i < PANIC_BITS; i++) { + if (reason & (1<<((PANIC_BITS-1)-i))) { pattern[2*i] = 400; pattern[2*i+1] = 100; } else { @@ -32,19 +53,16 @@ static void panic_create_pattern(led_pat } if ((i % 4) == 3) pattern[2*i+1] += 500; - if (i == 15) + if (i == (PANIC_BITS-1)) pattern[2*i+1] += 2500; } } void panic(unsigned int reason) { - /* - * We may one day be able to do something with the reason, like emit - * a final deathbed confession. So we'll provide the reasons in the - * caller and just ignore them for now. - */ - (void)reason; +#if PANIC_CHECKPOINT + reason = checkpoint; +#endif motor_kill(); ============================================================ --- panic.h 4ed7a80d4d14e77a557b91a5dcc39e621f765d6e +++ panic.h b263e2768b03feb4970bdeedf75a8ff0de240894 @@ -6,3 +6,11 @@ void panic(unsigned int reason); /* Panic code goes in the low 8 bits */ #define PANIC_WATCHDOG_TIMEOUT 0x100 +#define PANIC_SENSOR_FAIL 0x200 + +#ifdef PANIC_CHECKPOINT +extern unsigned int checkpoint; +#define CHECKPOINT(x) do { checkpoint = (x); } while (0) +#else +#define CHECKPOINT(x) +#endif ============================================================ --- /dev/null +++ sdcard.c 31209e9d85b303159ee1479d140e3f429ca8ec90 @@ -0,0 +1,678 @@ +/* sdcard.c */ + +#include "spi.h" +#include "types.h" +#include "uart.h" +#include "timer.h" +#include "event.h" +#include "log.h" + +#define spi_write_array(x) spi_write_bytes(x, sizeof(x)/sizeof(x[0])) + +#define SDCARD_COMMAND_TIMEOUT 0xffff + +char dummy_block[] = {0xff, 0xff, 0xff, 0xff, 0xff, + 0xff, 0xff, 0xff, 0xff, 0xff}; +char reset_command[] = {0x40, 0, 0, 0, 0, 0x95}; + +/* Voltage = 2.7-3.6V, check pattern = 0xaa, CRC matters for CMD8 */ +char sdcard_cmd8[] = {0x48, 0, 0, 0x01, 0xaa, 0x87}; + +char sdcard_cmd55[] = {0x77, 0, 0, 0, 0, 0xff}; + +char sdcard_acmd41[] = {0x69, 0, 0, 0, 0, 0xff}; +char sdcard_acmd41_hcs[] = {0x69, 0x40, 0, 0, 0, 0xff}; + +char sdcard_cmd58[] = {0x7a, 0, 0, 0, 0, 0xff}; + +/* 512 bytes block length */ +char sdcard_cmd16[] = {0x50, 0, 0, 2, 0, 0xff}; + +/* Read CSD */ +char sdcard_cmd9[] = {0x49, 0, 0, 0, 0, 0xff}; + + +static bool high_capacity; + +#ifdef SDCARD_BOUNDARY_128K +/* 128K */ +#define SDCARD_BOUNDARY_MASK 0xff +#define SDCARD_BOUNDARY_SIZE 0x100 +#else +/* 32K */ +#define SDCARD_BOUNDARY_MASK 0x3f +#define SDCARD_BOUNDARY_SIZE 0x40 +#endif + +unsigned int sdcard_sector; +unsigned int sdcard_offset; +unsigned int sdcard_size; /* defined as number of sectors */ + +#define SDCARD_IDLE 0 +#define SDCARD_WRITE_GAP 1 +#define SDCARD_WRITING_BLOCK 2 +#define SDCARD_STOPPING 3 +#define SDCARD_ERROR 4 + +unsigned int sdcard_active; + +static bool sdcard_command(char *command, unsigned int command_length, + char *response, unsigned int response_length, bool wait_busy); + +bool sdcard_write(unsigned int address, char *buffer, unsigned int length); +bool sdcard_read_csd(char *buffer); +void sdcard_prepare(void); + +/* SD card (SPI mode initialisation) + +power on + +CMD0+ +CMD8 +if (no response from CMD8) { + // legacy (MMC) card + CMD58 (optional, read OCR) - no or bad response = don't use card + while (ACMD41(arg 0x00) & in_idle_state_mask) + ; + done +} else { + // SD card + if (response from CMD8 was present but invalid + (check pattern not matched)) + retry CMD8; + CMD58 (optional, read OCR) + while (ACMD41(arg HCS=1) & in_idle_state_mask) + ; + CMD58 (Get CCS) + if (CCS) + done - high capacity SD card + else + done - standard SD card +} + + +*/ + +bool init_sdcard(void) +{ + char response[16]; + unsigned int i; + + unsigned int read_bl_len, c_size_mult, c_size; + unsigned int block_len, mult, blocknr; + + putstr("Initialising SPI\r\n"); + + init_spi(); + + high_capacity = FALSE; + + putstr("Sending 80 clocks\r\n"); + + spi_transaction_start(); + spi_write_array(dummy_block); + spi_transaction_stop(); + + putstr("Sending reset command\r\n"); + + if (!sdcard_command(reset_command, sizeof(reset_command), + response, 1, FALSE)) + return FALSE; + + putstr("Reset command successful. Checking response.\r\n"); + + puthex(response[0]); + + putstr("\r\n"); + + if (response[0] != 0x01) + return FALSE; + + putstr("Sending CMD8\r\n"); + + if (!sdcard_command(sdcard_cmd8, sizeof(sdcard_cmd8), + response, 5, FALSE)) + { + putstr("No response. Legacy device.\r\n"); + /* Legacy device */ + do { + if (!sdcard_command(sdcard_cmd55, sizeof(sdcard_cmd55), + response, 1, FALSE)) + return FALSE; + if (response[0] != 0x01) + return FALSE; + if (!sdcard_command(sdcard_acmd41, + sizeof(sdcard_acmd41), + response, 1, FALSE)) + return FALSE; + } while (response[0] & 1); + putstr("ACMD41 gave us the right response.\r\n"); + } else { + putstr("We got a response. Not a legacy device.\r\n"); + /* Not legacy device */ + for (i = 1; i < 4; i++) { + if (response[i] != sdcard_cmd8[i]) { + /* We should really retry here. Meh. */ + return FALSE; + } + } + putstr("Response OK. Safe to continue.\r\n"); + do { + if (!sdcard_command(sdcard_cmd55, sizeof(sdcard_cmd55), + response, 1, FALSE)) + return FALSE; + if (response[0] != 0x01) + return FALSE; + if (!sdcard_command(sdcard_acmd41_hcs, + sizeof(sdcard_acmd41_hcs), + response, 1, FALSE)) + return FALSE; + } while (response[0] & 1); + putstr("ACMD41 gave us the right response.\r\n"); + if (!sdcard_command(sdcard_cmd58, sizeof(sdcard_cmd58), + response, 5, FALSE)) + return FALSE; + putstr("OCR register retrieved.\r\n"); + if ((response[1] & 0x80) == 0) + return FALSE; + putstr("Chip isn't still powering up.\r\n"); + if (response[1] & 0x40) { + putstr("We have a high capacity device.\r\n"); + high_capacity = TRUE; + } else { + putstr("We have a low capacity device.\r\n"); + high_capacity = FALSE; + } + } + + spi_speedup(); + + /* Set block length to 512 */ + if (!sdcard_command(sdcard_cmd16, sizeof(sdcard_cmd16), + response, 1, FALSE)) + return FALSE; + + putstr("Determining card size.\r\n"); + + if (!sdcard_read_csd(response)) + return FALSE; + + putstr("Read CSD\r\n"); + + switch ((response[0] & 0xc0) >> 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<<read_bl_len; + mult = 1<<(c_size_mult+2); + blocknr = (c_size+1) * mult; + sdcard_size = blocknr * block_len / 512; + break; + case 1: + /* CSD Version 2.0 */ + c_size = ((response[7] & 0x3f) << 16) | + (response[8] << 8) | response[9]; + sdcard_size = (c_size+1) * 1024; + break; + default: + /* Unrecognised CSD version */ + putstr("Unrecognised CSD version\r\n"); + return FALSE; + } + + putstr("SD initialisation sequence complete.\r\n"); + putstr("size = "); + putint(sdcard_size / 2); + putstr("KB\r\n"); + + putstr("Initialising logging system.\r\n"); + sdcard_prepare(); + + return TRUE; +} + +static bool sdcard_command_innards(char *command, unsigned int command_length, + char *response, unsigned int response_length, bool wait_busy) +{ + char byte; + unsigned int i; + + spi_write_bytes(command, command_length); + + i = 0; + + do + { + byte = spi_read_byte(); + i++; + } while (((byte & 0x80) != 0) && (i < SDCARD_COMMAND_TIMEOUT)); + + if (byte & 0x80) + return FALSE; + + if (response_length > 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; +} + ============================================================ --- /dev/null +++ sensors.c 129fa873f84fea1677a1d27563afd5402f8d6677 @@ -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"); +} ============================================================ --- /dev/null +++ spi.c 82c405280f971eecb205e97ea0b6fc92d23bea85 @@ -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"); +} ============================================================ --- stick.c 57244f98d116ac872a524b6de2c760af161fd0a0 +++ stick.c 50fffafb58abb3d5935c27843ff50c4bd4ec5300 @@ -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,12 +105,24 @@ void stick_input(void) { 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) stick_debug_calibrate(); ============================================================ --- timer.c 7124cf1b41c7dfff9aa12b521db3b19870d7c718 +++ timer.c bcc39911ffda1137e85438b93c640cfb61c66a65 @@ -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_set_period(unsigned int perio 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; } ============================================================ --- timer.h 35eaca1c3fe810fd864583e55e91ab518ea25bd5 +++ timer.h 77f91cb86b26421f2d28236d25d384e9a025e939 @@ -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]) ============================================================ --- uart.c 7a38c486dc1280695e1e62c6d3a76d6c9f849f67 +++ uart.c 2ed02bb0e471483e0e77958a43e2e05516d3c127 @@ -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_rxbuf[UART_RXBUFSIZE]; 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 putch(char c) { } 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,8 +89,39 @@ 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) { bool active = FALSE; @@ -93,11 +131,13 @@ void __attribute__((interrupt("IRQ"))) u * 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); switch (source & 0x0e) { @@ -136,6 +176,8 @@ void __attribute__((interrupt("IRQ"))) u break; } + CHECKPOINT((checkpoint & 0x00ff) | 0x0200); + interrupt_clear(); } ============================================================ --- uart.h 86972bb7765797e0b5e660710beed2a9bbd9ec13 +++ uart.h 0e400c96fd36e3b821c79f86ab43102f9be607c7 @@ -6,6 +6,7 @@ void putch(char c); #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); ============================================================ --- watchdog.c 97962bde1fc11e8fea8fae642ed8e0bcdd78468a +++ watchdog.c d26ee7fc5e47c24a05fdd00cb771f966b4a87102 @@ -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) { ============================================================ --- wmp.c e6627850f4ae970c9aee0dbb4c6f1933f0cb31c7 +++ /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"); -}