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())
-		putstr("WMP initialisation failed\r\n");
+	if (!sensors_init())
+		putstr("Sensor initialisation failed\r\n");
 
 	/* Flight is potentially live after this. */
-	timer_set_period(5*TIMER_MS);
-	wmp_start_zero();
+	timer_set_period(TIMER_MS(5));
+#if 1
+	sensors_start_zero();
+#endif
 
 	led_init();
 
@@ -267,8 +176,12 @@ int main(void) {
 
 	/* Good luck! */
 	while (1) {
+		CHECKPOINT(0);
 		led_update();
-		event_dispatch();
+		CHECKPOINT(1);
+		if (!event_dispatch())
+			sdcard_poll();
+		CHECKPOINT(2);
 		watchdog_check();
 	}
 
@@ -276,6 +189,40 @@ int main(void) {
 }
 
 static int power = 0;
+static unsigned int sd_address = 0;
+
+unsigned int read_number(void)
+{
+	unsigned int number;
+	unsigned int base;
+	int digit;
+	char c;
+
+	number = 0;
+	base = 10;
+
+	while (1) {
+		if (getch(&c)) {
+			digit = -1;
+			if ((c == 0x0a) || (c == 0x0d))
+				break;
+			putch(c);
+			if (c == 'x')
+				base = 16;
+			if ((c >= '0') && (c <= '9'))
+				digit = c - '0';
+			if ((c >= 'A') && (c <= 'F'))
+				digit = c - 'A' + 10;
+
+			if ((digit >= 0) && (digit < (int)base)) {
+				number = number * base;
+				number += digit;
+			}
+		}
+	}
+	putstr("\r\n");
+	return number;
+}
 
 void menu_handler(void)
 {
@@ -283,7 +230,7 @@ void menu_handler(void)
 	char c;
 
 	while (getch(&c)) {
-#if 1
+#if 0
 		continue; /* Yes, let's just ignore UART input now. */
 #endif
 		if (c == 0x0a)
@@ -294,84 +241,37 @@ void menu_handler(void)
 		case 0x0a:
 		case 0x0d:
 			break;
-		case 'A':
-			reply("apple");
-			break;
-		case 'C':
-			count++;
-			putstr("The current count is ");
-			putint(count);
-			reply(".");
-			break;
 		case 'H':
 		case '?':
 			reply("Help is not available. Try a psychiatrist.");
 			break;
-		case 'T':
-			putstr(" I2C status is: ");
-			puthex(i2c_statreg());
-			reply(".");
-			putstr("I2C register is: ");
-			puthex(i2c_conreg());
+		case 'D':
+			sensors_dump();
+			break;
+		case 'N':
+			putstr("The time is ");
+			puthex(timer_read());
 			reply(".");
 			break;
 		case 'I':
-			putstr("Initialising WMP... ");
-			if (wmp_init())
-				reply("done");
-			else
-				reply("FAIL");
-			break;
-		case 'M':
-			putstr("Reading from WMP... ");
-			if (wmp_sample()) {
-				putstr("(");
-				puthex(wmp_roll);
-				putstr(", ");
-				puthex(wmp_pitch);
-				putstr(", ");
-				puthex(wmp_yaw);
-				reply(").");
-			} else
-				reply("FAIL");
-			break;
-		case 'L':
-			minmax_sample();
+			init_sdcard();
 			break;
-		case 'V':
-			average_sample();
-			break;
-		case 'D':
-			putstr("Reading calibration data... ");
-			if (wmp_read_calibration_data()) {
-				putstr("\r\n");
-				for (i = 0; i < 0x10 ; i++) {
-					puthex(wmp_calibration_data[i]);
-					putstr(" ");
-				}
-				putstr("\r\n");
-				for (i = 0x10; i < 0x20 ; i++) {
-					puthex(wmp_calibration_data[i]);
-					putstr(" ");
-				}
-				putstr("\r\n");
+		case 'R':
+			sd_address = 0;
+		case 'S':
+			if (sdcard_read(sd_address++, buffer, 512)) {
+				dump_buffer(buffer, 512, (sd_address-1) * 512);
+				reply ("SD card read success");
 			} else {
-				reply("FAIL");
+				reply("SD card read failed");
 			}
 			break;
-		case 'N':
-			putstr("The time is ");
-			puthex(timer_read());
+		case 'A':
+			sd_address = read_number();
+			putstr("SD read address set to 0x");
+			puthex(sd_address);
 			reply(".");
 			break;
-		case 'P':
-			putstr("Initialising timer... ");
-			/* We want a 100Hz loop but two samples per iteration.
-			 * So, we go for 200Hz. */
-			timer_set_period(5*TIMER_MS);
-			reply("done");
-			wmp_start_zero();
-			break;
 		case 'W':
 			for (i = 0; i < 4; i++) {
 				putstr("Width ");
@@ -386,6 +286,14 @@ void menu_handler(void)
 				putstr("ALL INVALID!\r\n");
 			}
 			break;
+#if 0
+		case 'T':
+			sdcard_start_write();
+			break;
+#endif
+		case 'L':
+			spi_drain();
+			break;
 		case '0' & 0xdf:
 			set_thrust(0, 0.0);
 			set_thrust(1, 0.0);

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++) {
-		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_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)
 {
-	/*
-	 * 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

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