diff --git a/Makefile b/Makefile
index 799abbc..90f1c94 100644
--- a/Makefile
+++ b/Makefile
@@ -4,14 +4,23 @@ 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+=fisqrt.c
+CSRCS+=fisqrt.c stick.c trig.c motor.c
+
+#PROJOPTS=-DUSE_UART -DSEND_DCM
 
 COPTIM?=-O1
-CFLAGS=-march=armv4t -msoft-float $(COPTIM) -Wall -Werror -Wextra
+CFLAGS=-march=armv4t -msoft-float $(COPTIM) -Wall -Werror -Wextra $(PROJOPTS)
 
 LDSCRIPT=lpc2103_flash.ld
+
+# To build with the Clang Static Analyzer, use
+#     scan-build --use-cc=arm-elf-gcc make
+# And uncomment the following line:
 CC=arm-elf-gcc
+
 OBJCOPY=arm-elf-objcopy
+LINT=splint
+LINTFLAGS=-booltype bool -nolib +charint
 
 CLEANOBJS=$(OBJS) $(NAME).hex $(NAME).elf $(NAME).bin $(NAME).map .depend
 
@@ -45,4 +54,7 @@ clean:
 depend:
 	$(CC) -MM $(CFLAGS) -nostdlib -nostartfiles $(CSRCS) >.depend
 
+lint:
+	$(LINT) $(LINTFLAGS) $(CSRCS)
+
 .sinclude ".depend"
diff --git a/dcm.c b/dcm.c
index dea6e7c..d830612 100644
--- a/dcm.c
+++ b/dcm.c
@@ -8,6 +8,7 @@
 #include "matrix.h"
 #include "dcm.h"
 #include "uart.h"
+#include "motor.h"
 
 #define GRAVITY 9.80665f
 
@@ -27,13 +28,15 @@ float dcm[3*3] = {1, 0, 0,
 float omega_p[3] = {0.0, 0.0, 0.0};
 float omega_i[3] = {0.0, 0.0, 0.0};
 
+float omega_x, omega_y, omega_z;
+
 float delta_t = 0.01;
 
 void dcm_update(float x, float y, float z)
 {
-	float omega_x = x + omega_i[0] + omega_p[0];
-	float omega_y = y + omega_i[1] + omega_p[1];
-	float omega_z = z + omega_i[2] + omega_p[2];
+	omega_x = x + omega_i[0] + omega_p[0];
+	omega_y = y + omega_i[1] + omega_p[1];
+	omega_z = z + omega_i[2] + omega_p[2];
 
 	float tx = delta_t * omega_x;
 	float ty = delta_t * omega_y;
@@ -182,9 +185,12 @@ void dcm_drift_correction(float x, float y, float z)
 		omega_i[i] += error[i] * (KI_ROLLPITCH * weight);
 	}
 
+#if 0
 	putstr("w: ");
 	putint_s((int)(weight * 100000.0f));
 	putstr("\r\n");
+#endif
+
 #if 0
 	putstr("p: ");
 	putint_s((int)(omega_p[0] * 100000.0f));
@@ -202,6 +208,22 @@ void dcm_drift_correction(float x, float y, float z)
 #endif
 }
 
+void dcm_attitude_error(float roll, float pitch, float yaw)
+{
+	/* dcm[6] = sine of pitch */
+	/* dcm[7] = sine of roll */
+
+	/* pitch error = pitch - dcm[6] */
+	/* roll error = roll - dcm[7] */
+
+	/* That was the theory. In practice, there appears to be some
+	   confusion over axes. Pitch and roll seem.. reversed. */
+
+	/* TODO: What if we are upside down? */
+
+	motor_pid_update(roll, dcm[6], pitch, -dcm[7], yaw, -omega_z);
+}
+
 void dcm_dump(void)
 {
 	putstr("dcm: ");
diff --git a/dcm.h b/dcm.h
index 4aacf6c..c2e068e 100644
--- a/dcm.h
+++ b/dcm.h
@@ -2,6 +2,8 @@
 
 #include "types.h"
 
+extern float delta_t;
+
 void dcm_update(float omega_x, float omega_y, float omega_z);
 void dcm_normalise(void);
 bool dcm_renormalise(float *v);
@@ -9,4 +11,4 @@ void dcm_dump(void);
 void dcm_send_packet(void);
 void dcm_setvector(float x, float y, float z);
 void dcm_drift_correction(float x, float y, float z);
-
+void dcm_attitude_error(float x, float y, float z);
diff --git a/interrupt.h b/interrupt.h
index 7dbf8f4..1e1d406 100644
--- a/interrupt.h
+++ b/interrupt.h
@@ -35,6 +35,7 @@
 #define I_PRIORITY_I2C0		0
 #define I_PRIORITY_UART0	1
 #define I_PRIORITY_TIMER0	2
+#define I_PRIORITY_TIMER1	3
 
 #define interrupt_clear() do { VICVectAddr = 0xff; } while (0)
 
diff --git a/main.c b/main.c
index 0aab5ba..5f0d124 100644
--- a/main.c
+++ b/main.c
@@ -1,3 +1,4 @@
+/* main.c */
 
 #include "wmp.h"
 #include "i2c.h"
@@ -5,8 +6,10 @@
 #include "uart.h"
 #include "interrupt.h"
 #include "event.h"
+#include "stick.h"
 
-#define PINSEL0 (*((volatile unsigned char *) 0xE002C000))
+#define PINSEL0 (*((volatile unsigned int *) 0xE002C000))
+#define PINSEL1 (*((volatile unsigned int *) 0xE002C004))
 #define FP0XDIR (*((volatile unsigned int *) 0x3FFFC000))
 #define FP0XVAL (*((volatile unsigned int *) 0x3FFFC014))
 
@@ -15,18 +18,26 @@
 
 void init_pins(void)
 {
-	PINSEL0 = 0x00000055; /* P0.0 and P0.1 assigned to UART */
+	PINSEL0 = 0x00a88055; /* P0.0 and P0.1 assigned to UART */
 			      /* P0.2 and P0.3 assigned to I2C  */
+	                      /* P0.7 and P0.9 assigned to MAT2.[02] */
+	                      /* P0.10 and P0.11 assigned to CAP1.[01] */
+	PINSEL1 = 0x20000828; /* P0.21 and P0.30 assigned to MAT3.[03] */
+	                      /* P0.17 and P0.18 assigned to CAP1.[23] */
 	SCS = 1;
 	FP0XDIR = 0x04000000; /* P0.26 is an output */
 	FP0XVAL = 0x0;
 }
 
+#ifdef USE_UART
 void reply(char *str)
 {
 	putstr(str);
 	putstr("\r\n");
 }
+#else
+#define reply(x) ((void)0)
+#endif
 
 unsigned int count = 0;
 
@@ -167,6 +178,8 @@ void timer_event_handler(void)
 void menu_handler(void);
 
 int main(void) {
+	armed = FALSE;
+
 	init_interrupt();
 	init_uart();
 	init_i2c();
@@ -183,6 +196,21 @@ int main(void) {
 
 	putstr("prompt> ");
 
+	FP0XVAL &= ~0x04000000;
+	timer_delay_ms(1000);
+	FP0XVAL |= 0x04000000;
+	timer_delay_ms(1000);
+	FP0XVAL &= ~0x04000000;
+	if (!wmp_init())
+		putstr("WMP initialisation failed\r\n");
+
+	/* Flight is potentially live after this. */
+	timer_set_period(5*TIMER_MS);
+	wmp_start_zero();
+
+	FP0XVAL |= 0x04000000;
+
+	/* Good luck! */
 	while (1) {
 		FP0XVAL ^= 0x04000000;
 		event_dispatch();
@@ -197,6 +225,9 @@ void menu_handler(void)
 	char c;
 
 	while (getch(&c)) {
+#if 1
+		continue; /* Yes, let's just ignore UART input now. */
+#endif
 		if (c == 0x0a)
 			continue;
 		putch(c);
@@ -283,6 +314,58 @@ void menu_handler(void)
 			reply("done");
 			wmp_start_zero();
 			break;
+		case 'W':
+			for (i = 0; i < 4; i++) {
+				putstr("Width ");
+				putint(i);
+				putstr(": ");
+				putint(timer_input(i));
+				if (!timer_valid(i))
+					putstr(" (invalid)");
+				putstr("\r\n");
+			}
+			if (!timer_allvalid()) {
+				putstr("ALL INVALID!\r\n");
+			}
+			break;
+		case '0' & 0xdf:
+			timer_set_pwm_value(0, 0);
+			timer_set_pwm_value(1, 0);
+			timer_set_pwm_value(2, 0);
+			timer_set_pwm_value(3, 0);
+			break;
+#if 0
+		case '1' & 0xdf:
+			timer_set_pwm_value(0, PWM_MAX/2);
+			break;
+		case '2' & 0xdf:
+			timer_set_pwm_value(1, PWM_MAX/2);
+			break;
+		case '3' & 0xdf:
+			timer_set_pwm_value(2, PWM_MAX/2);
+			break;
+		case '4' & 0xdf:
+			timer_set_pwm_value(3, PWM_MAX/2);
+			break;
+		case '5' & 0xdf:
+			timer_set_pwm_value(0, PWM_MAX);
+			break;
+		case '6' & 0xdf:
+			timer_set_pwm_value(1, PWM_MAX);
+			break;
+		case '7' & 0xdf:
+			timer_set_pwm_value(2, PWM_MAX);
+			break;
+		case '8' & 0xdf:
+			timer_set_pwm_value(3, PWM_MAX);
+			break;
+#endif
+		case '9' & 0xdf:
+			timer_set_pwm_invalid(0);
+			timer_set_pwm_invalid(1);
+			timer_set_pwm_invalid(2);
+			timer_set_pwm_invalid(3);
+			break;
 		default:
 			reply("Unrecognised command.");
 			break;
diff --git a/motor.c b/motor.c
new file mode 100644
index 0000000..61e1601
--- /dev/null
+++ b/motor.c
@@ -0,0 +1,153 @@
+/* motor.c */
+
+#include "stick.h"
+#include "timer.h"
+#include "dcm.h"
+#include "uart.h"
+
+float integral[3] = {0.0f, 0.0f, 0.0f};
+float last[3];
+
+float throttle = 0.0f;
+
+#define Kp 0.2
+#define Ki 0.04
+#define Kd 0.08
+#define Ka 0.0
+
+#define Kp_y 0.2
+#define Ki_y 0.00
+#define Kd_y 0.00
+#define Ka_y 0.0
+
+
+/*
+ * Perform a PID loop iteration.
+ * roll and pitch are absolute values
+ * yaw is, currently, a rate.
+ */
+void motor_pid_update(float troll, float mroll,
+		      float tpitch, float mpitch,
+		      float tyaw, float myaw)
+{
+	float derivative[3];
+	float out[3];
+	float motor[3];
+	float roll, pitch, yaw;
+	float error, max_error;
+	float min_motor;
+	int i;
+
+	roll  = troll  - mroll;
+	pitch = tpitch - mpitch;
+	yaw   = tyaw   - myaw;
+
+#if 0
+        if ((stick_counter % 100) == 0) {
+		putstr("{");
+		putint_s((int)(tyaw * 10000));
+		putstr(", ");
+		putint_s((int)(myaw * 10000));
+		putstr("}\r\n");
+	}
+#endif
+
+	integral[0] += roll * delta_t;
+	integral[1] += pitch * delta_t;
+	integral[2] += yaw * delta_t;
+
+	/* The measurements are the opposite sign to the error */
+	derivative[0] = (-mroll  - last[0]) / delta_t;
+	derivative[1] = (-mpitch - last[1]) / delta_t;
+	derivative[2] = (-myaw   - last[2]) / delta_t;
+
+	last[0] = -mroll;
+	last[1] = -mpitch;
+	last[2] = -myaw;
+
+	out[0] = roll  * Kp   + integral[0] * Ki   + derivative[0] * Kd;
+	out[1] = pitch * Kp   + integral[1] * Ki   + derivative[1] * Kd;
+	out[2] = yaw   * Kp_y + integral[2] * Ki_y + derivative[2] * Kd_y;
+
+	if (armed) {
+		/* Front right */
+		motor[0] = throttle + out[0] + out[1] + out[2];
+		/* Front left */
+		motor[1] = throttle - out[0] + out[1] - out[2];
+		/* Rear left */
+		motor[2] = throttle - out[0] - out[1] + out[2];
+		/* Rear right */
+		motor[3] = throttle + out[0] - out[1] - out[2];
+	} else {
+		motor[0] = 0.0;
+		motor[1] = 0.0;
+		motor[2] = 0.0;
+		motor[3] = 0.0;
+	}
+
+	max_error = 0.0;
+	min_motor = 1.0;
+	for (i = 0; i < 3; i++) {
+		if (motor[i] < 0.0)
+			motor[i] = 0.0;
+		if (motor[i] > 1.0f) {
+			error = motor[i] - 1.0f;
+			if (error > max_error)
+				max_error = error;
+		}
+		if (motor[i] < min_motor)
+			min_motor = motor[i];
+	}
+
+	if (max_error > 0.0) {
+		for (i = 0; i < 3; i++) {
+			motor[i] -= max_error;
+			if (motor[i] < 0.0)
+				motor[i] = 0.0;
+		}
+	}
+
+	if (throttle <= 0.0) {
+		motor[0] = 0.0;
+		motor[1] = 0.0;
+		motor[2] = 0.0;
+		motor[3] = 0.0;
+		integral[0] = 0.0;
+		integral[1] = 0.0;
+		integral[2] = 0.0;
+	}
+	if (max_error < min_motor) {
+		float new_throttle2, new_out[3];
+		new_throttle2 = (motor[0] + motor[1] + motor[2] + motor[3])/2.0;
+		new_out[0] = (motor[0] + motor[3] - new_throttle2)/2.0;
+		new_out[1] = (motor[0] + motor[1] - new_throttle2)/2.0;
+		new_out[2] = (motor[0] + motor[2] - new_throttle2)/2.0;
+
+		/* Anti-windup */
+		for (i = 0; i < 3; i++) {
+			if (new_out[i] > 1.0)
+				integral[i] -= (new_out[i]-1.0) * Ka;
+			if (new_out[i] < 0.0)
+				integral[i] -= (new_out[i]) * Ka;
+		}
+	}
+
+	timer_set_pwm_value(0, (int)(motor[0] * PWM_MAX));
+	timer_set_pwm_value(1, (int)(motor[1] * PWM_MAX));
+	timer_set_pwm_value(2, (int)(motor[2] * PWM_MAX));
+	timer_set_pwm_value(3, (int)(motor[3] * PWM_MAX));
+}
+
+void motor_kill(void) {
+	throttle = 0.0;
+	timer_set_pwm_value(0, 0);
+	timer_set_pwm_value(1, 0);
+	timer_set_pwm_value(2, 0);
+	timer_set_pwm_value(3, 0);
+}
+
+void motor_set_throttle(float t) {
+	if (armed)
+		throttle = t;
+}
+
diff --git a/motor.h b/motor.h
new file mode 100644
index 0000000..c0bdb90
--- /dev/null
+++ b/motor.h
@@ -0,0 +1,9 @@
+/* motor.h */
+
+extern float temp_kp;
+
+void motor_pid_update(float troll, float mroll,
+		      float tpitch, float mpitch,
+		      float tyaw, float myaw);
+void motor_kill(void);
+void motor_set_throttle(float t);
diff --git a/stick.c b/stick.c
new file mode 100644
index 0000000..2e35bed
--- /dev/null
+++ b/stick.c
@@ -0,0 +1,147 @@
+/* stick.c */
+
+#ifdef WE_HAVE_SQRT
+#include <math.h>
+#else
+#include "fisqrt.h"
+#endif
+#include "matrix.h"
+#include "stick.h"
+#include "dcm.h"
+#include "uart.h"
+#include "timer.h"
+#include "trig.h"
+#include "motor.h"
+#include "wmp.h"
+
+#define TWO_PI 6.28318531f
+#define PI 3.14159265f
+
+#define MIN_X 15830
+#define MAX_X 28300
+#define CENTRE_X 22100
+
+#define MIN_Y 18530
+#define MAX_Y 28200
+#define CENTRE_Y 22100
+
+#define MIN_Z 15800
+#define MAX_Z 28304
+#define CENTRE_Z 22100
+
+#define MIN_THR 16500
+#define MAX_THR 28275
+
+#define MIN_REAL_THR 15830
+
+#define CENTRE_ZONE 100
+
+/* Full scale is a roll/pitch angle of 30 degrees from the vertical */
+#define SCALE_X (TWO_PI*30.0/360.0 / (MAX_X-CENTRE_X))
+#define SCALE_Y (TWO_PI*30.0/360.0 / (MAX_Y-CENTRE_Y))
+
+/* Full scale is a complete rotation in one second */
+#define SCALE_Z (TWO_PI / (MAX_Z-CENTRE_Z))
+
+/* 0 is min throttle, 1 is max throttle */
+#define SCALE_THROTTLE (1.0f/(MAX_THR - MIN_THR))
+
+float stick_yaw = 0.0f;
+
+unsigned int stick_counter;
+
+bool armed = FALSE;
+
+void stick_update(float x, float y, float z)
+{
+	float tz = delta_t * z;
+
+	stick_yaw += tz;
+
+	if (stick_yaw <= -PI)
+	    stick_yaw += TWO_PI;
+
+	if (stick_yaw > PI)
+	    stick_yaw -= TWO_PI;
+
+#if 0
+	z = stick_yaw;
+#endif
+
+	x = sine(x);
+	y = sine(y);
+#if 0
+	z = 1.0/fisqrt(1-x*x-y*y);
+#endif
+
+	dcm_attitude_error(x, y, z);
+}
+
+void stick_input(void) {
+	float x, y, z, throttle;
+	if (timer_allvalid()) {
+		x = timer_input(0);
+		y = timer_input(1);
+		throttle = timer_input(2);
+		z = timer_input(3);
+
+		if (!armed) {
+			if ((throttle < MIN_THR) &&
+			    (x > (CENTRE_X - CENTRE_ZONE)) &&
+			    (x < (CENTRE_X + CENTRE_ZONE)) &&
+			    (y > (CENTRE_Y - CENTRE_ZONE)) &&
+			    (y < (CENTRE_Y + CENTRE_ZONE)) &&
+			    (z > (CENTRE_Z - CENTRE_ZONE)) &&
+			    (z < (CENTRE_Z + CENTRE_ZONE)) &&
+			    (wmp_zero == FALSE)) {
+				putstr("ARMED!!!\r\n");
+				armed = TRUE;
+			}
+
+		}
+
+		x -= CENTRE_X;
+		y -= CENTRE_Y;
+		z -= CENTRE_Z;
+		x = x * SCALE_X;
+		y = y * SCALE_Y;
+		z = z * SCALE_Z;
+		throttle = (throttle - MIN_THR) * SCALE_THROTTLE;
+		if (throttle < 0.0)
+			throttle = 0.0;
+	} else {
+		x = 0.0f;
+		y = 0.0f;
+		z = 0.0f;
+		throttle = 0.0f;
+	}
+
+	motor_set_throttle(throttle);
+
+	/* So the controls are back to front. Let's fix that. */
+	x = -x;
+	y = -y;
+	z = -z;
+
+#if 0
+	if ((stick_counter % 100) == 0) {
+		putstr("[");
+		putint_s((int)(z * 10000));
+		putstr("] ");
+	}
+#endif
+
+#if 1
+	stick_update(x, y, z);
+#else
+	if ((stick_counter % 100) == 0)
+		stick_update(x, y, z);
+#endif
+
+/*
+	if ((stick_counter & 3) == 0)
+		stick_send_packet();
+*/
+	stick_counter++;
+}
+
diff --git a/stick.h b/stick.h
new file mode 100644
index 0000000..10c9051
--- /dev/null
+++ b/stick.h
@@ -0,0 +1,10 @@
+/* stick.h */
+
+#include "types.h"
+
+extern bool armed;
+
+extern unsigned int stick_counter;
+
+void stick_input(void);
+void stick_update(float x, float y, float omega_z);
diff --git a/timer.c b/timer.c
index 1e96e76..e118e00 100644
--- a/timer.c
+++ b/timer.c
@@ -3,8 +3,12 @@
 #include "uart.h"
 #include "event.h"
 
+#define FP0XVAL (*((volatile unsigned int *) 0x3FFFC014))
+
 #define TIMER0BASE  0xE0004000
 #define TIMER1BASE  0xE0008000
+#define TIMER2BASE  0xE0070000
+#define TIMER3BASE  0xE0074000
 
 #define IR    0x00
 #define TCR   0x04
@@ -28,6 +32,15 @@
 #define TREG(x) (((volatile unsigned char *)TIMER0BASE)[x])
 #define TWREG(x) (((volatile unsigned int *)TIMER0BASE)[(x)/sizeof(unsigned int)])
 
+#define T1REG(x) (((volatile unsigned char *)TIMER1BASE)[x])
+#define T1WREG(x) (((volatile unsigned int *)TIMER1BASE)[(x)/sizeof(unsigned int)])
+
+#define T2REG(x) (((volatile unsigned char *)TIMER2BASE)[x])
+#define T2WREG(x) (((volatile unsigned int *)TIMER2BASE)[(x)/sizeof(unsigned int)])
+
+#define T3REG(x) (((volatile unsigned char *)TIMER3BASE)[x])
+#define T3WREG(x) (((volatile unsigned int *)TIMER3BASE)[(x)/sizeof(unsigned int)])
+
 #define TCR_ENABLE (1<<0)
 #define TCR_RESET  (1<<1)
 
@@ -44,7 +57,14 @@
 #define MR3R (1<<10)
 #define MR3S (1<<11)
 
+
+volatile unsigned int timer1_rising[4];
+volatile unsigned int timer1_width[4];
+
+unsigned int timer_map[] = {0, 3, 2, 1};
+
 void __attribute__((interrupt("IRQ"))) timer_interrupt_handler(void);
+void __attribute__((interrupt("IRQ"))) timer1_interrupt_handler(void);
 
 void timer_event_handler(void);
 
@@ -58,6 +78,52 @@ void init_timer(void)
 	TWREG(PC) = 0;
 
 	TREG(TCR) = TCR_ENABLE;
+
+	interrupt_register(TIMER1, timer1_interrupt_handler);
+
+	T1REG(TCR) = TCR_ENABLE | TCR_RESET;
+
+	T1REG(CTCR) = 0; /* Use PCLK */
+	T1WREG(TC) = 0;
+	T1WREG(PR) = TIMER_PRESCALE ;
+	T1WREG(PC) = 0;
+
+	T1WREG(CCR) = 0x00000fff;
+
+	T1REG(TCR) = TCR_ENABLE;
+
+	T2REG(TCR) = TCR_ENABLE | TCR_RESET;
+	T2REG(CTCR) = 0; /* Use PCLK */
+	T2WREG(PR) = 0; // Prescaling
+	T2WREG(PC) = 0; // Reset the prescale counter
+	T2WREG(TC) = 0; // Reset the counter
+
+	T2WREG(MCR) = 0x0400; // Reset on MR3 match
+	T2WREG(PWM) = 0x0000000d; // Enable PWMs
+
+	T2WREG(MR3) = PWM_PERIOD; // Period duration
+
+	/* This is chosen to be an invalid output. */
+	T2WREG(MR1) = 1; // Pulse width
+	T2WREG(MR0) = 1; // Pulse width
+
+	T3REG(TCR) = TCR_ENABLE | TCR_RESET;
+	T3REG(CTCR) = 0; /* Use PCLK */
+	T3WREG(PR) = 0; // Prescaling
+	T3WREG(PC) = 0; // Reset the prescale counter
+	T3WREG(TC) = 0; // Reset the counter
+
+	T3WREG(MCR) = 0x0010; // Reset on MR1 match
+	T3WREG(PWM) = 0x0000000b; // Enable PWMs
+
+	T3WREG(MR1) = PWM_PERIOD; // Period duration
+
+	/* This is chosen to be an invalid output. */
+	T3WREG(MR3) = 1; // Pulse width
+	T3WREG(MR0) = 1; // Pulse width
+
+	T2REG(TCR) = TCR_ENABLE;
+	T3REG(TCR) = TCR_ENABLE;
 }
 
 unsigned int timer_read(void)
@@ -92,3 +158,109 @@ void __attribute__((interrupt("IRQ"))) timer_interrupt_handler(void)
 
 	interrupt_clear();
 }
+
+void __attribute__((interrupt("IRQ"))) timer1_interrupt_handler(void)
+{
+	unsigned int ir;
+	unsigned int gpio;
+	ir = T1REG(IR);
+	T1REG(IR) = ir;
+
+	gpio = FP0XVAL;
+
+	if (ir & (1<<4)) {
+		/* Capture channel 0 */
+		if (gpio & (1<<10)) {
+			timer1_rising[0] = T1WREG(CR0);
+		} else {
+			timer1_width[0] = T1WREG(CR0) - timer1_rising[0];
+		}
+	}
+	if (ir & (1<<5)) {
+		/* Capture channel 1 */
+		if (gpio & (1<<11)) {
+			timer1_rising[1] = T1WREG(CR1);
+		} else {
+			timer1_width[1] = T1WREG(CR1) - timer1_rising[1];
+		}
+	}
+	if (ir & (1<<6)) {
+		/* Capture channel 2 */
+		if (gpio & (1<<17)) {
+			timer1_rising[2] = T1WREG(CR2);
+		} else {
+			timer1_width[2] = T1WREG(CR2) - timer1_rising[2];
+		}
+	}
+	if (ir & (1<<7)) {
+		/* Capture channel 3 */
+		if (gpio & (1<<18)) {
+			timer1_rising[3] = T1WREG(CR3);
+		} else {
+			timer1_width[3] = T1WREG(CR3) - timer1_rising[3];
+		}
+	}
+
+	interrupt_clear();
+}
+
+bool timer_valid(int channel) {
+	channel = TIMER_CH(channel);
+	/* Be careful here to ensure that this can't be in the past */
+	unsigned int chtime = timer1_rising[channel];	/* Atomic */
+	unsigned int time = T1WREG(TC);			/* Atomic */
+	return (time - chtime) < TIMER_INPUT_TIMEOUT;
+}
+
+bool timer_allvalid(void) {
+	unsigned int time;
+	unsigned int chtime[4];
+	int i;
+	/* Be careful here to ensure that this can't be in the past */
+	for (i = 0; i < 4; i++)
+		chtime[i] = timer1_rising[i];
+	time = T1WREG(TC);
+	for (i = 0; i < 4; i++)
+		if ((time - chtime[i]) >= TIMER_INPUT_TIMEOUT)
+			return FALSE;
+	return TRUE;
+}
+
+void timer_set_pwm_value(int channel, int value)
+{
+	value = PWM_PERIOD - (PWM_MAX + value);
+	switch (channel) {
+	case 0:
+		T2WREG(MR2) = value;
+		break;
+	case 1:
+		T2WREG(MR0) = value;
+		break;
+	case 2:
+		T3WREG(MR3) = value;
+		break;
+	case 3:
+		T3WREG(MR0) = value;
+		break;
+	}
+}
+
+void timer_set_pwm_invalid(int channel)
+{
+	int value = 1;
+	switch (channel) {
+	case 0:
+		T2WREG(MR2) = value;
+		break;
+	case 1:
+		T2WREG(MR0) = value;
+		break;
+	case 2:
+		T3WREG(MR3) = value;
+		break;
+	case 3:
+		T3WREG(MR0) = value;
+		break;
+	}
+}
+
diff --git a/timer.h b/timer.h
index 4dd8f8f..e54f613 100644
--- a/timer.h
+++ b/timer.h
@@ -1,6 +1,8 @@
 #ifndef __TIMER_H
 #define __TIMER_H
 
+#include "types.h"
+
 #define TIMER_PCLK 14745600
 #define TIMER_PRESCALE 0
 
@@ -8,12 +10,30 @@
 #define TIMER_MS (TIMER_SECOND/1000)
 #define TIMER_US (TIMER_SECOND/1000000)
 
+#define PWM_MAX 14745
+#if 0
+#define PWM_PERIOD 58980
+#endif
+#define PWM_PERIOD ((4*PWM_MAX)+1)
+
+#define TIMER_INPUT_TIMEOUT (TIMER_PCLK/10)
+
+#define TIMER_CH(x) (timer_map[x])
+
+extern volatile unsigned int timer1_width[];
+extern unsigned int timer_map[];
+
 void init_timer(void);
 unsigned int timer_read(void);
 void timer_delay_clocks(unsigned int clocks);
 void timer_set_period(unsigned int period);
+void timer_set_pwm_value(int channel, int value);
+void timer_set_pwm_invalid(int channel);
+bool timer_valid(int channel);
+bool timer_allvalid(void);
 
 #define timer_delay_us(x) timer_delay_clocks((x)*TIMER_US)
 #define timer_delay_ms(x) timer_delay_clocks((x)*TIMER_MS)
 
+#define timer_input(ch) (timer1_width[TIMER_CH(ch)])
 #endif /* __TIMER_H */
diff --git a/trig.c b/trig.c
new file mode 100644
index 0000000..510ad64
--- /dev/null
+++ b/trig.c
@@ -0,0 +1,44 @@
+/* trig.c */
+
+/* Cosine implementation from:
+ * http://www.ganssle.com/articles/atrig.htm
+ */
+
+double cosine(double x)
+{
+double p0,p1,p2,p3,p4,p5,y,t,absx,frac,pi2;
+int quad;
+p0= 0.999999999781;
+p1=-0.499999993585;
+p2= 0.041666636258;
+p3=-0.0013888361399;
+p4= 0.00002476016134;
+p5=-0.00000026051495;
+pi2=1.570796326794896; 		/* pi/2 */
+absx=x;
+if (x<0) absx=-absx; 	     /* absolute value of input */
+quad=(int) (absx/pi2);       	/* quadrant (0 to 3) */
+if (quad > 3) {
+	int q = quad & ~3; /* round down to the next multiple of 4 */
+	absx = absx / (pi2 * quad);
+	quad -= q;
+	t = 0.0; /* hello, compiler warnings */
+}
+frac= (absx/pi2) - quad;     	/* fractional part of input */
+if(quad==0) t=frac * pi2;
+if(quad==1) t=(1-frac) * pi2;
+if(quad==2) t=frac * pi2;
+if(quad==3) t=(frac-1) * pi2;
+t=t * t;
+y=p0 + (p1*t) + (p2*t*t) + (p3*t*t*t) + (p4*t*t*t*t) + (p5*t*t*t*t*t);
+if(quad==2 || quad==1) y=-y;  /* correct sign */
+return(y);
+};
+
+double sine(double x)
+{
+	double pi2=1.570796326794896; 		/* pi/2 */
+	x = x - pi2;
+	if (x < -pi2) x += pi2*4;
+	return cosine(x);
+}
diff --git a/trig.h b/trig.h
new file mode 100644
index 0000000..793ff9d
--- /dev/null
+++ b/trig.h
@@ -0,0 +1,5 @@
+/* trig.h */
+
+double cosine(double x);
+double sine(double x);
+
diff --git a/types.h b/types.h
index 44767e2..5e38bdd 100644
--- a/types.h
+++ b/types.h
@@ -7,6 +7,6 @@ typedef int bool;
 
 #define NULL 0
 
-typedef int size_t;
+typedef unsigned int size_t;
 
 #endif /* __TYPES_H */
diff --git a/uart.c b/uart.c
index 99b5235..774f043 100644
--- a/uart.c
+++ b/uart.c
@@ -38,6 +38,8 @@ volatile bool tx_running;
 
 void __attribute__((interrupt("IRQ"))) uart_interrupt_handler(void);
 
+#ifdef USE_UART
+
 void init_uart(void)
 {
 	UREG(FDR) = 0x10; /* DivAddVal = 0, MulVal = 1 */
@@ -205,3 +207,4 @@ bool getch(char *c) {
 	uart_rxread = (uart_rxread + 1) % UART_RXBUFSIZE;
 	return TRUE;
 }
+#endif
diff --git a/uart.h b/uart.h
index cff52fc..94debdb 100644
--- a/uart.h
+++ b/uart.h
@@ -3,6 +3,7 @@
 
 #include "types.h"
 
+#ifdef USE_UART
 void init_uart(void);
 void putch(char c);
 void putstr(char *s);
@@ -10,5 +11,14 @@ void putint(unsigned int n);
 void putint_s(int n);
 void puthex(unsigned int n);
 bool getch(char *c);
+#else
+#define init_uart() ((void)0)
+#define putch(x) ((void)0)
+#define putstr(x) ((void)0)
+#define putint(x) ((void)0)
+#define putint_s(x) ((void)0)
+#define puthex(x) ((void)0)
+#define getch(x) (FALSE)
+#endif
 
 #endif /* __UART_H */
diff --git a/wmp.c b/wmp.c
index 62681e7..2e9b1dd 100644
--- a/wmp.c
+++ b/wmp.c
@@ -1,9 +1,11 @@
+/* wmp.c */
 
 #include "wmp.h"
 #include "i2c.h"
 #include "uart.h"
 #include "dcm.h"
 #include "fisqrt.h"
+#include "stick.h"
 
 #define WMP_ZERO_COUNT 100
 
@@ -115,6 +117,7 @@ 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)
@@ -201,24 +204,28 @@ void wmp_process_gyro_sample(void)
 
 		wmp_generation++;
 
-#if 1
-		if ((wmp_generation % 2) == 0)
+#if SEND_DCM
+		if ((wmp_generation % 20) == 0)
 			dcm_send_packet();
 #endif
 
 	} else if (wmp_zero) {
-		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");
+		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");
+			}
 		}
 	}
 }
@@ -268,6 +275,7 @@ void wmp_process_accel_sample(void)
 	 * on that so we'll just fudge it here.
 	 */
 	dcm_drift_correction(x, -y, -z);
+	stick_input();
 }
 
 void wmp_event_handler(void)
@@ -287,6 +295,7 @@ void wmp_start_zero(void)
 {
 	wmp_zero = TRUE;
 	wmp_update = FALSE;
+	wmp_discard = 100;
 	wmp_generation = 0;
 	putstr("Starting zero\r\n");
 }
diff --git a/wmp.h b/wmp.h
index e9a3bc8..f71ed19 100644
--- a/wmp.h
+++ b/wmp.h
@@ -13,6 +13,8 @@ extern bool wmp_yaw_fast;
 extern bool wmp_pitch_fast;
 extern bool wmp_roll_fast;
 
+extern bool wmp_zero;
+
 bool wmp_init(void);
 bool wmp_sample(void);
 bool wmp_read_calibration_data(void);