Bye bye Wii Motion Plus, hello MPU6050. Also, increase control loop

to 200Hz, add SD card logging, and a number of other changes.

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

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

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));

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_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

@@ -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

@@ -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

@@ -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 */

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 */
 };
 

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)
 

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;
+}

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())
+	if (!sensors_init())
-		putstr("WMP initialisation failed\r\n");
+		putstr("Sensor initialisation failed\r\n");
 
 	/* Flight is potentially live after this. */
-	timer_set_period(5*TIMER_MS);
+	timer_set_period(TIMER_MS(5));
-	wmp_start_zero();
+#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,83 +241,36 @@ 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());
-			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();
-			break;
-		case 'V':
-			average_sample();
-			break;
 		case 'D':
-			putstr("Reading calibration data... ");
+			sensors_dump();
-			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");
-			} else {
-				reply("FAIL");
-			}
 			break;
 		case 'N':
 			putstr("The time is ");
 			puthex(timer_read());
 			reply(".");
 			break;
-		case 'P':
+		case 'I':
-			putstr("Initialising timer... ");
+			init_sdcard();
-			/* We want a 100Hz loop but two samples per iteration.
+			break;
-			 * So, we go for 200Hz. */
+		case 'R':
-			timer_set_period(5*TIMER_MS);
+			sd_address = 0;
-			reply("done");
+		case 'S':
-			wmp_start_zero();
+			if (sdcard_read(sd_address++, buffer, 512)) {
+				dump_buffer(buffer, 512, (sd_address-1) * 512);
+				reply ("SD card read success");
+			} else {
+				reply("SD card read failed");
+			}
+			break;
+		case 'A':
+			sd_address = read_number();
+			putstr("SD read address set to 0x");
+			puthex(sd_address);
+			reply(".");
 			break;
 		case 'W':
 			for (i = 0; i < 4; i++) {
@@ -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

@@ -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) {

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<<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

@@ -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++) {
+	for (i = 0; i < PANIC_BITS; i++) {
-		if (reason & (1<<(15-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_pattern *pattern, unsigned int reason)
 		}
 		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)
 {
-	/*
+#if PANIC_CHECKPOINT
-	 * We may one day be able to do something with the reason, like emit
+	reason = checkpoint;
-	 * a final deathbed confession. So we'll provide the reasons in the
+#endif
-	 * caller and just ignore them for now.
-	 */
-	(void)reason;
 
 	motor_kill();
 

panic.h

@@ -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

sdcard.c

@@ -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;
+}
+

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");
+}

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");
+}

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);
+		xi = timer_input(0);
-		y = timer_input(1);
+		yi = timer_input(1);
-		throttle = timer_input(2);
+		throttlei = timer_input(2);
-		z = timer_input(3);
+		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)

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;
 }

timer.h

@@ -3,13 +3,14 @@
 
 #include "types.h"
 
-#define TIMER_PCLK 14745600
+#define TIMER_PCLK 58982400
-#define TIMER_PRESCALE 9215
+#define TIMER_PRESCALE 36863
-#define TIMER0_PRESCALE 0
+#define TIMER0_PRESCALE 3
 
 #define TIMER_SECOND (TIMER_PCLK/(TIMER_PRESCALE+1))
-#define TIMER_MS (TIMER_SECOND/1000)
+/* Since we're using awkward numbers, this gives better accuracy */
-#define TIMER_US (TIMER_SECOND/1000000)
+#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

@@ -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 unsigned int uart_txread;
-volatile int uart_txwrite;
+volatile unsigned int uart_txwrite;
-volatile int uart_rxread;
+volatile unsigned int uart_rxread;
-volatile int uart_rxwrite;
+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,9 +63,42 @@ 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;
+		}
+	}
+
+	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;
+		}
+	}
+	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;
@@ -82,6 +117,9 @@ void putch(char c) {
 			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;
+	unsigned int local_txwrite;
-	int local_txread;
+	unsigned int local_txread;
-	int local_rxwrite;
+	unsigned int local_rxwrite;
-	int local_rxread;
+	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();
 }
 

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);

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) {

wmp.c

@@ -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");
-}