Merge branch 'branch-1.3' into debian

Conflicts: ChangeLog altoslib/AltosRecordMM.java altosui/Makefile.am altosui/altos-windows.nsi.in configure.ac debian/changelog debian/control doc/Makefile doc/altusmetrum.xsl doc/release-notes-1.2.1.xsl doc/release-notes-1.2.xsl
author: Bdale Garbee <bdale@gag.com> 2013-12-19 01:38:40 -0700
committer: Bdale Garbee <bdale@gag.com> 2013-12-19 01:38:40 -0700
commit: 575bbaf976c5840fd0e308549c45a466fdec1352 (patch)
tree: 11bfb498348bf7687bffc24699c4b1a998988ee4 /src
parent: b825116df173b77e2cab217a7b76112c742f9279 (diff)
parent: bc3610d8cecbfed40c62d4dcb93fc9a4d2a7c9e3 (diff)
253 files changed, 14463 insertions, 1653 deletions
diff --git a/src/.gitignore b/src/.gitignore
index cae36ae6..a8628fae 100644
--- a/src/.gitignore
+++ b/src/.gitignore
@@ -1,3 +1,4 @@
+Makedefs
 altitude.h
 altitude-pa.h
 ao_whiten.h
diff --git a/src/Makedefs.in b/src/Makedefs.in
new file mode 100644
index 00000000..a52f96fa
--- /dev/null
+++ b/src/Makedefs.in
@@ -0,0 +1,15 @@
+ARM_CC=@ARM_CC@
+HAVE_ARM_M3_CC=@HAVE_ARM_M3_CC@
+HAVE_ARM_M0_CC=@HAVE_ARM_M0_CC@
+PDCLIB_INCLUDES=@PDCLIB_INCLUDES@
+PDCLIB_LIBS_M0=@PDCLIB_LIBS_M0@
+PDCLIB_LIBS_M3=@PDCLIB_LIBS_M3@
+PDCLIB_ROOT=@PDCLIB_ROOT@
+HAVE_PDCLIB=@HAVE_PDCLIB@
+
+SDCC=@SDCC@
+HAVE_SDCC=@HAVE_SDCC@
+
+AVR_CC=@AVR_CC@
+AVR_OBJCOPY=@AVR_OBJCOPY@
+HAVE_AVR_CC=@HAVE_AVR_CC@
diff --git a/src/Makefile b/src/Makefile
index 11f149dd..392262d4 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -13,43 +13,59 @@ vpath load_csv.5c kalman
 vpath matrix.5c kalman
 
 include Version
+TOPDIR=.
+include Makedefs
 
 SDCCDIRS=\
 	telemetrum-v1.2 telemetrum-v1.1 telemetrum-v1.0 \
-	teledongle-v0.2 teledongle-v0.1 \
-	telemini-v1.0 telenano-v0.1 \
+	teledongle-v0.2 \
+	telemini-v1.0 \
 	telebt-v1.0 \
-	telemetrum-v0.1-sky telemetrum-v0.1-sirf \
-	telelaunch-v0.1 tidongle test \
 	teleterra-v0.2 teleshield-v0.1 \
-	telefire-v0.1 \
-	spiradio-v0.1
+	telefire-v0.1 telefire-v0.2 \
+	telemini-v2.0
 
-AVRDIRS=\
-	telescience-v0.1 telescience-pwm telepyro-v0.1 micropeak
-
-ARMDIRS=\
+ARMM3DIRS=\
 	telemega-v0.1 telemega-v0.1/flash-loader \
-	telemega-v0.3 telemega-v0.3/flash-loader \
+	telemega-v1.0 telemega-v1.0/flash-loader \
+	telemetrum-v2.0 telemetrum-v2.0/flash-loader \
 	megadongle-v0.1 megadongle-v0.1/flash-loader \
-	telegps-v0.1 telegps-v0.1/flash-loader \
-	stm-bringup stm-demo \
+	telegps-v0.3 telegps-v0.3/flash-loader \
 	telelco-v0.2 telelco-v0.2/flash-loader \
-	telescience-v0.2
+	telescience-v0.2 telescience-v0.2/flash-loader
+
+ARMM0DIRS=\
+	easymini-v1.0 easymini-v1.0/flash-loader
+
+AVRDIRS=\
+	telescience-v0.1 telescience-pwm micropeak nanopeak-v0.1
+
+SUBDIRS=
 
-ifneq ($(shell which sdcc),)
-	SUBDIRS += $(SDCCDIRS)
+ifeq ($(strip $(HAVE_PDCLIB)),yes)
+PDCLIB=pdclib
+CLEAN_PDCLIB=clean-pdclib
 endif
 
-ifneq ($(shell which avr-gcc),)
-	SUBDIRS += $(AVRDIRS)
+ifeq ($(strip $(HAVE_SDCC)),yes)
+SUBDIRS+=$(SDCCDIRS)
 endif
 
-ifneq ($(shell which arm-none-eabi-gcc),)
-	SUBDIRS += $(ARMDIRS)
+ifeq ($(strip $(HAVE_ARM_M3_CC)),yes)
+SUBDIRS+=$(ARMM3DIRS)
+foo=bar
 endif
 
-ALLDIRS=$(SDCCDIRS) $(AVRDIRS) $(ARMDIRS)
+ifeq ($(strip $(HAVE_ARM_M0_CC)),yes)
+SUBDIRS+=$(ARMM0DIRS)
+baz=bletch
+endif
+
+ifeq ($(strip $(HAVE_AVR_CC)),yes)
+SUBDIRS += $(AVRDIRS)
+endif
+
+ALLDIRS=$(SDCCDIRS) $(ARMM3DIRS) $(ARMM0DIRS) $(AVRDIRS)
 
 all: all-local all-recursive
 
@@ -81,7 +97,7 @@ uninstall:
 
 all-recursive: all-local
 
-all-local: altitude.h altitude-pa.h ao_kalman.h ao_whiten.h
+all-local: altitude.h altitude-pa.h ao_kalman.h ao_whiten.h $(PDCLIB)
 
 altitude.h: make-altitude
 	nickle $< > $@
@@ -95,5 +111,13 @@ ao_kalman.h: make-kalman kalman.5c kalman_filter.5c load_csv.5c matrix.5c
 ao_whiten.h: make-whiten
 	nickle $< > $@
 
-clean-local:
+clean-local: $(CLEAN_PDCLIB)
 	rm -f altitude.h ao_kalman.h
+
+pdclib:
+	mkdir -p $(PDCLIB_ROOT)/include $(PDCLIB_ROOT)/lib 
+	cd ../pdclib && make && make prefix=`pwd`/../pdclib-root install
+
+clean-pdclib:
+	rm -rf $(PDCLIB_ROOT)
+	cd ../pdclib && make clean
diff --git a/src/aes/ao_aes.c b/src/aes/ao_aes.c
index 71ffadb1..52463f5d 100644
--- a/src/aes/ao_aes.c
+++ b/src/aes/ao_aes.c
@@ -310,6 +310,10 @@ void xrijndaelEncrypt(word32 block[], roundkey *rkk)
   xKeyAddition(block, block2, rp, BC);
 }
 
+#if NOTUSED
+/* We don't actually need this in AltOS, so don't bother including it */
+
+/* Decryption of one block. */
 static
 void xrijndaelDecrypt(word32 block[], roundkey *rkk)
 {
@@ -352,6 +356,7 @@ void xrijndaelDecrypt(word32 block[], roundkey *rkk)
 
   xKeyAddition(block, block, rp, BC);
 }
+#endif
 
 uint8_t ao_aes_mutex;
 static uint8_t key[16];
diff --git a/src/attiny/ao_arch_funcs.h b/src/attiny/ao_arch_funcs.h
index 8c9d1ae6..d4584a9f 100644
--- a/src/attiny/ao_arch_funcs.h
+++ b/src/attiny/ao_arch_funcs.h
@@ -20,21 +20,16 @@
  */
 
 #define ao_spi_get_mask(reg,mask,bus,speed) do {	\
-		(reg) &= ~(mask);		\
+		(reg) &= ~(mask);			\
 	} while (0)
 
 #define ao_spi_put_mask(reg,mask,bus) do {	\
 		(reg) |= (mask);		\
 	} while (0)
 
-#define ao_spi_get_bit(reg,bit,pin,bus,speed) do {	\
-		(pin) = 0;			\
-	} while (0)
-
-#define ao_spi_put_bit(reg,bit,pin,bus) do {	\
-		(pin) = 1;			\
-	} while (0)
+#define ao_spi_get_bit(reg,bit,pin,bus,speed) ao_spi_get_mask(reg,(1<<(bit)),bus,speed)
 
+#define ao_spi_put_bit(reg,bit,pin,bus) ao_spi_put_mask(reg,(1<<(bit)),bus)
 
 #define ao_gpio_token_paster(x,y)		x ## y
 #define ao_gpio_token_evaluator(x,y)	ao_gpio_token_paster(x,y)
@@ -46,6 +41,8 @@
 			PORTB &= ~(1 << bit);	\
 	} while (0)
 
+#define ao_gpio_get(port, bit, pin)	((PORTB >> (bit)) & 1)
+
 /*
  * The SPI mutex must be held to call either of these
  * functions -- this mutex covers the entire SPI operation,
diff --git a/src/micropeak/ao_async.c b/src/attiny/ao_async.c
index 3556f54c..3556f54c 100644
--- a/src/micropeak/ao_async.c
+++ b/src/attiny/ao_async.c
diff --git a/src/micropeak/ao_async.h b/src/attiny/ao_async.h
index 1b239712..1b239712 100644
--- a/src/micropeak/ao_async.h
+++ b/src/attiny/ao_async.h
diff --git a/src/attiny/ao_exti.h b/src/attiny/ao_exti.h
index 2ea4f47d..85bb2fba 100644
--- a/src/attiny/ao_exti.h
+++ b/src/attiny/ao_exti.h
@@ -30,5 +30,6 @@ ao_exti_setup_port(uint8_t pin, uint8_t mode, void (*callback)(void));
 #define ao_exti_init()
 
 #define AO_EXTI_MODE_RISING	1
+#define AO_EXTI_PIN_NOCONFIGURE	0
 
 #endif /* _AO_EXTI_H_ */
diff --git a/src/avr-demo/Makefile b/src/avr-demo/Makefile
index 93295166..6d9bfea2 100644
--- a/src/avr-demo/Makefile
+++ b/src/avr-demo/Makefile
@@ -11,18 +11,12 @@ vpath load_csv.5c ../kalman
 vpath matrix.5c ../kalman
 vpath ao-make-product.5c ../util
 
+include ../avr/Makefile.defs
+
 MCU=atmega32u4
 DUDECPUTYPE=m32u4
 #PROGRAMMER=stk500v2 -P usb
-PROGRAMMER=usbtiny
-LOADCMD=avrdude
 LOADARG=-p $(DUDECPUTYPE) -c $(PROGRAMMER) -e -U flash:w:
-CC=avr-gcc
-OBJCOPY=avr-objcopy
-
-ifndef VERSION
-include ../Version
-endif
 
 INC = \
 	ao.h \
diff --git a/src/avr/Makefile.defs b/src/avr/Makefile.defs
new file mode 100644
index 00000000..eeb9a881
--- /dev/null
+++ b/src/avr/Makefile.defs
@@ -0,0 +1,16 @@
+ifndef TOPDIR
+TOPDIR=..
+endif
+
+ifndef VERSION
+include $(TOPDIR)/Version
+endif
+
+include $(TOPDIR)/Makedefs
+
+CC=$(AVR_CC)
+OBJCOPY=$(AVR_OBJCOPY)
+LDSCRIPTS=/usr/lib/avr/lib/ldscripts
+
+PROGRAMMER=usbtiny
+LOADCMD=avrdude
diff --git a/src/cc1111/Makefile.cc1111 b/src/cc1111/Makefile.cc1111
index 0e19603b..78b653b3 100644
--- a/src/cc1111/Makefile.cc1111
+++ b/src/cc1111/Makefile.cc1111
@@ -1,4 +1,5 @@
-CC=sdcc
+include ../Makedefs
+CC=$(SDCC)
 
 CFLAGS=--model-small --debug --opt-code-speed -DCODESIZE=$(CODESIZE)
 
diff --git a/src/cc1111/ao_adc.c b/src/cc1111/ao_adc.c
index ed76179b..1689ebef 100644
--- a/src/cc1111/ao_adc.c
+++ b/src/cc1111/ao_adc.c
@@ -19,23 +19,26 @@
 
 volatile __xdata struct ao_data	ao_data_ring[AO_DATA_RING];
 volatile __data uint8_t		ao_data_head;
+#if (AO_DATA_ALL & ~(AO_DATA_ADC))
+volatile __data uint8_t		ao_data_present;
+#endif
+
+#ifdef TELENANO_V_0_1
+# define AO_ADC_FIRST_PIN	1
+#endif
+
+#if HAS_ACCEL_REF
+# define AO_ADC_FIRST_PIN	2
+#endif
 
 #ifndef AO_ADC_FIRST_PIN
-#define AO_ADC_FIRST_PIN	0
+# define AO_ADC_FIRST_PIN	0
 #endif
 
 void
 ao_adc_poll(void)
 {
-#if HAS_ACCEL_REF
-	ADCCON3 = ADCCON3_EREF_VDD | ADCCON3_EDIV_512 | 2;
-#else
-# ifdef TELENANO_V_0_1
-	ADCCON3 = ADCCON3_EREF_VDD | ADCCON3_EDIV_512 | 1;
-# else
 	ADCCON3 = ADCCON3_EREF_VDD | ADCCON3_EDIV_512 | AO_ADC_FIRST_PIN;
-# endif
-#endif
 }
 
 void
@@ -82,6 +85,7 @@ ao_adc_isr(void) __interrupt 1
 		else
 #endif
 			ADCCON3 = ADCCON3_EREF_VDD | ADCCON3_EDIV_512 | sequence;
+		return;
 	}
 #endif
 
@@ -141,15 +145,18 @@ ao_adc_isr(void) __interrupt 1
 	if (sequence) {
 		/* Start next conversion */
 		ADCCON3 = sequence;
+		return;
 	}
 #endif /* telemini || telenano */
 
-#ifdef TELEFIRE_V_0_1
+#if defined(TELEFIRE_V_0_1) || defined(TELEFIRE_V_0_2)
 	a = (uint8_t __xdata *) (&ao_data_ring[ao_data_head].adc.sense[0] + sequence - AO_ADC_FIRST_PIN);
 	a[0] = ADCL;
 	a[1] = ADCH;
-	if (sequence < 5)
+	if (sequence < 5) {
 		ADCCON3 = ADCCON3_EREF_VDD | ADCCON3_EDIV_512 | (sequence + 1);
+		return;
+	}
 #define GOT_ADC
 #endif /* TELEFIRE_V_0_1 */
 
@@ -157,21 +164,22 @@ ao_adc_isr(void) __interrupt 1
 	a = (uint8_t __xdata *) (&ao_data_ring[ao_data_head].adc.batt);
 	a[0] = ADCL;
 	a[1] = ADCH;
-	if (0)
-		;
 #define GOT_ADC
 #endif	
 
+#ifdef FETCH_ADC
+	FETCH_ADC();
+#define GOT_ADC
+#endif
+
 #ifndef GOT_ADC
 #error No known ADC configuration set
 #endif
 
-	else {
-		/* record this conversion series */
-		ao_data_ring[ao_data_head].tick = ao_time();
-		ao_data_head = ao_data_ring_next(ao_data_head);
-		ao_wakeup(DATA_TO_XDATA(&ao_data_head));
-	}
+	/* record this conversion series */
+	ao_data_ring[ao_data_head].tick = ao_time();
+	ao_data_head = ao_data_ring_next(ao_data_head);
+	ao_wakeup(DATA_TO_XDATA(&ao_data_head));
 }
 
 static void
diff --git a/src/cc1111/ao_arch.h b/src/cc1111/ao_arch.h
index 9097557f..34235b08 100644
--- a/src/cc1111/ao_arch.h
+++ b/src/cc1111/ao_arch.h
@@ -321,4 +321,9 @@ void
 ao_serial1_tx_isr(void) ao_arch_interrupt(14);
 #endif
 
+#if HAS_EXTI_0
+void
+ao_p0_isr(void) __interrupt(13);
+#endif
+
 #endif /* _AO_ARCH_H_ */
diff --git a/src/cc1111/ao_arch_funcs.h b/src/cc1111/ao_arch_funcs.h
index 8f1cc094..ea340dfd 100644
--- a/src/cc1111/ao_arch_funcs.h
+++ b/src/cc1111/ao_arch_funcs.h
@@ -19,46 +19,74 @@
  * ao_spi.c
  */
 
-extern __xdata uint8_t	ao_spi_mutex;
+#if !HAS_SPI_0 && !HAS_SPI_1
+#define HAS_SPI_0	1
+#define SPI_0_ALT_2	1
+#endif
+
+#if HAS_SPI_0 && HAS_SPI_1
+#define MULTI_SPI	1
+#define N_SPI		2
+#else
+#define MULTI_SPI	0
+#define N_SPI		1
+#endif
+
+extern __xdata uint8_t	ao_spi_mutex[N_SPI];
+
+#if MULTI_SPI
+#define ao_spi_get(bus)	ao_mutex_get(&ao_spi_mutex[bus])
+#define ao_spi_put(bus)	ao_mutex_put(&ao_spi_mutex[bus])
+#else
+#define ao_spi_get(bus)	ao_mutex_get(&ao_spi_mutex[0])
+#define ao_spi_put(bus)	ao_mutex_put(&ao_spi_mutex[0])
+#endif
 
 #define AO_SPI_SPEED_FAST	17
 #define AO_SPI_SPEED_200kHz	13
 
-#define ao_spi_set_speed(speed) (U0GCR = (UxGCR_CPOL_NEGATIVE |		\
-					  UxGCR_CPHA_FIRST_EDGE |	\
-					  UxGCR_ORDER_MSB |		\
-					  ((speed) << UxGCR_BAUD_E_SHIFT)))
+#if MULTI_SPI
+#define ao_spi_set_speed(bus,speed) (*(bus ? &U1GCR : &U0GCR) =(UxGCR_CPOL_NEGATIVE | \
+								UxGCR_CPHA_FIRST_EDGE |	\
+								UxGCR_ORDER_MSB | \
+								((speed) << UxGCR_BAUD_E_SHIFT)))
+#else
+#define ao_spi_set_speed(bus,speed) (U0GCR = (UxGCR_CPOL_NEGATIVE |	\
+					      UxGCR_CPHA_FIRST_EDGE |	\
+					      UxGCR_ORDER_MSB |		\
+					      ((speed) << UxGCR_BAUD_E_SHIFT)))
+#endif
 
 #define ao_spi_get_slave(bus) do {			\
-		ao_mutex_get(&ao_spi_mutex);		\
-		ao_spi_set_speed(AO_SPI_SPEED_FAST);	\
+		ao_spi_get(bus);			\
+		ao_spi_set_speed(bus,AO_SPI_SPEED_FAST);	\
 	} while (0)
 
 #define ao_spi_put_slave(bus) do {		\
-		ao_mutex_put(&ao_spi_mutex);	\
+		ao_spi_put(bus);		\
 	} while (0)
 
 #define ao_spi_get_mask(reg,mask,bus,speed) do {	\
-		ao_mutex_get(&ao_spi_mutex);		\
-		ao_spi_set_speed(speed);		\
+		ao_spi_get(bus);			\
+		ao_spi_set_speed(bus,speed);		\
 		(reg) &= ~(mask);			\
 	} while (0)
 
 #define ao_spi_put_mask(reg,mask,bus) do {		\
 	(reg) |= (mask); \
-	ao_mutex_put(&ao_spi_mutex); \
+	ao_spi_put(bus); \
 	} while (0)
 
 
 #define ao_spi_get_bit(reg,bit,pin,bus,speed) do {	\
-		ao_mutex_get(&ao_spi_mutex);	\
-		ao_spi_set_speed(speed);	\
-		pin = 0;			\
+		ao_spi_get(bus);			\
+		ao_spi_set_speed(bus,speed);		\
+		pin = 0;				\
 	} while (0)
 
 #define ao_spi_put_bit(reg,bit,pin,bus) do {	\
 		pin = 1;			\
-		ao_mutex_put(&ao_spi_mutex);	\
+		ao_spi_put(bus);		\
 	} while (0)
 
 
@@ -68,6 +96,13 @@ extern __xdata uint8_t	ao_spi_mutex;
  * from chip select low to chip select high
  */
 
+#if MULTI_SPI
+void
+ao_spi_send(void __xdata *block, uint16_t len, uint8_t bus) __reentrant;
+
+void
+ao_spi_recv(void __xdata *block, uint16_t len, uint8_t bus) __reentrant;
+#else
 void
 ao_spi_send_bus(void __xdata *block, uint16_t len) __reentrant;
 
@@ -76,6 +111,7 @@ ao_spi_recv_bus(void __xdata *block, uint16_t len) __reentrant;
 
 #define ao_spi_send(block, len, bus) ao_spi_send_bus(block, len)
 #define ao_spi_recv(block, len, bus) ao_spi_recv_bus(block, len)
+#endif
 
 #if AO_SPI_SLAVE
 void
@@ -88,10 +124,15 @@ ao_spi_recv_wait(void);
 void
 ao_spi_init(void);
 
-#define ao_spi_init_cs(port, mask) do {		\
-		SPI_CS_PORT |= mask;		\
-		SPI_CS_DIR |= mask;		\
-		SPI_CS_SEL &= ~mask;		\
+#define token_paster(x,y)	x ## y
+#define token_paster3(x,y,z)	x ## y ## z
+#define token_evaluator(x,y)	token_paster(x,y)
+#define token_evaluator3(x,y,z)	token_paster3(x,y,z)
+
+#define ao_spi_init_cs(port, mask) do {			\
+		port |= mask;				\
+		token_evaluator(port,DIR) |= mask;	\
+		token_evaluator(port,SEL) &= ~mask;	\
 	} while (0)
 
 #define cc1111_enable_output(port,dir,sel,pin,bit,v) do {	\
@@ -102,7 +143,7 @@ ao_spi_init(void);
 
 #define disable_unreachable	_Pragma("disable_warning 126")
 
-#define token_paster(x,y)	x ## y
-#define token_evaluator(x,y)	token_paster(x,y)
 #define ao_enable_output(port,bit,pin,v) cc1111_enable_output(port,token_evaluator(port,DIR), token_evaluator(port,SEL), pin, bit, v)
 #define ao_gpio_set(port, bit, pin, v) ((pin) = (v))
+#define ao_gpio_get(port, bit, pin) (pin)
+
diff --git a/src/cc1111/ao_exti.c b/src/cc1111/ao_exti.c
new file mode 100644
index 00000000..537f6252
--- /dev/null
+++ b/src/cc1111/ao_exti.c
@@ -0,0 +1,33 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+#include <ao_exti.h>
+
+#if HAS_EXTI_0
+__xdata void 	(*ao_int_callback)(void);
+
+void
+ao_p0_isr(void) __interrupt(13)
+{
+	if (P0IF && (P0IFG & (AO_MS5607_MISO_MASK))) {
+		(*ao_int_callback)();
+	}
+	P0IFG = 0;
+	P0IF = 0;
+}
+#endif
diff --git a/src/cc1111/ao_exti.h b/src/cc1111/ao_exti.h
new file mode 100644
index 00000000..49fca5d2
--- /dev/null
+++ b/src/cc1111/ao_exti.h
@@ -0,0 +1,56 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_EXTI_H_
+#define _AO_EXTI_H_
+
+#define AO_EXTI_MODE_RISING	1
+#define AO_EXTI_MODE_FALLING	2
+#define AO_EXTI_MODE_PULL_UP	4
+#define AO_EXTI_MODE_PULL_DOWN	8
+#define AO_EXTI_PRIORITY_LOW	16
+#define AO_EXTI_PRIORITY_MED	0
+#define AO_EXTI_PRIORITY_HIGH	32
+
+extern void 	(*ao_int_callback)(void);
+
+#define ao_exti_setup(gpio, pin, mode, callback) do { \
+		ao_int_callback = callback;	      \
+	} while (0)
+
+#define ao_exti_set_mode(gpio, pin, mode) do { \
+	} while (0)
+
+#define ao_exti_set_callback(port, pin, callback) do {	\
+		ao_int_callback = callback;		\
+	} while (0)
+
+#define ao_exti_init()
+
+#define ao_exti_enable(port, pin) do {		\
+		P0IFG &= ~(1 << pin);		\
+		P0IF = 0;			\
+		PICTL |= PICTL_P0IENL;		\
+		IEN1 |= IEN1_P0IE;		\
+	} while (0)
+
+#define ao_exti_disable(port, pin) do {		\
+		IEN1 &= ~IEN1_P0IE;		\
+		PICTL &= ~PICTL_P0IENL;		\
+	} while (0)
+
+#endif /* _AO_EXTI_H_ */
diff --git a/src/cc1111/ao_radio.c b/src/cc1111/ao_radio.c
index 88b8f686..190647ce 100644
--- a/src/cc1111/ao_radio.c
+++ b/src/cc1111/ao_radio.c
@@ -374,7 +374,7 @@ ao_radio_recv(__xdata void *packet, uint8_t size, uint8_t timeout) __reentrant
 	}
 #if NEED_RADIO_RSSI
 	else
-		ao_radio_rssi = AO_RSSI_FROM_RADIO(((uint8_t *)packet)[size - 1]);
+		ao_radio_rssi = AO_RSSI_FROM_RADIO(((uint8_t *)packet)[size - 2]);
 #endif
 	ao_radio_put();
 	return ao_radio_dma_done;
diff --git a/src/cc1111/ao_spi.c b/src/cc1111/ao_spi.c
index cdef6bda..fb08f3f5 100644
--- a/src/cc1111/ao_spi.c
+++ b/src/cc1111/ao_spi.c
@@ -18,10 +18,6 @@
 #include "ao.h"
 
 /* Default pin usage for existing Altus Metrum devices */
-#if !HAS_SPI_0 && !HAS_SPI_1
-#define HAS_SPI_0	1
-#define SPI_0_ALT_2	1
-#endif
 
 #ifndef SPI_CONST
 #define SPI_CONST	0xff
@@ -61,62 +57,107 @@
  */
 
 #if HAS_SPI_0
-#define SPI_CSR		U0CSR
-#define SPI_BUF		U0DBUFXADDR
-#define SPI_BAUD	U0BAUD
-#define SPI_GCR		U0GCR
-#define SPI_CFG_MASK	PERCFG_U0CFG_ALT_MASK
-#define SPI_DMA_TX	DMA_CFG0_TRIGGER_UTX0
-#define SPI_DMA_RX	DMA_CFG0_TRIGGER_URX0
+#define SPI_BUF_0	&U0DBUFXADDR
+#define SPI_CSR_0	U0CSR
+#define SPI_BAUD_0	U0BAUD
+#define SPI_GCR_0	U0GCR
+#define SPI_CFG_MASK_0	PERCFG_U0CFG_ALT_MASK
+#define SPI_DMA_TX_0	DMA_CFG0_TRIGGER_UTX0
+#define SPI_DMA_RX_0	DMA_CFG0_TRIGGER_URX0
 
 #if SPI_0_ALT_1
-#define SPI_CFG		PERCFG_U0CFG_ALT_1
-#define SPI_SEL		P0SEL
-#define SPI_BITS	(1 << 3) | (1 << 2) | (1 << 5)
-#define SPI_CSS_BIT	(1 << 4)
+#define SPI_CFG_0	PERCFG_U0CFG_ALT_1
+#define SPI_SEL_0	P0SEL
+#define SPI_BITS_0	(1 << 3) | (1 << 2) | (1 << 5)
+#define SPI_CSS_BIT_0	(1 << 4)
 #endif
 
 #if SPI_0_ALT_2
-#define SPI_CFG		PERCFG_U0CFG_ALT_2
-#define SPI_SEL		P1SEL
-#define SPI_PRI		P2SEL_PRI3P1_USART0
-#define SPI_BITS	(1 << 5) | (1 << 4) | (1 << 3)
-#define SPI_CSS_BIT	(1 << 2)
+#define SPI_CFG_0	PERCFG_U0CFG_ALT_2
+#define SPI_SEL_0	P1SEL
+#define SPI_PRI_0	P2SEL_PRI3P1_USART0
+#define SPI_BITS_0	(1 << 5) | (1 << 4) | (1 << 3)
+#define SPI_CSS_BIT_0	(1 << 2)
 #endif
 
 #endif
 
 #if HAS_SPI_1
-#define SPI_CSR		U1CSR
-#define SPI_BUF		U1DBUFXADDR
-#define SPI_BAUD	U1BAUD
-#define SPI_GCR		U1GCR
-#define SPI_CFG_MASK	PERCFG_U1CFG_ALT_MASK
-#define SPI_DMA_TX	DMA_CFG0_TRIGGER_UTX1
-#define SPI_DMA_RX	DMA_CFG0_TRIGGER_URX1
+#define SPI_BUF_1	&U1DBUFXADDR
+#define SPI_CSR_1	U1CSR
+#define SPI_BAUD_1	U1BAUD
+#define SPI_GCR_1	U1GCR
+#define SPI_CFG_MASK_1	PERCFG_U1CFG_ALT_MASK
+#define SPI_DMA_TX_1	DMA_CFG0_TRIGGER_UTX1
+#define SPI_DMA_RX_1	DMA_CFG0_TRIGGER_URX1
 
 #if SPI_1_ALT_1
-#define SPI_CFG		PERCFG_U1CFG_ALT_1
-#define SPI_SEL		P0SEL
-#define SPI_BITS	(1 << 4) | (1 << 5) | (1 << 3)
-#define SPI_CSS_BIT	(1 << 2)
+#define SPI_CFG_1	PERCFG_U1CFG_ALT_1
+#define SPI_SEL_1	P0SEL
+#define SPI_BITS_1	(1 << 4) | (1 << 5) | (1 << 3)
+#define SPI_CSS_BIT_1	(1 << 2)
 #endif
 
 #if SPI_1_ALT_2
-#define SPI_CFG		PERCFG_U1CFG_ALT_2
-#define SPI_SEL		P1SEL
-#define SPI_PRI		P2SEL_PRI3P1_USART1
-#define SPI_BITS	(1 << 6) | (1 << 7) | (1 << 5)
-#define SPI_CSS_BIT	(1 << 4)
+#define SPI_CFG_1	PERCFG_U1CFG_ALT_2
+#define SPI_SEL_1	P1SEL
+#define SPI_PRI_1	P2SEL_PRI3P1_USART1
+#define SPI_BITS_1	(1 << 6) | (1 << 7) | (1 << 5)
+#define SPI_CSS_BIT_1	(1 << 4)
 #endif
 
 #endif
 
+#if MULTI_SPI
+
+#define SPI_BUF(bus)		((bus) ? SPI_BUF_1 : SPI_BUF_0)
+#define SPI_CSR(bus)		((bus) ? SPI_CSR_1 : SPI_CSR_0)
+#define SPI_BAUD(bus)		((bus) ? SPI_BAUD_1 : SPI_BAUD_0)
+#define SPI_GCR(bus)		((bus) ? SPI_GCR_1 : SPI_GCR_0)
+#define SPI_CFG_MASK(bus)	((bus) ? SPI_CFG_MASK_1 : SPI_CFG_MASK_0)
+#define SPI_DMA_TX(bus)		((bus) ? SPI_DMA_TX_1 : SPI_DMA_TX_0)
+#define SPI_DMA_RX(bus)		((bus) ? SPI_DMA_RX_1 : SPI_DMA_RX_0)
+#define SPI_CFG(bus)		((bus) ? SPI_CFG_1 : SPI_CFG_0)
+#define SPI_SEL(bus)		((bus) ? SPI_SEL_1 : SPI_SEL_0)
+#define SPI_BITS(bus)		((bus) ? SPI_BITS_1 : SPI_BITS_0)
+#define SPI_CSS_BIT(bus)	((bus) ? SPI_CSS_BIT_1 : SPI_CSS_BIT_0)
+
+#else
+
+#if HAS_SPI_0
+#define SPI_BUF(bus)		SPI_BUF_0
+#define SPI_CSR(bus)		SPI_CSR_0
+#define SPI_BAUD(bus)		SPI_BAUD_0
+#define SPI_GCR(bus)		SPI_GCR_0
+#define SPI_CFG_MASK(bus)	SPI_CFG_MASK_0
+#define SPI_DMA_TX(bus)		SPI_DMA_TX_0
+#define SPI_DMA_RX(bus)		SPI_DMA_RX_0
+#define SPI_CFG(bus)		SPI_CFG_0
+#define SPI_SEL(bus)		SPI_SEL_0
+#define SPI_BITS(bus)		SPI_BITS_0
+#define SPI_CSS_BIT(bus)	SPI_CSS_BIT_0
+#endif
+#if HAS_SPI_1
+#define SPI_BUF(bus)		SPI_BUF_1
+#define SPI_CSR(bus)		SPI_CSR_1
+#define SPI_BAUD(bus)		SPI_BAUD_1
+#define SPI_GCR(bus)		SPI_GCR_1
+#define SPI_CFG_MASK(bus)	SPI_CFG_MASK_1
+#define SPI_DMA_TX(bus)		SPI_DMA_TX_1
+#define SPI_DMA_RX(bus)		SPI_DMA_RX_1
+#define SPI_CFG(bus)		SPI_CFG_1
+#define SPI_SEL(bus)		SPI_SEL_1
+#define SPI_BITS(bus)		SPI_BITS_1
+#define SPI_CSS_BIT(bus)	SPI_CSS_BIT_1
+#endif
+
+#endif /* MULTI_SPI */
+
 #if AO_SPI_SLAVE
-#define CSS		SPI_CSS_BIT
+#define CSS(bus)		SPI_CSS_BIT(bus)
 #define UxCSR_DIRECTION	UxCSR_SLAVE
 #else
-#define CSS		0
+#define CSS(bus)		0
 #define UxCSR_DIRECTION	UxCSR_MASTER
 #endif
 
@@ -124,15 +165,16 @@
  * operation, from CS low to CS high. This means that any SPI
  * user must protect the SPI bus with this mutex
  */
-__xdata uint8_t	ao_spi_mutex;
-__xdata uint8_t ao_spi_dma_in_done;
-__xdata uint8_t ao_spi_dma_out_done;
+__xdata uint8_t	ao_spi_mutex[N_SPI];
+__xdata uint8_t ao_spi_dma_in_done[N_SPI];
+__xdata uint8_t ao_spi_dma_out_done[N_SPI];
 
-uint8_t	ao_spi_dma_out_id;
-uint8_t ao_spi_dma_in_id;
+uint8_t	ao_spi_dma_out_id[N_SPI];
+uint8_t ao_spi_dma_in_id[N_SPI];
 
 static __xdata uint8_t ao_spi_const;
 
+
 /* Send bytes over SPI.
  *
  * This sets up two DMA engines, one writing the data and another reading
@@ -140,45 +182,52 @@ static __xdata uint8_t ao_spi_const;
  * is complete, as the transmit register is double buffered and hence signals
  * completion one byte before the transfer is actually complete
  */
+#if MULTI_SPI
+void
+ao_spi_send(void __xdata *block, uint16_t len, uint8_t bus) __reentrant
+#else
 void
 ao_spi_send_bus(void __xdata *block, uint16_t len) __reentrant
+#define bus	0
+#endif
 {
-	ao_dma_set_transfer(ao_spi_dma_in_id,
-			    &SPI_BUF,
+	ao_dma_set_transfer(ao_spi_dma_in_id[bus],
+			    SPI_BUF(bus),
 			    &ao_spi_const,
 			    len,
 			    DMA_CFG0_WORDSIZE_8 |
 			    DMA_CFG0_TMODE_SINGLE |
-			    SPI_DMA_RX,
+			    SPI_DMA_RX(bus),
 			    DMA_CFG1_SRCINC_0 |
 			    DMA_CFG1_DESTINC_0 |
 			    DMA_CFG1_PRIORITY_NORMAL);
-	ao_dma_set_transfer(ao_spi_dma_out_id,
+	ao_dma_set_transfer(ao_spi_dma_out_id[bus],
 			    block,
-			    &SPI_BUF,
+			    SPI_BUF(bus),
 			    len,
 			    DMA_CFG0_WORDSIZE_8 |
 			    DMA_CFG0_TMODE_SINGLE |
-			    SPI_DMA_TX,
+			    SPI_DMA_TX(bus),
 			    DMA_CFG1_SRCINC_1 |
 			    DMA_CFG1_DESTINC_0 |
 			    DMA_CFG1_PRIORITY_NORMAL);
 
-	ao_dma_start(ao_spi_dma_in_id);
-	ao_dma_start(ao_spi_dma_out_id);
-	ao_dma_trigger(ao_spi_dma_out_id);
+	ao_dma_start(ao_spi_dma_in_id[bus]);
+	ao_dma_start(ao_spi_dma_out_id[bus]);
+	ao_dma_trigger(ao_spi_dma_out_id[bus]);
 #if !AO_SPI_SLAVE
-	__critical while (!ao_spi_dma_in_done)
-		ao_sleep(&ao_spi_dma_in_done);
+	__critical while (!ao_spi_dma_in_done[bus])
+		ao_sleep(&ao_spi_dma_in_done[bus]);
 #endif
+#undef bus
 }
 
 #if AO_SPI_SLAVE
 void
 ao_spi_send_wait(void)
 {
-	__critical while (!ao_spi_dma_in_done)
-		ao_sleep(&ao_spi_dma_in_done);
+	__critical while (!ao_spi_dma_in_done[0])
+		ao_sleep(&ao_spi_dma_in_done[0]);
 }
 #endif
 
@@ -188,16 +237,22 @@ ao_spi_send_wait(void)
  * writing constant values to the SPI transmitter as that is what
  * clocks the data coming in.
  */
+#if MULTI_SPI
+void
+ao_spi_recv(void __xdata *block, uint16_t len, uint8_t bus) __reentrant
+#else
 void
 ao_spi_recv_bus(void __xdata *block, uint16_t len) __reentrant
+#define bus 0
+#endif
 {
-	ao_dma_set_transfer(ao_spi_dma_in_id,
-			    &SPI_BUF,
+	ao_dma_set_transfer(ao_spi_dma_in_id[bus],
+			    SPI_BUF(bus),
 			    block,
 			    len,
 			    DMA_CFG0_WORDSIZE_8 |
 			    DMA_CFG0_TMODE_SINGLE |
-			    SPI_DMA_RX,
+			    SPI_DMA_RX(bus),
 			    DMA_CFG1_SRCINC_0 |
 			    DMA_CFG1_DESTINC_1 |
 			    DMA_CFG1_PRIORITY_NORMAL);
@@ -205,24 +260,24 @@ ao_spi_recv_bus(void __xdata *block, uint16_t len) __reentrant
 	ao_spi_const = SPI_CONST;
 
 #if !AO_SPI_SLAVE
-	ao_dma_set_transfer(ao_spi_dma_out_id,
+	ao_dma_set_transfer(ao_spi_dma_out_id[bus],
 			    &ao_spi_const,
-			    &SPI_BUF,
+			    SPI_BUF(bus),
 			    len,
 			    DMA_CFG0_WORDSIZE_8 |
 			    DMA_CFG0_TMODE_SINGLE |
-			    SPI_DMA_TX,
+			    SPI_DMA_TX(bus),
 			    DMA_CFG1_SRCINC_0 |
 			    DMA_CFG1_DESTINC_0 |
 			    DMA_CFG1_PRIORITY_NORMAL);
 #endif
 
-	ao_dma_start(ao_spi_dma_in_id);
+	ao_dma_start(ao_spi_dma_in_id[bus]);
 #if !AO_SPI_SLAVE
-	ao_dma_start(ao_spi_dma_out_id);
-	ao_dma_trigger(ao_spi_dma_out_id);
-	__critical while (!ao_spi_dma_in_done)
-		ao_sleep(&ao_spi_dma_in_done);
+	ao_dma_start(ao_spi_dma_out_id[bus]);
+	ao_dma_trigger(ao_spi_dma_out_id[bus]);
+	__critical while (!ao_spi_dma_in_done[bus])
+		ao_sleep(&ao_spi_dma_in_done[bus]);
 #endif
 }
 
@@ -230,17 +285,43 @@ ao_spi_recv_bus(void __xdata *block, uint16_t len) __reentrant
 void
 ao_spi_recv_wait(void)
 {
-	__critical while (!ao_spi_dma_in_done)
-		ao_sleep(&ao_spi_dma_in_done);
+	__critical while (!ao_spi_dma_in_done[0])
+		ao_sleep(&ao_spi_dma_in_done[0]);
 }
 #endif
 
+/* Set up the USART.
+ *
+ * SPI master/slave mode
+ */
+/* Set the baud rate and signal parameters
+ *
+ * The cc1111 is limited to a 24/8 MHz SPI clock.
+ * Every peripheral I've ever seen goes faster than that,
+ * so set the clock to 3MHz (BAUD_E 17, BAUD_M 0)
+ */
+#define SPI_INIT(bus,o)	do {						\
+		/* Set up the USART pin assignment */			\
+		PERCFG = (PERCFG & ~SPI_CFG_MASK(bus)) | SPI_CFG(bus);	\
+									\
+		/* Make the SPI pins be controlled by the USART peripheral */ \
+		SPI_SEL(bus) |= SPI_BITS(bus) | CSS(bus);		\
+		SPI_CSR(bus) = (UxCSR_MODE_SPI | UxCSR_RE | UxCSR_DIRECTION); \
+		SPI_BAUD(bus) = 0;					\
+		SPI_GCR(bus) = (UxGCR_CPOL_NEGATIVE |			\
+				UxGCR_CPHA_FIRST_EDGE |			\
+				UxGCR_ORDER_MSB |			\
+				(17 << UxGCR_BAUD_E_SHIFT));		\
+		/* Set up OUT DMA */					\
+		ao_spi_dma_out_id[o] = ao_dma_alloc(&ao_spi_dma_out_done[o]); \
+									\
+		/* Set up IN DMA */					\
+		ao_spi_dma_in_id[o] = ao_dma_alloc(&ao_spi_dma_in_done[o]);	\
+	} while (0)
+
 void
 ao_spi_init(void)
 {
-	/* Set up the USART pin assignment */
-	PERCFG = (PERCFG & ~SPI_CFG_MASK) | SPI_CFG;
-
 	/* Ensure that SPI USART takes precidence over the other USART
 	 * for pins that they share
 	 */
@@ -248,30 +329,10 @@ ao_spi_init(void)
 	P2SEL = (P2SEL & ~P2SEL_PRI3P1_MASK) | SPI_PRI;
 #endif
 
-	/* Make the SPI pins be controlled by the USART peripheral */
-	SPI_SEL |= SPI_BITS | CSS;
-
-	/* Set up OUT DMA */
-	ao_spi_dma_out_id = ao_dma_alloc(&ao_spi_dma_out_done);
-
-	/* Set up IN DMA */
-	ao_spi_dma_in_id = ao_dma_alloc(&ao_spi_dma_in_done);
-
-	/* Set up the USART.
-	 *
-	 * SPI master/slave mode
-	 */
-	SPI_CSR = (UxCSR_MODE_SPI | UxCSR_RE | UxCSR_DIRECTION);
-
-	/* Set the baud rate and signal parameters
-	 *
-	 * The cc1111 is limited to a 24/8 MHz SPI clock.
-	 * Every peripheral I've ever seen goes faster than that,
-	 * so set the clock to 3MHz (BAUD_E 17, BAUD_M 0)
-	 */
-	SPI_BAUD = 0;
-	SPI_GCR = (UxGCR_CPOL_NEGATIVE |
-		   UxGCR_CPHA_FIRST_EDGE |
-		   UxGCR_ORDER_MSB |
-		   (17 << UxGCR_BAUD_E_SHIFT));
+#if HAS_SPI_0
+	SPI_INIT(0, 0);
+#endif
+#if HAS_SPI_1
+	SPI_INIT(1, MULTI_SPI);
+#endif
 }
diff --git a/src/cc1111/ao_timer.c b/src/cc1111/ao_timer.c
index a64b5aba..75cc4ce8 100644
--- a/src/cc1111/ao_timer.c
+++ b/src/cc1111/ao_timer.c
@@ -39,6 +39,9 @@ void ao_timer_isr(void) __interrupt 9
 	if (++ao_adc_count == ao_adc_interval) {
 		ao_adc_count = 0;
 		ao_adc_poll();
+#if (AO_DATA_ALL & ~(AO_DATA_ADC))
+		ao_wakeup(DATA_TO_XDATA(&ao_adc_count));
+#endif
 	}
 #endif
 }
@@ -92,6 +95,13 @@ ao_clock_init(void)
 	while (!(SLEEP & SLEEP_XOSC_STB))
 		;
 
+	/* Power down the unused HFRC oscillator */
+	SLEEP |= SLEEP_OSC_PD;
+
+	/* Wait for HFRC to power down */
+	while ((SLEEP & SLEEP_HFRC_STB) != 0)
+		;
+	
 	/* Crank up the timer tick and system clock speed */
 	CLKCON = ((CLKCON & ~(CLKCON_TICKSPD_MASK | CLKCON_CLKSPD_MASK)) |
 		  (CLKCON_TICKSPD_1 | CLKCON_CLKSPD_1));
diff --git a/src/cc1111/ao_usb.c b/src/cc1111/ao_usb.c
index a655d1be..b0ab409d 100644
--- a/src/cc1111/ao_usb.c
+++ b/src/cc1111/ao_usb.c
@@ -201,6 +201,11 @@ ao_usb_ep0_setup(void)
 				ao_usb_ep0_queue_byte(0);
 				break;
 			case AO_USB_REQ_SET_ADDRESS:
+#if USB_FORCE_FLIGHT_IDLE
+				/* Go to idle mode if USB is connected
+				 */
+				ao_flight_force_idle = 1;
+#endif
 				ao_usb_set_address(ao_usb_setup.value);
 				break;
 			case AO_USB_REQ_GET_DESCRIPTOR:
diff --git a/src/core/ao.h b/src/core/ao.h
index 0ad3e4aa..0b634a79 100644
--- a/src/core/ao.h
+++ b/src/core/ao.h
@@ -43,6 +43,12 @@
 #define HAS_TASK	1
 #endif
 
+#ifndef AO_PORT_TYPE
+#define AO_PORT_TYPE uint8_t
+#endif
+
+typedef AO_PORT_TYPE ao_port_t;
+
 #if HAS_TASK
 #include <ao_task.h>
 #else
@@ -68,6 +74,8 @@
 #define AO_PANIC_SPI		13	/* SPI communication failure */
 #define AO_PANIC_CRASH		14	/* Processor crashed */
 #define AO_PANIC_BUFIO		15	/* Mis-using bufio API */
+#define AO_PANIC_EXTI		16	/* Mis-using exti API */
+#define AO_PANIC_FAST_TIMER	17	/* Mis-using fast timer API */
 #define AO_PANIC_SELF_TEST_CC1120	0x40 | 1	/* Self test failure */
 #define AO_PANIC_SELF_TEST_HMC5883	0x40 | 2	/* Self test failure */
 #define AO_PANIC_SELF_TEST_MPU6000	0x40 | 3	/* Self test failure */
@@ -174,7 +182,7 @@ void
 ao_cmd_hex(void);
 
 void
-ao_cmd_decimal(void);
+ao_cmd_decimal(void) __reentrant;
 
 /* Read a single hex nibble off stdin. */
 uint8_t
@@ -334,6 +342,10 @@ ao_spi_slave(void);
 #define AO_GPS_DATE_VALID	(1 << 6)
 #define AO_GPS_COURSE_VALID	(1 << 7)
 
+#define AO_GPS_NEW_DATA		1
+#define AO_GPS_NEW_TRACKING	2
+
+extern __xdata uint8_t ao_gps_new;
 extern __pdata uint16_t ao_gps_tick;
 extern __xdata uint8_t ao_gps_mutex;
 extern __xdata struct ao_telemetry_location ao_gps_data;
@@ -380,6 +392,9 @@ void
 ao_gps_tracking_print(__xdata struct ao_gps_tracking_orig *gps_tracking_data);
 
 void
+ao_gps_show(void) __reentrant;
+
+void
 ao_gps_init(void);
 
 /*
@@ -693,6 +708,8 @@ struct ao_ignition {
 	uint8_t firing;
 };
 
+extern __code char * __code ao_igniter_status_names[];
+
 extern __xdata struct ao_ignition ao_ignition[2];
 
 enum ao_igniter_status
@@ -722,7 +739,7 @@ extern __xdata uint8_t ao_force_freq;
 #endif
 
 #define AO_CONFIG_MAJOR	1
-#define AO_CONFIG_MINOR	14
+#define AO_CONFIG_MINOR	15
 
 #define AO_AES_LEN 16
 
@@ -756,6 +773,11 @@ struct ao_config {
 #if HAS_RADIO_AMP
 	uint8_t		radio_amp;		/* minor version 14 */
 #endif
+#if HAS_GYRO
+	int16_t		accel_zero_along;	/* minor version 15 */
+	int16_t		accel_zero_across;	/* minor version 15 */
+	int16_t		accel_zero_through;	/* minor version 15 */
+#endif
 };
 
 #define AO_IGNITE_MODE_DUAL		0
diff --git a/src/core/ao_adc.h b/src/core/ao_adc.h
index 0dd87080..373db1c4 100644
--- a/src/core/ao_adc.h
+++ b/src/core/ao_adc.h
@@ -28,10 +28,6 @@ ao_adc_poll(void);
 void
 ao_adc_sleep(void);
 
-/* Get a copy of the last complete sample set */
-void
-ao_data_get(__xdata struct ao_data *packet);
-
 /* Initialize the A/D converter */
 void
 ao_adc_init(void);
diff --git a/src/core/ao_cmd.c b/src/core/ao_cmd.c
index 188b8bb4..4ebaa607 100644
--- a/src/core/ao_cmd.c
+++ b/src/core/ao_cmd.c
@@ -206,9 +206,9 @@ ao_cmd_hex(void)
 }
 
 void
-ao_cmd_decimal(void)
+ao_cmd_decimal(void) __reentrant
 {
-	__pdata uint8_t	r = ao_cmd_lex_error;
+	uint8_t	r = ao_cmd_lex_error;
 
 	ao_cmd_lex_u32 = 0;
 	ao_cmd_white();
@@ -290,9 +290,6 @@ version(void)
 	       , ao_log_format
 #endif
 		);
-#if HAS_MS5607
-	ao_ms5607_info();
-#endif
 	printf("software-version %s\n", ao_version);
 }
 #endif
diff --git a/src/core/ao_config.c b/src/core/ao_config.c
index 73608a55..a30ec64a 100644
--- a/src/core/ao_config.c
+++ b/src/core/ao_config.c
@@ -17,7 +17,7 @@
 
 #include "ao.h"
 #include "ao_log.h"
-#include <ao_storage.h>
+#include <ao_config.h>
 #if HAS_FLIGHT
 #include <ao_sample.h>
 #include <ao_data.h>
@@ -28,6 +28,9 @@ __pdata uint8_t ao_config_loaded;
 __pdata uint8_t ao_config_dirty;
 __xdata uint8_t ao_config_mutex;
 
+#ifndef AO_CONFIG_DEFAULT_APRS_INTERVAL
+#define AO_CONFIG_DEFAULT_APRS_INTERVAL	0
+#endif
 #define AO_CONFIG_DEFAULT_MAIN_DEPLOY	250
 #define AO_CONFIG_DEFAULT_RADIO_CHANNEL	0
 #define AO_CONFIG_DEFAULT_CALLSIGN	"N0CALL"
@@ -47,20 +50,22 @@ __xdata uint8_t ao_config_mutex;
 #define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX	((uint32_t) 192 * (uint32_t) 1024)
 #endif
 #endif
+#ifndef AO_CONFIG_DEFAULT_RADIO_POWER
 #define AO_CONFIG_DEFAULT_RADIO_POWER		0x60
+#endif
 #define AO_CONFIG_DEFAULT_RADIO_AMP		0
 
 #if HAS_EEPROM
 static void
 _ao_config_put(void)
 {
-	ao_storage_setup();
-	ao_storage_erase(ao_storage_config);
-	ao_storage_write(ao_storage_config, &ao_config, sizeof (ao_config));
+	ao_config_setup();
+	ao_config_erase();
+	ao_config_write(0, &ao_config, sizeof (ao_config));
 #if HAS_FLIGHT
 	ao_log_write_erase(0);
 #endif
-	ao_storage_flush();
+	ao_config_flush();
 }
 
 void
@@ -92,8 +97,8 @@ _ao_config_get(void)
 	 * but ao_storage_setup *also* sets ao_storage_config, which we
 	 * need before calling ao_storage_read here
 	 */
-	ao_storage_setup();
-	ao_storage_read(ao_storage_config, &ao_config, sizeof (ao_config));
+	ao_config_setup();
+	ao_config_read(0, &ao_config, sizeof (ao_config));
 #endif
 	if (ao_config.major != AO_CONFIG_MAJOR) {
 		ao_config.major = AO_CONFIG_MAJOR;
@@ -122,8 +127,10 @@ _ao_config_get(void)
 			ao_config.radio_cal = ao_radio_cal;
 #endif
 		/* Fixups for minor version 4 */
+#if HAS_FLIGHT
 		if (minor < 4)
 			ao_config.flight_log_max = AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX;
+#endif
 		/* Fixupes for minor version 5 */
 		if (minor < 5)
 			ao_config.ignite_mode = AO_CONFIG_DEFAULT_IGNITE_MODE;
@@ -142,7 +149,7 @@ _ao_config_get(void)
 			memset(&ao_config.pyro, '\0', sizeof (ao_config.pyro));
 #endif
 		if (minor < 13)
-			ao_config.aprs_interval = 0;
+			ao_config.aprs_interval = AO_CONFIG_DEFAULT_APRS_INTERVAL;
 #if HAS_RADIO_POWER
 		if (minor < 14)
 			ao_config.radio_power = AO_CONFIG_DEFAULT_RADIO_POWER;
@@ -151,6 +158,19 @@ _ao_config_get(void)
 		if (minor  < 14)
 			ao_config.radio_amp = AO_CONFIG_DEFAULT_RADIO_AMP;
 #endif
+#if HAS_GYRO
+		if (minor < 15) {
+			ao_config.accel_zero_along = 0;
+			ao_config.accel_zero_across = 0;
+			ao_config.accel_zero_through = 0;
+
+			/* Reset the main accel offsets to force
+			 * re-calibration
+			 */
+			ao_config.accel_plus_g = 0;
+			ao_config.accel_minus_g = 0;
+		}
+#endif
 		ao_config.minor = AO_CONFIG_MINOR;
 		ao_config_dirty = 1;
 	}
@@ -270,17 +290,34 @@ ao_config_accel_calibrate_show(void) __reentrant
 {
 	printf("Accel cal +1g: %d -1g: %d\n",
 	       ao_config.accel_plus_g, ao_config.accel_minus_g);
+#if HAS_GYRO
+	printf ("IMU cal along %d across %d through %d\n",
+		ao_config.accel_zero_along,
+		ao_config.accel_zero_across,
+		ao_config.accel_zero_through);
+#endif
 }
 
 #define ACCEL_CALIBRATE_SAMPLES	1024
 #define ACCEL_CALIBRATE_SHIFT	10
 
+#if HAS_GYRO
+static int16_t accel_cal_along;
+static int16_t accel_cal_across;
+static int16_t accel_cal_through;
+#endif
+
 static int16_t
 ao_config_accel_calibrate_auto(char *orientation) __reentrant
 {
 	uint16_t	i;
 	int32_t		accel_total;
 	uint8_t		cal_data_ring;
+#if HAS_GYRO
+	int32_t		accel_along_total = 0;
+	int32_t		accel_across_total = 0;
+	int32_t		accel_through_total = 0;
+#endif
 
 	printf("Orient antenna %s and press a key...", orientation);
 	flush();
@@ -294,10 +331,20 @@ ao_config_accel_calibrate_auto(char *orientation) __reentrant
 		ao_sleep(DATA_TO_XDATA(&ao_sample_data));
 		while (i && cal_data_ring != ao_sample_data) {
 			accel_total += (int32_t) ao_data_accel(&ao_data_ring[cal_data_ring]);
+#if HAS_GYRO
+			accel_along_total += (int32_t) ao_data_along(&ao_data_ring[cal_data_ring]);
+			accel_across_total += (int32_t) ao_data_across(&ao_data_ring[cal_data_ring]);
+			accel_through_total += (int32_t) ao_data_through(&ao_data_ring[cal_data_ring]);
+#endif
 			cal_data_ring = ao_data_ring_next(cal_data_ring);
 			i--;
 		}
 	}
+#if HAS_GYRO
+	accel_cal_along = accel_along_total >> ACCEL_CALIBRATE_SHIFT;
+	accel_cal_across = accel_across_total >> ACCEL_CALIBRATE_SHIFT;
+	accel_cal_through = accel_through_total >> ACCEL_CALIBRATE_SHIFT;
+#endif
 	return accel_total >> ACCEL_CALIBRATE_SHIFT;
 }
 
@@ -305,12 +352,28 @@ void
 ao_config_accel_calibrate_set(void) __reentrant
 {
 	int16_t	up, down;
+#if HAS_GYRO
+	int16_t	accel_along_up, accel_along_down;
+	int16_t	accel_across_up, accel_across_down;
+	int16_t	accel_through_up, accel_through_down;
+#endif
+	
 	ao_cmd_decimal();
 	if (ao_cmd_status != ao_cmd_success)
 		return;
 	if (ao_cmd_lex_i == 0) {
 		up = ao_config_accel_calibrate_auto("up");
+#if HAS_GYRO
+		accel_along_up = accel_cal_along;
+		accel_across_up = accel_cal_across;
+		accel_through_up = accel_cal_through;
+#endif
 		down = ao_config_accel_calibrate_auto("down");
+#if HAS_GYRO
+		accel_along_down = accel_cal_along;
+		accel_across_down = accel_cal_across;
+		accel_through_down = accel_cal_through;
+#endif
 	} else {
 		up = ao_cmd_lex_i;
 		ao_cmd_decimal();
@@ -326,6 +389,11 @@ ao_config_accel_calibrate_set(void) __reentrant
 	_ao_config_edit_start();
 	ao_config.accel_plus_g = up;
 	ao_config.accel_minus_g = down;
+#if HAS_GYRO
+	ao_config.accel_zero_along = (accel_along_up + accel_along_down) / 2;
+	ao_config.accel_zero_across = (accel_across_up + accel_across_down) / 2;
+	ao_config.accel_zero_through = (accel_through_up + accel_through_down) / 2;
+#endif
 	_ao_config_edit_finish();
 }
 #endif /* HAS_ACCEL */
@@ -399,7 +467,7 @@ void
 ao_config_log_set(void) __reentrant
 {
 	uint16_t	block = (uint16_t) (ao_storage_block >> 10);
-	uint16_t	config = (uint16_t) (ao_storage_config >> 10);
+	uint16_t	log_max = (uint16_t) (ao_storage_log_max >> 10);
 
 	ao_cmd_decimal();
 	if (ao_cmd_status != ao_cmd_success)
@@ -408,8 +476,8 @@ ao_config_log_set(void) __reentrant
 		printf("Storage must be empty before changing log size\n");
 	else if (block > 1024 && (ao_cmd_lex_i & (block - 1)))
 		printf("Flight log size must be multiple of %d kB\n", block);
-	else if (ao_cmd_lex_i > config)
-		printf("Flight log max %d kB\n", config);
+	else if (ao_cmd_lex_i > log_max)
+		printf("Flight log max %d kB\n", log_max);
 	else {
 		_ao_config_edit_start();
 		ao_config.flight_log_max = (uint32_t) ao_cmd_lex_i << 10;
@@ -589,7 +657,7 @@ static void
 ao_config_show(void) __reentrant;
 
 static void
-ao_config_write(void) __reentrant;
+ao_config_save(void) __reentrant;
 
 __code struct ao_config_var ao_config_vars[] = {
 #if HAS_FLIGHT
@@ -648,7 +716,7 @@ __code struct ao_config_var ao_config_vars[] = {
 	  ao_config_show,		0 },
 #if HAS_EEPROM
 	{ "w\0Write to eeprom",
-	  ao_config_write,		0 },
+	  ao_config_save,		0 },
 #endif
 	{ "?\0Help",
 	  ao_config_help,		0 },
@@ -693,11 +761,14 @@ ao_config_show(void) __reentrant
 	for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++)
 		if (ao_config_vars[cmd].show)
 			(*ao_config_vars[cmd].show)();
+#if HAS_MS5607
+	ao_ms5607_info();
+#endif
 }
 
 #if HAS_EEPROM
 static void
-ao_config_write(void) __reentrant
+ao_config_save(void) __reentrant
 {
 	uint8_t saved = 0;
 	ao_mutex_get(&ao_config_mutex);
diff --git a/src/core/ao_config.h b/src/core/ao_config.h
new file mode 100644
index 00000000..e101af8e
--- /dev/null
+++ b/src/core/ao_config.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_CONFIG_H_
+#define _AO_CONFIG_H_
+
+#ifndef USE_STORAGE_CONFIG
+#define USE_STORAGE_CONFIG 1
+#endif
+
+#ifndef USE_EEPROM_CONFIG
+#define USE_EEPROM_CONFIG 0
+#endif
+
+#if USE_STORAGE_CONFIG
+
+#include <ao_storage.h>
+
+#define ao_config_setup() 		ao_storage_setup()
+#define ao_config_erase()		ao_storage_erase(ao_storage_config)
+#define ao_config_write(pos,bytes, len)	ao_storage_write(ao_storage_config+(pos), bytes, len)
+#define ao_config_read(pos,bytes, len)	ao_storage_read(ao_storage_config+(pos), bytes, len)
+#define ao_config_flush()		ao_storage_flush()
+
+#endif
+
+#if USE_EEPROM_CONFIG
+
+#include <ao_eeprom.h>
+
+#define ao_config_setup()
+#define ao_config_erase()
+#define ao_config_write(pos,bytes, len)	ao_eeprom_write(pos, bytes, len)
+#define ao_config_read(pos,bytes, len)	ao_eeprom_read(pos, bytes, len)
+#define ao_config_flush()
+
+#endif
+
+#endif /* _AO_CONFIG_H_ */
diff --git a/src/stm/ao_data.c b/src/core/ao_data.c
index 38d2f7ff..6a3d02a1 100644
--- a/src/stm/ao_data.c
+++ b/src/core/ao_data.c
@@ -22,6 +22,7 @@ volatile __xdata struct ao_data	ao_data_ring[AO_DATA_RING];
 volatile __data uint8_t		ao_data_head;
 volatile __data uint8_t		ao_data_present;
 
+#ifndef ao_data_count
 void
 ao_data_get(__xdata struct ao_data *packet)
 {
@@ -32,3 +33,4 @@ ao_data_get(__xdata struct ao_data *packet)
 #endif
 	memcpy(packet, (void *) &ao_data_ring[i], sizeof (struct ao_data));
 }
+#endif
diff --git a/src/core/ao_data.h b/src/core/ao_data.h
index 7e2f85d8..e1d8a139 100644
--- a/src/core/ao_data.h
+++ b/src/core/ao_data.h
@@ -18,6 +18,8 @@
 #ifndef _AO_DATA_H_
 #define _AO_DATA_H_
 
+#define GRAVITY 9.80665
+
 #if HAS_ADC
 #define AO_DATA_ADC	(1 << 0)
 #else
@@ -82,6 +84,10 @@ struct ao_data {
 #define ao_data_ring_next(n)	(((n) + 1) & (AO_DATA_RING - 1))
 #define ao_data_ring_prev(n)	(((n) - 1) & (AO_DATA_RING - 1))
 
+/* Get a copy of the last complete sample set */
+void
+ao_data_get(__xdata struct ao_data *packet);
+
 extern volatile __xdata struct ao_data	ao_data_ring[AO_DATA_RING];
 extern volatile __data uint8_t		ao_data_head;
 extern volatile __data uint8_t		ao_data_present;
@@ -97,7 +103,7 @@ extern volatile __data uint8_t		ao_data_count;
  * signaled by the timer tick
  */
 #define AO_DATA_WAIT() do {				\
-		ao_sleep((void *) &ao_data_count);	\
+		ao_sleep(DATA_TO_XDATA ((void *) &ao_data_count));	\
 	} while (0)
 
 #endif /* AO_DATA_RING */
@@ -268,7 +274,11 @@ typedef int16_t accel_t;
 /* MMA655X is hooked up so that positive values represent negative acceleration */
 
 #define ao_data_accel(packet)			((packet)->mma655x)
+#if AO_MMA655X_INVERT
+#define ao_data_accel_cook(packet)		(4095 - (packet)->mma655x)
+#else
 #define ao_data_accel_cook(packet)		((packet)->mma655x)
+#endif
 #define ao_data_set_accel(packet, accel)	((packet)->mma655x = (accel))
 #define ao_data_accel_invert(accel)		(4095 - (accel))
 
@@ -292,8 +302,8 @@ typedef int16_t accel_t;
 
 #define HAS_GYRO	1
 
-typedef int16_t	gyro_t;
-typedef int32_t angle_t;
+typedef int16_t	gyro_t;		/* in raw sample units */
+typedef int16_t angle_t;	/* in degrees */
 
 /* Y axis is aligned with the direction of motion (along) */
 /* X axis is aligned in the other board axis (across) */
@@ -309,4 +319,16 @@ typedef int32_t angle_t;
 
 #endif
 
+#if !HAS_MAG && HAS_HMC5883
+
+#define HAS_MAG		1
+
+typedef int16_t ao_mag_t;		/* in raw sample units */
+
+#define ao_data_mag_along(packet)	((packet)->hmc5883.x)
+#define ao_data_mag_across(packet)	((packet)->hmc5883.y)
+#define ao_data_mag_through(packet)	((packet)->hmc5883.z)
+
+#endif
+
 #endif /* _AO_DATA_H_ */
diff --git a/src/core/ao_debounce.c b/src/core/ao_debounce.c
new file mode 100644
index 00000000..b9d67729
--- /dev/null
+++ b/src/core/ao_debounce.c
@@ -0,0 +1,163 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+#include <ao_debounce.h>
+#include <ao_fast_timer.h>
+
+static uint8_t			ao_debounce_initialized;
+static uint8_t			ao_debounce_running;
+static struct ao_debounce	*ao_debounce;
+
+static uint8_t	values[64];
+static uint8_t	n;
+
+#define d_step(n)	(((n) + 1) & 63)
+
+static void
+_ao_debounce_set(struct ao_debounce *debounce, uint8_t value)
+{
+	if (value != debounce->value) {
+		values[n] = value;
+		n = (n + 1) & 63;
+		debounce->value = value;
+		debounce->_set(debounce, value);
+	}
+	_ao_debounce_stop(debounce);
+}
+
+void
+ao_debounce_dump(void)
+{
+	uint8_t	s;
+
+	for (s = 0; s < n; s++) {
+		printf ("%d: %d\n",
+			s, values[s]);
+	}
+	n = 0;
+}
+
+/*
+ * Get the current value, set the result when we've
+ * reached the debounce count limit
+ */
+static void
+_ao_debounce_check(struct ao_debounce *debounce)
+{
+	uint8_t	next = debounce->_get(debounce);
+
+	if (next == debounce->current) {
+		if (debounce->count < debounce->hold) {
+			if (++debounce->count == debounce->hold)
+				_ao_debounce_set(debounce, debounce->current);
+		}
+	} else {
+		debounce->count = 0;
+		debounce->current = next;
+	}
+}
+
+static void
+_ao_debounce_isr(void)
+{
+	struct ao_debounce *debounce, *next;
+
+	for (debounce = ao_debounce; debounce; debounce = next) {
+		next = debounce->next;
+		_ao_debounce_check(debounce);
+	}
+}
+
+static void
+ao_debounce_on(void)
+{
+	ao_fast_timer_on(_ao_debounce_isr);
+}
+
+static void
+ao_debounce_off(void)
+{
+	ao_fast_timer_off(_ao_debounce_isr);
+}
+
+/*
+ * Start monitoring one pin
+ */
+void
+_ao_debounce_start(struct ao_debounce *debounce)
+{
+	uint32_t	m;
+
+	m = ao_arch_irqsave();
+	if (!debounce->running) {
+		debounce->running = 1;
+
+		/* Reset the counter */
+		debounce->count = 0;
+
+		/* Link into list */
+		debounce->next = ao_debounce;
+		ao_debounce = debounce;
+
+		/* Make sure the timer is running */
+		if (!ao_debounce_running++)
+			ao_debounce_on();
+
+		/* And go check the current value */
+		_ao_debounce_check(debounce);
+	}
+	ao_arch_irqrestore(m);
+}
+
+/*
+ * Stop monitoring one pin
+ */
+void
+_ao_debounce_stop(struct ao_debounce *debounce)
+{
+	struct ao_debounce **prev;
+	uint32_t m;
+
+	m = ao_arch_irqsave();
+	if (debounce->running) {
+		debounce->running = 0;
+
+		/* Unlink */
+		for (prev = &ao_debounce; (*prev); prev = &((*prev)->next)) {
+			if (*prev == debounce) {
+				*prev = debounce->next;
+				break;
+			}
+		}
+		debounce->next = NULL;
+
+		/* Turn off the timer if possible */
+		if (!--ao_debounce_running)
+			ao_debounce_off();
+	}
+	ao_arch_irqrestore(m);
+}
+
+void
+ao_debounce_init(void)
+{
+	if (ao_debounce_initialized)
+		return;
+	ao_debounce_initialized = 1;
+	ao_fast_timer_init();
+}
diff --git a/src/core/ao_debounce.h b/src/core/ao_debounce.h
new file mode 100644
index 00000000..19c620f5
--- /dev/null
+++ b/src/core/ao_debounce.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_DEBOUNCE_H_
+#define _AO_DEBOUNCE_H_
+
+struct ao_debounce {
+	struct ao_debounce	*next;
+
+	/* time that pin value must be stable before accepting */
+	uint8_t			hold;
+
+	/* last value reported to app; don't report it twice */
+	uint8_t			value;
+
+	/* current value received from pins */
+	uint8_t			current;
+
+	/* current count of intervals pin value has been stable */
+	uint8_t			count;
+
+	/* This pin is running */
+	uint8_t			running;
+
+	/* Get the current pin value */
+	uint8_t			(*_get)(struct ao_debounce *debounce);
+
+	/* The stable value has changed */
+	void 			(*_set)(struct ao_debounce *debounce, uint8_t value);
+};
+
+static inline void
+ao_debounce_config(struct ao_debounce *debounce,
+		   uint8_t (*_get)(struct ao_debounce *debounce),
+		   void (*_set)(struct ao_debounce *debounce, uint8_t value),
+		   uint8_t hold)
+{
+	debounce->next = 0;
+	debounce->hold = hold;
+	debounce->value = 0xff;
+	debounce->current = 0xff;
+	debounce->count = 0;
+	debounce->running = 0;
+	debounce->_get = _get;
+	debounce->_set = _set;
+}
+
+void
+_ao_debounce_start(struct ao_debounce *debounce);
+
+void
+_ao_debounce_stop(struct ao_debounce *debounce);
+
+void
+ao_debounce_init(void);
+
+void
+ao_debounce_dump(void);
+
+#endif /* _AO_DEBOUNCE_H_ */
diff --git a/src/core/ao_eeprom.h b/src/core/ao_eeprom.h
new file mode 100644
index 00000000..915522bf
--- /dev/null
+++ b/src/core/ao_eeprom.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_EEPROM_H_
+#define _AO_EEPROM_H_
+
+extern const ao_pos_t	ao_eeprom_total;
+
+/*
+ * Write to eeprom
+ */
+
+uint8_t
+ao_eeprom_write(ao_pos_t pos32, __xdata void *v, uint16_t len);
+
+/*
+ * Read from eeprom
+ */
+uint8_t
+ao_eeprom_read(ao_pos_t pos, __xdata void *v, uint16_t len);
+
+/*
+ * Initialize eeprom
+ */
+
+void
+ao_eeprom_init(void);
+
+#endif /* _AO_EEPROM_H_ */
diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c
index 9d9d4c6e..463ff4a2 100644
--- a/src/core/ao_flight.c
+++ b/src/core/ao_flight.c
@@ -20,6 +20,10 @@
 #include <ao_log.h>
 #endif
 
+#if HAS_MPU6000
+#include <ao_quaternion.h>
+#endif
+
 #ifndef HAS_ACCEL
 #error Please define HAS_ACCEL
 #endif
@@ -42,6 +46,11 @@ __pdata enum ao_flight_state	ao_flight_state;	/* current flight state */
 __pdata uint16_t		ao_boost_tick;		/* time of launch detect */
 __pdata uint16_t		ao_motor_number;	/* number of motors burned so far */
 
+#if HAS_IMU
+/* Any sensor can set this to mark the flight computer as 'broken' */
+__xdata uint8_t			ao_sensor_errors;
+#endif
+
 /*
  * track min/max data over a long interval to detect
  * resting
@@ -95,6 +104,9 @@ ao_flight(void)
 			    ao_config.accel_minus_g == 0 ||
 			    ao_ground_accel < ao_config.accel_plus_g - ACCEL_NOSE_UP ||
 			    ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP ||
+#if HAS_IMU
+			    ao_sensor_errors ||
+#endif
 			    ao_ground_height < -1000 ||
 			    ao_ground_height > 7000)
 			{
@@ -117,14 +129,14 @@ ao_flight(void)
  			{
 				/* Set pad mode - we can fly! */
 				ao_flight_state = ao_flight_pad;
-#if HAS_USB && HAS_RADIO && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE
+#if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE
 				/* Disable the USB controller in flight mode
 				 * to save power
 				 */
 				ao_usb_disable();
 #endif
 
-#if !HAS_ACCEL
+#if !HAS_ACCEL && PACKET_HAS_SLAVE
 				/* Disable packet mode in pad state on TeleMini */
 				ao_packet_slave_stop();
 #endif
@@ -134,8 +146,10 @@ ao_flight(void)
 				ao_rdf_set(1);
 				ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD);
 #endif
+#if HAS_LED
 				/* signal successful initialization by turning off the LED */
 				ao_led_off(AO_LED_RED);
+#endif
 			} else {
 				/* Set idle mode */
  				ao_flight_state = ao_flight_idle;
@@ -145,8 +159,10 @@ ao_flight(void)
 				ao_packet_slave_start();
 #endif
 
+#if HAS_LED
 				/* signal successful initialization by turning off the LED */
 				ao_led_off(AO_LED_RED);
+#endif
 			}
 			/* wakeup threads due to state change */
 			ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
@@ -190,8 +206,8 @@ ao_flight(void)
 
 #if HAS_GPS
 				/* Record current GPS position by waking up GPS log tasks */
-				ao_wakeup(&ao_gps_data);
-				ao_wakeup(&ao_gps_tracking_data);
+				ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
+				ao_wakeup(&ao_gps_new);
 #endif
 
 				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
@@ -356,6 +372,18 @@ ao_flight(void)
 				ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
 			}
 			break;
+#if HAS_FLIGHT_DEBUG
+		case ao_flight_test:
+#if HAS_GYRO
+			printf ("angle %4d pitch %7d yaw %7d roll %7d\n",
+				ao_sample_orient,
+				((ao_sample_pitch << 9) - ao_ground_pitch) >> 9,
+				((ao_sample_yaw << 9) - ao_ground_yaw) >> 9,
+				((ao_sample_roll << 9) - ao_ground_roll) >> 9);
+#endif
+			flush();
+			break;
+#endif /* HAS_FLIGHT_DEBUG */
 		default:
 			break;
 		}
@@ -406,8 +434,17 @@ ao_flight_dump(void)
 	printf ("  error_avg   %d\n", ao_error_h_sq_avg);
 }
 
+static void
+ao_gyro_test(void)
+{
+	ao_flight_state = ao_flight_test;
+	ao_getchar();
+	ao_flight_state = ao_flight_idle;
+}
+
 __code struct ao_cmds ao_flight_cmds[] = {
 	{ ao_flight_dump, 	"F\0Dump flight status" },
+	{ ao_gyro_test,		"G\0Test gyro code" },
 	{ 0, NULL },
 };
 #endif
diff --git a/src/core/ao_flight.h b/src/core/ao_flight.h
index b80202f0..c7c02ccf 100644
--- a/src/core/ao_flight.h
+++ b/src/core/ao_flight.h
@@ -33,13 +33,18 @@ enum ao_flight_state {
 	ao_flight_drogue = 6,
 	ao_flight_main = 7,
 	ao_flight_landed = 8,
-	ao_flight_invalid = 9
+	ao_flight_invalid = 9,
+	ao_flight_test = 10
 };
 
 extern __pdata enum ao_flight_state	ao_flight_state;
 extern __pdata uint16_t			ao_boost_tick;
 extern __pdata uint16_t			ao_motor_number;
 
+#if HAS_IMU
+extern __xdata uint8_t			ao_sensor_errors;
+#endif
+
 extern __pdata uint16_t			ao_launch_time;
 extern __pdata uint8_t			ao_flight_force_idle;
 
diff --git a/src/core/ao_gps_report.c b/src/core/ao_gps_report.c
index c52ef621..07201ac2 100644
--- a/src/core/ao_gps_report.c
+++ b/src/core/ao_gps_report.c
@@ -22,78 +22,68 @@ ao_gps_report(void)
 {
 	static __xdata struct ao_log_record		gps_log;
 	static __xdata struct ao_telemetry_location	gps_data;
+	static __xdata struct ao_telemetry_satellite	gps_tracking_data;
 	uint8_t	date_reported = 0;
+	uint8_t new;
 
 	for (;;) {
-		ao_sleep(&ao_gps_data);
+		while ((new = ao_gps_new) == 0)
+			ao_sleep(&ao_gps_new);
 		ao_mutex_get(&ao_gps_mutex);
-		ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
+		if (new & AO_GPS_NEW_DATA)
+			ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
+		if (new & AO_GPS_NEW_TRACKING)
+			ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
+		ao_gps_new = 0;
 		ao_mutex_put(&ao_gps_mutex);
 
-		if (!(gps_data.flags & AO_GPS_VALID))
-			continue;
-
-		gps_log.tick = ao_gps_tick;
-		gps_log.type = AO_LOG_GPS_TIME;
-		gps_log.u.gps_time.hour = gps_data.hour;
-		gps_log.u.gps_time.minute = gps_data.minute;
-		gps_log.u.gps_time.second = gps_data.second;
-		gps_log.u.gps_time.flags = gps_data.flags;
-		ao_log_data(&gps_log);
-		gps_log.type = AO_LOG_GPS_LAT;
-		gps_log.u.gps_latitude = gps_data.latitude;
-		ao_log_data(&gps_log);
-		gps_log.type = AO_LOG_GPS_LON;
-		gps_log.u.gps_longitude = gps_data.longitude;
-		ao_log_data(&gps_log);
-		gps_log.type = AO_LOG_GPS_ALT;
-		gps_log.u.gps_altitude.altitude = gps_data.altitude;
-		gps_log.u.gps_altitude.unused = 0xffff;
-		ao_log_data(&gps_log);
-		if (!date_reported && (gps_data.flags & AO_GPS_DATE_VALID)) {
-			gps_log.type = AO_LOG_GPS_DATE;
-			gps_log.u.gps_date.year = gps_data.year;
-			gps_log.u.gps_date.month = gps_data.month;
-			gps_log.u.gps_date.day = gps_data.day;
-			gps_log.u.gps_date.extra = 0;
-			date_reported = ao_log_data(&gps_log);
+		if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) {
+			gps_log.tick = ao_gps_tick;
+			gps_log.type = AO_LOG_GPS_TIME;
+			gps_log.u.gps_time.hour = gps_data.hour;
+			gps_log.u.gps_time.minute = gps_data.minute;
+			gps_log.u.gps_time.second = gps_data.second;
+			gps_log.u.gps_time.flags = gps_data.flags;
+			ao_log_data(&gps_log);
+			gps_log.type = AO_LOG_GPS_LAT;
+			gps_log.u.gps_latitude = gps_data.latitude;
+			ao_log_data(&gps_log);
+			gps_log.type = AO_LOG_GPS_LON;
+			gps_log.u.gps_longitude = gps_data.longitude;
+			ao_log_data(&gps_log);
+			gps_log.type = AO_LOG_GPS_ALT;
+			gps_log.u.gps_altitude.altitude = gps_data.altitude;
+			gps_log.u.gps_altitude.unused = 0xffff;
+			ao_log_data(&gps_log);
+			if (!date_reported && (gps_data.flags & AO_GPS_DATE_VALID)) {
+				gps_log.type = AO_LOG_GPS_DATE;
+				gps_log.u.gps_date.year = gps_data.year;
+				gps_log.u.gps_date.month = gps_data.month;
+				gps_log.u.gps_date.day = gps_data.day;
+				gps_log.u.gps_date.extra = 0;
+				date_reported = ao_log_data(&gps_log);
+			}
 		}
-	}
-}
-
-void
-ao_gps_tracking_report(void)
-{
-	static __xdata struct ao_log_record		gps_log;
-	static __xdata struct ao_telemetry_satellite	gps_tracking_data;
-	uint8_t	c, n;
-
-	for (;;) {
-		ao_sleep(&ao_gps_tracking_data);
-		ao_mutex_get(&ao_gps_mutex);
-		gps_log.tick = ao_gps_tick;
-		ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
-		ao_mutex_put(&ao_gps_mutex);
+		if (new & AO_GPS_NEW_TRACKING) {
+			uint8_t c, n;
 
-		if (!(n = gps_tracking_data.channels))
-			continue;
-
-		gps_log.type = AO_LOG_GPS_SAT;
-		for (c = 0; c < n; c++)
-			if ((gps_log.u.gps_sat.svid = gps_tracking_data.sats[c].svid))
-			{
-				gps_log.u.gps_sat.c_n = gps_tracking_data.sats[c].c_n_1;
-				ao_log_data(&gps_log);
+			if ((n = gps_tracking_data.channels) != 0) {
+				gps_log.type = AO_LOG_GPS_SAT;
+				for (c = 0; c < n; c++)
+					if ((gps_log.u.gps_sat.svid = gps_tracking_data.sats[c].svid))
+					{
+						gps_log.u.gps_sat.c_n = gps_tracking_data.sats[c].c_n_1;
+						ao_log_data(&gps_log);
+					}
 			}
+		}
 	}
 }
 
 __xdata struct ao_task ao_gps_report_task;
-__xdata struct ao_task ao_gps_tracking_report_task;
 
 void
 ao_gps_report_init(void)
 {
 	ao_add_task(&ao_gps_report_task, ao_gps_report, "gps_report");
-	ao_add_task(&ao_gps_tracking_report_task, ao_gps_tracking_report, "gps_tracking_report");
 }
diff --git a/src/core/ao_gps_report_mega.c b/src/core/ao_gps_report_mega.c
index e3af4307..d13885dd 100644
--- a/src/core/ao_gps_report_mega.c
+++ b/src/core/ao_gps_report_mega.c
@@ -23,66 +23,62 @@ ao_gps_report_mega(void)
 {
 	static __xdata struct ao_log_mega		gps_log;
 	static __xdata struct ao_telemetry_location	gps_data;
-	uint8_t	date_reported = 0;
-
-	for (;;) {
-		ao_sleep(&ao_gps_data);
-		ao_mutex_get(&ao_gps_mutex);
-		ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
-		ao_mutex_put(&ao_gps_mutex);
-
-		if (!(gps_data.flags & AO_GPS_VALID))
-			continue;
-
-		gps_log.tick = ao_gps_tick;
-		gps_log.type = AO_LOG_GPS_TIME;
-		gps_log.u.gps.latitude = gps_data.latitude;
-		gps_log.u.gps.longitude = gps_data.longitude;
-		gps_log.u.gps.altitude = gps_data.altitude;
-
-		gps_log.u.gps.hour = gps_data.hour;
-		gps_log.u.gps.minute = gps_data.minute;
-		gps_log.u.gps.second = gps_data.second;
-		gps_log.u.gps.flags = gps_data.flags;
-		gps_log.u.gps.year = gps_data.year;
-		gps_log.u.gps.month = gps_data.month;
-		gps_log.u.gps.day = gps_data.day;
-		ao_log_mega(&gps_log);
-	}
-}
-
-void
-ao_gps_tracking_report_mega(void)
-{
-	static __xdata struct ao_log_mega		gps_log;
 	static __xdata struct ao_telemetry_satellite	gps_tracking_data;
+	uint8_t	date_reported = 0;
+	uint8_t	new;
 	uint8_t	c, n, i;
 
 	for (;;) {
-		ao_sleep(&ao_gps_tracking_data);
+		while (!(new = ao_gps_new))
+			ao_sleep(&ao_gps_new);
 		ao_mutex_get(&ao_gps_mutex);
-		ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
+		if (new & AO_GPS_NEW_DATA)
+			ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
+		if (new & AO_GPS_NEW_TRACKING)
+			ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
+		ao_gps_new = 0;
 		ao_mutex_put(&ao_gps_mutex);
 
-		if (!(n = gps_tracking_data.channels))
-			continue;
+		if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) {
+			gps_log.tick = ao_gps_tick;
+			gps_log.type = AO_LOG_GPS_TIME;
+			gps_log.u.gps.latitude = gps_data.latitude;
+			gps_log.u.gps.longitude = gps_data.longitude;
+			gps_log.u.gps.altitude = gps_data.altitude;
 
-		gps_log.type = AO_LOG_GPS_SAT;
-		gps_log.tick = ao_gps_tick;
-		i = 0;
-		for (c = 0; c < n; c++)
-			if ((gps_log.u.gps_sat.sats[i].svid = gps_tracking_data.sats[c].svid))
-			{
-				gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1;
-				i++;
-			}
-		gps_log.u.gps_sat.channels = i;
-		ao_log_mega(&gps_log);
+			gps_log.u.gps.hour = gps_data.hour;
+			gps_log.u.gps.minute = gps_data.minute;
+			gps_log.u.gps.second = gps_data.second;
+			gps_log.u.gps.flags = gps_data.flags;
+			gps_log.u.gps.year = gps_data.year;
+			gps_log.u.gps.month = gps_data.month;
+			gps_log.u.gps.day = gps_data.day;
+			gps_log.u.gps.course = gps_data.course;
+			gps_log.u.gps.ground_speed = gps_data.ground_speed;
+			gps_log.u.gps.climb_rate = gps_data.climb_rate;
+			gps_log.u.gps.pdop = gps_data.pdop;
+			gps_log.u.gps.hdop = gps_data.hdop;
+			gps_log.u.gps.vdop = gps_data.vdop;
+			gps_log.u.gps.mode = gps_data.mode;
+			ao_log_mega(&gps_log);
+		}
+		if ((new & AO_GPS_NEW_TRACKING) && (n = gps_tracking_data.channels) != 0) {
+			gps_log.tick = ao_gps_tick;
+			gps_log.type = AO_LOG_GPS_SAT;
+			i = 0;
+			for (c = 0; c < n; c++)
+				if ((gps_log.u.gps_sat.sats[i].svid = gps_tracking_data.sats[c].svid))
+				{
+					gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1;
+					i++;
+				}
+			gps_log.u.gps_sat.channels = i;
+			ao_log_mega(&gps_log);
+		}
 	}
 }
 
 __xdata struct ao_task ao_gps_report_mega_task;
-__xdata struct ao_task ao_gps_tracking_report_mega_task;
 
 void
 ao_gps_report_mega_init(void)
@@ -90,7 +86,4 @@ ao_gps_report_mega_init(void)
 	ao_add_task(&ao_gps_report_mega_task,
 		    ao_gps_report_mega,
 		    "gps_report");
-	ao_add_task(&ao_gps_tracking_report_mega_task,
-		    ao_gps_tracking_report_mega,
-		    "gps_tracking_report");
 }
diff --git a/src/core/ao_gps_report_metrum.c b/src/core/ao_gps_report_metrum.c
new file mode 100644
index 00000000..fa038976
--- /dev/null
+++ b/src/core/ao_gps_report_metrum.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright © 2009 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "ao.h"
+#include "ao_log.h"
+
+void
+ao_gps_report_metrum(void)
+{
+	static __xdata struct ao_log_metrum		gps_log;
+	static __xdata struct ao_telemetry_location	gps_data;
+	static __xdata struct ao_telemetry_satellite	gps_tracking_data;
+	uint8_t	c, n, i, p, valid, packets;
+	uint8_t svid;
+	uint8_t	date_reported = 0;
+	uint8_t new;
+
+	for (;;) {
+		while (!(new = ao_gps_new))
+			ao_sleep(&ao_gps_new);
+		ao_mutex_get(&ao_gps_mutex);
+		if (new & AO_GPS_NEW_DATA)
+			ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
+		if (new & AO_GPS_NEW_TRACKING)
+			ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
+		ao_gps_new = 0;
+		ao_mutex_put(&ao_gps_mutex);
+
+		if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) {
+			gps_log.tick = ao_gps_tick;
+			gps_log.type = AO_LOG_GPS_POS;
+			gps_log.u.gps.latitude = gps_data.latitude;
+			gps_log.u.gps.longitude = gps_data.longitude;
+			gps_log.u.gps.altitude = gps_data.altitude;
+			ao_log_metrum(&gps_log);
+
+			gps_log.type = AO_LOG_GPS_TIME;
+			gps_log.u.gps_time.hour = gps_data.hour;
+			gps_log.u.gps_time.minute = gps_data.minute;
+			gps_log.u.gps_time.second = gps_data.second;
+			gps_log.u.gps_time.flags = gps_data.flags;
+			gps_log.u.gps_time.year = gps_data.year;
+			gps_log.u.gps_time.month = gps_data.month;
+			gps_log.u.gps_time.day = gps_data.day;
+			ao_log_metrum(&gps_log);
+		}
+
+		if ((new & AO_GPS_NEW_TRACKING) && (n = gps_tracking_data.channels)) {
+			gps_log.type = AO_LOG_GPS_SAT;
+			gps_log.tick = ao_gps_tick;
+			i = 0;
+			for (c = 0; c < n; c++) {
+				svid = gps_tracking_data.sats[c].svid;
+				if (svid != 0) {
+					if (i == 4) {
+						gps_log.u.gps_sat.channels = i;
+						gps_log.u.gps_sat.more = 1;
+						ao_log_metrum(&gps_log);
+						i = 0;
+					}
+					gps_log.u.gps_sat.sats[i].svid = svid;
+					gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1;
+					i++;
+				}
+			}
+			if (i) {
+				gps_log.u.gps_sat.channels = i;
+				gps_log.u.gps_sat.more = 0;
+				ao_log_metrum(&gps_log);
+			}
+		}
+	}
+}
+
+__xdata struct ao_task ao_gps_report_metrum_task;
+
+void
+ao_gps_report_metrum_init(void)
+{
+	ao_add_task(&ao_gps_report_metrum_task,
+		    ao_gps_report_metrum,
+		    "gps_report");
+}
diff --git a/src/core/ao_gps_show.c b/src/core/ao_gps_show.c
new file mode 100644
index 00000000..3a05e35a
--- /dev/null
+++ b/src/core/ao_gps_show.c
@@ -0,0 +1,39 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef AO_GPS_TEST
+#include <ao.h>
+#endif
+
+void
+ao_gps_show(void) __reentrant
+{
+	uint8_t	i;
+	ao_mutex_get(&ao_gps_mutex);
+	printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day);
+	printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second);
+	printf ("Lat/Lon: %ld %ld\n", (long) ao_gps_data.latitude, (long) ao_gps_data.longitude);
+	printf ("Alt: %d\n", ao_gps_data.altitude);
+	printf ("Flags: 0x%x\n", ao_gps_data.flags);
+	printf ("Sats: %d", ao_gps_tracking_data.channels);
+	for (i = 0; i < ao_gps_tracking_data.channels; i++)
+		printf (" %d %d",
+			ao_gps_tracking_data.sats[i].svid,
+			ao_gps_tracking_data.sats[i].c_n_1);
+	printf ("\ndone\n");
+	ao_mutex_put(&ao_gps_mutex);
+}
diff --git a/src/core/ao_ignite.c b/src/core/ao_ignite.c
index 74bd0c5a..9f2ec0a7 100644
--- a/src/core/ao_ignite.c
+++ b/src/core/ao_ignite.c
@@ -17,7 +17,11 @@
 
 #include "ao.h"
 #include <ao_data.h>
+#if AO_PYRO_NUM
+#include <ao_pyro.h>
+#endif
 
+#if HAS_IGNITE
 __xdata struct ao_ignition ao_ignition[2];
 
 void
@@ -150,6 +154,8 @@ ao_igniter(void)
 	}
 }
 
+#endif
+
 void
 ao_ignite_manual(void)
 {
@@ -157,33 +163,50 @@ ao_ignite_manual(void)
 	if (!ao_match_word("DoIt"))
 		return;
 	ao_cmd_white();
-	if (ao_cmd_lex_c == 'm') {
-		if(ao_match_word("main"))
-			ao_igniter_fire(ao_igniter_main);
-	} else {
-		if(ao_match_word("drogue"))
-			ao_igniter_fire(ao_igniter_drogue);
+#if HAS_IGNITE
+	if (ao_cmd_lex_c == 'm' && ao_match_word("main")) {
+		ao_igniter_fire(ao_igniter_main);
+		return;
+	}
+	if (ao_cmd_lex_c == 'd' && ao_match_word("drogue")) {
+		ao_igniter_fire(ao_igniter_drogue);
+		return;
+	}
+#endif
+#if AO_PYRO_NUM
+	if ('0' <= ao_cmd_lex_c && ao_cmd_lex_c <= '9') {
+		ao_pyro_manual(ao_cmd_lex_c - '0');
+		return;
 	}
+#endif
+	ao_cmd_status = ao_cmd_syntax_error;
 }
 
-static __code char * __code igniter_status_names[] = {
+__code char * __code ao_igniter_status_names[] = {
 	"unknown", "ready", "active", "open"
 };
 
+#if HAS_IGNITE
 void
 ao_ignite_print_status(enum ao_igniter igniter, __code char *name) __reentrant
 {
 	enum ao_igniter_status status = ao_igniter_status(igniter);
 	printf("Igniter: %6s Status: %s\n",
 	       name,
-	       igniter_status_names[status]);
+	       ao_igniter_status_names[status]);
 }
+#endif
 
 void
 ao_ignite_test(void)
 {
+#if HAS_IGNITE
 	ao_ignite_print_status(ao_igniter_drogue, "drogue");
 	ao_ignite_print_status(ao_igniter_main, "main");
+#endif
+#if AO_PYRO_NUM
+	ao_pyro_print_status();
+#endif
 }
 
 __code struct ao_cmds ao_ignite_cmds[] = {
@@ -192,6 +215,7 @@ __code struct ao_cmds ao_ignite_cmds[] = {
 	{ 0,	NULL },
 };
 
+#if HAS_IGNITE
 __xdata struct ao_task ao_igniter_task;
 
 void
@@ -200,11 +224,14 @@ ao_ignite_set_pins(void)
 	ao_enable_output(AO_IGNITER_DROGUE_PORT, AO_IGNITER_DROGUE_PIN, AO_IGNITER_DROGUE, 0);
 	ao_enable_output(AO_IGNITER_MAIN_PORT, AO_IGNITER_MAIN_PIN, AO_IGNITER_MAIN, 0);
 }
+#endif
 
 void
 ao_igniter_init(void)
 {
+#if HAS_IGNITE
 	ao_ignite_set_pins();
-	ao_cmd_register(&ao_ignite_cmds[0]);
 	ao_add_task(&ao_igniter_task, ao_igniter, "igniter");
+#endif
+	ao_cmd_register(&ao_ignite_cmds[0]);
 }
diff --git a/src/core/ao_int64.c b/src/core/ao_int64.c
new file mode 100644
index 00000000..aa23dbe0
--- /dev/null
+++ b/src/core/ao_int64.c
@@ -0,0 +1,158 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao_int64.h>
+
+__pdata ao_int64_t *__data ao_64r, *__data ao_64a, *__data ao_64b;
+
+void ao_plus64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, __pdata ao_int64_t *b) __FATTR {
+	__LOCAL uint32_t	t;
+
+	r->high = a->high + b->high;
+	t = a->low + b->low;
+	if (t < a->low)
+		r->high++;
+	r->low = t;
+}
+
+void ao_minus64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, __pdata ao_int64_t *b) __FATTR {
+	__LOCAL uint32_t	t;
+
+	r->high = a->high - b->high;
+	t = a->low - b->low;
+	if (t > a->low)
+		r->high--;
+	r->low = t;
+}
+
+void ao_rshift64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, uint8_t d) __FATTR {
+	if (d < 32) {
+		r->low = a->low >> d;
+		if (d)
+			r->low |= a->high << (32 - d);
+		r->high = (int32_t) a->high >> d;
+	} else {
+		d &= 0x1f;
+		r->low = (int32_t) a->high >> d;
+		r->high = 0;
+	}
+}
+
+void ao_lshift64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, uint8_t d) __FATTR {
+	if (d < 32) {
+		r->high = a->high << d;
+		if (d)
+			r->high |= a->low >> (32 - d);
+		r->low = a->low << d;
+	} else {
+		d &= 0x1f;
+		r->high = a->low << d;
+		r->low = 0;
+	}
+}
+
+static void ao_umul64_32_32(__ARG ao_int64_t *r, uint32_t a, uint32_t b) __reentrant {
+	__LOCAL uint32_t	s;
+	__LOCAL ao_int64_t	t;
+	r->low = (uint32_t) (uint16_t) a * (uint16_t) b;
+	r->high = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) (b >> 16);
+
+	s = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) b;
+
+	t.high = s >> 16;
+	t.low = s << 16;
+	ao_plus64(r, r, &t);
+
+	s = (uint32_t) (uint16_t) a * (uint16_t) (b >> 16);
+
+	t.high = s >> 16;
+	t.low = s << 16;
+	ao_plus64(r, r, &t);
+}
+
+void ao_neg64(__pdata ao_int64_t *r, __pdata ao_int64_t *a) __FATTR {
+	r->high = ~a->high;
+	if (!(r->low = ~a->low + 1))
+		r->high++;
+}
+
+void ao_mul64_32_32(__ARG ao_int64_t *r, int32_t a, int32_t b) __FATTR {
+	uint8_t		negative = 0;
+
+	if (a < 0) {
+		a = -a;
+		++negative;
+	}
+	if (b < 0) {
+		b = -b;
+		--negative;
+	}
+	ao_umul64_32_32(r, a, b);
+	if (negative)
+		ao_neg64(r, r);
+}
+
+static void ao_umul64(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG ao_int64_t *b) __reentrant {
+	__LOCAL ao_int64_t	r2, r3;
+
+	ao_umul64_32_32(&r2, a->high, b->low);
+	ao_umul64_32_32(&r3, a->low, b->high);
+	ao_umul64_32_32(r, a->low, b->low);
+
+	r->high += r2.low + r3.low;
+}
+
+static __ARG ao_int64_t	ap, bp;
+
+void ao_mul64(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG ao_int64_t *b) __FATTR {
+	uint8_t	negative = 0;
+
+	if (ao_int64_negativep(a)) {
+		ao_neg64(&ap, a);
+		a = &ap;
+		++negative;
+	}
+	if (ao_int64_negativep(b)) {
+		ao_neg64(&bp, b);
+		b = &bp;
+		--negative;
+	}
+	ao_umul64(r, a, b);
+	if (negative)
+		ao_neg64(r, r);
+}
+
+static void ao_umul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, uint16_t b) __reentrant {
+	__LOCAL uint32_t h;
+
+	h = a->high * b;
+	ao_umul64_32_32(r, a->low, b);
+	r->high += h;
+}
+
+void ao_mul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG uint16_t b) __FATTR {
+	uint8_t		negative = 0;
+
+	if ((int32_t) a->high < 0) {
+		ao_neg64(&ap, a);
+		a = &ap;
+		negative++;
+	} else
+		ao_umul64_64_16(r, a, b);
+	if (negative)
+		ao_neg64(r, r);
+}
diff --git a/src/core/ao_int64.h b/src/core/ao_int64.h
new file mode 100644
index 00000000..b16db58c
--- /dev/null
+++ b/src/core/ao_int64.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_INT64_H_
+#define _AO_INT64_H_
+
+#include <stdint.h>
+
+typedef struct {
+	uint32_t	high;
+	uint32_t	low;
+} ao_int64_t;
+
+#define __FATTR
+#define __ARG __pdata
+#define __LOCAL static __pdata
+
+void ao_plus64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, __pdata ao_int64_t *ao_64b) __FATTR;
+void ao_minus64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, __pdata ao_int64_t *ao_64b) __FATTR;
+void ao_neg64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a) __FATTR;
+void ao_rshift64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, uint8_t d) __FATTR;
+void ao_lshift64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, uint8_t d) __FATTR;
+void ao_mul64_32_32(__ARG ao_int64_t *r, __ARG int32_t a, __ARG int32_t b) __FATTR;
+void ao_mul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG uint16_t b) __FATTR;
+void ao_mul64(__ARG ao_int64_t * __ARG r, __ARG ao_int64_t * __ARG a, __ARG ao_int64_t *__ARG b) __FATTR;
+
+#define ao_int64_init32(r, a) (((r)->high = 0), (r)->low = (a))
+#define ao_int64_init64(r, a, b) (((r)->high = (a)), (r)->low = (b))
+
+#define ao_cast64(a) (((int64_t) (a)->high << 32) | (a)->low)
+
+#define ao_int64_negativep(a)	(((int32_t) (a)->high) < 0)
+
+#endif /* _AO_INT64_H_ */
diff --git a/src/core/ao_kalman.c b/src/core/ao_kalman.c
index 59ffd8b2..7fd4f889 100644
--- a/src/core/ao_kalman.c
+++ b/src/core/ao_kalman.c
@@ -40,9 +40,9 @@ static __pdata int32_t		ao_k_accel;
 __pdata int16_t			ao_height;
 __pdata int16_t			ao_speed;
 __pdata int16_t			ao_accel;
-__pdata int16_t			ao_max_height;
+__xdata int16_t			ao_max_height;
 static __pdata int32_t		ao_avg_height_scaled;
-__pdata int16_t			ao_avg_height;
+__xdata int16_t			ao_avg_height;
 
 __pdata int16_t			ao_error_h;
 __pdata int16_t			ao_error_h_sq_avg;
@@ -292,7 +292,4 @@ ao_kalman(void)
 	else 
 #endif
 		ao_avg_height = (ao_avg_height_scaled + 63) >> 7;
-#ifdef AO_FLIGHT_TEST
-	ao_sample_prev_tick = ao_sample_tick;
-#endif
 }
diff --git a/src/core/ao_log.c b/src/core/ao_log.c
index 7884ec3c..701c81ab 100644
--- a/src/core/ao_log.c
+++ b/src/core/ao_log.c
@@ -17,6 +17,7 @@
 
 #include "ao.h"
 #include <ao_log.h>
+#include <ao_config.h>
 
 __pdata uint32_t ao_log_current_pos;
 __pdata uint32_t ao_log_end_pos;
@@ -48,7 +49,7 @@ static __xdata struct ao_log_erase erase;
 static uint32_t
 ao_log_erase_pos(uint8_t i)
 {
-	return i * sizeof (struct ao_log_erase) + AO_STORAGE_ERASE_LOG;
+	return i * sizeof (struct ao_log_erase) + AO_CONFIG_MAX_SIZE;
 }
 
 void
@@ -56,14 +57,14 @@ ao_log_write_erase(uint8_t pos)
 {
 	erase.unused = 0x00;
 	erase.flight = ao_flight_number;
-	ao_storage_write(ao_log_erase_pos(pos),  &erase, sizeof (erase));
-	ao_storage_flush();
+	ao_config_write(ao_log_erase_pos(pos),  &erase, sizeof (erase));
+	ao_config_flush();
 }
 
 static void
 ao_log_read_erase(uint8_t pos)
 {
-	ao_storage_read(ao_log_erase_pos(pos), &erase, sizeof (erase));
+	ao_config_read(ao_log_erase_pos(pos), &erase, sizeof (erase));
 }
 
 
@@ -87,7 +88,7 @@ ao_log_erase_mark(void)
 static uint8_t
 ao_log_slots()
 {
-	return (uint8_t) (ao_storage_config / ao_config.flight_log_max);
+	return (uint8_t) (ao_storage_log_max / ao_config.flight_log_max);
 }
 
 uint32_t
@@ -278,6 +279,11 @@ ao_log_init(void)
 
 	ao_cmd_register(&ao_log_cmds[0]);
 
+#ifndef HAS_ADC
+#error Define HAS_ADC for ao_log.c
+#endif
+#if HAS_ADC
 	/* Create a task to log events to eeprom */
 	ao_add_task(&ao_log_task, ao_log, "log");
+#endif
 }
diff --git a/src/core/ao_log.h b/src/core/ao_log.h
index a68a40dd..09f31188 100644
--- a/src/core/ao_log.h
+++ b/src/core/ao_log.h
@@ -44,6 +44,9 @@ extern __pdata enum ao_flight_state ao_log_state;
 #define AO_LOG_FORMAT_TELEMETRY		3	/* 32 byte ao_telemetry records */
 #define AO_LOG_FORMAT_TELESCIENCE	4	/* 32 byte typed telescience records */
 #define AO_LOG_FORMAT_TELEMEGA		5	/* 32 byte typed telemega records */
+#define AO_LOG_FORMAT_EASYMINI		6	/* 16-byte MS5607 baro only, 3.0V supply */
+#define AO_LOG_FORMAT_TELEMETRUM	7	/* 16-byte typed telemetrum records */
+#define AO_LOG_FORMAT_TELEMINI		8	/* 16-byte MS5607 baro only, 3.3V supply */
 #define AO_LOG_FORMAT_NONE		127	/* No log at all */
 
 extern __code uint8_t ao_log_format;
@@ -134,6 +137,7 @@ ao_log_full(void);
 #define AO_LOG_GPS_ALT		'H'
 #define AO_LOG_GPS_SAT		'V'
 #define AO_LOG_GPS_DATE		'Y'
+#define AO_LOG_GPS_POS		'P'
 
 #define AO_LOG_POS_NONE		(~0UL)
 
@@ -235,7 +239,8 @@ struct ao_log_mega {
 			int16_t		v_pbatt;	/* 6 */
 			int16_t		n_sense;	/* 8 */
 			int16_t		sense[10];	/* 10 */
-		} volt;					/* 30 */
+			uint16_t	pyro;		/* 30 */
+		} volt;					/* 32 */
 		/* AO_LOG_GPS_TIME */
 		struct {
 			int32_t		latitude;	/* 4 */
@@ -248,8 +253,14 @@ struct ao_log_mega {
 			uint8_t		year;		/* 18 */
 			uint8_t		month;		/* 19 */
 			uint8_t		day;		/* 20 */
-			uint8_t		pad;		/* 21 */
-		} gps;	/* 22 */
+			uint8_t		course;		/* 21 */
+			uint16_t	ground_speed;	/* 22 */
+			int16_t		climb_rate;	/* 24 */
+			uint8_t		pdop;		/* 26 */
+			uint8_t		hdop;		/* 27 */
+			uint8_t		vdop;		/* 28 */
+			uint8_t		mode;		/* 29 */
+		} gps;	/* 30 */
 		/* AO_LOG_GPS_SAT */
 		struct {
 			uint16_t	channels;	/* 4 */
@@ -261,6 +272,98 @@ struct ao_log_mega {
 	} u;
 };
 
+struct ao_log_metrum {
+	char			type;			/* 0 */
+	uint8_t			csum;			/* 1 */
+	uint16_t		tick;			/* 2 */
+	union {						/* 4 */
+		/* AO_LOG_FLIGHT */
+		struct {
+			uint16_t	flight;		/* 4 */
+			int16_t		ground_accel;	/* 6 */
+			uint32_t	ground_pres;	/* 8 */
+			uint32_t	ground_temp;	/* 12 */
+		} flight;	/* 16 */
+		/* AO_LOG_STATE */
+		struct {
+			uint16_t	state;		/* 4 */
+			uint16_t	reason;		/* 6 */
+		} state;	/* 8 */
+		/* AO_LOG_SENSOR */
+		struct {
+			uint32_t	pres;		/* 4 */
+			uint32_t	temp;		/* 8 */
+			int16_t		accel;		/* 12 */
+		} sensor;	/* 14 */
+		/* AO_LOG_TEMP_VOLT */
+		struct {
+			int16_t		v_batt;		/* 4 */
+			int16_t		sense_a;	/* 6 */
+			int16_t		sense_m;	/* 8 */
+		} volt;		/* 10 */
+		/* AO_LOG_GPS_POS */
+		struct {
+			int32_t		latitude;	/* 4 */
+			int32_t		longitude;	/* 8 */
+			int16_t		altitude;	/* 12 */
+		} gps;		/* 14 */
+		/* AO_LOG_GPS_TIME */
+		struct {
+			uint8_t		hour;		/* 4 */
+			uint8_t		minute;		/* 5 */
+			uint8_t		second;		/* 6 */
+			uint8_t		flags;		/* 7 */
+			uint8_t		year;		/* 8 */
+			uint8_t		month;		/* 9 */
+			uint8_t		day;		/* 10 */
+			uint8_t		pad;		/* 11 */
+		} gps_time;	/* 12 */
+		/* AO_LOG_GPS_SAT (up to three packets) */
+		struct {
+			uint8_t	channels;		/* 4 */
+			uint8_t	more;			/* 5 */
+			struct {
+				uint8_t	svid;
+				uint8_t c_n;
+			} sats[4];			/* 6 */
+		} gps_sat;				/* 14 */
+		uint8_t		raw[12];		/* 4 */
+	} u;	/* 16 */
+};
+
+struct ao_log_mini {
+	char		type;				/* 0 */
+	uint8_t		csum;				/* 1 */
+	uint16_t	tick;				/* 2 */
+	union {						/* 4 */
+		/* AO_LOG_FLIGHT */
+		struct {
+			uint16_t	flight;		/* 4 */
+			uint16_t	r6;
+			uint32_t	ground_pres;	/* 8 */
+		} flight;
+		/* AO_LOG_STATE */
+		struct {
+			uint16_t	state;		/* 4 */
+			uint16_t	reason;		/* 6 */
+		} state;
+		/* AO_LOG_SENSOR */
+		struct {
+			uint8_t		pres[3];	/* 4 */
+			uint8_t		temp[3];	/* 7 */
+			int16_t		sense_a;	/* 10 */
+			int16_t		sense_m;	/* 12 */
+			int16_t		v_batt;		/* 14 */
+		} sensor;				/* 16 */
+	} u;						/* 16 */
+};							/* 16 */
+
+#define ao_log_pack24(dst,value) do {		\
+		(dst)[0] = (value);		\
+		(dst)[1] = (value) >> 8;	\
+		(dst)[2] = (value) >> 16;	\
+	} while (0)
+
 /* Write a record to the eeprom log */
 uint8_t
 ao_log_data(__xdata struct ao_log_record *log) __reentrant;
@@ -268,7 +371,16 @@ ao_log_data(__xdata struct ao_log_record *log) __reentrant;
 uint8_t
 ao_log_mega(__xdata struct ao_log_mega *log) __reentrant;
 
+uint8_t
+ao_log_metrum(__xdata struct ao_log_metrum *log) __reentrant;
+
+uint8_t
+ao_log_mini(__xdata struct ao_log_mini *log) __reentrant;
+
 void
 ao_log_flush(void);
 
+void
+ao_gps_report_metrum_init(void);
+
 #endif /* _AO_LOG_H_ */
diff --git a/src/core/ao_log_mega.c b/src/core/ao_log_mega.c
index abf953a6..768947d5 100644
--- a/src/core/ao_log_mega.c
+++ b/src/core/ao_log_mega.c
@@ -65,6 +65,7 @@ ao_log_dump_check_data(void)
 	return 1;
 }
 
+#if HAS_ADC
 static __data uint8_t	ao_log_data_pos;
 
 /* a hack to make sure that ao_log_megas fill the eeprom block in even units */
@@ -151,6 +152,7 @@ ao_log(void)
 				log.u.volt.n_sense = AO_ADC_NUM_SENSE;
 				for (i = 0; i < AO_ADC_NUM_SENSE; i++)
 					log.u.volt.sense[i] = ao_data_ring[ao_log_data_pos].adc.sense[i];
+				log.u.volt.pyro = ao_pyro_fired;
 				ao_log_mega(&log);
 				next_other = log.tick + AO_OTHER_INTERVAL;
 			}
@@ -181,6 +183,7 @@ ao_log(void)
 			ao_sleep(&ao_log_running);
 	}
 }
+#endif
 
 uint16_t
 ao_log_flight(uint8_t slot)
diff --git a/src/core/ao_log_metrum.c b/src/core/ao_log_metrum.c
new file mode 100644
index 00000000..43441e7a
--- /dev/null
+++ b/src/core/ao_log_metrum.c
@@ -0,0 +1,175 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "ao.h"
+#include <ao_log.h>
+#include <ao_data.h>
+#include <ao_flight.h>
+
+static __xdata uint8_t	ao_log_mutex;
+static __xdata struct ao_log_metrum log;
+
+__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMETRUM;
+
+static uint8_t
+ao_log_csum(__xdata uint8_t *b) __reentrant
+{
+	uint8_t	sum = 0x5a;
+	uint8_t	i;
+
+	for (i = 0; i < sizeof (struct ao_log_metrum); i++)
+		sum += *b++;
+	return -sum;
+}
+
+uint8_t
+ao_log_metrum(__xdata struct ao_log_metrum *log) __reentrant
+{
+	uint8_t wrote = 0;
+	/* set checksum */
+	log->csum = 0;
+	log->csum = ao_log_csum((__xdata uint8_t *) log);
+	ao_mutex_get(&ao_log_mutex); {
+		if (ao_log_current_pos >= ao_log_end_pos && ao_log_running)
+			ao_log_stop();
+		if (ao_log_running) {
+			wrote = 1;
+			ao_storage_write(ao_log_current_pos,
+					 log,
+					 sizeof (struct ao_log_metrum));
+			ao_log_current_pos += sizeof (struct ao_log_metrum);
+		}
+	} ao_mutex_put(&ao_log_mutex);
+	return wrote;
+}
+
+static uint8_t
+ao_log_dump_check_data(void)
+{
+	if (ao_log_csum((uint8_t *) &log) != 0)
+		return 0;
+	return 1;
+}
+
+#if HAS_ADC
+static __data uint8_t	ao_log_data_pos;
+
+/* a hack to make sure that ao_log_metrums fill the eeprom block in even units */
+typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_metrum))] ;
+
+#ifndef AO_SENSOR_INTERVAL_ASCENT
+#define AO_SENSOR_INTERVAL_ASCENT	1
+#define AO_SENSOR_INTERVAL_DESCENT	10
+#define AO_OTHER_INTERVAL		32
+#endif
+
+void
+ao_log(void)
+{
+	__pdata uint16_t	next_sensor, next_other;
+	uint8_t			i;
+
+	ao_storage_setup();
+
+	ao_log_scan();
+
+	while (!ao_log_running)
+		ao_sleep(&ao_log_running);
+
+#if HAS_FLIGHT
+	log.type = AO_LOG_FLIGHT;
+	log.tick = ao_sample_tick;
+#if HAS_ACCEL
+	log.u.flight.ground_accel = ao_ground_accel;
+#endif
+	log.u.flight.ground_pres = ao_ground_pres;
+	log.u.flight.flight = ao_flight_number;
+	ao_log_metrum(&log);
+#endif
+
+	/* Write the whole contents of the ring to the log
+	 * when starting up.
+	 */
+	ao_log_data_pos = ao_data_ring_next(ao_data_head);
+	next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick;
+	ao_log_state = ao_flight_startup;
+	for (;;) {
+		/* Write samples to EEPROM */
+		while (ao_log_data_pos != ao_data_head) {
+			log.tick = ao_data_ring[ao_log_data_pos].tick;
+			if ((int16_t) (log.tick - next_sensor) >= 0) {
+				log.type = AO_LOG_SENSOR;
+#if HAS_MS5607
+				log.u.sensor.pres = ao_data_ring[ao_log_data_pos].ms5607_raw.pres;
+				log.u.sensor.temp = ao_data_ring[ao_log_data_pos].ms5607_raw.temp;
+#endif
+				log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]);
+				ao_log_metrum(&log);
+				if (ao_log_state <= ao_flight_coast)
+					next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT;
+				else
+					next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT;
+			}
+			if ((int16_t) (log.tick - next_other) >= 0) {
+				log.type = AO_LOG_TEMP_VOLT;
+				log.u.volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt;
+				log.u.volt.sense_a = ao_data_ring[ao_log_data_pos].adc.sense_a;
+				log.u.volt.sense_m = ao_data_ring[ao_log_data_pos].adc.sense_m;
+				ao_log_metrum(&log);
+				next_other = log.tick + AO_OTHER_INTERVAL;
+			}
+			ao_log_data_pos = ao_data_ring_next(ao_log_data_pos);
+		}
+#if HAS_FLIGHT
+		/* Write state change to EEPROM */
+		if (ao_flight_state != ao_log_state) {
+			ao_log_state = ao_flight_state;
+			log.type = AO_LOG_STATE;
+			log.tick = ao_time();
+			log.u.state.state = ao_log_state;
+			log.u.state.reason = 0;
+			ao_log_metrum(&log);
+
+			if (ao_log_state == ao_flight_landed)
+				ao_log_stop();
+		}
+#endif
+
+		ao_log_flush();
+
+		/* Wait for a while */
+		ao_delay(AO_MS_TO_TICKS(100));
+
+		/* Stop logging when told to */
+		while (!ao_log_running)
+			ao_sleep(&ao_log_running);
+	}
+}
+#endif
+
+uint16_t
+ao_log_flight(uint8_t slot)
+{
+	if (!ao_storage_read(ao_log_pos(slot),
+			     &log,
+			     sizeof (struct ao_log_metrum)))
+		return 0;
+
+	if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT)
+		return log.u.flight.flight;
+	return 0;
+}
diff --git a/src/micropeak/ao_log_micro.c b/src/core/ao_log_micro.c
index d665efb5..d665efb5 100644
--- a/src/micropeak/ao_log_micro.c
+++ b/src/core/ao_log_micro.c
diff --git a/src/micropeak/ao_log_micro.h b/src/core/ao_log_micro.h
index 976852ee..976852ee 100644
--- a/src/micropeak/ao_log_micro.h
+++ b/src/core/ao_log_micro.h
diff --git a/src/core/ao_log_mini.c b/src/core/ao_log_mini.c
new file mode 100644
index 00000000..99a85982
--- /dev/null
+++ b/src/core/ao_log_mini.c
@@ -0,0 +1,162 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "ao.h"
+#include <ao_log.h>
+#include <ao_data.h>
+#include <ao_flight.h>
+
+static __xdata uint8_t	ao_log_mutex;
+static __xdata struct ao_log_mini log;
+
+__code uint8_t ao_log_format = AO_LOG_FORMAT;
+
+static uint8_t
+ao_log_csum(__xdata uint8_t *b) __reentrant
+{
+	uint8_t	sum = 0x5a;
+	uint8_t	i;
+
+	for (i = 0; i < sizeof (struct ao_log_mini); i++)
+		sum += *b++;
+	return -sum;
+}
+
+uint8_t
+ao_log_mini(__xdata struct ao_log_mini *log) __reentrant
+{
+	uint8_t wrote = 0;
+	/* set checksum */
+	log->csum = 0;
+	log->csum = ao_log_csum((__xdata uint8_t *) log);
+	ao_mutex_get(&ao_log_mutex); {
+		if (ao_log_current_pos >= ao_log_end_pos && ao_log_running)
+			ao_log_stop();
+		if (ao_log_running) {
+			wrote = 1;
+			ao_storage_write(ao_log_current_pos,
+					 log,
+					 sizeof (struct ao_log_mini));
+			ao_log_current_pos += sizeof (struct ao_log_mini);
+		}
+	} ao_mutex_put(&ao_log_mutex);
+	return wrote;
+}
+
+static uint8_t
+ao_log_dump_check_data(void)
+{
+	if (ao_log_csum((uint8_t *) &log) != 0)
+		return 0;
+	return 1;
+}
+
+static __data uint8_t	ao_log_data_pos;
+
+/* a hack to make sure that ao_log_minis fill the eeprom block in even units */
+typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_mini))] ;
+
+#ifndef AO_SENSOR_INTERVAL_ASCENT
+#define AO_SENSOR_INTERVAL_ASCENT	1
+#define AO_SENSOR_INTERVAL_DESCENT	10
+#endif
+
+void
+ao_log(void)
+{
+	__pdata uint16_t	next_sensor, next_other;
+
+	ao_storage_setup();
+
+	ao_log_scan();
+
+	while (!ao_log_running)
+		ao_sleep(&ao_log_running);
+
+#if HAS_FLIGHT
+	log.type = AO_LOG_FLIGHT;
+	log.tick = ao_sample_tick;
+	log.u.flight.flight = ao_flight_number;
+	log.u.flight.ground_pres = ao_ground_pres;
+	ao_log_mini(&log);
+#endif
+
+	/* Write the whole contents of the ring to the log
+	 * when starting up.
+	 */
+	ao_log_data_pos = ao_data_ring_next(ao_data_head);
+	next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick;
+	ao_log_state = ao_flight_startup;
+	for (;;) {
+		/* Write samples to EEPROM */
+		while (ao_log_data_pos != ao_data_head) {
+			log.tick = ao_data_ring[ao_log_data_pos].tick;
+			if ((int16_t) (log.tick - next_sensor) >= 0) {
+				log.type = AO_LOG_SENSOR;
+				ao_log_pack24(log.u.sensor.pres,
+					      ao_data_ring[ao_log_data_pos].ms5607_raw.pres);
+				ao_log_pack24(log.u.sensor.temp,
+					      ao_data_ring[ao_log_data_pos].ms5607_raw.temp);
+				log.u.sensor.sense_a = ao_data_ring[ao_log_data_pos].adc.sense_a;
+				log.u.sensor.sense_m = ao_data_ring[ao_log_data_pos].adc.sense_m;
+				log.u.sensor.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt;
+				ao_log_mini(&log);
+				if (ao_log_state <= ao_flight_coast)
+					next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT;
+				else
+					next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT;
+			}
+			ao_log_data_pos = ao_data_ring_next(ao_log_data_pos);
+		}
+#if HAS_FLIGHT
+		/* Write state change to EEPROM */
+		if (ao_flight_state != ao_log_state) {
+			ao_log_state = ao_flight_state;
+			log.type = AO_LOG_STATE;
+			log.tick = ao_time();
+			log.u.state.state = ao_log_state;
+			log.u.state.reason = 0;
+			ao_log_mini(&log);
+
+			if (ao_log_state == ao_flight_landed)
+				ao_log_stop();
+		}
+#endif
+
+		ao_log_flush();
+
+		/* Wait for a while */
+		ao_delay(AO_MS_TO_TICKS(100));
+
+		/* Stop logging when told to */
+		while (!ao_log_running)
+			ao_sleep(&ao_log_running);
+	}
+}
+
+uint16_t
+ao_log_flight(uint8_t slot)
+{
+	if (!ao_storage_read(ao_log_pos(slot),
+			     &log,
+			     sizeof (struct ao_log_mini)))
+		return 0;
+
+	if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT)
+		return log.u.flight.flight;
+	return 0;
+}
diff --git a/src/core/ao_log_telem.c b/src/core/ao_log_telem.c
index 23ebf7dd..095aca37 100644
--- a/src/core/ao_log_telem.c
+++ b/src/core/ao_log_telem.c
@@ -23,7 +23,7 @@ __code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMETRY;
 
 static __data uint8_t			ao_log_monitor_pos;
 __pdata enum ao_flight_state		ao_flight_state;
-__pdata int16_t				ao_max_height;	/* max of ao_height */
+__xdata int16_t				ao_max_height;	/* max of ao_height */
 __pdata int16_t				sense_d, sense_m;
 __pdata uint8_t				ao_igniter_present;
 
diff --git a/src/core/ao_log_tiny.c b/src/core/ao_log_tiny.c
index 492658ea..67767dc9 100644
--- a/src/core/ao_log_tiny.c
+++ b/src/core/ao_log_tiny.c
@@ -105,7 +105,7 @@ ao_log(void)
 		 */
 		ao_sleep(DATA_TO_XDATA(&ao_sample_data));
 		while (ao_log_data != ao_sample_data) {
-			sum += ao_data_ring[ao_log_data].adc.pres;
+			sum += ao_data_pres(&ao_data_ring[ao_log_data]);
 			count++;
 			ao_log_data = ao_data_ring_next(ao_log_data);
 		}
diff --git a/src/micropeak/ao_microflight.c b/src/core/ao_microflight.c
index 714bb90a..f680e400 100644
--- a/src/micropeak/ao_microflight.c
+++ b/src/core/ao_microflight.c
@@ -33,7 +33,7 @@ ao_microsample(void)
 	ao_microkalman_correct();
 }
 
-#define NUM_PA_HIST	16
+#define NUM_PA_HIST	(GROUND_AVG)
 
 #define SKIP_PA_HIST(i,j)	(((i) + (j)) & (NUM_PA_HIST - 1))
 
@@ -60,11 +60,11 @@ ao_microflight(void)
 	h = 0;
 	for (;;) {
 		time += SAMPLE_SLEEP;
-		if (sample_count == 0)
+		if ((sample_count & 0x1f) == 0)
 			ao_led_on(AO_LED_REPORT);
 		ao_delay_until(time);
 		ao_microsample();
-		if (sample_count == 0)
+		if ((sample_count & 0x1f) == 0)
 			ao_led_off(AO_LED_REPORT);
 		pa_hist[h] = pa;
 		h = SKIP_PA_HIST(h,1);
@@ -85,10 +85,10 @@ ao_microflight(void)
 		}
 	}
 
-	/* Go back and find the first sample a decent interval above the ground */
+	/* Go back and find the last sample close to the ground */
 	pa_min = pa_ground - LAND_DETECT;
-	for (i = SKIP_PA_HIST(h,2); i != h; i = SKIP_PA_HIST(i,2)) {
-		if (pa_hist[i] < pa_min)
+	for (i = SKIP_PA_HIST(h,-2); i != SKIP_PA_HIST(h,2); i = SKIP_PA_HIST(i,-2)) {
+		if (pa_hist[i] >= pa_min)
 			break;
 	}
 
diff --git a/src/micropeak/ao_microkalman.c b/src/core/ao_microkalman.c
index 0684ea2b..0684ea2b 100644
--- a/src/micropeak/ao_microkalman.c
+++ b/src/core/ao_microkalman.c
diff --git a/src/core/ao_product.c b/src/core/ao_product.c
index ec91b978..b9327bac 100644
--- a/src/core/ao_product.c
+++ b/src/core/ao_product.c
@@ -27,6 +27,12 @@ const char ao_product[] = AO_iProduct_STRING;
 #define LE_WORD(x)    ((x)&0xFF),((uint8_t) (((uint16_t) (x))>>8))
 
 #if HAS_USB
+
+/* Maximum power in mA */
+#ifndef AO_USB_MAX_POWER
+#define AO_USB_MAX_POWER	100
+#endif
+
 #include "ao_usb.h"
 /* USB descriptors in one giant block of bytes */
 AO_ROMCONFIG_SYMBOL(0x00aa) uint8_t ao_usb_descriptors [] =
@@ -55,7 +61,7 @@ AO_ROMCONFIG_SYMBOL(0x00aa) uint8_t ao_usb_descriptors [] =
 	0x01,			/*  bConfigurationValue */
 	0x00,			/*  iConfiguration */
 	0xC0,			/*  bmAttributes */
-	0x32,			/*  bMaxPower */
+	AO_USB_MAX_POWER >> 1,	/*  bMaxPower, 2mA units */
 
 	/* Control class interface */
 	0x09,
diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c
index aac8fda5..a260aa99 100644
--- a/src/core/ao_pyro.c
+++ b/src/core/ao_pyro.c
@@ -15,10 +15,12 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  */
 
+#ifndef AO_FLIGHT_TEST
 #include <ao.h>
-#include <ao_pyro.h>
 #include <ao_sample.h>
 #include <ao_flight.h>
+#endif
+#include <ao_pyro.h>
 
 #if IS_COMPANION
 #include <ao_companion.h>
@@ -31,6 +33,42 @@
 
 #define ao_lowbit(x)	((x) & (-x))
 
+#ifndef AO_FLIGHT_TEST
+enum ao_igniter_status
+ao_pyro_status(uint8_t p)
+{
+	__xdata struct ao_data packet;
+	__pdata int16_t value;
+
+	ao_arch_critical(
+		ao_data_get(&packet);
+		);
+
+	value = (AO_IGNITER_CLOSED>>1);
+	value = AO_SENSE_PYRO(&packet, p);
+	if (value < AO_IGNITER_OPEN)
+		return ao_igniter_open;
+	else if (value > AO_IGNITER_CLOSED)
+		return ao_igniter_ready;
+	else
+		return ao_igniter_unknown;
+}
+
+void
+ao_pyro_print_status(void)
+{
+	uint8_t	p;
+
+	for(p = 0; p < AO_PYRO_NUM; p++) {
+		enum ao_igniter_status status = ao_pyro_status(p);
+		printf("Igniter: %d Status: %s\n",
+		       p, ao_igniter_status_names[status]);
+	}
+}
+#endif
+
+uint16_t	ao_pyro_fired;
+
 /*
  * Given a pyro structure, figure out
  * if the current flight state satisfies all
@@ -75,13 +113,13 @@ ao_pyro_ready(struct ao_pyro *pyro)
 				continue;
 			break;
 
-#if HAS_ORIENT
+#if HAS_GYRO
 		case ao_pyro_orient_less:
-			if (ao_orient <= pyro->orient_less)
+			if (ao_sample_orient <= pyro->orient_less)
 				continue;
 			break;
 		case ao_pyro_orient_greater:
-			if (ao_orient >= pyro->orient_greater)
+			if (ao_sample_orient >= pyro->orient_greater)
 				continue;
 			break;
 #endif
@@ -130,107 +168,116 @@ ao_pyro_ready(struct ao_pyro *pyro)
 	return TRUE;
 }
 
-#define ao_pyro_fire_port(port, bit, pin) do {	\
-		ao_gpio_set(port, bit, pin, 1);	\
-		ao_delay(AO_MS_TO_TICKS(50));	\
-		ao_gpio_set(port, bit, pin, 0);	\
-	} while (0)
-
-
+#ifndef AO_FLIGHT_TEST
 static void
-ao_pyro_fire(uint8_t p)
+ao_pyro_pin_set(uint8_t p, uint8_t v)
 {
 	switch (p) {
 #if AO_PYRO_NUM > 0
-	case 0: ao_pyro_fire_port(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0); break;
+	case 0: ao_gpio_set(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0, v); break;
 #endif
 #if AO_PYRO_NUM > 1
-	case 1: ao_pyro_fire_port(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1); break;
+	case 1: ao_gpio_set(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, v); break;
 #endif
 #if AO_PYRO_NUM > 2
-	case 2: ao_pyro_fire_port(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2); break;
+	case 2: ao_gpio_set(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, v); break;
 #endif
 #if AO_PYRO_NUM > 3
-	case 3: ao_pyro_fire_port(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3); break;
+	case 3: ao_gpio_set(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, v); break;
 #endif
 #if AO_PYRO_NUM > 4
-	case 4: ao_pyro_fire_port(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4); break;
+	case 4: ao_gpio_set(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, v); break;
 #endif
 #if AO_PYRO_NUM > 5
-	case 5: ao_pyro_fire_port(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5); break;
+	case 5: ao_gpio_set(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, v); break;
 #endif
 #if AO_PYRO_NUM > 6
-	case 6: ao_pyro_fire_port(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6); break;
+	case 6: ao_gpio_set(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, v); break;
 #endif
 #if AO_PYRO_NUM > 7
-	case 7: ao_pyro_fire_port(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7); break;
+	case 7: ao_gpio_set(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, v); break;
 #endif
 	default: break;
 	}
-	ao_delay(AO_MS_TO_TICKS(50));
 }
+#endif
 
 uint8_t	ao_pyro_wakeup;
 
 static void
-ao_pyro(void)
+ao_pyro_pins_fire(uint16_t fire)
 {
-	uint8_t		p, any_waiting;
-	struct ao_pyro	*pyro;
+	uint8_t p;
 
-	ao_config_get();
-	while (ao_flight_state < ao_flight_boost)
-		ao_sleep(&ao_flight_state);
+	for (p = 0; p < AO_PYRO_NUM; p++) {
+		if (fire & (1 << p))
+			ao_pyro_pin_set(p, 1);
+	}
+	ao_delay(AO_MS_TO_TICKS(50));
+	for (p = 0; p < AO_PYRO_NUM; p++) {
+		if (fire & (1 << p)) {
+			ao_pyro_pin_set(p, 0);
+			ao_config.pyro[p].fired = 1;
+			ao_pyro_fired |= (1 << p);
+		}
+	}
+	ao_delay(AO_MS_TO_TICKS(50));
+}
 
-	for (;;) {
-		ao_alarm(AO_MS_TO_TICKS(100));
-		ao_sleep(&ao_pyro_wakeup);
-		ao_clear_alarm();
-		any_waiting = 0;
-		for (p = 0; p < AO_PYRO_NUM; p++) {
-			pyro = &ao_config.pyro[p];
+static uint8_t
+ao_pyro_check(void)
+{
+	struct ao_pyro	*pyro;
+	uint8_t		p, any_waiting;
+	uint16_t	fire = 0;
+	
+	any_waiting = 0;
+	for (p = 0; p < AO_PYRO_NUM; p++) {
+		pyro = &ao_config.pyro[p];
 
-			/* Ignore igniters which have already fired
-			 */
-			if (pyro->fired)
-				continue;
+		/* Ignore igniters which have already fired
+		 */
+		if (pyro->fired)
+			continue;
 
-			/* Ignore disabled igniters
-			 */
-			if (!pyro->flags)
-				continue;
+		/* Ignore disabled igniters
+		 */
+		if (!pyro->flags)
+			continue;
 
-			any_waiting = 1;
-			/* Check pyro state to see if it shoule fire
-			 */
-			if (!pyro->delay_done) {
-				if (!ao_pyro_ready(pyro))
-					continue;
-
-				/* If there's a delay set, then remember when
-				 * it expires
-				 */
-				if (pyro->flags & ao_pyro_delay)
-					pyro->delay_done = ao_time() + pyro->delay;
-			}
+		any_waiting = 1;
+		/* Check pyro state to see if it should fire
+		 */
+		if (!pyro->delay_done) {
+			if (!ao_pyro_ready(pyro))
+				continue;
 
-			/* Check to see if we're just waiting for
-			 * the delay to expire
+			/* If there's a delay set, then remember when
+			 * it expires
 			 */
-			if (pyro->delay_done) {
-				if ((int16_t) (ao_time() - pyro->delay_done) < 0)
-					continue;
+			if (pyro->flags & ao_pyro_delay) {
+				pyro->delay_done = ao_time() + pyro->delay;
+				if (!pyro->delay_done)
+					pyro->delay_done = 1;
 			}
+		}
 
-			ao_pyro_fire(p);
+		/* Check to see if we're just waiting for
+		 * the delay to expire
+		 */
+		if (pyro->delay_done) {
+			if ((int16_t) (ao_time() - pyro->delay_done) < 0)
+				continue;
 		}
-		if (!any_waiting)
-			break;
+
+		fire |= (1 << p);
 	}
-	ao_exit();
-}
 
-__xdata struct ao_task ao_pyro_task;
+	if (fire)
+		ao_pyro_pins_fire(fire);
+
+	return any_waiting;
+}
 
 #define NO_VALUE	0xff
 
@@ -263,7 +310,7 @@ const struct {
 	{ "h<",	ao_pyro_height_less,	offsetof(struct ao_pyro, height_less), HELP("height less (m)") },
 	{ "h>",	ao_pyro_height_greater,	offsetof(struct ao_pyro, height_greater), HELP("height greater (m)") },
 
-#if HAS_ORIENT
+#if HAS_GYRO
 	{ "o<",	ao_pyro_orient_less,	offsetof(struct ao_pyro, orient_less), HELP("orient less (deg)") },
 	{ "o>",	ao_pyro_orient_greater,	offsetof(struct ao_pyro, orient_greater), HELP("orient greater (deg)")  },
 #endif
@@ -283,6 +330,34 @@ const struct {
 	{ "", ao_pyro_none,		NO_VALUE, HELP(NULL) },
 };
 
+#define NUM_PYRO_VALUES (sizeof ao_pyro_values / sizeof ao_pyro_values[0])
+
+#ifndef AO_FLIGHT_TEST
+static void
+ao_pyro(void)
+{
+	uint8_t		any_waiting;
+
+	ao_config_get();
+	while (ao_flight_state < ao_flight_boost)
+		ao_sleep(&ao_flight_state);
+
+	for (;;) {
+		ao_alarm(AO_MS_TO_TICKS(100));
+		ao_sleep(&ao_pyro_wakeup);
+		ao_clear_alarm();
+		if (ao_flight_state >= ao_flight_landed)
+			break;
+		any_waiting = ao_pyro_check();
+		if (!any_waiting)
+			break;
+	}
+	ao_exit();
+}
+
+__xdata struct ao_task ao_pyro_task;
+
+
 static void
 ao_pyro_print_name(uint8_t v)
 {
@@ -400,25 +475,17 @@ ao_pyro_set(void)
 	_ao_config_edit_finish();
 }
 
-static void
-ao_pyro_manual(void)
+void
+ao_pyro_manual(uint8_t p)
 {
-	ao_cmd_white();
-	if (!ao_match_word("DoIt"))
-		return;
-	ao_cmd_white();
-	ao_cmd_decimal();
-	if (ao_cmd_lex_i < 0 || AO_PYRO_NUM <= ao_cmd_lex_i)
+	printf ("ao_pyro_manual %d\n", p);
+	if (p >= AO_PYRO_NUM) {
+		ao_cmd_status = ao_cmd_syntax_error;
 		return;
-	ao_pyro_fire(ao_cmd_lex_i);
-
+	}
+	ao_pyro_pins_fire(1 << p);
 }
 
-const struct ao_cmds ao_pyro_cmds[] = {
-	{ ao_pyro_manual,	"P DoIt <n>\0Fire igniter" },
-	{ 0, NULL }
-};
-
 void
 ao_pyro_init(void)
 {
@@ -446,6 +513,6 @@ ao_pyro_init(void)
 #if AO_PYRO_NUM > 7
 	ao_enable_output(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, 0);
 #endif
-	ao_cmd_register(&ao_pyro_cmds[0]);
 	ao_add_task(&ao_pyro_task, ao_pyro, "pyro");
 }
+#endif
diff --git a/src/core/ao_pyro.h b/src/core/ao_pyro.h
index cde850ad..0c5642d6 100644
--- a/src/core/ao_pyro.h
+++ b/src/core/ao_pyro.h
@@ -63,6 +63,8 @@ struct ao_pyro {
 
 extern uint8_t	ao_pyro_wakeup;
 
+extern uint16_t	ao_pyro_fired;
+
 void
 ao_pyro_set(void);
 
@@ -72,4 +74,10 @@ ao_pyro_show(void);
 void
 ao_pyro_init(void);
 
+void
+ao_pyro_manual(uint8_t p);
+
+void
+ao_pyro_print_status(void);
+
 #endif
diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h
new file mode 100644
index 00000000..044f1607
--- /dev/null
+++ b/src/core/ao_quaternion.h
@@ -0,0 +1,249 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_QUATERNION_H_
+#define _AO_QUATERNION_H_
+
+#include <math.h>
+
+struct ao_quaternion {
+	float	r;		/* real bit */
+	float	x, y, z;	/* imaginary bits */
+};
+
+static inline void ao_quaternion_multiply(struct ao_quaternion *r,
+					  const struct ao_quaternion *a,
+					  const struct ao_quaternion *b)
+{
+	struct ao_quaternion	t;
+#define T(_a,_b)	(((a)->_a) * ((b)->_b))
+
+/*
+ * Quaternions
+ *
+ *	ii = jj = kk = ijk = -1;
+ *
+ *	kji = 1;
+ *
+ * 	ij = k;		ji = -k;
+ *	kj = -i;	jk = i;
+ *	ik = -j;	ki = j;
+ *
+ * Multiplication p * q:
+ *
+ *	(pr + ipx + jpy + kpz) (qr + iqx + jqy + kqz) =
+ *
+ *		( pr * qr +  pr * iqx +  pr * jqy +  pr * kqz) +
+ *		(ipx * qr + ipx * iqx + ipx * jqy + ipx * kqz) +
+ *		(jpy * qr + jpy * iqx + jpy * jqy + jpy * kqz) +
+ *		(kpz * qr + kpz * iqx + kpz * jqy + kpz * kqz) =
+ *
+ *
+ *		 (pr * qr) + i(pr * qx) + j(pr * qy) + k(pr * qz) +
+ *		i(px * qr) -  (px * qx) + k(px * qy) - j(px * qz) +
+ *		j(py * qr) - k(py * qx) -  (py * qy) + i(py * qz) +
+ *		k(pz * qr) + j(pz * qx) - i(pz * qy) -  (pz * qz) =
+ *
+ *		1 * ( (pr * qr) - (px * qx) - (py * qy) - (pz * qz) ) +
+ *		i * ( (pr * qx) + (px * qr) + (py * qz) - (pz * qy) ) +
+ *		j * ( (pr * qy) - (px * qz) + (py * qr) + (pz * qx) ) +
+ *		k * ( (pr * qz) + (px * qy) - (py * qx) + (pz * qr);
+ */
+
+	t.r = T(r,r) - T(x,x) - T(y,y) - T(z,z);
+	t.x = T(r,x) + T(x,r) + T(y,z) - T(z,y);
+	t.y = T(r,y) - T(x,z) + T(y,r) + T(z,x);
+	t.z = T(r,z) + T(x,y) - T(y,x) + T(z,r);
+#undef T
+	*r = t;
+}
+
+static inline void ao_quaternion_conjugate(struct ao_quaternion *r,
+					   const struct ao_quaternion *a)
+{
+	r->r = a->r;
+	r->x = -a->x;
+	r->y = -a->y;
+	r->z = -a->z;
+}
+
+static inline float ao_quaternion_normal(const struct ao_quaternion *a)
+{
+#define S(_a)	(((a)->_a) * ((a)->_a))
+	return S(r) + S(x) + S(y) + S(z);
+#undef S
+}
+
+static inline void ao_quaternion_scale(struct ao_quaternion *r,
+				       const struct ao_quaternion *a,
+				       float b)
+{
+	r->r = a->r * b;
+	r->x = a->x * b;
+	r->y = a->y * b;
+	r->z = a->z * b;
+}
+
+static inline void ao_quaternion_normalize(struct ao_quaternion *r,
+					   const struct ao_quaternion *a)
+{
+	float	n = ao_quaternion_normal(a);
+
+	if (n > 0)
+		ao_quaternion_scale(r, a, 1/sqrtf(n));
+	else
+		*r = *a;
+}
+
+static inline float ao_quaternion_dot(const struct ao_quaternion *a,
+				      const struct ao_quaternion *b)
+{
+#define T(_a)	(((a)->_a) * ((b)->_a))
+	return T(r) + T(x) + T(y) + T(z);
+#undef T
+}
+				     
+
+static inline void ao_quaternion_rotate(struct ao_quaternion *r,
+					const struct ao_quaternion *a,
+					const struct ao_quaternion *b)
+{
+	struct ao_quaternion	c;
+	struct ao_quaternion	t;
+
+	ao_quaternion_multiply(&t, b, a);
+	ao_quaternion_conjugate(&c, b);
+	ao_quaternion_multiply(r, &t, &c);
+}
+
+/*
+ * Compute a rotation quaternion between two vectors
+ *
+ *	cos(θ) + u * sin(θ)
+ *
+ * where θ is the angle between the two vectors and u
+ * is a unit vector axis of rotation
+ */
+
+static inline void ao_quaternion_vectors_to_rotation(struct ao_quaternion *r,
+						     const struct ao_quaternion *a,
+						     const struct ao_quaternion *b)
+{
+	/*
+	 * The cross product will point orthogonally to the two
+	 * vectors, forming our rotation axis. The length will be
+	 * sin(θ), so these values are already multiplied by that.
+	 */
+
+	float x = a->y * b->z - a->z * b->y;
+	float y = a->z * b->x - a->x * b->z;
+	float z = a->x * b->y - a->y * b->x;
+
+	float s_2 = x*x + y*y + z*z;
+	float s = sqrtf(s_2);
+
+	/* cos(θ) = a · b / (|a| |b|).
+	 *
+	 * a and b are both unit vectors, so the divisor is one
+	 */
+	float c = a->x*b->x + a->y*b->y + a->z*b->z;
+
+	float c_half = sqrtf ((1 + c) / 2);
+	float s_half = sqrtf ((1 - c) / 2);
+
+	/*
+	 * Divide out the sine factor from the
+	 * cross product, then multiply in the
+	 * half sine factor needed for the quaternion
+	 */
+	float s_scale = s_half / s;
+
+	r->x = x * s_scale;
+	r->y = y * s_scale;
+	r->z = z * s_scale;
+
+	r->r = c_half;
+
+	ao_quaternion_normalize(r, r);
+}
+
+static inline void ao_quaternion_init_vector(struct ao_quaternion *r,
+					     float x, float y, float z)
+{
+	r->r = 0;
+	r->x = x;
+	r->y = y;
+	r->z = z;
+}
+
+static inline void ao_quaternion_init_rotation(struct ao_quaternion *r,
+					       float x, float y, float z,
+					       float s, float c)
+{
+	r->r = c;
+	r->x = s * x;
+	r->y = s * y;
+	r->z = s * z;
+}
+
+static inline void ao_quaternion_init_zero_rotation(struct ao_quaternion *r)
+{
+	r->r = 1;
+	r->x = r->y = r->z = 0;
+}
+
+/*
+ * The sincosf from newlib just calls sinf and cosf. This is a bit
+ * faster, if slightly less precise
+ */
+
+static inline void
+ao_sincosf(float a, float *s, float *c) {
+	float	_s = sinf(a);
+	*s = _s;
+	*c = sqrtf(1 - _s*_s);
+}
+
+/*
+ * Initialize a quaternion from 1/2 euler rotation angles (in radians).
+ *
+ * Yes, it would be nicer if there were a faster way, but because we
+ * sample the gyros at only 100Hz, we end up getting angles too large
+ * to take advantage of sin(x) ≃ x.
+ *
+ * We might be able to use just a couple of elements of the sin taylor
+ * series though, instead of the whole sin function?
+ */
+
+static inline void ao_quaternion_init_half_euler(struct ao_quaternion *r,
+						 float x, float y, float z)
+{
+	float	s_x, c_x;
+	float	s_y, c_y;
+	float	s_z, c_z;
+
+	ao_sincosf(x, &s_x, &c_x);
+	ao_sincosf(y, &s_y, &c_y);
+	ao_sincosf(z, &s_z, &c_z);
+
+	r->r = c_x * c_y * c_z + s_x * s_y * s_z;
+	r->x = s_x * c_y * c_z - c_x * s_y * s_z;
+	r->y = c_x * s_y * c_z + s_x * c_y * s_z;
+	r->z = c_x * c_y * s_z - s_x * s_y * c_z;
+}
+
+#endif /* _AO_QUATERNION_H_ */
diff --git a/src/micropeak/ao_report_tiny.c b/src/core/ao_report_micro.c
index 0e8e287f..0e8e287f 100644
--- a/src/micropeak/ao_report_tiny.c
+++ b/src/core/ao_report_micro.c
diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c
index dec44f9f..adf8399d 100644
--- a/src/core/ao_sample.c
+++ b/src/core/ao_sample.c
@@ -20,6 +20,10 @@
 #include <ao_data.h>
 #endif
 
+#if HAS_GYRO
+#include <ao_quaternion.h>
+#endif
+
 /*
  * Current sensor values
  */
@@ -44,8 +48,7 @@ __pdata accel_t		ao_sample_accel_through;
 __pdata gyro_t		ao_sample_roll;
 __pdata gyro_t		ao_sample_pitch;
 __pdata gyro_t		ao_sample_yaw;
-__pdata angle_t		ao_sample_angle;
-__pdata angle_t		ao_sample_roll_angle;
+__pdata angle_t		ao_sample_orient;
 #endif
 
 __data uint8_t		ao_sample_data;
@@ -67,9 +70,9 @@ __pdata int32_t		ao_accel_scale;		/* sensor to m/s² conversion */
 __pdata accel_t		ao_ground_accel_along;
 __pdata accel_t		ao_ground_accel_across;
 __pdata accel_t		ao_ground_accel_through;
-__pdata gyro_t		ao_ground_pitch;
-__pdata gyro_t		ao_ground_yaw;
-__pdata gyro_t		ao_ground_roll;
+__pdata int32_t		ao_ground_pitch;
+__pdata int32_t		ao_ground_yaw;
+__pdata int32_t		ao_ground_roll;
 #endif
 
 static __pdata uint8_t	ao_preflight;		/* in preflight mode */
@@ -86,6 +89,7 @@ __pdata int32_t	ao_sample_accel_through_sum;
 __pdata int32_t ao_sample_pitch_sum;
 __pdata int32_t ao_sample_yaw_sum;
 __pdata int32_t	ao_sample_roll_sum;
+static struct ao_quaternion ao_rotation;
 #endif
 
 static void
@@ -120,20 +124,95 @@ ao_sample_preflight_set(void)
 	ao_ground_accel_along = ao_sample_accel_along_sum >> 9;
 	ao_ground_accel_across = ao_sample_accel_across_sum >> 9;
 	ao_ground_accel_through = ao_sample_accel_through_sum >> 9;
-	ao_ground_pitch = ao_sample_pitch_sum >> 9;
-	ao_ground_yaw = ao_sample_yaw_sum >> 9;
-	ao_ground_roll = ao_sample_roll_sum >> 9;
+	ao_ground_pitch = ao_sample_pitch_sum;
+	ao_ground_yaw = ao_sample_yaw_sum;
+	ao_ground_roll = ao_sample_roll_sum;
 	ao_sample_accel_along_sum = 0;
 	ao_sample_accel_across_sum = 0;
 	ao_sample_accel_through_sum = 0;
 	ao_sample_pitch_sum = 0;
 	ao_sample_yaw_sum = 0;
 	ao_sample_roll_sum = 0;
-	ao_sample_angle = 0;
+	ao_sample_orient = 0;
+
+	struct ao_quaternion	orient;
+
+	/* Take the pad IMU acceleration values and compute our current direction
+	 */
+
+	ao_quaternion_init_vector(&orient,
+				  (ao_ground_accel_across - ao_config.accel_zero_across),
+				  (ao_ground_accel_through - ao_config.accel_zero_through),
+				  (ao_ground_accel_along - ao_config.accel_zero_along));
+
+	ao_quaternion_normalize(&orient,
+				&orient);
+
+	/* Here's up */
+
+	struct ao_quaternion	up = { .r = 0, .x = 0, .y = 0, .z = 1 };
+
+	if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP)
+		up.z = -1;
+
+	/* Compute rotation to get from up to our current orientation, set
+	 * that as the current rotation vector
+	 */
+	ao_quaternion_vectors_to_rotation(&ao_rotation, &up, &orient);
 #endif	
 	nsamples = 0;
 }
 
+#if HAS_GYRO
+
+#define TIME_DIV	200.0f
+
+static void
+ao_sample_rotate(void)
+{
+#ifdef AO_FLIGHT_TEST
+	float	dt = (ao_sample_tick - ao_sample_prev_tick) / TIME_DIV;
+#else
+	static const float dt = 1/TIME_DIV;
+#endif
+	float	x = ao_mpu6000_gyro((float) ((ao_sample_pitch << 9) - ao_ground_pitch) / 512.0f) * dt;
+	float	y = ao_mpu6000_gyro((float) ((ao_sample_yaw << 9) - ao_ground_yaw) / 512.0f) * dt;
+	float	z = ao_mpu6000_gyro((float) ((ao_sample_roll << 9) - ao_ground_roll) / 512.0f) * dt;
+	struct ao_quaternion	rot;
+
+	ao_quaternion_init_half_euler(&rot, x, y, z);
+	ao_quaternion_multiply(&ao_rotation, &rot, &ao_rotation);
+
+	/* And normalize to make sure it remains a unit vector */
+	ao_quaternion_normalize(&ao_rotation, &ao_rotation);
+
+	/* Compute pitch angle from vertical by taking the pad
+	 * orientation vector and rotating it by the current total
+	 * rotation value. That will be a unit vector pointing along
+	 * the airframe axis. The Z value will be the cosine of the
+	 * change in the angle from vertical since boost.
+	 *
+	 * rot = ao_rotation * vertical * ao_rotation°
+	 * rot = ao_rotation * (0,0,0,1) * ao_rotation°
+	 *     = ((a.z, a.y, -a.x, a.r) * (a.r, -a.x, -a.y, -a.z)) .z
+	 *
+	 *     = (-a.z * -a.z) + (a.y * -a.y) - (-a.x * -a.x) + (a.r * a.r)
+	 *     = a.z² - a.y² - a.x² + a.r²
+	 *
+	 * rot = ao_rotation * (0, 0, 0, -1) * ao_rotation°
+	 *     = ((-a.z, -a.y, a.x, -a.r) * (a.r, -a.x, -a.y, -a.z)) .z
+	 *
+	 *     = (a.z * -a.z) + (-a.y * -a.y) - (a.x * -a.x) + (-a.r * a.r)
+	 *     = -a.z² + a.y² + a.x² - a.r²
+	 */
+
+	float rotz;
+	rotz = ao_rotation.z * ao_rotation.z - ao_rotation.y * ao_rotation.y - ao_rotation.x * ao_rotation.x + ao_rotation.r * ao_rotation.r;
+
+	ao_sample_orient = acosf(rotz) * (float) (180.0/M_PI);
+}
+#endif
+
 static void
 ao_sample_preflight(void)
 {
@@ -232,9 +311,12 @@ ao_sample(void)
 				ao_sample_preflight_update();
 			ao_kalman();
 #if HAS_GYRO
-			/* do quaternion stuff here... */
+			ao_sample_rotate();
 #endif
 		}
+#ifdef AO_FLIGHT_TEST
+		ao_sample_prev_tick = ao_sample_tick;
+#endif
 		ao_sample_data = ao_data_ring_next(ao_sample_data);
 	}
 	return !ao_preflight;
@@ -264,7 +346,7 @@ ao_sample_init(void)
 	ao_sample_pitch = 0;
 	ao_sample_yaw = 0;
 	ao_sample_roll = 0;
-	ao_sample_angle = 0;
+	ao_sample_orient = 0;
 #endif
 	ao_sample_data = ao_data_head;
 	ao_preflight = TRUE;
diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h
index a2dac979..16d4c507 100644
--- a/src/core/ao_sample.h
+++ b/src/core/ao_sample.h
@@ -64,8 +64,6 @@
  * for all further flight computations
  */
 
-#define GRAVITY 9.80665
-
 /*
  * Above this height, the baro sensor doesn't work
  */
@@ -115,9 +113,16 @@ extern __pdata int32_t	ao_accel_scale;		/* sensor to m/s² conversion */
 extern __pdata accel_t	ao_ground_accel_along;
 extern __pdata accel_t	ao_ground_accel_across;
 extern __pdata accel_t	ao_ground_accel_through;
-extern __pdata gyro_t	ao_ground_pitch;
-extern __pdata gyro_t	ao_ground_yaw;
-extern __pdata gyro_t	ao_ground_roll;
+extern __pdata int32_t	ao_ground_pitch;	/* * 512 */
+extern __pdata int32_t	ao_ground_yaw;		/* * 512 */
+extern __pdata int32_t	ao_ground_roll;		/* * 512 */
+extern __pdata accel_t	ao_sample_accel_along;
+extern __pdata accel_t	ao_sample_accel_across;
+extern __pdata accel_t	ao_sample_accel_through;
+extern __pdata gyro_t	ao_sample_roll;
+extern __pdata gyro_t	ao_sample_pitch;
+extern __pdata gyro_t	ao_sample_yaw;
+extern __pdata angle_t	ao_sample_orient;
 #endif
 
 void ao_sample_init(void);
@@ -136,8 +141,8 @@ uint8_t ao_sample(void);
 extern __pdata int16_t			ao_height;	/* meters */
 extern __pdata int16_t			ao_speed;	/* m/s * 16 */
 extern __pdata int16_t			ao_accel;	/* m/s² * 16 */
-extern __pdata int16_t			ao_max_height;	/* max of ao_height */
-extern __pdata int16_t			ao_avg_height;	/* running average of height */
+extern __xdata int16_t			ao_max_height;	/* max of ao_height */
+extern __xdata int16_t			ao_avg_height;	/* running average of height */
 
 extern __pdata int16_t			ao_error_h;
 extern __pdata int16_t			ao_error_h_sq_avg;
diff --git a/src/core/ao_sample_profile.c b/src/core/ao_sample_profile.c
index 1d9ed414..d3743d12 100644
--- a/src/core/ao_sample_profile.c
+++ b/src/core/ao_sample_profile.c
@@ -20,11 +20,11 @@
 #include <ao_task.h>
 
 #ifndef AO_SAMPLE_PROFILE_LOW_PC
-#define AO_SAMPLE_PROFILE_LOW_PC	0x08000000
+#define AO_SAMPLE_PROFILE_LOW_PC	0x08002000
 #endif
 
 #ifndef AO_SAMPLE_PROFILE_HIGH_PC
-#define AO_SAMPLE_PROFILE_HIGH_PC	(AO_SAMPLE_PROFILE_LOW_PC + 44 * 1024)
+#define AO_SAMPLE_PROFILE_HIGH_PC	0x0800f000
 #endif
 
 #ifndef AO_SAMPLE_PROFILE_SHIFT
diff --git a/src/core/ao_storage.c b/src/core/ao_storage.c
index adf7e4d4..6eddae7f 100644
--- a/src/core/ao_storage.c
+++ b/src/core/ao_storage.c
@@ -154,7 +154,7 @@ ao_storage_zapall(void) __reentrant
 	ao_cmd_white();
 	if (!ao_match_word("DoIt"))
 		return;
-	for (pos = 0; pos < ao_storage_config; pos += ao_storage_block)
+	for (pos = 0; pos < ao_storage_log_max; pos += ao_storage_block)
 		ao_storage_erase(pos);
 }
 
diff --git a/src/core/ao_storage.h b/src/core/ao_storage.h
index ea946399..6cc6fcb7 100644
--- a/src/core/ao_storage.h
+++ b/src/core/ao_storage.h
@@ -35,14 +35,22 @@ extern __pdata ao_pos_t	ao_storage_total;
 /* Block size - device is erased in these units. At least 256 bytes */
 extern __pdata ao_pos_t	ao_storage_block;
 
+#ifndef USE_STORAGE_CONFIG
+#define USE_STORAGE_CONFIG 1
+#endif
+
+#if USE_STORAGE_CONFIG
 /* Byte offset of config block. Will be ao_storage_block bytes long */
 extern __pdata ao_pos_t	ao_storage_config;
 
+#define ao_storage_log_max	ao_storage_config
+#else
+#define ao_storage_log_max	ao_storage_total
+#endif
+
 /* Storage unit size - device reads and writes must be within blocks of this size. Usually 256 bytes. */
 extern __pdata uint16_t ao_storage_unit;
 
-#define AO_STORAGE_ERASE_LOG	(ao_storage_config + AO_CONFIG_MAX_SIZE)
-
 /* Initialize above values. Can only be called once the OS is running */
 void
 ao_storage_setup(void) __reentrant;
diff --git a/src/core/ao_task.c b/src/core/ao_task.c
index 0aad6508..bafb4943 100644
--- a/src/core/ao_task.c
+++ b/src/core/ao_task.c
@@ -109,6 +109,8 @@ ao_task_validate_alarm_queue(void)
 				ao_panic(3);
 		}
 	}
+	if (ao_task_alarm_tick != ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm)
+		ao_panic(4);
 }
 #else
 #define ao_task_validate_alarm_queue()
@@ -123,6 +125,7 @@ ao_task_to_alarm_queue(struct ao_task *task)
 	ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) {
 		if ((int16_t) (alarm->alarm - task->alarm) >= 0) {
 			ao_list_insert(&task->alarm_queue, alarm->alarm_queue.prev);
+			ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm;
 			ao_task_validate_alarm_queue();
 			return;
 		}
@@ -420,7 +423,7 @@ ao_sleep(__xdata void *wchan)
 }
 
 void
-ao_wakeup(__xdata void *wchan)
+ao_wakeup(__xdata void *wchan) __reentrant
 {
 #if HAS_TASK_QUEUE
 	struct ao_task	*sleep, *next;
diff --git a/src/core/ao_task.h b/src/core/ao_task.h
index 1a4b5b6b..9c56b480 100644
--- a/src/core/ao_task.h
+++ b/src/core/ao_task.h
@@ -45,7 +45,10 @@ struct ao_task {
 #endif
 };
 
+#ifndef AO_NUM_TASKS
 #define AO_NUM_TASKS		16	/* maximum number of tasks */
+#endif
+
 #define AO_NO_TASK		0	/* no task id */
 
 extern __xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS];
@@ -67,7 +70,7 @@ ao_sleep(__xdata void *wchan);
 
 /* Wake all tasks sleeping on wchan */
 void
-ao_wakeup(__xdata void *wchan);
+ao_wakeup(__xdata void *wchan) __reentrant;
 
 /* set an alarm to go off in 'delay' ticks */
 void
diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c
index c3bbfec5..c118d007 100644
--- a/src/core/ao_telemetry.c
+++ b/src/core/ao_telemetry.c
@@ -19,9 +19,16 @@
 #include "ao_log.h"
 #include "ao_product.h"
 
+#ifndef HAS_RDF
+#define HAS_RDF 1
+#endif
+
 static __pdata uint16_t ao_telemetry_interval;
 static __pdata uint8_t ao_rdf = 0;
+
+#if HAS_RDF
 static __pdata uint16_t ao_rdf_time;
+#endif
 
 #if HAS_APRS
 static __pdata uint16_t ao_aprs_time;
@@ -33,6 +40,10 @@ static __pdata uint16_t ao_aprs_time;
 #define AO_SEND_MEGA	1
 #endif
 
+#if defined (TELEMETRUM_V_2_0)
+#define AO_SEND_METRUM	1
+#endif
+
 #if defined(TELEMETRUM_V_0_1) || defined(TELEMETRUM_V_0_2) || defined(TELEMETRUM_V_1_0) || defined(TELEMETRUM_V_1_1) || defined(TELEBALLOON_V_1_1) || defined(TELEMETRUM_V_1_2)
 #define AO_TELEMETRY_SENSOR	AO_TELEMETRY_SENSOR_TELEMETRUM
 #endif
@@ -104,6 +115,7 @@ ao_send_mega_sensor(void)
 	telemetry.generic.tick = packet->tick;
 	telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR;
 
+	telemetry.mega_sensor.orient = ao_sample_orient;
 	telemetry.mega_sensor.accel = ao_data_accel(packet);
 	telemetry.mega_sensor.pres = ao_data_pres(packet);
 	telemetry.mega_sensor.temp = ao_data_temp(packet);
@@ -164,6 +176,87 @@ ao_send_mega_data(void)
 }
 #endif /* AO_SEND_MEGA */
 
+#ifdef AO_SEND_METRUM
+/* Send telemetrum sensor packet */
+static void
+ao_send_metrum_sensor(void)
+{
+	__xdata	struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)];
+
+	telemetry.generic.tick = packet->tick;
+	telemetry.generic.type = AO_TELEMETRY_METRUM_SENSOR;
+
+	telemetry.metrum_sensor.state = ao_flight_state;
+	telemetry.metrum_sensor.accel = ao_data_accel(packet);
+	telemetry.metrum_sensor.pres = ao_data_pres(packet);
+	telemetry.metrum_sensor.temp = ao_data_temp(packet);
+
+	telemetry.metrum_sensor.acceleration = ao_accel;
+	telemetry.metrum_sensor.speed = ao_speed;
+	telemetry.metrum_sensor.height = ao_height;
+
+	telemetry.metrum_sensor.v_batt = packet->adc.v_batt;
+	telemetry.metrum_sensor.sense_a = packet->adc.sense_a;
+	telemetry.metrum_sensor.sense_m = packet->adc.sense_m;
+
+	ao_radio_send(&telemetry, sizeof (telemetry));
+}
+
+static __pdata int8_t ao_telemetry_metrum_data_max;
+static __pdata int8_t ao_telemetry_metrum_data_cur;
+
+/* Send telemetrum data packet */
+static void
+ao_send_metrum_data(void)
+{
+	if (--ao_telemetry_metrum_data_cur <= 0) {
+		__xdata	struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)];
+		uint8_t	i;
+
+		telemetry.generic.tick = packet->tick;
+		telemetry.generic.type = AO_TELEMETRY_METRUM_DATA;
+
+		telemetry.metrum_data.ground_pres = ao_ground_pres;
+		telemetry.metrum_data.ground_accel = ao_ground_accel;
+		telemetry.metrum_data.accel_plus_g = ao_config.accel_plus_g;
+		telemetry.metrum_data.accel_minus_g = ao_config.accel_minus_g;
+
+		ao_radio_send(&telemetry, sizeof (telemetry));
+		ao_telemetry_metrum_data_cur = ao_telemetry_metrum_data_max;
+	}
+}
+#endif /* AO_SEND_METRUM */
+
+#ifdef AO_SEND_MINI
+
+static void
+ao_send_mini(void)
+{
+	__xdata	struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)];
+			
+	telemetry.generic.tick = packet->tick;
+	telemetry.generic.type = AO_TELEMETRY_MINI;
+
+	telemetry.mini.state = ao_flight_state;
+
+	telemetry.mini.v_batt = packet->adc.v_batt;
+	telemetry.mini.sense_a = packet->adc.sense_a;
+	telemetry.mini.sense_m = packet->adc.sense_m;
+
+	telemetry.mini.pres = ao_data_pres(packet);
+	telemetry.mini.temp = ao_data_temp(packet);
+
+	telemetry.mini.acceleration = ao_accel;
+	telemetry.mini.speed = ao_speed;
+	telemetry.mini.height = ao_height;
+
+	telemetry.mini.ground_pres = ao_ground_pres;
+
+	ao_radio_send(&telemetry, sizeof (telemetry));
+}
+
+#endif /* AO_SEND_MINI */
+
 #ifdef AO_SEND_ALL_BARO
 static uint8_t		ao_baro_sample;
 
@@ -198,7 +291,11 @@ ao_send_configuration(void)
 	{
 		telemetry.generic.type = AO_TELEMETRY_CONFIGURATION;
 		telemetry.configuration.device = AO_idProduct_NUMBER;
+#if HAS_LOG
 		telemetry.configuration.flight = ao_log_full() ? 0 : ao_flight_number;
+#else
+		telemetry.configuration.flight = ao_flight_number;
+#endif
 		telemetry.configuration.config_major = AO_CONFIG_MAJOR;
 		telemetry.configuration.config_minor = AO_CONFIG_MINOR;
 		telemetry.configuration.apogee_delay = ao_config.apogee_delay;
@@ -295,12 +392,14 @@ ao_telemetry(void)
 	for (;;) {
 		while (ao_telemetry_interval == 0)
 			ao_sleep(&telemetry);
-		time = ao_rdf_time = ao_time();
+		time = ao_time();
+#if HAS_RDF
+		ao_rdf_time = time;
+#endif
 #if HAS_APRS
 		ao_aprs_time = time;
 #endif
 		while (ao_telemetry_interval) {
-
 #if HAS_APRS
 			if (!(ao_config.radio_enable & AO_RADIO_DISABLE_TELEMETRY))
 #endif
@@ -308,14 +407,23 @@ ao_telemetry(void)
 #ifdef AO_SEND_ALL_BARO
 				ao_send_baro();
 #endif
+
 #if HAS_FLIGHT
-#ifdef AO_SEND_MEGA
+# ifdef AO_SEND_MEGA
 				ao_send_mega_sensor();
 				ao_send_mega_data();
-#else
+# endif
+# ifdef AO_SEND_METRUM
+				ao_send_metrum_sensor();
+				ao_send_metrum_data();
+# endif
+# ifdef AO_SEND_MINI
+				ao_send_mini();
+# endif
+# ifdef AO_TELEMETRY_SENSOR
 				ao_send_sensor();
-#endif
-#endif
+# endif
+#endif /* HAS_FLIGHT */
 
 #if HAS_COMPANION
 				if (ao_companion_running)
@@ -328,23 +436,25 @@ ao_telemetry(void)
 #endif
 			}
 #ifndef AO_SEND_ALL_BARO
+#if HAS_RDF
 			if (ao_rdf &&
 #if HAS_APRS
 			    !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) &&
-#endif
+#endif /* HAS_APRS */
 			    (int16_t) (ao_time() - ao_rdf_time) >= 0)
 			{
 #if HAS_IGNITE_REPORT
 				uint8_t	c;
-#endif
+#endif /* HAS_IGNITE_REPORT */
 				ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS;
 #if HAS_IGNITE_REPORT
 				if (ao_flight_state == ao_flight_pad && (c = ao_report_igniter()))
 					ao_radio_continuity(c);
 				else
-#endif
+#endif /* HAS_IGNITE_REPORT*/
 					ao_radio_rdf();
 			}
+#endif /* HAS_RDF */
 #if HAS_APRS
 			if (ao_config.aprs_interval != 0 &&
 			    (int16_t) (ao_time() - ao_aprs_time) >= 0)
@@ -352,8 +462,8 @@ ao_telemetry(void)
 				ao_aprs_time = ao_time() + AO_SEC_TO_TICKS(ao_config.aprs_interval);
 				ao_aprs_send();
 			}
-#endif
-#endif
+#endif /* HAS_APRS */
+#endif /* !AO_SEND_ALL_BARO */
 			time += ao_telemetry_interval;
 			delay = time - ao_time();
 			if (delay > 0) {
@@ -383,6 +493,12 @@ ao_telemetry_set_interval(uint16_t interval)
 		cur++;
 	ao_telemetry_mega_data_cur = cur;
 #endif
+#if AO_SEND_METRUM
+	ao_telemetry_metrum_data_max = AO_SEC_TO_TICKS(1) / interval;
+	if (ao_telemetry_metrum_data_max > cur)
+		cur++;
+	ao_telemetry_metrum_data_cur = cur;
+#endif
 
 #if HAS_COMPANION
 	if (!ao_companion_setup.update_period)
@@ -411,6 +527,7 @@ ao_telemetry_set_interval(uint16_t interval)
 	ao_wakeup(&telemetry);
 }
 
+#if HAS_RDF
 void
 ao_rdf_set(uint8_t rdf)
 {
@@ -421,6 +538,7 @@ ao_rdf_set(uint8_t rdf)
 		ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS;
 	}
 }
+#endif
 
 __xdata struct ao_task	ao_telemetry_task;
 
diff --git a/src/core/ao_telemetry.h b/src/core/ao_telemetry.h
index f2d201de..237a35ab 100644
--- a/src/core/ao_telemetry.h
+++ b/src/core/ao_telemetry.h
@@ -162,7 +162,7 @@ struct ao_telemetry_mega_sensor {
 	uint16_t	tick;		/*  2 */
 	uint8_t		type;		/*  4 */
 
-	uint8_t		pad5;		/*  5 */
+	uint8_t		orient;		/*  5 angle from vertical */
 	int16_t		accel;		/*  6 Z axis */
 
 	int32_t		pres;		/*  8 Pa * 10 */
@@ -207,6 +207,72 @@ struct ao_telemetry_mega_data {
 };
 
 
+#define AO_TELEMETRY_METRUM_SENSOR	0x0A
+
+struct ao_telemetry_metrum_sensor {
+	uint16_t	serial;		/*  0 */
+	uint16_t	tick;		/*  2 */
+	uint8_t		type;		/*  4 */
+
+	uint8_t         state;          /*  5 flight state */
+	int16_t		accel;		/*  6 Z axis */
+
+	int32_t		pres;		/*  8 Pa * 10 */
+	int16_t		temp;		/* 12 °C * 100 */
+
+	int16_t         acceleration;   /* 14 m/s² * 16 */
+	int16_t         speed;          /* 16 m/s * 16 */
+	int16_t         height;         /* 18 m */
+
+	int16_t		v_batt;		/* 20 battery voltage */
+	int16_t		sense_a;	/* 22 apogee continuity sense */
+	int16_t		sense_m;	/* 24 main continuity sense */
+
+	uint8_t		pad[6];		/* 26 */
+	/* 32 */
+};
+	
+#define AO_TELEMETRY_METRUM_DATA	0x0B
+
+struct ao_telemetry_metrum_data {
+	uint16_t	serial;		/*  0 */
+	uint16_t	tick;		/*  2 */
+	uint8_t		type;		/*  4 */
+
+	int32_t		ground_pres;	/* 8 average pres on pad */
+	int16_t		ground_accel;	/* 12 average accel on pad */
+	int16_t		accel_plus_g;	/* 14 accel calibration at +1g */
+	int16_t		accel_minus_g;	/* 16 accel calibration at -1g */
+
+	uint8_t		pad[14];	/* 18 */
+	/* 32 */
+};
+
+#define AO_TELEMETRY_MINI		0x10
+
+struct ao_telemetry_mini {
+	uint16_t	serial;		/*  0 */
+	uint16_t	tick;		/*  2 */
+	uint8_t		type;		/*  4 */
+
+	uint8_t         state;          /*  5 flight state */
+	int16_t		v_batt;		/*  6 battery voltage */
+	int16_t		sense_a;	/*  8 apogee continuity */
+	int16_t		sense_m;	/* 10 main continuity */
+
+	int32_t		pres;		/* 12 Pa * 10 */
+	int16_t		temp;		/* 16 °C * 100 */
+
+	int16_t         acceleration;   /* 18 m/s² * 16 */
+	int16_t         speed;          /* 20 m/s * 16 */
+	int16_t         height;         /* 22 m */
+
+	int32_t		ground_pres;	/* 24 average pres on pad */
+
+	int32_t		pad28;		/* 28 */
+	/* 32 */
+};
+
 /* #define AO_SEND_ALL_BARO */
 
 #define AO_TELEMETRY_BARO		0x80
@@ -240,6 +306,9 @@ union ao_telemetry_all {
 	struct ao_telemetry_companion		companion;
 	struct ao_telemetry_mega_sensor		mega_sensor;
 	struct ao_telemetry_mega_data		mega_data;
+	struct ao_telemetry_metrum_sensor	metrum_sensor;
+	struct ao_telemetry_metrum_data		metrum_data;
+	struct ao_telemetry_mini		mini;
 	struct ao_telemetry_baro		baro;
 };
 
diff --git a/src/core/ao_usb.h b/src/core/ao_usb.h
index 6bc77608..35e64e65 100644
--- a/src/core/ao_usb.h
+++ b/src/core/ao_usb.h
@@ -102,8 +102,11 @@ extern __code __at (0x00aa) uint8_t ao_usb_descriptors [];
 #define AO_USB_INT_EP		1
 #define AO_USB_INT_SIZE		8
 
+#ifndef AO_USB_OUT_EP
 #define AO_USB_OUT_EP		4
 #define AO_USB_IN_EP		5
+#endif
+
 /*
  * USB bulk packets can only come in 8, 16, 32 and 64
  * byte sizes, so we'll use 64 for everything
diff --git a/src/drivers/ao_74hc165.c b/src/drivers/ao_74hc165.c
new file mode 100644
index 00000000..143f4e3f
--- /dev/null
+++ b/src/drivers/ao_74hc165.c
@@ -0,0 +1,58 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+/*
+ * 74HC165 driver.
+ * Reads a single byte from the shift register
+ */
+
+#include <ao.h>
+#include <ao_74hc165.h>
+
+uint8_t
+ao_74hc165_read(void)
+{
+	static __xdata state;
+	ao_spi_get(AO_74HC165_SPI_BUS);
+	ao_spi_set_speed(AO_74HC165_SPI_BUS, AO_SPI_SPEED_FAST);
+	AO_74HC165_CS = 1;
+	ao_spi_recv(&state, 1, AO_74HC165_SPI_BUS);
+	AO_74HC165_CS = 0;
+	ao_spi_put(AO_74HC165_SPI_BUS);
+	return state;
+}
+
+static void
+ao_74hc165_cmd(void)
+{
+	uint8_t	v;
+
+	v = ao_74hc165_read();
+	printf ("Switches: 0x%02x\n", v);
+}
+
+static const struct ao_cmds ao_74hc165_cmds[] = {
+	{ ao_74hc165_cmd, "L\0Show 74hc165" },
+	{ 0, NULL }
+};
+
+void
+ao_74hc165_init(void)
+{
+	ao_enable_output(AO_74HC165_CS_PORT, AO_74HC165_CS_PIN, AO_74HC165_CS, 0);
+	ao_cmd_register(&ao_74hc165_cmds[0]);
+}
diff --git a/src/drivers/ao_74hc165.h b/src/drivers/ao_74hc165.h
new file mode 100644
index 00000000..3ae51353
--- /dev/null
+++ b/src/drivers/ao_74hc165.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_74HC165_H_
+#define _AO_74HC165_H_
+
+uint8_t
+ao_74hc165_read(void);
+
+void
+ao_74hc165_init(void);
+
+#endif /* _AO_74HC165_H_ */
diff --git a/src/drivers/ao_button.c b/src/drivers/ao_button.c
index a507c909..25c0cd5c 100644
--- a/src/drivers/ao_button.c
+++ b/src/drivers/ao_button.c
@@ -18,6 +18,7 @@
 #include <ao.h>
 #include <ao_button.h>
 #include <ao_exti.h>
+#include <ao_debounce.h>
 #if AO_EVENT
 #include <ao_event.h>
 #define ao_button_queue(b,v)	ao_event_put_isr(AO_EVENT_BUTTON, b, v)
@@ -25,55 +26,71 @@
 #define ao_button_queue(b,v)
 #endif
 
-static uint8_t		ao_button[AO_BUTTON_COUNT];
-static AO_TICK_TYPE	ao_button_time[AO_BUTTON_COUNT];
+#define AO_BUTTON_DEBOUNCE_HOLD	10
 
-#define AO_DEBOUNCE	AO_MS_TO_TICKS(20)
+static struct ao_debounce	ao_button_debounce[AO_BUTTON_COUNT];
 
 #define port(q)	AO_BUTTON_ ## q ## _PORT
 #define bit(q) AO_BUTTON_ ## q
 #define pin(q) AO_BUTTON_ ## q ## _PIN
 
-static void
-ao_button_do(uint8_t b, uint8_t v)
-{
-	/* Debounce */
-	if ((AO_TICK_SIGNED) (ao_tick_count - ao_button_time[b]) < AO_DEBOUNCE)
-		return;
-
-	/* pins are inverted */
-	v = !v;
-	if (ao_button[b] != v) {
-		ao_button[b] = v;
-		ao_button_time[b] = ao_tick_count;
-		ao_button_queue(b, v);
-		ao_wakeup(&ao_button[b]);
-	}
-}
+/* pins are inverted */
+#define ao_button_value(b)	!ao_gpio_get(port(b), bit(b), pin(b))
 
-#define ao_button_update(b)	ao_button_do(b, ao_gpio_get(port(b), bit(b), pin(b)))
-
-static void
-ao_button_isr(void)
+static uint8_t
+_ao_button_get(struct ao_debounce *debounce)
 {
+	uint8_t	b = debounce - ao_button_debounce;
+
+	switch (b) {
 #if AO_BUTTON_COUNT > 0
-	ao_button_update(0);
+	case 0: return ao_button_value(0);
 #endif
 #if AO_BUTTON_COUNT > 1
-	ao_button_update(1);
+	case 1: return ao_button_value(1);
 #endif
 #if AO_BUTTON_COUNT > 2
-	ao_button_update(2);
+	case 2: return ao_button_value(2);
 #endif
 #if AO_BUTTON_COUNT > 3
-	ao_button_update(3);
+	case 3: return ao_button_value(3);
 #endif
 #if AO_BUTTON_COUNT > 4
-	ao_button_update(4);
+	case 4: return ao_button_value(4);
 #endif
+	}
+}
+
+static void
+_ao_button_set(struct ao_debounce *debounce, uint8_t value)
+{
+	uint8_t b = debounce - ao_button_debounce;
+
+	ao_button_queue(b, value);
+}
+
+
+#define ao_button_update(b)	ao_button_do(b, ao_gpio_get(port(b), bit(b), pin(b)))
+
+static void
+ao_button_debounce_init(struct ao_debounce *debounce) {
+	ao_debounce_config(debounce,
+			   _ao_button_get,
+			   _ao_button_set,
+			   AO_BUTTON_DEBOUNCE_HOLD);
+}
+
+static void
+ao_button_isr(void)
+{
+	uint8_t	b;
+
+	for (b = 0; b < AO_BUTTON_COUNT; b++)
+		_ao_debounce_start(&ao_button_debounce[b]);
 }
 
 #define init(b) do {							\
+		ao_button_debounce_init(&ao_button_debounce[b]);	\
 		ao_enable_port(port(b));				\
 									\
 		ao_exti_setup(port(b), bit(b),				\
@@ -91,4 +108,14 @@ ao_button_init(void)
 #if AO_BUTTON_COUNT > 1
 	init(1);
 #endif
+#if AO_BUTTON_COUNT > 2
+	init(2);
+#endif
+#if AO_BUTTON_COUNT > 3
+	init(3);
+#endif
+#if AO_BUTTON_COUNT > 4
+	init(4);
+#endif
+	ao_debounce_init();
 }
diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c
index 772014ee..37d04927 100644
--- a/src/drivers/ao_cc1120.c
+++ b/src/drivers/ao_cc1120.c
@@ -326,6 +326,8 @@ static const uint16_t packet_setup[] = {
 				 (0 << CC1120_PKT_CFG0_PKG_BIT_LEN) |
 				 (0 << CC1120_PKT_CFG0_UART_MODE_EN) |
 				 (0 << CC1120_PKT_CFG0_UART_SWAP_EN)),
+        CC1120_PREAMBLE_CFG1,	((CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_4_BYTES << CC1120_PREAMBLE_CFG1_NUM_PREAMBLE) |
+				 (CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_AA << CC1120_PREAMBLE_CFG1_PREAMBLE_WORD)),
 	AO_CC1120_MARC_GPIO_IOCFG,		CC1120_IOCFG_GPIO_CFG_MARC_MCU_WAKEUP,
 };
 
@@ -389,6 +391,8 @@ static const uint16_t rdf_setup[] = {
 				 (0 << CC1120_PKT_CFG0_PKG_BIT_LEN) |
 				 (0 << CC1120_PKT_CFG0_UART_MODE_EN) |
 				 (0 << CC1120_PKT_CFG0_UART_SWAP_EN)),
+        CC1120_PREAMBLE_CFG1,	((CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_NONE << CC1120_PREAMBLE_CFG1_NUM_PREAMBLE) |
+				 (CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_AA << CC1120_PREAMBLE_CFG1_PREAMBLE_WORD)),
 };
 
 /*
@@ -433,6 +437,33 @@ static const uint16_t aprs_setup[] = {
 				 (CC1120_PKT_CFG1_ADDR_CHECK_CFG_NONE << CC1120_PKT_CFG1_ADDR_CHECK_CFG) |
 				 (CC1120_PKT_CFG1_CRC_CFG_DISABLED << CC1120_PKT_CFG1_CRC_CFG) |
 				 (0 << CC1120_PKT_CFG1_APPEND_STATUS)),
+        CC1120_PREAMBLE_CFG1,	((CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_NONE << CC1120_PREAMBLE_CFG1_NUM_PREAMBLE) |
+				 (CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_AA << CC1120_PREAMBLE_CFG1_PREAMBLE_WORD)),
+};
+
+/*
+ * For Test mode, we want an unmodulated carrier. To do that, we
+ * set the deviation to zero and enable a preamble so that the radio
+ * turns on before we send any data
+ */
+
+static const uint16_t test_setup[] = {
+	CC1120_DEVIATION_M,	0,
+	CC1120_MODCFG_DEV_E,	((CC1120_MODCFG_DEV_E_MODEM_MODE_NORMAL << CC1120_MODCFG_DEV_E_MODEM_MODE) |
+				 (CC1120_MODCFG_DEV_E_MOD_FORMAT_2_GFSK << CC1120_MODCFG_DEV_E_MOD_FORMAT) |
+				 (0 << CC1120_MODCFG_DEV_E_DEV_E)),
+	CC1120_DRATE2,		((APRS_DRATE_E << CC1120_DRATE2_DATARATE_E) |
+				 (((APRS_DRATE_M >> 16) & CC1120_DRATE2_DATARATE_M_19_16_MASK) << CC1120_DRATE2_DATARATE_M_19_16)),
+	CC1120_DRATE1,		((APRS_DRATE_M >> 8) & 0xff),
+	CC1120_DRATE0,		((APRS_DRATE_M >> 0) & 0xff),
+	CC1120_PKT_CFG2,	((CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR << CC1120_PKT_CFG2_CCA_MODE) |
+				 (CC1120_PKT_CFG2_PKT_FORMAT_NORMAL << CC1120_PKT_CFG2_PKT_FORMAT)),
+	CC1120_PKT_CFG1,	((0 << CC1120_PKT_CFG1_WHITE_DATA) |
+				 (CC1120_PKT_CFG1_ADDR_CHECK_CFG_NONE << CC1120_PKT_CFG1_ADDR_CHECK_CFG) |
+				 (CC1120_PKT_CFG1_CRC_CFG_DISABLED << CC1120_PKT_CFG1_CRC_CFG) |
+				 (0 << CC1120_PKT_CFG1_APPEND_STATUS)),
+        CC1120_PREAMBLE_CFG1,	((CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_4_BYTES << CC1120_PREAMBLE_CFG1_NUM_PREAMBLE) |
+				 (CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_AA << CC1120_PREAMBLE_CFG1_PREAMBLE_WORD)),
 };
 
 #define AO_PKT_CFG0_INFINITE ((0 << CC1120_PKT_CFG0_RESERVED7) |	\
@@ -456,8 +487,9 @@ static uint16_t ao_radio_mode;
 #define AO_RADIO_MODE_BITS_PACKET_RX	16
 #define AO_RADIO_MODE_BITS_RDF		32
 #define AO_RADIO_MODE_BITS_APRS		64
-#define AO_RADIO_MODE_BITS_INFINITE	128
-#define AO_RADIO_MODE_BITS_FIXED	256
+#define AO_RADIO_MODE_BITS_TEST		128
+#define AO_RADIO_MODE_BITS_INFINITE	256
+#define AO_RADIO_MODE_BITS_FIXED	512
 
 #define AO_RADIO_MODE_NONE		0
 #define AO_RADIO_MODE_PACKET_TX_BUF	(AO_RADIO_MODE_BITS_PACKET | AO_RADIO_MODE_BITS_PACKET_TX | AO_RADIO_MODE_BITS_TX_BUF)
@@ -467,6 +499,7 @@ static uint16_t ao_radio_mode;
 #define AO_RADIO_MODE_APRS_BUF		(AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_INFINITE | AO_RADIO_MODE_BITS_TX_BUF)
 #define AO_RADIO_MODE_APRS_LAST_BUF	(AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_FIXED | AO_RADIO_MODE_BITS_TX_BUF)
 #define AO_RADIO_MODE_APRS_FINISH	(AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_FIXED | AO_RADIO_MODE_BITS_TX_FINISH)
+#define AO_RADIO_MODE_TEST		(AO_RADIO_MODE_BITS_TEST | AO_RADIO_MODE_BITS_INFINITE | AO_RADIO_MODE_BITS_TX_BUF)
 
 static void
 ao_radio_set_mode(uint16_t new_mode)
@@ -504,6 +537,10 @@ ao_radio_set_mode(uint16_t new_mode)
 		for (i = 0; i < sizeof (aprs_setup) / sizeof (aprs_setup[0]); i += 2)
 			ao_radio_reg_write(aprs_setup[i], aprs_setup[i+1]);
 
+	if (changes & AO_RADIO_MODE_BITS_TEST)
+		for (i = 0; i < sizeof (test_setup) / sizeof (test_setup[0]); i += 2)
+			ao_radio_reg_write(test_setup[i], test_setup[i+1]);
+
 	if (changes & AO_RADIO_MODE_BITS_INFINITE)
 		ao_radio_reg_write(CC1120_PKT_CFG0, AO_PKT_CFG0_INFINITE);
 
@@ -652,6 +689,7 @@ ao_radio_test_cmd(void)
 		ao_packet_slave_stop();
 #endif
 		ao_radio_get(0xff);
+		ao_radio_set_mode(AO_RADIO_MODE_TEST);
 		ao_radio_strobe(CC1120_STX);
 #if CC1120_TRACE
 		{ int t; 
diff --git a/src/drivers/ao_cc1120.h b/src/drivers/ao_cc1120.h
index 5d226b64..a1d78c01 100644
--- a/src/drivers/ao_cc1120.h
+++ b/src/drivers/ao_cc1120.h
@@ -404,6 +404,8 @@
 #define CC1120_MARC_SPARE	(CC1120_EXTENDED_BIT | 0x03)
 #define CC1120_ECG_CFG		(CC1120_EXTENDED_BIT | 0x04)
 #define CC1120_SOFT_TX_DATA_CFG	(CC1120_EXTENDED_BIT | 0x05)
+#define  CC1120_SOFT_TX_DATA_CFG_SYMBOL_MAP_CFG		5
+#define  CC1120_SOFT_TX_DATA_CFG_SOFT_TX_DATA_EN	0
 #define CC1120_EXT_CTRL		(CC1120_EXTENDED_BIT | 0x06)
 #define CC1120_RCCAL_FINE	(CC1120_EXTENDED_BIT | 0x07)
 #define CC1120_RCCAL_COARSE	(CC1120_EXTENDED_BIT | 0x08)
diff --git a/src/drivers/ao_cc115l.c b/src/drivers/ao_cc115l.c
index 05e6a762..0fa1e899 100644
--- a/src/drivers/ao_cc115l.c
+++ b/src/drivers/ao_cc115l.c
@@ -245,6 +245,8 @@ ao_radio_idle(void)
 		uint8_t	state = ao_radio_strobe(CC115L_SIDLE);
 		if ((state >> CC115L_STATUS_STATE) == CC115L_STATUS_STATE_IDLE)
 			break;
+		if ((state >> CC115L_STATUS_STATE) == CC115L_STATUS_STATE_TX_FIFO_UNDERFLOW)
+			ao_radio_strobe(CC115L_SFTX);
 	}
 	/* Flush any pending TX bytes */
 	ao_radio_strobe(CC115L_SFTX);
@@ -476,6 +478,8 @@ ao_radio_setup(void)
 
 	ao_config_get();
 
+	ao_radio_strobe(CC115L_SCAL);
+
 	ao_radio_configured = 1;
 }
 
@@ -494,7 +498,6 @@ static void
 ao_radio_get(void)
 {
 	static uint32_t	last_radio_setting;
-	static uint8_t	last_power_setting;
 
 	ao_mutex_get(&ao_radio_mutex);
 	if (!ao_radio_configured)
@@ -505,10 +508,6 @@ ao_radio_get(void)
 		ao_radio_reg_write(CC115L_FREQ0, ao_config.radio_setting);
 		last_radio_setting = ao_config.radio_setting;
 	}
-	if (ao_config.radio_power != last_power_setting) {
-		ao_radio_reg_write(CC115L_PA, ao_config.radio_power);
-		last_power_setting = ao_config.radio_power;
-	}
 }
 
 static void
@@ -614,6 +613,20 @@ ao_radio_rdf_abort(void)
 	ao_wakeup(&ao_radio_wake);
 }
 
+#define POWER_STEP	0x08
+
+static void
+ao_radio_stx(void)
+{
+	uint8_t	power;
+	ao_radio_pa_on();
+	ao_radio_reg_write(CC115L_PA, 0);
+	ao_radio_strobe(CC115L_STX);
+	for (power = POWER_STEP; power < ao_config.radio_power; power += POWER_STEP)
+		ao_radio_reg_write(CC115L_PA, power);
+	ao_radio_reg_write(CC115L_PA, ao_config.radio_power);
+}
+
 static void
 ao_radio_test_cmd(void)
 {
@@ -633,11 +646,10 @@ ao_radio_test_cmd(void)
 		ao_packet_slave_stop();
 #endif
 		ao_radio_get();
-		ao_radio_set_len(0xff);
-		ao_radio_set_mode(AO_RADIO_MODE_RDF|AO_RADIO_MODE_BITS_FIXED);
 		ao_radio_strobe(CC115L_SFTX);
-		ao_radio_pa_on();
-		ao_radio_strobe(CC115L_STX);
+		ao_radio_set_len(0xff);
+		ao_radio_set_mode(AO_RADIO_MODE_RDF);
+		ao_radio_stx();
 		radio_on = 1;
 	}
 	if (mode == 3) {
@@ -655,12 +667,14 @@ ao_radio_test_cmd(void)
 	}
 }
 
+#if CC115L_TRACE
 static inline int16_t
 ao_radio_gpio_bits(void)
 {
 	return AO_CC115L_DONE_INT_PORT->idr & ((1 << AO_CC115L_FIFO_INT_PIN) |
 					       (1 << AO_CC115L_DONE_INT_PIN));
 }
+#endif
 
 static void
 ao_radio_wait_fifo(void)
@@ -738,6 +752,10 @@ _ao_radio_send_lots(ao_radio_fill_func fill, uint8_t mode)
 	uint8_t	fifo_space;
 
 	fifo_space = CC115L_FIFO_SIZE;
+	ao_radio_abort = 0;
+
+	ao_radio_strobe(CC115L_SFTX);
+
 	ao_radio_done = 0;
 	ao_radio_fifo = 0;
 	while (!done) {
@@ -784,8 +802,7 @@ _ao_radio_send_lots(ao_radio_fill_func fill, uint8_t mode)
 			ao_exti_enable(AO_CC115L_DONE_INT_PORT, AO_CC115L_DONE_INT_PIN);
 
 			if (!started) {
-				ao_radio_pa_on();
-				ao_radio_strobe(CC115L_STX);
+				ao_radio_stx();
 				started = 1;
 			}
 		}
diff --git a/src/drivers/ao_event.c b/src/drivers/ao_event.c
index 440ef2de..c428125d 100644
--- a/src/drivers/ao_event.c
+++ b/src/drivers/ao_event.c
@@ -25,11 +25,6 @@
 #define ao_event_queue_empty()	(ao_event_queue_insert == ao_event_queue_remove)
 #define ao_event_queue_full()	(ao_event_queue_next(ao_event_queue_insert) == ao_event_queue_remove)
 
-/*
- * Whether a sequence of events from the same device should be collapsed
- */
-#define ao_event_can_collapse(type)	((type) == AO_EVENT_QUADRATURE)
-
 struct ao_event ao_event_queue[AO_EVENT_QUEUE];
 uint8_t		ao_event_queue_insert;
 uint8_t		ao_event_queue_remove;
@@ -48,17 +43,9 @@ ao_event_get(struct ao_event *ev)
 
 /* called with interrupts disabled */
 void
-ao_event_put_isr(uint8_t type, uint8_t unit, uint32_t value)
+ao_event_put_isr(uint8_t type, uint8_t unit, int32_t value)
 {
 	if (!ao_event_queue_full()) {
-
-		if (ao_event_can_collapse(type) && !ao_event_queue_empty()) {
-			uint8_t	prev = ao_event_queue_prev(ao_event_queue_insert);
-
-			if (ao_event_queue[prev].type == type &&
-			    ao_event_queue[prev].unit == unit)
-				ao_event_queue_insert = prev;
-		}
 		ao_event_queue[ao_event_queue_insert] = (struct ao_event) {
 			.type = type,
 			.unit = unit,
@@ -71,7 +58,7 @@ ao_event_put_isr(uint8_t type, uint8_t unit, uint32_t value)
 }
 
 void
-ao_event_put(uint8_t type, uint8_t unit, uint32_t value)
+ao_event_put(uint8_t type, uint8_t unit, int32_t value)
 {
 	ao_arch_critical(ao_event_put_isr(type, unit, value););
 }
diff --git a/src/drivers/ao_event.h b/src/drivers/ao_event.h
index 25c49c35..ed9a7433 100644
--- a/src/drivers/ao_event.h
+++ b/src/drivers/ao_event.h
@@ -26,16 +26,16 @@ struct ao_event {
 	uint8_t		type;
 	uint8_t		unit;
 	uint16_t	tick;
-	uint32_t	value;
+	int32_t		value;
 };
 
 uint8_t
 ao_event_get(struct ao_event *ev);
 
 void
-ao_event_put_isr(uint8_t type, uint8_t unit, uint32_t value);
+ao_event_put_isr(uint8_t type, uint8_t unit, int32_t value);
 
 void
-ao_event_put(uint8_t type, uint8_t unit, uint32_t value);
+ao_event_put(uint8_t type, uint8_t unit, int32_t value);
 
 #endif /* _AO_EVENT_H_ */
diff --git a/src/drivers/ao_gps_sirf.c b/src/drivers/ao_gps_sirf.c
index 91fc948b..d89435b9 100644
--- a/src/drivers/ao_gps_sirf.c
+++ b/src/drivers/ao_gps_sirf.c
@@ -19,6 +19,7 @@
 #include "ao.h"
 #endif
 
+__xdata uint8_t ao_gps_new;
 __xdata uint8_t ao_gps_mutex;
 __pdata uint16_t ao_gps_tick;
 __xdata struct ao_telemetry_location	ao_gps_data;
@@ -422,8 +423,9 @@ ao_gps(void) __reentrant
 			else
 				ao_gps_data.v_error = ao_sirf_data.v_error / 100;
 #endif
+			ao_gps_new |= AO_GPS_NEW_DATA;
 			ao_mutex_put(&ao_gps_mutex);
-			ao_wakeup(&ao_gps_data);
+			ao_wakeup(&ao_gps_new);
 			break;
 		case 4:
 			ao_mutex_get(&ao_gps_mutex);
@@ -432,8 +434,9 @@ ao_gps(void) __reentrant
 				ao_gps_tracking_data.sats[i].svid = ao_sirf_tracker_data.sats[i].svid;
 				ao_gps_tracking_data.sats[i].c_n_1 = ao_sirf_tracker_data.sats[i].c_n_1;
 			}
+			ao_gps_new |= AO_GPS_NEW_TRACKING;
 			ao_mutex_put(&ao_gps_mutex);
-			ao_wakeup(&ao_gps_tracking_data);
+			ao_wakeup(&ao_gps_new);
 			break;
 		}
 	}
diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c
index d2f67e6b..944a37f9 100644
--- a/src/drivers/ao_gps_skytraq.c
+++ b/src/drivers/ao_gps_skytraq.c
@@ -32,6 +32,7 @@
 #define ao_gps_set_speed	ao_serial1_set_speed
 #endif
 
+__xdata uint8_t ao_gps_new;
 __xdata uint8_t ao_gps_mutex;
 static __data char ao_gps_char;
 static __data uint8_t ao_gps_cksum;
@@ -293,10 +294,11 @@ ao_nmea_gga(void)
 
 	if (!ao_gps_error) {
 		ao_mutex_get(&ao_gps_mutex);
+		ao_gps_new |= AO_GPS_NEW_DATA;
 		ao_gps_tick = ao_gps_next_tick;
 		ao_xmemcpy(&ao_gps_data, PDATA_TO_XDATA(&ao_gps_next), sizeof (ao_gps_data));
 		ao_mutex_put(&ao_gps_mutex);
-		ao_wakeup(&ao_gps_data);
+		ao_wakeup(&ao_gps_new);
 	}
 }
 
@@ -352,9 +354,10 @@ ao_nmea_gsv(void)
 		ao_gps_tracking_next.channels = 0;
 	else if (done) {
 		ao_mutex_get(&ao_gps_mutex);
+		ao_gps_new |= AO_GPS_NEW_TRACKING;
 		ao_xmemcpy(&ao_gps_tracking_data, PDATA_TO_XDATA(&ao_gps_tracking_next), sizeof(ao_gps_tracking_data));
 		ao_mutex_put(&ao_gps_mutex);
-		ao_wakeup(&ao_gps_tracking_data);
+		ao_wakeup(&ao_gps_new);
 	}
 }
 
@@ -483,25 +486,6 @@ ao_gps(void) __reentrant
 
 __xdata struct ao_task ao_gps_task;
 
-static void
-gps_dump(void) __reentrant
-{
-	uint8_t	i;
-	ao_mutex_get(&ao_gps_mutex);
-	printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day);
-	printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second);
-	printf ("Lat/Lon: %ld %ld\n", (long) ao_gps_data.latitude, (long) ao_gps_data.longitude);
-	printf ("Alt: %d\n", ao_gps_data.altitude);
-	printf ("Flags: 0x%x\n", ao_gps_data.flags);
-	printf ("Sats: %d", ao_gps_tracking_data.channels);
-	for (i = 0; i < ao_gps_tracking_data.channels; i++)
-		printf (" %d %d",
-			ao_gps_tracking_data.sats[i].svid,
-			ao_gps_tracking_data.sats[i].c_n_1);
-	printf ("\ndone\n");
-	ao_mutex_put(&ao_gps_mutex);
-}
-
 static __code uint8_t ao_gps_115200[] = {
 	SKYTRAQ_MSG_3(5,0,5,0)	/* Set to 115200 baud */
 };
@@ -532,7 +516,7 @@ gps_update(void) __reentrant
 }
 
 __code struct ao_cmds ao_gps_cmds[] = {
-	{ gps_dump, 	"g\0Display GPS" },
+	{ ao_gps_show, 	"g\0Display GPS" },
 	{ gps_update,	"U\0Update GPS firmware" },
 	{ 0, NULL },
 };
diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c
index 22300df3..4fb90746 100644
--- a/src/drivers/ao_gps_ublox.c
+++ b/src/drivers/ao_gps_ublox.c
@@ -21,16 +21,37 @@
 
 #include "ao_gps_ublox.h"
 
+#define AO_UBLOX_DEBUG 0
+
+#include <stdarg.h>
+
+__xdata uint8_t ao_gps_new;
 __xdata uint8_t ao_gps_mutex;
 __pdata uint16_t ao_gps_tick;
 __xdata struct ao_telemetry_location	ao_gps_data;
 __xdata struct ao_telemetry_satellite	ao_gps_tracking_data;
 
-static const char ao_gps_set_nmea[] = "\r\n$PUBX,41,1,3,1,57600,0*2d\r\n";
+#undef AO_SERIAL_SPEED_UBLOX
 
-const char ao_gps_config[] = {
+#ifndef AO_SERIAL_SPEED_UBLOX
+#define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_9600
+#endif
 
-};
+#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_57600
+#define SERIAL_SPEED_STRING	"57600"
+#define SERIAL_SPEED_CHECKSUM	"2d"
+#endif
+#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_19200
+#define SERIAL_SPEED_STRING	"19200"
+#define SERIAL_SPEED_CHECKSUM	"23"
+#endif
+#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_9600
+#define SERIAL_SPEED_STRING	"9600"
+#define SERIAL_SPEED_CHECKSUM	"16"
+#endif
+
+static const char ao_gps_set_nmea[] =
+	"\r\n$PUBX,41,1,3,1," SERIAL_SPEED_STRING ",0*" SERIAL_SPEED_CHECKSUM "\r\n";
 
 struct ao_ublox_cksum {
 	uint8_t	a, b;
@@ -39,13 +60,34 @@ struct ao_ublox_cksum {
 static __pdata struct ao_ublox_cksum ao_ublox_cksum;
 static __pdata uint16_t ao_ublox_len;
 
-#ifndef ao_ublox_getchar
-#define ao_ublox_getchar	ao_serial1_getchar
-#define ao_ublox_putchar	ao_serial1_putchar
-#define ao_ublox_set_speed	ao_serial1_set_speed
+#if AO_UBLOX_DEBUG
+
+static uint8_t ao_gps_dbg_enable;
+
+#define DBG_PROTO	1
+#define DBG_CHAR	2
+#define DBG_INIT	4
+
+static void ao_gps_dbg(int level, char *format, ...) {
+	va_list a;
+
+	if (level & ao_gps_dbg_enable) {
+		va_start(a, format);
+		vprintf(format, a);
+		va_end(a);
+		flush();
+	}
+}
+
+#else
+#define ao_gps_dbg(fmt, ...)
 #endif
 
-#define ao_ublox_byte()	((uint8_t) ao_ublox_getchar())
+static inline uint8_t ao_ublox_byte(void) {
+	uint8_t	c = (uint8_t) ao_gps_getchar();
+	ao_gps_dbg(DBG_CHAR, " %02x", c);
+	return c;
+}
 
 static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c)
 {
@@ -61,7 +103,8 @@ static void ao_ublox_init_cksum(void)
 static void ao_ublox_put_u8(uint8_t c)
 {
 	add_cksum(&ao_ublox_cksum, c);
-	ao_ublox_putchar(c);
+	ao_gps_dbg(DBG_CHAR, " (%02x)", c);
+	ao_gps_putchar(c);
 }
 
 static void ao_ublox_put_i8(int8_t c)
@@ -183,6 +226,7 @@ ao_ublox_parse(void __xdata *target, const struct ublox_packet_parse *parse) __r
 			break;
 		}
 	}
+	ao_gps_dbg(DBG_PROTO, "\n");
 }
 
 /*
@@ -326,6 +370,7 @@ ao_ublox_parse_nav_svinfo(void)
 {
 	uint8_t	nsat;
 	nav_svinfo_nsat = 0;
+
 	ao_ublox_parse(&nav_svinfo, nav_svinfo_packet);
 	for (nsat = 0; nsat < nav_svinfo.num_ch && ao_ublox_len >= 12; nsat++) {
 		if (nsat < NAV_SVINFO_MAX_SAT) {
@@ -334,6 +379,17 @@ ao_ublox_parse_nav_svinfo(void)
 			ublox_discard(12);
 		}
 	}
+#if AO_UBLOX_DEBUG
+	ao_gps_dbg(DBG_PROTO, "svinfo num_ch %d flags %02x\n", nav_svinfo.num_ch, nav_svinfo.flags);
+	for (nsat = 0; nsat < nav_svinfo.num_ch; nsat++)
+		ao_gps_dbg(DBG_PROTO, "\t%d: chn %d svid %d flags %02x quality %d cno %d\n",
+			    nsat,
+			    nav_svinfo_sat[nsat].chn,
+			    nav_svinfo_sat[nsat].svid,
+			    nav_svinfo_sat[nsat].flags,
+			    nav_svinfo_sat[nsat].quality,
+			    nav_svinfo_sat[nsat].cno);
+#endif
 }
 
 /*
@@ -402,45 +458,60 @@ ao_ublox_parse_nav_velned(void)
  */
 
 static void
-ao_gps_setup(void)
+ao_gps_delay(void)
 {
-	uint8_t	i, k;
-	ao_ublox_set_speed(AO_SERIAL_SPEED_9600);
+	uint8_t i;
 
 	/*
 	 * A bunch of nulls so the start bit
 	 * is clear
 	 */
+
 	for (i = 0; i < 64; i++)
-		ao_ublox_putchar(0x00);
+		ao_gps_putchar(0x00);
+}
+
+static void
+ao_gps_setup(void)
+{
+	uint8_t	i, k;
+
+	ao_delay(AO_SEC_TO_TICKS(3));
+
+	ao_gps_dbg(DBG_INIT, "Set speed 9600\n");
+	ao_gps_set_speed(AO_SERIAL_SPEED_9600);
 
 	/*
 	 * Send the baud-rate setting and protocol-setting
 	 * command three times
 	 */
-	for (k = 0; k < 3; k++)
+	for (k = 0; k < 3; k++) {
+		ao_gps_delay();
+
+		ao_gps_dbg(DBG_INIT, "Send initial setting\n");
 		for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
-			ao_ublox_putchar(ao_gps_set_nmea[i]);
+			ao_gps_putchar(ao_gps_set_nmea[i]);
+	}
+
+	ao_gps_delay();
 
+#if AO_SERIAL_SPEED_UBLOX != AO_SERIAL_SPEED_9600
+	ao_gps_dbg(DBG_INIT, "Set speed high\n");
 	/*
 	 * Increase the baud rate
 	 */
-	ao_ublox_set_speed(AO_SERIAL_SPEED_57600);
+	ao_gps_set_speed(AO_SERIAL_SPEED_UBLOX);
+#endif
 
-	/*
-	 * Pad with nulls to give the chip
-	 * time to see the baud rate switch
-	 */
-	for (i = 0; i < 64; i++)
-		ao_ublox_putchar(0x00);
+	ao_gps_delay();
 }
 
 static void
 ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len)
 {
 	ao_ublox_init_cksum();
-	ao_ublox_putchar(0xb5);
-	ao_ublox_putchar(0x62);
+	ao_gps_putchar(0xb5);
+	ao_gps_putchar(0x62);
 	ao_ublox_put_u8(class);
 	ao_ublox_put_u8(id);
 	ao_ublox_put_u8(len);
@@ -450,8 +521,8 @@ ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len)
 static void
 ao_ublox_putend(void)
 {
-	ao_ublox_putchar(ao_ublox_cksum.a);
-	ao_ublox_putchar(ao_ublox_cksum.b);
+	ao_gps_putchar(ao_ublox_cksum.a);
+	ao_gps_putchar(ao_ublox_cksum.b);
 }
 
 static void
@@ -547,7 +618,7 @@ ao_gps(void) __reentrant
 	/* Enable all of the messages we want */
 	for (i = 0; i < sizeof (ublox_enable_nav); i++)
 		ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], 1);
-	
+
 	ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE),
 					 UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G,
 					 UBLOX_CFG_NAV5_FIXMODE_3D,
@@ -578,6 +649,8 @@ ao_gps(void) __reentrant
 		ao_ublox_len = header_byte();
 		ao_ublox_len |= header_byte() << 8;
 
+		ao_gps_dbg(DBG_PROTO, "class %02x id %02x len %d\n", class, id, ao_ublox_len);
+
 		if (ao_ublox_len > 1023)
 			continue;
 
@@ -618,8 +691,10 @@ ao_gps(void) __reentrant
 			break;
 		}
 
-		if (ao_ublox_len != 0)
+		if (ao_ublox_len != 0) {
+			ao_gps_dbg(DBG_PROTO, "len left %d\n", ao_ublox_len);
 			continue;
+		}
 
 		/* verify checksum and end sequence */
 		cksum.a = ao_ublox_byte();
@@ -628,9 +703,9 @@ ao_gps(void) __reentrant
 			continue;
 
 		switch (class) {
-		case 0x01:
+		case UBLOX_NAV:
 			switch (id) {
-			case 0x21:
+			case UBLOX_NAV_TIMEUTC:
 				ao_mutex_get(&ao_gps_mutex);
 				ao_gps_tick = ao_time();
 
@@ -645,7 +720,7 @@ ao_gps(void) __reentrant
 				}
 				if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC))
 					ao_gps_data.flags |= AO_GPS_DATE_VALID;
-				
+
 				ao_gps_data.altitude = nav_posllh.alt_msl / 1000;
 				ao_gps_data.latitude = nav_posllh.lat;
 				ao_gps_data.longitude = nav_posllh.lon;
@@ -667,7 +742,7 @@ ao_gps(void) __reentrant
 				ao_gps_data.ground_speed = nav_velned.g_speed;
 				ao_gps_data.climb_rate = -nav_velned.vel_d;
 				ao_gps_data.course = nav_velned.heading / 200000;
-				
+
 				ao_gps_tracking_data.channels = 0;
 
 				struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0];
@@ -686,8 +761,8 @@ ao_gps(void) __reentrant
 				}
 
 				ao_mutex_put(&ao_gps_mutex);
-				ao_wakeup(&ao_gps_data);
-				ao_wakeup(&ao_gps_tracking_data);
+				ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
+				ao_wakeup(&ao_gps_new);
 				break;
 			}
 			break;
@@ -695,10 +770,32 @@ ao_gps(void) __reentrant
 	}
 }
 
+#if AO_UBLOX_DEBUG
+static void ao_gps_option(void)
+{
+	ao_cmd_hex();
+	if (ao_cmd_status != ao_cmd_success) {
+		ao_cmd_status = ao_cmd_success;
+		ao_gps_show();
+	} else {
+		ao_gps_dbg_enable = ao_cmd_lex_i;
+		printf ("gps debug set to %d\n", ao_gps_dbg_enable);
+	}
+}
+#else
+#define ao_gps_option ao_gps_show
+#endif
+
+__code struct ao_cmds ao_gps_cmds[] = {
+	{ ao_gps_option, 	"g\0Display GPS" },
+	{ 0, NULL },
+};
+
 __xdata struct ao_task ao_gps_task;
 
 void
 ao_gps_init(void)
 {
+	ao_cmd_register(&ao_gps_cmds[0]);
 	ao_add_task(&ao_gps_task, ao_gps, "gps");
 }
diff --git a/src/drivers/ao_m25.c b/src/drivers/ao_m25.c
index 390637d7..e6c7bb4d 100644
--- a/src/drivers/ao_m25.c
+++ b/src/drivers/ao_m25.c
@@ -82,11 +82,11 @@ __pdata uint16_t	ao_storage_unit;
 
 #if M25_MAX_CHIPS > 1
 static uint8_t ao_m25_size[M25_MAX_CHIPS];	/* number of sectors in each chip */
-static uint8_t ao_m25_pin[M25_MAX_CHIPS];	/* chip select pin for each chip */
+static ao_port_t ao_m25_pin[M25_MAX_CHIPS];	/* chip select pin for each chip */
 static uint8_t ao_m25_numchips;			/* number of chips detected */
 #endif
 static uint8_t ao_m25_total;			/* total sectors available */
-static uint8_t ao_m25_wip;			/* write in progress */
+static ao_port_t ao_m25_wip;			/* write in progress */
 
 static __xdata uint8_t ao_m25_mutex;
 
@@ -112,7 +112,7 @@ static __xdata uint8_t	ao_m25_instruction[4];
  * Block until the specified chip is done writing
  */
 static void
-ao_m25_wait_wip(uint8_t cs)
+ao_m25_wait_wip(ao_port_t cs)
 {
 	if (ao_m25_wip & cs) {
 		M25_SELECT(cs);
@@ -132,7 +132,7 @@ ao_m25_wait_wip(uint8_t cs)
  * so that future operations will block until the WIP bit goes off
  */
 static void
-ao_m25_write_enable(uint8_t cs)
+ao_m25_write_enable(ao_port_t cs)
 {
 	M25_SELECT(cs);
 	ao_m25_instruction[0] = M25_WREN;
@@ -146,7 +146,7 @@ ao_m25_write_enable(uint8_t cs)
  * Returns the number of 64kB sectors
  */
 static uint8_t
-ao_m25_read_capacity(uint8_t cs)
+ao_m25_read_capacity(ao_port_t cs)
 {
 	uint8_t	capacity;
 	M25_SELECT(cs);
@@ -166,12 +166,13 @@ ao_m25_read_capacity(uint8_t cs)
 	return 1 << (capacity - 0x10);
 }
 
-static uint8_t
+static ao_port_t
 ao_m25_set_address(uint32_t pos)
 {
-	uint8_t	chip;
+	ao_port_t	mask;
 #if M25_MAX_CHIPS > 1
 	uint8_t	size;
+	uint8_t		chip;
 
 	for (chip = 0; chip < ao_m25_numchips; chip++) {
 		size = ao_m25_size[chip];
@@ -182,16 +183,16 @@ ao_m25_set_address(uint32_t pos)
 	if (chip == ao_m25_numchips)
 		return 0xff;
 
-	chip = ao_m25_pin[chip];
+	mask = ao_m25_pin[chip];
 #else
-	chip = AO_M25_SPI_CS_MASK;
+	mask = AO_M25_SPI_CS_MASK;
 #endif
-	ao_m25_wait_wip(chip);
+	ao_m25_wait_wip(mask);
 
 	ao_m25_instruction[1] = pos >> 16;
 	ao_m25_instruction[2] = pos >> 8;
 	ao_m25_instruction[3] = pos;
-	return chip;
+	return mask;
 }
 
 /*
@@ -239,7 +240,7 @@ ao_m25_scan(void)
 uint8_t
 ao_storage_erase(uint32_t pos) __reentrant
 {
-	uint8_t	cs;
+	ao_port_t	cs;
 
 	if (pos >= ao_storage_total || pos + ao_storage_block > ao_storage_total)
 		return 0;
@@ -249,7 +250,6 @@ ao_storage_erase(uint32_t pos) __reentrant
 
 	cs = ao_m25_set_address(pos);
 
-	ao_m25_wait_wip(cs);
 	ao_m25_write_enable(cs);
 
 	ao_m25_instruction[0] = M25_SE;
@@ -268,7 +268,7 @@ ao_storage_erase(uint32_t pos) __reentrant
 uint8_t
 ao_storage_device_write(uint32_t pos, __xdata void *d, uint16_t len) __reentrant
 {
-	uint8_t	cs;
+	ao_port_t	cs;
 
 	if (pos >= ao_storage_total || pos + len > ao_storage_total)
 		return 0;
@@ -295,7 +295,7 @@ ao_storage_device_write(uint32_t pos, __xdata void *d, uint16_t len) __reentrant
 uint8_t
 ao_storage_device_read(uint32_t pos, __xdata void *d, uint16_t len) __reentrant
 {
-	uint8_t	cs;
+	ao_port_t	cs;
 
 	if (pos >= ao_storage_total || pos + len > ao_storage_total)
 		return 0;
@@ -332,7 +332,7 @@ void
 ao_storage_device_info(void) __reentrant
 {
 #if M25_DEBUG
-	uint8_t	cs;
+	ao_port_t	cs;
 #endif
 #if M25_MAX_CHIPS > 1
 	uint8_t chip;
diff --git a/src/drivers/ao_mma655x.c b/src/drivers/ao_mma655x.c
index 28fe1e08..ce83a5a3 100644
--- a/src/drivers/ao_mma655x.c
+++ b/src/drivers/ao_mma655x.c
@@ -206,10 +206,7 @@ ao_mma655x_setup(void)
 	ao_mma655x_reg_write(AO_MMA655X_AXISCFG,
 			     AXISCFG_VALUE |
 			     (1 << AO_MMA655X_AXISCFG_ST));
-	for (i = 0; i < 10; i++) {
-		a_st = ao_mma655x_value();
-		printf ("SELF-TEST %2d = %6d\n", i, a_st);
-	}
+	a_st = ao_mma655x_value();
 
 	stdefl = ao_mma655x_reg_read(AO_MMA655X_STDEFL);
 
@@ -218,11 +215,6 @@ ao_mma655x_setup(void)
 			     (0 << AO_MMA655X_AXISCFG_ST));
 	a = ao_mma655x_value();
 
-	for (i = 0; i < 10; i++) {
-		a = ao_mma655x_value();
-		printf("NORMAL   %2d = %6d\n", i, a);
-	}
-
 	ao_mma655x_reg_write(AO_MMA655X_DEVCFG,
 			     DEVCFG_VALUE | (1 << AO_MMA655X_DEVCFG_ENDINIT));
 	s0 = ao_mma655x_reg_read(AO_MMA655X_SN0);
@@ -234,8 +226,6 @@ ao_mma655x_setup(void)
 	serial = lot & 0x1fff;
 	lot >>= 12;
 	pn = ao_mma655x_reg_read(AO_MMA655X_PN);
-	printf ("MMA655X lot %d serial %d number %d\n", lot, serial, pn);
-
 }
 
 uint16_t	ao_mma655x_current;
diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index c65aecbc..f8ce7346 100644
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -19,84 +19,94 @@
 #include <ao_mpu6000.h>
 #include <ao_exti.h>
 
+#if HAS_MPU6000
+
 static uint8_t	ao_mpu6000_wake;
 static uint8_t	ao_mpu6000_configured;
 
-#define ao_mpu6000_spi_get() 	ao_spi_get_bit(AO_MPU6000_SPI_CS_PORT,	\
-					       AO_MPU6000_SPI_CS_PIN,	\
-					       AO_MPU6000_SPI_CS,	\
-					       AO_MPU6000_SPI_BUS,	\
-					       AO_SPI_SPEED_1MHz)
+#ifndef AO_MPU6000_I2C_INDEX
+#define AO_MPU6000_SPI	1
+#else
+#define AO_MPU6000_SPI	0
+#endif
+
+#if AO_MPU6000_SPI
+
+#define ao_mpu6000_spi_get()	ao_spi_get(AO_MPU6000_SPI_BUS, AO_SPI_SPEED_1MHz)
+#define ao_mpu6000_spi_put()	ao_spi_put(AO_MPU6000_SPI_BUS)
+
+#define ao_mpu6000_spi_start() 	ao_spi_set_cs(AO_MPU6000_SPI_CS_PORT,	\
+					      (1 << AO_MPU6000_SPI_CS_PIN))
+
+#define ao_mpu6000_spi_end() 	ao_spi_clr_cs(AO_MPU6000_SPI_CS_PORT,	\
+					      (1 << AO_MPU6000_SPI_CS_PIN))
 
-#define ao_mpu6000_spi_put() 	ao_spi_put_bit(AO_MPU6000_SPI_CS_PORT,	\
-					       AO_MPU6000_SPI_CS_PIN,	\
-					       AO_MPU6000_SPI_CS,	\
-					       AO_MPU6000_SPI_BUS)
+#endif
 
 
 static void
-ao_mpu6000_reg_write(uint8_t addr, uint8_t value)
+_ao_mpu6000_reg_write(uint8_t addr, uint8_t value)
 {
 	uint8_t	d[2] = { addr, value };
-#ifdef AO_MPU6000_I2C_INDEX
+#if AO_MPU6000_SPI
+	ao_mpu6000_spi_start();
+	ao_spi_send(d, 2, AO_MPU6000_SPI_BUS);
+	ao_mpu6000_spi_end();
+#else
 	ao_i2c_get(AO_MPU6000_I2C_INDEX);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
 	ao_i2c_send(d, 2, AO_MPU6000_I2C_INDEX, TRUE);
 	ao_i2c_put(AO_MPU6000_I2C_INDEX);
-#else
-	ao_mpu6000_spi_get();
-	ao_spi_send(d, 2, AO_MPU6000_SPI_BUS);
-	ao_mpu6000_spi_put();
 #endif
 }
 
 static void
-ao_mpu6000_read(uint8_t addr, void *data, uint8_t len)
+_ao_mpu6000_read(uint8_t addr, void *data, uint8_t len)
 {
-#ifdef AO_MPU6000_I2C_INDEX
+#if AO_MPU6000_SPI
+	addr |= 0x80;
+	ao_mpu6000_spi_start();
+	ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
+	ao_spi_recv(data, len, AO_MPU6000_SPI_BUS);
+	ao_mpu6000_spi_end();
+#else
 	ao_i2c_get(AO_MPU6000_I2C_INDEX);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
 	ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_READ);
 	ao_i2c_recv(data, len, AO_MPU6000_I2C_INDEX, TRUE);
 	ao_i2c_put(AO_MPU6000_I2C_INDEX);
-#else
-	addr |= 0x80;
-	ao_mpu6000_spi_get();
-	ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
-	ao_spi_recv(data, len, AO_MPU6000_SPI_BUS);
-	ao_mpu6000_spi_put();
 #endif
 }
 
 static uint8_t
-ao_mpu6000_reg_read(uint8_t addr)
+_ao_mpu6000_reg_read(uint8_t addr)
 {
 	uint8_t	value;
-#ifdef AO_MPU6000_I2C_INDEX
+#if AO_MPU6000_SPI
+	addr |= 0x80;
+	ao_mpu6000_spi_start();
+	ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
+	ao_spi_recv(&value, 1, AO_MPU6000_SPI_BUS);
+	ao_mpu6000_spi_end();
+#else
 	ao_i2c_get(AO_MPU6000_I2C_INDEX);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
 	ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_READ);
 	ao_i2c_recv(&value, 1, AO_MPU6000_I2C_INDEX, TRUE);
 	ao_i2c_put(AO_MPU6000_I2C_INDEX);
-#else
-	addr |= 0x80;
-	ao_mpu6000_spi_get();
-	ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
-	ao_spi_recv(&value, 1, AO_MPU6000_SPI_BUS);
-	ao_mpu6000_spi_put();
 #endif
 	return value;
 }
 
 static void
-ao_mpu6000_sample(struct ao_mpu6000_sample *sample)
+_ao_mpu6000_sample(struct ao_mpu6000_sample *sample)
 {
 	uint16_t	*d = (uint16_t *) sample;
 	int		i = sizeof (*sample) / 2;
 
-	ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, sample, sizeof (*sample));
+	_ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, sample, sizeof (*sample));
 #if __BYTE_ORDER == __LITTLE_ENDIAN
 	/* byte swap */
 	while (i--) {
@@ -108,6 +118,7 @@ ao_mpu6000_sample(struct ao_mpu6000_sample *sample)
 
 #define G	981	/* in cm/s² */
 
+#if 0
 static int16_t /* cm/s² */
 ao_mpu6000_accel(int16_t v)
 {
@@ -119,16 +130,17 @@ ao_mpu6000_gyro(int16_t v)
 {
 	return (int16_t) ((v * (int32_t) 20000) / 32767);
 }
+#endif
 
 static uint8_t
 ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which)
 {
 	int16_t	diff = test - normal;
 
-	if (diff < MPU6000_ST_ACCEL(16) / 2) {
+	if (diff < MPU6000_ST_ACCEL(16) / 4) {
 		return 1;
 	}
-	if (diff > MPU6000_ST_ACCEL(16) * 2) {
+	if (diff > MPU6000_ST_ACCEL(16) * 4) {
 		return 1;
 	}
 	return 0;
@@ -141,142 +153,168 @@ ao_mpu6000_gyro_check(int16_t normal, int16_t test, char *which)
 
 	if (diff < 0)
 		diff = -diff;
-	if (diff < MPU6000_ST_GYRO(2000) / 2) {
+	if (diff < MPU6000_ST_GYRO(2000) / 4) {
 		return 1;
 	}
-	if (diff > MPU6000_ST_GYRO(2000) * 2) {
+	if (diff > MPU6000_ST_GYRO(2000) * 4) {
 		return 1;
 	}
 	return 0;
 }
 
 static void
-ao_mpu6000_setup(void)
+_ao_mpu6000_wait_alive(void)
+{
+	uint8_t	i;
+
+	/* Wait for the chip to wake up */
+	for (i = 0; i < 30; i++) {
+		ao_delay(AO_MS_TO_TICKS(100));
+		if (_ao_mpu6000_reg_read(MPU6000_WHO_AM_I) == 0x68)
+			break;
+	}
+	if (i == 30)
+		ao_panic(AO_PANIC_SELF_TEST_MPU6000);
+}
+
+#define ST_TRIES	10
+
+static void
+_ao_mpu6000_setup(void)
 {
 	struct ao_mpu6000_sample	normal_mode, test_mode;
-	int				errors =0;
+	int				errors;
+	int				st_tries;
 
 	if (ao_mpu6000_configured)
 		return;
 
+	_ao_mpu6000_wait_alive();
+
 	/* Reset the whole chip */
 	
-	ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
-			     (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET));
+	_ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
+			      (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET));
 
 	/* Wait for it to reset. If we talk too quickly, it appears to get confused */
-	ao_delay(AO_MS_TO_TICKS(100));
 
-	/* Reset signal conditioning */
-	ao_mpu6000_reg_write(MPU6000_USER_CONTROL,
-			     (0 << MPU6000_USER_CONTROL_FIFO_EN) |
-			     (0 << MPU6000_USER_CONTROL_I2C_MST_EN) |
-			     (0 << MPU6000_USER_CONTROL_I2C_IF_DIS) |
-			     (0 << MPU6000_USER_CONTROL_FIFO_RESET) |
-			     (0 << MPU6000_USER_CONTROL_I2C_MST_RESET) |
-			     (1 << MPU6000_USER_CONTROL_SIG_COND_RESET));
+	_ao_mpu6000_wait_alive();
 
-	while (ao_mpu6000_reg_read(MPU6000_USER_CONTROL) & (1 << MPU6000_USER_CONTROL_SIG_COND_RESET))
-		ao_yield();
+	/* Reset signal conditioning, disabling I2C on SPI systems */
+	_ao_mpu6000_reg_write(MPU6000_USER_CTRL,
+			      (0 << MPU6000_USER_CTRL_FIFO_EN) |
+			      (0 << MPU6000_USER_CTRL_I2C_MST_EN) |
+			      (AO_MPU6000_SPI << MPU6000_USER_CTRL_I2C_IF_DIS) |
+			      (0 << MPU6000_USER_CTRL_FIFO_RESET) |
+			      (0 << MPU6000_USER_CTRL_I2C_MST_RESET) |
+			      (1 << MPU6000_USER_CTRL_SIG_COND_RESET));
+
+	while (_ao_mpu6000_reg_read(MPU6000_USER_CTRL) & (1 << MPU6000_USER_CTRL_SIG_COND_RESET))
+		ao_delay(AO_MS_TO_TICKS(10));
 
 	/* Reset signal paths */
-	ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
-			     (1 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
-			     (1 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
-			     (1 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
+	_ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
+			      (1 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
+			      (1 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
+			      (1 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
 
-	ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
-			     (0 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
-			     (0 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
-			     (0 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
+	_ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
+			      (0 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
+			      (0 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
+			      (0 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
 
 	/* Select clocks, disable sleep */
-	ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
-			     (0 << MPU6000_PWR_MGMT_1_DEVICE_RESET) |
-			     (0 << MPU6000_PWR_MGMT_1_SLEEP) |
-			     (0 << MPU6000_PWR_MGMT_1_CYCLE) |
-			     (0 << MPU6000_PWR_MGMT_1_TEMP_DIS) |
-			     (MPU6000_PWR_MGMT_1_CLKSEL_PLL_X_AXIS << MPU6000_PWR_MGMT_1_CLKSEL));
+	_ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
+			      (0 << MPU6000_PWR_MGMT_1_DEVICE_RESET) |
+			      (0 << MPU6000_PWR_MGMT_1_SLEEP) |
+			      (0 << MPU6000_PWR_MGMT_1_CYCLE) |
+			      (0 << MPU6000_PWR_MGMT_1_TEMP_DIS) |
+			      (MPU6000_PWR_MGMT_1_CLKSEL_PLL_X_AXIS << MPU6000_PWR_MGMT_1_CLKSEL));
 
-	/* Set sample rate divider to sample at full speed
-	ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, 0);
+	/* Set sample rate divider to sample at full speed */
+	_ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, 0);
 
 	/* Disable filtering */
-	ao_mpu6000_reg_write(MPU6000_CONFIG,
-			     (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
-			     (MPU6000_CONFIG_DLPF_CFG_260_256 << MPU6000_CONFIG_DLPF_CFG));
-
-	/* Configure accelerometer to +/-16G in self-test mode */
-	ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
-			     (1 << MPU600_ACCEL_CONFIG_XA_ST) |
-			     (1 << MPU600_ACCEL_CONFIG_YA_ST) |
-			     (1 << MPU600_ACCEL_CONFIG_ZA_ST) |
-			     (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
-
-	/* Configure gyro to +/- 2000°/s in self-test mode */
-	ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
-			     (1 << MPU600_GYRO_CONFIG_XG_ST) |
-			     (1 << MPU600_GYRO_CONFIG_YG_ST) |
-			     (1 << MPU600_GYRO_CONFIG_ZG_ST) |
-			     (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
-
-	ao_delay(AO_MS_TO_TICKS(200));
-	ao_mpu6000_sample(&test_mode);
+	_ao_mpu6000_reg_write(MPU6000_CONFIG,
+			      (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
+			      (MPU6000_CONFIG_DLPF_CFG_260_256 << MPU6000_CONFIG_DLPF_CFG));
 
 #if TRIDGE
 	// read the product ID rev c has 1/2 the sensitivity of rev d
-    _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID);
-    //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
-
-    if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) ||
-        (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) {
-        // Accel scale 8g (4096 LSB/g)
-        // Rev C has different scaling than rev D
-        register_write(MPUREG_ACCEL_CONFIG,1<<3);
-    } else {
-        // Accel scale 8g (4096 LSB/g)
-        register_write(MPUREG_ACCEL_CONFIG,2<<3);
-    }
-    hal.scheduler->delay(1);
-
+	_mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID);
+	//Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
+
+	if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) ||
+	    (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) {
+		// Accel scale 8g (4096 LSB/g)
+		// Rev C has different scaling than rev D
+		register_write(MPUREG_ACCEL_CONFIG,1<<3);
+	} else {
+		// Accel scale 8g (4096 LSB/g)
+		register_write(MPUREG_ACCEL_CONFIG,2<<3);
+	}
+	hal.scheduler->delay(1);
 #endif
 
-	/* Configure accelerometer to +/-16G */
-	ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
-			     (0 << MPU600_ACCEL_CONFIG_XA_ST) |
-			     (0 << MPU600_ACCEL_CONFIG_YA_ST) |
-			     (0 << MPU600_ACCEL_CONFIG_ZA_ST) |
-			     (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
-
-	/* Configure gyro to +/- 2000°/s */
-	ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
-			     (0 << MPU600_GYRO_CONFIG_XG_ST) |
-			     (0 << MPU600_GYRO_CONFIG_YG_ST) |
-			     (0 << MPU600_GYRO_CONFIG_ZG_ST) |
-			     (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
-
-	ao_delay(AO_MS_TO_TICKS(10));
-	ao_mpu6000_sample(&normal_mode);
+	for (st_tries = 0; st_tries < ST_TRIES; st_tries++) {
+		errors = 0;
+
+		/* Configure accelerometer to +/-16G in self-test mode */
+		_ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
+				      (1 << MPU600_ACCEL_CONFIG_XA_ST) |
+				      (1 << MPU600_ACCEL_CONFIG_YA_ST) |
+				      (1 << MPU600_ACCEL_CONFIG_ZA_ST) |
+				      (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
+
+		/* Configure gyro to +/- 2000°/s in self-test mode */
+		_ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
+				      (1 << MPU600_GYRO_CONFIG_XG_ST) |
+				      (1 << MPU600_GYRO_CONFIG_YG_ST) |
+				      (1 << MPU600_GYRO_CONFIG_ZG_ST) |
+				      (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
+
+		ao_delay(AO_MS_TO_TICKS(200));
+		_ao_mpu6000_sample(&test_mode);
+
+		/* Configure accelerometer to +/-16G */
+		_ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
+				      (0 << MPU600_ACCEL_CONFIG_XA_ST) |
+				      (0 << MPU600_ACCEL_CONFIG_YA_ST) |
+				      (0 << MPU600_ACCEL_CONFIG_ZA_ST) |
+				      (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
+
+		/* Configure gyro to +/- 2000°/s */
+		_ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
+				      (0 << MPU600_GYRO_CONFIG_XG_ST) |
+				      (0 << MPU600_GYRO_CONFIG_YG_ST) |
+				      (0 << MPU600_GYRO_CONFIG_ZG_ST) |
+				      (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
+
+		ao_delay(AO_MS_TO_TICKS(200));
+		_ao_mpu6000_sample(&normal_mode);
 	
-	errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x");
-	errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y");
-	errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z");
-
-	errors += ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x");
-	errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y");
-	errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z");
+		errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x");
+		errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y");
+		errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z");
+
+		errors += ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x");
+		errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y");
+		errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z");
+		if (!errors)
+			break;
+	}
 
-	if (errors)
-		ao_panic(AO_PANIC_SELF_TEST_MPU6000);
+	if (st_tries == ST_TRIES)
+		ao_sensor_errors = 1;
 
 	/* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */
-	ao_mpu6000_reg_write(MPU6000_CONFIG,
-			     (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
-			     (MPU6000_CONFIG_DLPF_CFG_94_98 << MPU6000_CONFIG_DLPF_CFG));
+	_ao_mpu6000_reg_write(MPU6000_CONFIG,
+			      (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
+			      (MPU6000_CONFIG_DLPF_CFG_94_98 << MPU6000_CONFIG_DLPF_CFG));
 
 	/* Set sample rate divider to sample at 200Hz (v = gyro/rate - 1) */
-	ao_mpu6000_reg_write(MPU6000_SMPRT_DIV,
-			     1000 / 200 - 1);
+	_ao_mpu6000_reg_write(MPU6000_SMPRT_DIV,
+			      1000 / 200 - 1);
 	
 	ao_delay(AO_MS_TO_TICKS(100));
 	ao_mpu6000_configured = 1;
@@ -287,10 +325,20 @@ struct ao_mpu6000_sample	ao_mpu6000_current;
 static void
 ao_mpu6000(void)
 {
-	ao_mpu6000_setup();
+	/* ao_mpu6000_init already grabbed the SPI bus and mutex */
+	_ao_mpu6000_setup();
+#if AO_MPU6000_SPI
+	ao_mpu6000_spi_put();
+#endif
 	for (;;)
 	{
-		ao_mpu6000_sample(&ao_mpu6000_current);
+#if AO_MPU6000_SPI
+		ao_mpu6000_spi_get();
+#endif
+		_ao_mpu6000_sample(&ao_mpu6000_current);
+#if AO_MPU6000_SPI
+		ao_mpu6000_spi_put();
+#endif	
 		ao_arch_critical(
 			AO_DATA_PRESENT(AO_DATA_MPU6000);
 			AO_DATA_WAIT();
@@ -326,5 +374,20 @@ ao_mpu6000_init(void)
 	ao_mpu6000_configured = 0;
 
 	ao_add_task(&ao_mpu6000_task, ao_mpu6000, "mpu6000");
+	
+#if AO_MPU6000_SPI
+	ao_spi_init_cs(AO_MPU6000_SPI_CS_PORT, (1 << AO_MPU6000_SPI_CS_PIN));
+
+	/* Pretend to be the mpu6000 task. Grab the SPI bus right away and
+	 * hold it for the task so that nothing else uses the SPI bus before
+	 * we get the I2C mode disabled in the chip
+	 */
+
+	ao_cur_task = &ao_mpu6000_task;
+	ao_spi_get(AO_MPU6000_SPI_BUS, AO_SPI_SPEED_1MHz);
+	ao_cur_task = NULL;
+#endif	
+
 	ao_cmd_register(&ao_mpu6000_cmds[0]);
 }
+#endif
diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h
index f01e9e83..dc3a9fbf 100644
--- a/src/drivers/ao_mpu6000.h
+++ b/src/drivers/ao_mpu6000.h
@@ -18,6 +18,10 @@
 #ifndef _AO_MPU6000_H_
 #define _AO_MPU6000_H_
 
+#ifndef M_PI
+#define M_PI 3.1415926535897832384626433
+#endif
+
 #define MPU6000_ADDR_WRITE	0xd0
 #define MPU6000_ADDR_READ	0xd1
 
@@ -133,13 +137,13 @@
 #define MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET	1
 #define MPU6000_SIGNAL_PATH_RESET_TEMP_RESET	0
 
-#define MPU6000_USER_CONTROL		0x6a
-#define MPU6000_USER_CONTROL_FIFO_EN		6
-#define MPU6000_USER_CONTROL_I2C_MST_EN		5
-#define MPU6000_USER_CONTROL_I2C_IF_DIS		4
-#define MPU6000_USER_CONTROL_FIFO_RESET		2
-#define MPU6000_USER_CONTROL_I2C_MST_RESET	1
-#define MPU6000_USER_CONTROL_SIG_COND_RESET	0
+#define MPU6000_USER_CTRL		0x6a
+#define MPU6000_USER_CTRL_FIFO_EN		6
+#define MPU6000_USER_CTRL_I2C_MST_EN		5
+#define MPU6000_USER_CTRL_I2C_IF_DIS		4
+#define MPU6000_USER_CTRL_FIFO_RESET		2
+#define MPU6000_USER_CTRL_I2C_MST_RESET		1
+#define MPU6000_USER_CTRL_SIG_COND_RESET	0
 
 #define MPU6000_PWR_MGMT_1	0x6b
 #define MPU6000_PWR_MGMT_1_DEVICE_RESET		7
@@ -166,9 +170,20 @@
 /* Self test gyro is approximately 50°/s */
 #define MPU6000_ST_GYRO(full_scale)	((int16_t) (((int32_t) 32767 * (int32_t) 50) / (full_scale)))
 
-#define MPU6000_GYRO_FULLSCALE	2000
+#define MPU6000_GYRO_FULLSCALE	((float) 2000 * M_PI/180.0)
+
+static inline float
+ao_mpu6000_gyro(float sensor) {
+	return sensor * ((float) (MPU6000_GYRO_FULLSCALE / 32767.0));
+}
+
 #define MPU6000_ACCEL_FULLSCALE	16
 
+static inline float
+ao_mpu6000_accel(int16_t sensor) {
+	return (float) sensor * ((float) (MPU6000_ACCEL_FULLSCALE * GRAVITY / 32767.0));
+}
+
 struct ao_mpu6000_sample {
 	int16_t		accel_x;
 	int16_t		accel_y;
diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c
index bd57400e..58ab9197 100644
--- a/src/drivers/ao_ms5607.c
+++ b/src/drivers/ao_ms5607.c
@@ -21,19 +21,17 @@
 
 #if HAS_MS5607 || HAS_MS5611
 
-static struct ao_ms5607_prom	ms5607_prom;
-static uint8_t	  		ms5607_configured;
+static __xdata struct ao_ms5607_prom	ms5607_prom;
+static __xdata uint8_t	  		ms5607_configured;
 
 static void
 ao_ms5607_start(void) {
-	ao_spi_get(AO_MS5607_SPI_INDEX,AO_SPI_SPEED_FAST);
-	ao_gpio_set(AO_MS5607_CS_PORT, AO_MS5607_CS_PIN, AO_MS5607_CS, 0);
+	ao_spi_get_bit(AO_MS5607_CS_PORT, AO_MS5607_CS_PIN, AO_MS5607_CS, AO_MS5607_SPI_INDEX, AO_SPI_SPEED_FAST);
 }
 
 static void
 ao_ms5607_stop(void) {
-	ao_gpio_set(AO_MS5607_CS_PORT, AO_MS5607_CS_PIN, AO_MS5607_CS, 1);
-	ao_spi_put(AO_MS5607_SPI_INDEX);
+	ao_spi_put_bit(AO_MS5607_CS_PORT, AO_MS5607_CS_PIN, AO_MS5607_CS, AO_MS5607_SPI_INDEX);
 }
 
 static void
@@ -42,7 +40,7 @@ ao_ms5607_reset(void) {
 
 	cmd = AO_MS5607_RESET;
 	ao_ms5607_start();
-	ao_spi_send(&cmd, 1, AO_MS5607_SPI_INDEX);
+	ao_spi_send(DATA_TO_XDATA(&cmd), 1, AO_MS5607_SPI_INDEX);
 	ao_delay(AO_MS_TO_TICKS(10));
 	ao_ms5607_stop();
 }
@@ -71,17 +69,17 @@ ao_ms5607_crc(uint8_t *prom)
 }
 
 static void
-ao_ms5607_prom_read(struct ao_ms5607_prom *prom)
+ao_ms5607_prom_read(__xdata struct ao_ms5607_prom *prom)
 {
-	uint8_t		addr;
-	uint8_t		crc;
-	uint16_t	*r;
+	uint8_t			addr;
+	uint8_t			crc;
+	__xdata uint16_t	*r;
 
-	r = (uint16_t *) prom;
+	r = (__xdata uint16_t *) prom;
 	for (addr = 0; addr < 8; addr++) {
 		uint8_t	cmd = AO_MS5607_PROM_READ(addr);
 		ao_ms5607_start();
-		ao_spi_send(&cmd, 1, AO_MS5607_SPI_INDEX);
+		ao_spi_send(DATA_TO_XDATA(&cmd), 1, AO_MS5607_SPI_INDEX);
 		ao_spi_recv(r, 2, AO_MS5607_SPI_INDEX);
 		ao_ms5607_stop();
 		r++;
@@ -116,25 +114,25 @@ ao_ms5607_setup(void)
 	ao_ms5607_prom_read(&ms5607_prom);
 }
 
-static volatile uint8_t	ao_ms5607_done;
+static __xdata volatile uint8_t	ao_ms5607_done;
 
 static void
 ao_ms5607_isr(void)
 {
 	ao_exti_disable(AO_MS5607_MISO_PORT, AO_MS5607_MISO_PIN);
 	ao_ms5607_done = 1;
-	ao_wakeup((void *) &ao_ms5607_done);
+	ao_wakeup((__xdata void *) &ao_ms5607_done);
 }
 
 static uint32_t
 ao_ms5607_get_sample(uint8_t cmd) {
-	uint8_t	reply[3];
-	uint8_t read;
+	__xdata uint8_t	reply[3];
+	__xdata uint8_t read;
 
 	ao_ms5607_done = 0;
 
 	ao_ms5607_start();
-	ao_spi_send(&cmd, 1, AO_MS5607_SPI_INDEX);
+	ao_spi_send(DATA_TO_XDATA(&cmd), 1, AO_MS5607_SPI_INDEX);
 
 	ao_exti_enable(AO_MS5607_MISO_PORT, AO_MS5607_MISO_PIN);
 
@@ -142,7 +140,8 @@ ao_ms5607_get_sample(uint8_t cmd) {
 	ao_spi_put(AO_MS5607_SPI_INDEX);
 #endif
 	ao_arch_block_interrupts();
-	while (!ao_ms5607_done)
+	while (!ao_gpio_get(AO_MS5607_MISO_PORT, AO_MS5607_MISO_PIN, AO_MS5607_MISO) &&
+	       !ao_ms5607_done)
 		ao_sleep((void *) &ao_ms5607_done);
 	ao_arch_release_interrupts();
 #if AO_MS5607_PRIVATE_PINS
@@ -175,16 +174,20 @@ ao_ms5607_get_sample(uint8_t cmd) {
 #define AO_CONVERT_D2	token_evaluator(AO_MS5607_CONVERT_D2_, AO_MS5607_TEMP_OVERSAMPLE)
 
 void
-ao_ms5607_sample(struct ao_ms5607_sample *sample)
+ao_ms5607_sample(__xdata struct ao_ms5607_sample *sample)
 {
 	sample->pres = ao_ms5607_get_sample(AO_CONVERT_D1);
 	sample->temp = ao_ms5607_get_sample(AO_CONVERT_D2);
 }
 
+#ifdef _CC1111_H_
+#include "ao_ms5607_convert_8051.c"
+#else
 #include "ao_ms5607_convert.c"
+#endif
 
 #if HAS_TASK
-struct ao_ms5607_sample	ao_ms5607_current;
+__xdata struct ao_ms5607_sample	ao_ms5607_current;
 
 static void
 ao_ms5607(void)
@@ -193,10 +196,10 @@ ao_ms5607(void)
 	for (;;)
 	{
 		ao_ms5607_sample(&ao_ms5607_current);
-		ao_arch_critical(
-			AO_DATA_PRESENT(AO_DATA_MS5607);
-			AO_DATA_WAIT();
-			);
+		ao_arch_block_interrupts();
+		AO_DATA_PRESENT(AO_DATA_MS5607);
+		AO_DATA_WAIT();
+		ao_arch_release_interrupts();
 	}
 }
 
@@ -218,11 +221,11 @@ ao_ms5607_info(void)
 static void
 ao_ms5607_dump(void)
 {
-	struct ao_ms5607_value value;
+	__xdata struct ao_ms5607_value value;
 
 	ao_ms5607_convert(&ao_ms5607_current, &value);
-	printf ("Pressure:    %8u %8d\n", ao_ms5607_current.pres, value.pres);
-	printf ("Temperature: %8u %8d\n", ao_ms5607_current.temp, value.temp);
+	printf ("Pressure:    %8lu %8ld\n", ao_ms5607_current.pres, value.pres);
+	printf ("Temperature: %8lu %8ld\n", ao_ms5607_current.temp, value.temp);
 	printf ("Altitude: %ld\n", ao_pa_to_altitude(value.pres));
 }
 
@@ -249,17 +252,9 @@ ao_ms5607_init(void)
 	 */
 	ao_exti_setup(AO_MS5607_MISO_PORT,
 		      AO_MS5607_MISO_PIN,
-		      AO_EXTI_MODE_RISING,
+		      AO_EXTI_MODE_RISING|
+		      AO_EXTI_PIN_NOCONFIGURE,
 		      ao_ms5607_isr);
-
-#ifdef STM_MODER_ALTERNATE
-	/* Reset the pin from INPUT to ALTERNATE so that SPI works
-	 * This needs an abstraction at some point...
-	 */
-	stm_moder_set(AO_MS5607_MISO_PORT,
-		      AO_MS5607_MISO_PIN,
-		      STM_MODER_ALTERNATE);
-#endif
 }
 
 #endif
diff --git a/src/drivers/ao_ms5607.h b/src/drivers/ao_ms5607.h
index b2f98a59..206efd64 100644
--- a/src/drivers/ao_ms5607.h
+++ b/src/drivers/ao_ms5607.h
@@ -56,7 +56,7 @@ struct ao_ms5607_value {
 	int32_t		temp;	/* in °C * 100 */
 };
 
-extern struct ao_ms5607_sample	ao_ms5607_current;
+extern __xdata struct ao_ms5607_sample	ao_ms5607_current;
 
 void
 ao_ms5607_setup(void);
@@ -68,12 +68,13 @@ void
 ao_ms5607_info(void);
 
 void
-ao_ms5607_sample(struct ao_ms5607_sample *sample);
+ao_ms5607_sample(__xdata struct ao_ms5607_sample *sample);
 
 void
-ao_ms5607_convert(struct ao_ms5607_sample *sample, struct ao_ms5607_value *value);
+ao_ms5607_convert(__xdata struct ao_ms5607_sample *sample,
+		  __xdata struct ao_ms5607_value *value);
 
 void
-ao_ms5607_get_prom(struct ao_ms5607_prom *prom);
+ao_ms5607_get_prom(__data struct ao_ms5607_prom *prom);
 
 #endif /* _AO_MS5607_H_ */
diff --git a/src/drivers/ao_ms5607_convert.c b/src/drivers/ao_ms5607_convert.c
index e61d19ed..bfb952a4 100644
--- a/src/drivers/ao_ms5607_convert.c
+++ b/src/drivers/ao_ms5607_convert.c
@@ -42,11 +42,14 @@ ao_ms5607_convert(struct ao_ms5607_sample *sample, struct ao_ms5607_value *value
 		int32_t TEMPM = TEMP - 2000;
 		int64_t OFF2 = (61 * (int64_t) TEMPM * (int64_t) TEMPM) >> 4;
 		int64_t SENS2 = 2 * (int64_t) TEMPM * (int64_t) TEMPM;
-		if (TEMP < 1500) {
+		if (TEMP < -1500) {
 			int32_t TEMPP = TEMP + 1500;
-			int64_t TEMPP2 = TEMPP * TEMPP;
-			OFF2 = OFF2 + 15 * TEMPP2;
-			SENS2 = SENS2 + 8 * TEMPP2;
+			/* You'd think this would need a 64-bit int, but
+			 * that would imply a temperature below -327.67°C...
+			 */
+			int32_t TEMPP2 = TEMPP * TEMPP;
+			OFF2 = OFF2 + (int64_t) 15 * TEMPP2;
+			SENS2 = SENS2 + (int64_t) 8 * TEMPP2;
 		}
 		TEMP -= T2;
 		OFF -= OFF2;
diff --git a/src/drivers/ao_ms5607_convert_8051.c b/src/drivers/ao_ms5607_convert_8051.c
new file mode 100644
index 00000000..f3a48c46
--- /dev/null
+++ b/src/drivers/ao_ms5607_convert_8051.c
@@ -0,0 +1,136 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao_ms5607.h>
+#include <ao_int64.h>
+
+#if HAS_MS5611
+#define SHIFT_OFF	16
+#define SHIFT_TCO	7
+#define SHIFT_SENS	15
+#define SHFIT_TCS	8
+#else
+#define SHIFT_OFF	17
+#define SHIFT_TCO	6
+#define SHIFT_SENS	16
+#define SHIFT_TCS	7
+#endif
+
+void
+ao_ms5607_convert(__xdata struct ao_ms5607_sample *sample,
+		  __xdata struct ao_ms5607_value *value)
+{
+	__LOCAL int32_t	dT;
+	__LOCAL int32_t TEMP;
+	__LOCAL ao_int64_t OFF;
+	__LOCAL ao_int64_t SENS;
+	__LOCAL ao_int64_t a;
+
+	dT = sample->temp - ((int32_t) ms5607_prom.tref << 8);
+	
+	/* TEMP = 2000 + (((int64_t) dT * ms5607_prom.tempsens) >> 23); */
+	ao_mul64_32_32(&a, dT, ms5607_prom.tempsens);
+	ao_rshift64(&a, &a, 23);
+	TEMP = 2000 + a.low;
+	/* */
+
+	/* OFF = ((int64_t) ms5607_prom.off << SHIFT_OFF) + (((int64_t) ms5607_prom.tco * dT) >> SHIFT_TCO);*/
+#if SHIFT_OFF > 16
+	OFF.high = ms5607_prom.off >> (32 - SHIFT_OFF);
+#else
+	OFF.high = 0;
+#endif
+	OFF.low = (uint32_t) ms5607_prom.off << SHIFT_OFF;
+	ao_mul64_32_32(&a, ms5607_prom.tco, dT);
+	ao_rshift64(&a, &a, SHIFT_TCO);
+	ao_plus64(&OFF, &OFF, &a);
+	/**/
+
+	/* SENS = ((int64_t) ms5607_prom.sens << SHIFT_SENS) + (((int64_t) ms5607_prom.tcs * dT) >> SHIFT_TCS); */
+	SENS.high = 0;
+	SENS.low = (uint32_t) ms5607_prom.sens << SHIFT_SENS;
+	ao_mul64_32_32(&a, ms5607_prom.tcs, dT);
+	ao_rshift64(&a, &a, SHIFT_TCS);
+	ao_plus64(&SENS, &SENS, &a);
+	/**/
+
+	if (TEMP < 2000) {
+		__LOCAL int32_t	T2;
+		__LOCAL int32_t TEMPM;
+		__LOCAL ao_int64_t OFF2;
+		__LOCAL ao_int64_t SENS2;
+
+		/* T2 = ((int64_t) dT * (int64_t) dT) >> 31; */
+		ao_mul64_32_32(&a, dT, dT);
+		T2 = (a.low >> 31) | (a.high << 1);
+		/**/
+
+		TEMPM = TEMP - 2000;
+
+		/* OFF2 = (61 * (int64_t) TEMPM * (int64_t) TEMPM) >> 4; */
+		ao_mul64_32_32(&OFF2, TEMPM, TEMPM);
+		ao_mul64_64_16(&OFF2, &OFF2, 61);
+		ao_rshift64(&OFF2, &OFF2, 4);
+		/**/
+		
+		/* SENS2 = 2 * (int64_t) TEMPM * (int64_t) TEMPM; */
+		ao_mul64_32_32(&SENS2, TEMPM, TEMPM);
+		ao_lshift64(&SENS2, &SENS2, 1);
+		/**/
+
+		if (TEMP < -1500) {
+			int32_t TEMPP;
+			int32_t TEMPP2;
+
+			TEMPP = TEMP + 1500;
+			TEMPP2 = TEMPP * TEMPP;
+
+			/* OFF2 = OFF2 + 15 * TEMPP2; */
+			ao_mul64_32_32(&a, 15, TEMPP2);
+			ao_plus64(&OFF2, &OFF2, &a);
+			/**/
+
+			/* SENS2 = SENS2 + 8 * TEMPP2; */
+			a.high = 0;
+			a.low = TEMPP2;
+			ao_lshift64(&a, &a, 3);
+			ao_plus64(&SENS2, &SENS2, &a);
+			/**/
+		}
+		TEMP -= T2;
+
+		/* OFF -= OFF2; */
+		ao_minus64(&OFF, &OFF, &OFF2);
+		/**/
+
+		/* SENS -= SENS2; */
+		ao_minus64(&SENS, &SENS, &SENS2);
+		/**/
+	}
+
+	/* value->pres = ((((int64_t) sample->pres * SENS) >> 21) - OFF) >> 15; */
+	a.high = 0;
+	a.low = sample->pres;
+	ao_mul64(&a, &a, &SENS);
+	ao_rshift64(&a, &a, 21);
+	ao_minus64(&a, &a, &OFF);
+	ao_rshift64(&a, &a, 15);
+	value->pres = a.low;
+	/**/
+	
+	value->temp = TEMP;
+}
diff --git a/src/drivers/ao_pad.c b/src/drivers/ao_pad.c
index 6cec98ab..62ae68e9 100644
--- a/src/drivers/ao_pad.c
+++ b/src/drivers/ao_pad.c
@@ -17,7 +17,7 @@
 
 #include <ao.h>
 #include <ao_pad.h>
-#include <ao_74hc497.h>
+#include <ao_74hc165.h>
 #include <ao_radio_cmac.h>
 
 static __xdata uint8_t ao_pad_ignite;
@@ -27,6 +27,7 @@ static __pdata uint8_t	ao_pad_armed;
 static __pdata uint16_t	ao_pad_arm_time;
 static __pdata uint8_t	ao_pad_box;
 static __xdata uint8_t	ao_pad_disabled;
+static __pdata uint16_t	ao_pad_packet_time;
 
 #define DEBUG	1
 
@@ -135,6 +136,12 @@ ao_pad_monitor(void)
 			query.arm_status = AO_PAD_ARM_STATUS_UNKNOWN;
 			arm_beep_time = 0;
 		}
+		if ((ao_time() - ao_pad_packet_time) > AO_SEC_TO_TICKS(2))
+			cur |= AO_LED_RED;
+		else if (ao_radio_cmac_rssi < -90)
+			cur |= AO_LED_AMBER;
+		else
+			cur |= AO_LED_GREEN;
 
 		for (c = 0; c < AO_PAD_NUM; c++) {
 			int16_t		sense = packet->adc.sense[c];
@@ -171,9 +178,10 @@ ao_pad_monitor(void)
 			query.igniter_status[c] = status;
 		}
 		if (cur != prev) {
-			PRINTD("change leds from %02x to %02x mask %02x\n",
-			       prev, cur, AO_LED_CONTINUITY_MASK|AO_LED_ARMED);
-			ao_led_set_mask(cur, AO_LED_CONTINUITY_MASK | AO_LED_ARMED);
+			PRINTD("change leds from %02x to %02x\n",
+			       prev, cur);
+			FLUSHD();
+			ao_led_set(cur);
 			prev = cur;
 		}
 
@@ -182,10 +190,7 @@ ao_pad_monitor(void)
 
 		if (ao_pad_armed) {
 			ao_strobe(1);
-			if (sample & 2)
-				ao_siren(1);
-			else
-				ao_siren(0);
+			ao_siren(1);
 			beeping = 1;
 		} else if (query.arm_status == AO_PAD_ARM_STATUS_ARMED && !beeping) {
 			if (arm_beep_time == 0) {
@@ -218,6 +223,21 @@ ao_pad_enable(void)
 	ao_wakeup (&ao_pad_disabled);
 }
 
+#if HAS_74HC165
+static uint8_t
+ao_pad_read_box(void)
+{
+	uint8_t		byte = ao_74hc165_read();
+	uint8_t		h, l;
+
+	h = byte >> 4;
+	l = byte & 0xf;
+	return h * 10 + l;
+}
+#else
+#define ao_pad_read_box()	0
+#endif
+
 static void
 ao_pad(void)
 {
@@ -226,18 +246,20 @@ ao_pad(void)
 
 	ao_pad_box = 0;
 	ao_led_set(0);
-	ao_led_on(AO_LED_POWER);
 	for (;;) {
 		FLUSHD();
 		while (ao_pad_disabled)
 			ao_sleep(&ao_pad_disabled);
 		ret = ao_radio_cmac_recv(&command, sizeof (command), 0);
-		PRINTD ("cmac_recv %d\n", ret);
+		PRINTD ("cmac_recv %d %d\n", ret, ao_radio_cmac_rssi);
 		if (ret != AO_RADIO_CMAC_OK)
 			continue;
+		ao_pad_packet_time = ao_time();
 		
-		PRINTD ("tick %d box %d cmd %d channels %02x\n",
-			command.tick, command.box, command.cmd, command.channels);
+		ao_pad_box = ao_pad_read_box();
+
+		PRINTD ("tick %d box %d (me %d) cmd %d channels %02x\n",
+			command.tick, command.box, ao_pad_box, command.cmd, command.channels);
 
 		switch (command.cmd) {
 		case AO_LAUNCH_ARM:
@@ -327,7 +349,7 @@ ao_pad_test(void)
 	}
 
 	for (c = 0; c < AO_PAD_NUM; c++) {
-		printf ("Pad %d: ");
+		printf ("Pad %d: ", c);
 		switch (query.igniter_status[c]) {
 		case AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_CLOSED:	printf ("No igniter. Relay closed\n"); break;
 		case AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN:	printf ("No igniter. Relay open\n"); break;
@@ -362,6 +384,26 @@ ao_pad_set_debug(void)
 	if (ao_cmd_status == ao_cmd_success)
 		ao_pad_debug = ao_cmd_lex_i != 0;
 }
+
+
+static void
+ao_pad_alarm_debug(void)
+{
+	uint8_t	which, value;
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	which = ao_cmd_lex_i;
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	value = ao_cmd_lex_i;
+	printf ("Set %s to %d\n", which ? "siren" : "strobe", value);
+	if (which)
+		ao_siren(value);
+	else
+		ao_strobe(value);
+}
 #endif
 
 __code struct ao_cmds ao_pad_cmds[] = {
@@ -369,6 +411,7 @@ __code struct ao_cmds ao_pad_cmds[] = {
 	{ ao_pad_manual,	"i <key> <n>\0Fire igniter. <key> is doit with D&I" },
 #if DEBUG
 	{ ao_pad_set_debug,	"D <0 off, 1 on>\0Debug" },
+	{ ao_pad_alarm_debug,	"S <0 strobe, 1 siren> <0 off, 1 on>\0Set alarm output" },
 #endif
 	{ 0, NULL }
 };
diff --git a/src/drivers/ao_pca9922.c b/src/drivers/ao_pca9922.c
index 6d8d18d8..d376b968 100644
--- a/src/drivers/ao_pca9922.c
+++ b/src/drivers/ao_pca9922.c
@@ -30,9 +30,12 @@ ao_led_apply(void)
 	/* Don't try the SPI bus during initialization */
 	if (!ao_cur_task)
 		return;
-	ao_spi_get_bit(AO_PCA9922_CS_PORT, AO_PCA9922_CS_PIN, AO_PCA9922_CS, AO_PCA9922_SPI_BUS, AO_SPI_SPEED_FAST);
+	ao_spi_get(AO_PCA9922_SPI_BUS);
+	ao_spi_set_speed(AO_PCA9922_SPI_BUS,AO_SPI_SPEED_FAST);
+	AO_PCA9922_CS = 1;
 	ao_spi_send(&ao_led_state, 1, AO_PCA9922_SPI_BUS);
-	ao_spi_put_bit(AO_PCA9922_CS_PORT, AO_PCA9922_CS_PIN, AO_PCA9922_CS, AO_PCA9922_SPI_BUS);
+	AO_PCA9922_CS = 0;
+	ao_spi_put(AO_PCA9922_SPI_BUS);
 }
 
 void
diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c
index 6cc2467a..d07488d0 100644
--- a/src/drivers/ao_quadrature.c
+++ b/src/drivers/ao_quadrature.c
@@ -18,16 +18,12 @@
 #include <ao.h>
 #include <ao_quadrature.h>
 #include <ao_exti.h>
-#if AO_EVENT
+#include <ao_fast_timer.h>
 #include <ao_event.h>
-#define ao_quadrature_queue(q)	ao_event_put_isr(AO_EVENT_QUADRATURE, q, ao_quadrature_count[q])
-#else
-#define ao_quadrature_queue(q)
-#endif
 
 __xdata int32_t ao_quadrature_count[AO_QUADRATURE_COUNT];
-
 static uint8_t	ao_quadrature_state[AO_QUADRATURE_COUNT];
+static int8_t	ao_quadrature_raw[AO_QUADRATURE_COUNT];
 
 #define BIT(a,b)	((a) | ((b) << 1))
 #define STATE(old_a, old_b, new_a, new_b)	(((BIT(old_a, old_b) << 2) | BIT(new_a, new_b)))
@@ -35,41 +31,76 @@ static uint8_t	ao_quadrature_state[AO_QUADRATURE_COUNT];
 #define port(q)	AO_QUADRATURE_ ## q ## _PORT
 #define bita(q) AO_QUADRATURE_ ## q ## _A
 #define bitb(q) AO_QUADRATURE_ ## q ## _B
+#define pina(q) AO_QUADRATURE_ ## q ## _A ## _PIN
+#define pinb(q) AO_QUADRATURE_ ## q ## _B ## _PIN
+#define isr(q)  ao_quadrature_isr_ ## q
 
-#define ao_quadrature_update(q) do {					\
-		ao_quadrature_state[q] = ((ao_quadrature_state[q] & 3) << 2); \
-		ao_quadrature_state[q] |= ao_gpio_get(port(q), bita(q), 0); \
-		ao_quadrature_state[q] |= ao_gpio_get(port(q), bitb(q), 0) << 1; \
-	} while (0)
-	
+static inline uint16_t
+ao_quadrature_read(struct stm_gpio *gpio, uint8_t pin_a, uint8_t pin_b) {
+	uint16_t	v = stm_gpio_get_all(gpio);
+
+	return ~((((v >> pin_a) & 1) | (((v >> pin_b) & 1) << 1))) & 3;
+}
+
+#define _ao_quadrature_get(q)	ao_quadrature_read(port(q), bita(q), bitb(q))
 
 static void
-ao_quadrature_isr(void)
+_ao_quadrature_queue(uint8_t q, int8_t step)
 {
-	uint8_t	q;
-#if AO_QUADRATURE_COUNT > 0
-	ao_quadrature_update(0);
-#endif
-#if AO_QUADRATURE_COUNT > 1
-	ao_quadrature_update(1);
+	ao_quadrature_count[q] += step;
+#if AO_EVENT
+	ao_event_put_isr(AO_EVENT_QUADRATURE, q, step);
 #endif
+	ao_wakeup(&ao_quadrature_count[q]);
+}
 
-	for (q = 0; q < AO_QUADRATURE_COUNT; q++) {
-		switch (ao_quadrature_state[q]) {
-		case STATE(0, 1, 0, 0):
-			ao_quadrature_count[q]++;
-			break;
-		case STATE(1, 0, 0, 0):
-			ao_quadrature_count[q]--;
-			break;
-		default:
-			continue;
-		}
-		ao_quadrature_queue(q);
-		ao_wakeup(&ao_quadrature_count[q]);
+static const int8_t	step[16] = {
+	[STATE(0,0,0,0)] = 0,
+	[STATE(0,0,0,1)] = -1,
+	[STATE(0,0,1,0)] = 1,
+	[STATE(0,0,1,1)] = 0,
+	[STATE(0,1,0,0)] = 1,
+	[STATE(0,1,1,0)] = 0,
+	[STATE(0,1,1,1)] = -1,
+	[STATE(1,0,0,0)] = -1,
+	[STATE(1,0,0,1)] = 0,
+	[STATE(1,0,1,0)] = 0,
+	[STATE(1,0,1,1)] = 1,
+	[STATE(1,1,0,0)] = 0,
+	[STATE(1,1,0,1)] = 1,
+	[STATE(1,1,1,0)] = -1,
+	[STATE(1,1,1,1)] = 0
+};
+
+static void
+_ao_quadrature_set(uint8_t q, uint8_t value) {
+	uint8_t v;
+	
+	v = ao_quadrature_state[q] & 3;
+	value = value & 3;
+
+	if (v == value)
+		return;
+	
+	ao_quadrature_state[q] = (v << 2) | value;
+
+	ao_quadrature_raw[q] += step[ao_quadrature_state[q]];
+	if (value == 0) {
+		if (ao_quadrature_raw[q] == 4)
+			_ao_quadrature_queue(q, 1);
+		else if (ao_quadrature_raw[q] == -4)
+			_ao_quadrature_queue(q, -1);
+		ao_quadrature_raw[q] = 0;
 	}
 }
 
+static void
+ao_quadrature_isr(void)
+{
+	_ao_quadrature_set(0, _ao_quadrature_get(0));
+	_ao_quadrature_set(1, _ao_quadrature_get(1));
+}
+
 int32_t
 ao_quadrature_poll(uint8_t q)
 {
@@ -92,6 +123,15 @@ ao_quadrature_test(void)
 
 	ao_cmd_decimal();
 	q = ao_cmd_lex_i;
+	if (q >= AO_QUADRATURE_COUNT) {
+		ao_cmd_status = ao_cmd_syntax_error;
+		return;
+	}
+	printf ("count %d state %x raw %d\n",
+		ao_quadrature_count[q],
+		ao_quadrature_state[q],
+		ao_quadrature_raw[q]);
+#if 0
 	for (;;) {
 		int32_t	c;
 		flush();
@@ -100,6 +140,7 @@ ao_quadrature_test(void)
 		if (c == 100)
 			break;
 	}
+#endif
 }
 
 static const struct ao_cmds ao_quadrature_cmds[] = {
@@ -107,18 +148,9 @@ static const struct ao_cmds ao_quadrature_cmds[] = {
 	{ 0, NULL }
 };
 
-#define init(q) do {							\
-		ao_enable_port(port(q));				\
-									\
-		ao_exti_setup(port(q), bita(q),				\
-			      AO_QUADRATURE_MODE|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, \
-			      ao_quadrature_isr);			\
-		ao_exti_enable(port(q), bita(q));			\
-									\
-		ao_exti_setup(port(q), bitb(q),				\
-			      AO_QUADRATURE_MODE|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, \
-			      ao_quadrature_isr);			\
-		ao_exti_enable(port(q), bitb(q));			\
+#define init(q) do {					\
+		ao_enable_input(port(q), bita(q), 0);	\
+		ao_enable_input(port(q), bitb(q), 0);	\
 	} while (0)
 
 void
@@ -130,5 +162,7 @@ ao_quadrature_init(void)
 #if AO_QUADRATURE_COUNT > 1
 	init(1);
 #endif
+	ao_fast_timer_init();
+	ao_fast_timer_on(ao_quadrature_isr);
 	ao_cmd_register(&ao_quadrature_cmds[0]);
 }
diff --git a/src/drivers/ublox-csum.5c b/src/drivers/ublox-csum.5c
new file mode 100644
index 00000000..4e0c7c5a
--- /dev/null
+++ b/src/drivers/ublox-csum.5c
@@ -0,0 +1,23 @@
+#!/usr/bin/nickle
+string[] speeds = { "57600", "19200", "9600" };
+
+string make_set_nmea(string speed) {
+	return sprintf ("PUBX,41,1,3,1,%s,0", speed);
+}
+
+int csum(string x) {
+	int csum = 0;
+	for (int i = 0; i < String::length(x); i++)
+		csum ^= x[i];
+	return csum;
+}
+
+for (int i = 0; i < dim(speeds); i++) {
+	string s = make_set_nmea(speeds[i]);
+	int c = csum(s);
+	printf ("/* $%s* */\n", s);
+	printf ("#define SERIAL_SPEED_STRING   \"%s\"\n", speeds[i]);
+	printf ("#define SERIAL_SPEED_CHECKSUM \"%02x\"\n", c);
+}
+	
+	
diff --git a/src/easymini-v1.0/.gitignore b/src/easymini-v1.0/.gitignore
new file mode 100644
index 00000000..e5f7d586
--- /dev/null
+++ b/src/easymini-v1.0/.gitignore
@@ -0,0 +1,2 @@
+ao_product.h
+*.elf
diff --git a/src/easymini-v1.0/Makefile b/src/easymini-v1.0/Makefile
new file mode 100644
index 00000000..7dae2692
--- /dev/null
+++ b/src/easymini-v1.0/Makefile
@@ -0,0 +1,83 @@
+#
+# AltOS build
+#
+#
+
+include ../lpc/Makefile.defs
+
+INC = \
+	ao.h \
+	ao_arch.h \
+	ao_arch_funcs.h \
+	ao_pins.h \
+	ao_product.h \
+	lpc.h
+
+#
+# Common AltOS sources
+#
+ALTOS_SRC = \
+	ao_interrupt.c \
+	ao_boot_chain.c \
+	ao_romconfig.c \
+	ao_product.c \
+	ao_mutex.c \
+	ao_panic.c \
+	ao_stdio.c \
+	ao_storage.c \
+	ao_report.c \
+	ao_ignite.c \
+	ao_flight.c \
+	ao_kalman.c \
+	ao_sample.c \
+	ao_data.c \
+	ao_convert_pa.c \
+	ao_task.c \
+	ao_log.c \
+	ao_log_mini.c \
+	ao_cmd.c \
+	ao_config.c \
+	ao_timer_lpc.c \
+	ao_exti_lpc.c \
+	ao_usb_lpc.c \
+	ao_spi_lpc.c \
+	ao_adc_lpc.c \
+	ao_beep_lpc.c \
+	ao_m25.c \
+	ao_ms5607.c
+
+PRODUCT=EasyMini-v1.0
+PRODUCT_DEF=-DEASYMINI_V_1_0
+IDPRODUCT=0x0026
+
+CFLAGS = $(PRODUCT_DEF) $(LPC_CFLAGS) -g -Os
+
+PROGNAME=easymini-v1.0
+PROG=$(PROGNAME)-$(VERSION).elf
+HEX=$(PROGNAME)-$(VERSION).ihx
+
+SRC=$(ALTOS_SRC) ao_easymini.c
+OBJ=$(SRC:.c=.o)
+
+all: $(PROG) $(HEX)
+
+$(PROG): Makefile $(OBJ) altos.ld
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS)
+
+ao_product.h: ao-make-product.5c ../Version
+	$(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@
+
+$(OBJ): $(INC)
+
+load: $(PROG)
+	lpc-load $(PROG)
+
+distclean:	clean
+
+clean:
+	rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx
+	rm -f ao_product.h
+
+install:
+
+uninstall:
diff --git a/src/easymini-v1.0/ao_easymini.c b/src/easymini-v1.0/ao_easymini.c
new file mode 100644
index 00000000..97230b61
--- /dev/null
+++ b/src/easymini-v1.0/ao_easymini.c
@@ -0,0 +1,46 @@
+/*
+ * Copyright © 2011 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+#include <ao_exti.h>
+
+void
+main(void)
+{
+	ao_clock_init();
+	ao_task_init();
+	ao_timer_init();
+	ao_exti_init();
+
+	ao_beep_init();
+
+	ao_adc_init();
+	ao_spi_init();
+	ao_storage_init();
+
+	ao_usb_init();
+
+	ao_cmd_init();
+	ao_flight_init();
+	ao_ms5607_init();
+	ao_log_init();
+	ao_report_init();
+	ao_igniter_init();
+	ao_config_init();
+
+	ao_start_scheduler();
+}
diff --git a/src/easymini-v1.0/ao_pins.h b/src/easymini-v1.0/ao_pins.h
new file mode 100644
index 00000000..e721030d
--- /dev/null
+++ b/src/easymini-v1.0/ao_pins.h
@@ -0,0 +1,136 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#define HAS_BEEP	1
+#define	HAS_LED		0
+
+#define AO_STACK_SIZE	384
+
+#define IS_FLASH_LOADER	0
+
+/* Crystal on the board */
+#define AO_LPC_CLKIN	12000000
+
+/* Main clock frequency. 48MHz for USB so we don't use the USB PLL */
+#define AO_LPC_CLKOUT	48000000
+
+/* System clock frequency */
+#define AO_LPC_SYSCLK	24000000
+
+#define HAS_USB		1
+
+#define HAS_USB_CONNECT	0
+#define HAS_USB_VBUS	0
+#define HAS_USB_PULLUP	1
+#define AO_USB_PULLUP_PORT	0
+#define AO_USB_PULLUP_PIN	20
+
+#define PACKET_HAS_SLAVE	0
+
+#define AO_LOG_FORMAT		AO_LOG_FORMAT_EASYMINI
+
+/* USART */
+
+#define HAS_SERIAL		0
+#define USE_SERIAL_0_STDIN	1
+#define SERIAL_0_18_19		1
+#define SERIAL_0_14_15		0
+#define SERIAL_0_17_18		0
+#define SERIAL_0_26_27		0
+
+/* SPI */
+
+#define HAS_SPI_0		1
+#define SPI_SCK0_P0_6		1
+#define HAS_SPI_1		1
+#define SPI_SCK1_P1_15		1
+#define SPI_MISO1_P0_22		1
+#define SPI_MOSI1_P0_21		1
+
+/* M25 */
+
+#define M25_MAX_CHIPS		1
+#define AO_M25_SPI_CS_PORT	0
+#define AO_M25_SPI_CS_MASK	(1 << 23)
+#define AO_M25_SPI_BUS		1
+
+/* MS5607 */
+
+#define HAS_MS5607		1
+#define HAS_MS5611		0
+#define AO_MS5607_PRIVATE_PINS	0
+#define AO_MS5607_CS_PORT	0
+#define AO_MS5607_CS_PIN	7
+#define AO_MS5607_CS_MASK	(1 << AO_MS5607_CS_PIN)
+#define AO_MS5607_MISO_PORT	0
+#define AO_MS5607_MISO_PIN	8
+#define AO_MS5607_MISO_MASK	(1 << AO_MS5607_MISO_PIN)
+#define AO_MS5607_SPI_INDEX	0
+
+#define HAS_ACCEL		0
+#define HAS_GPS			0
+#define HAS_RADIO		0
+#define HAS_FLIGHT		1
+#define HAS_EEPROM		1
+#define HAS_TELEMETRY		0
+#define HAS_APRS		0
+#define HAS_LOG			1
+#define USE_INTERNAL_FLASH	0
+#define HAS_IGNITE		1
+#define HAS_IGNITE_REPORT	1
+
+#define AO_DATA_RING		16
+
+/*
+ * ADC
+ */
+
+#define HAS_ADC			1
+
+#define AO_NUM_ADC		3
+
+#define AO_ADC_0		1
+#define AO_ADC_1		1
+#define AO_ADC_2		1
+
+struct ao_adc {
+	int16_t		sense_a;
+	int16_t		sense_m;
+	int16_t		v_batt;
+};
+
+/*
+ * Igniter
+ */
+
+#define AO_IGNITER_CLOSED	400
+#define AO_IGNITER_OPEN		60
+
+#define AO_IGNITER_DROGUE_PORT	0
+#define AO_IGNITER_DROGUE_PIN	2
+#define AO_IGNITER_SET_DROGUE(v)	ao_gpio_set(AO_IGNITER_DROGUE_PORT, AO_IGNITER_DROGUE_PIN, AO_IGNITER_DROGUE, v)
+
+#define AO_IGNITER_MAIN_PORT	0
+#define AO_IGNITER_MAIN_PIN	3
+#define AO_IGNITER_SET_MAIN(v)		ao_gpio_set(AO_IGNITER_MAIN_PORT, AO_IGNITER_MAIN_PIN, AO_IGNITER_MAIN, v)
+
+#define AO_SENSE_DROGUE(p)	((p)->adc.sense_a)
+#define AO_SENSE_MAIN(p)	((p)->adc.sense_m)
+
+#define AO_ADC_DUMP(p) \
+	printf("tick: %5u apogee: %5d main: %5d batt: %5d\n", \
+	       (p)->tick, (p)->adc.sense_a, (p)->adc.sense_m, (p)->adc.v_batt)
diff --git a/src/easymini-v1.0/flash-loader/Makefile b/src/easymini-v1.0/flash-loader/Makefile
new file mode 100644
index 00000000..78bb4092
--- /dev/null
+++ b/src/easymini-v1.0/flash-loader/Makefile
@@ -0,0 +1,8 @@
+#
+# AltOS flash loader build
+#
+#
+
+TOPDIR=../..
+HARDWARE=easymini-v1.0
+include $(TOPDIR)/lpc/Makefile-flash.defs
diff --git a/src/easymini-v1.0/flash-loader/ao_pins.h b/src/easymini-v1.0/flash-loader/ao_pins.h
new file mode 100644
index 00000000..4330151d
--- /dev/null
+++ b/src/easymini-v1.0/flash-loader/ao_pins.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_PINS_H_
+#define _AO_PINS_H_
+
+#include <ao_flash_lpc_pins.h>
+
+#define AO_BOOT_PIN		1
+#define AO_BOOT_APPLICATION_GPIO	0
+#define AO_BOOT_APPLICATION_PIN		19
+#define AO_BOOT_APPLICATION_VALUE	1
+#define AO_BOOT_APPLICATION_MODE	AO_EXTI_MODE_PULL_UP
+
+#define HAS_USB_PULLUP	1
+#define AO_USB_PULLUP_PORT	0
+#define AO_USB_PULLUP_PIN	20
+
+#endif /* _AO_PINS_H_ */
diff --git a/src/lpc/Makefile-flash.defs b/src/lpc/Makefile-flash.defs
new file mode 100644
index 00000000..4a245d10
--- /dev/null
+++ b/src/lpc/Makefile-flash.defs
@@ -0,0 +1,61 @@
+include $(TOPDIR)/lpc/Makefile-lpc.defs
+
+INC = \
+	ao.h \
+	ao_arch.h \
+	ao_arch_funcs.h \
+	ao_flash_pins.h \
+	ao_flash_lpc_pins.h \
+	ao_flash_task.h \
+	ao_pins.h \
+	ao_product.h \
+	Makefile
+
+#
+# Common AltOS sources
+#
+SRC = \
+	ao_interrupt.c \
+	ao_romconfig.c \
+	ao_boot_chain.c \
+	ao_boot_pin.c \
+	ao_product.c \
+	ao_notask.c \
+	ao_timer_lpc.c \
+	ao_usb_lpc.c \
+	ao_flash_lpc.c \
+	ao_flash_task.c \
+	ao_flash_loader_lpc.c
+
+OBJ=$(SRC:.c=.o)
+
+PRODUCT=AltosFlash
+PRODUCT_DEF=-DALTOS_FLASH
+IDPRODUCT=0x000a
+
+CFLAGS = $(PRODUCT_DEF) $(LPC_CFLAGS) -g -Os
+
+LDFLAGS=$(CFLAGS) -L$(TOPDIR)/lpc -Wl,-Taltos-loader.ld
+
+PROGNAME=altos-flash
+PROG=$(HARDWARE)-$(PROGNAME)-$(VERSION).elf
+
+$(PROG): Makefile $(OBJ) altos-loader.ld
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS)
+
+ao_product.h: ao-make-product.5c $(TOPDIR)/Version
+	$(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@
+
+$(OBJ): $(INC)
+
+all: $(PROG)
+
+distclean:	clean
+
+clean:
+	rm -f *.o $(HARDWARE)-$(PROGNAME)-*.elf
+	rm -f ao_product.h
+
+install:
+
+uninstall:
diff --git a/src/lpc/Makefile-lpc.defs b/src/lpc/Makefile-lpc.defs
new file mode 100644
index 00000000..3d55cf67
--- /dev/null
+++ b/src/lpc/Makefile-lpc.defs
@@ -0,0 +1,45 @@
+ifndef TOPDIR
+TOPDIR=..
+endif
+
+include $(TOPDIR)/Makedefs
+
+vpath % $(TOPDIR)/lpc:$(TOPDIR)/product:$(TOPDIR)/drivers:$(TOPDIR)/core:$(TOPDIR)/util:$(TOPDIR)/kalman:$(TOPDIR/aes):$(TOPDIR)
+vpath make-altitude $(TOPDIR)/util
+vpath make-kalman $(TOPDIR)/util
+vpath kalman.5c $(TOPDIR)/kalman
+vpath kalman_filter.5c $(TOPDIR)/kalman
+vpath load_csv.5c $(TOPDIR)/kalman
+vpath matrix.5c $(TOPDIR)/kalman
+vpath ao-make-product.5c $(TOPDIR)/util
+
+.SUFFIXES: .elf .ihx
+
+.elf.ihx:
+	$(ELFTOHEX) --output=$@ $*.elf
+
+
+ifndef VERSION
+include $(TOPDIR)/Version
+endif
+
+ELFTOHEX=$(TOPDIR)/../ao-tools/ao-elftohex/ao-elftohex
+CC=$(ARM_CC)
+
+AO_CFLAGS=-I. -I$(TOPDIR)/lpc -I$(TOPDIR)/core -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR) $(PDCLIB_INCLUDES) 
+LPC_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m0 -mthumb -ffreestanding -nostdlib $(AO_CFLAGS)
+
+NICKLE=nickle
+
+LIBS=$(PDCLIB_LIBS_M0) -lgcc
+
+V=0
+# The user has explicitly enabled quiet compilation.
+ifeq ($(V),0)
+quiet = @printf "  $1 $2 $@\n"; $($1)
+endif
+# Otherwise, print the full command line.
+quiet ?= $($1)
+
+.c.o:
+	$(call quiet,CC) -c $(CFLAGS) -o $@ $<
diff --git a/src/lpc/Makefile.defs b/src/lpc/Makefile.defs
new file mode 100644
index 00000000..b6d739c2
--- /dev/null
+++ b/src/lpc/Makefile.defs
@@ -0,0 +1,15 @@
+ifndef TOPDIR
+TOPDIR=..
+endif
+
+include $(TOPDIR)/lpc/Makefile-lpc.defs
+include $(TOPDIR)/Makedefs
+
+LDFLAGS=$(CFLAGS) -L$(TOPDIR)/lpc -Wl,-Taltos.ld
+
+ao_serial_lpc.h: $(TOPDIR)/lpc/baud_rate ao_pins.h
+	nickle $(TOPDIR)/lpc/baud_rate `awk '/AO_LPC_CLKOUT/{print $$3}' ao_pins.h` > $@
+
+ao_serial_lpc.o: ao_serial_lpc.h
+
+.DEFAULT_GOAL=all
diff --git a/src/lpc/altos-loader.ld b/src/lpc/altos-loader.ld
new file mode 100644
index 00000000..9df6e456
--- /dev/null
+++ b/src/lpc/altos-loader.ld
@@ -0,0 +1,80 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+MEMORY {
+	rom : ORIGIN = 0x00000000, LENGTH = 4K
+	ram : ORIGIN = 0x10000000, LENGTH = 4k - 128 - 32
+	usb (!x) : ORIGIN = 0x20004000 + 2K - 256, LENGTH = 256
+	stack (!w) : ORIGIN = 0x10000000 + 4K - 128 - 32, LENGTH = 128
+}
+
+INCLUDE registers.ld
+
+EXTERN (lpc_interrupt_vector)
+
+SECTIONS {
+	/*
+	 * Rom contents
+	 */
+
+	.interrupt : {
+		__text_start__ = .;
+		*(.interrupt)	/* Interrupt vectors */
+
+	} > rom
+
+	.text ORIGIN(rom) + 0x100 : {
+		ao_romconfig.o(.romconfig*)
+		ao_product.o(.romconfig*)
+
+		*(.text*)	/* Executable code */
+		*(.ARM.exidx* .gnu.linkonce.armexidx.*)
+		*(.rodata*)	/* Constants */
+		__text_end__ = .;
+	} > rom
+
+	/* Boot data which must live at the start of ram so that
+	 * the application and bootloader share the same addresses.
+	 * This must be all uninitialized data
+	 */
+	.boot ORIGIN(ram) + SIZEOF(.interrupt) (NOLOAD) : {
+		__boot_start__ = .;
+		*(.boot*)
+		__boot_end__ = .;
+	} >ram
+
+	/* Data -- relocated to RAM, but written to ROM
+	 */
+	.data : {
+		__data_start__ = .;
+		*(.data*)	/* initialized data */
+		__data_end__ = .;
+	} >ram AT>rom
+
+
+	.bss : {
+		__bss_start__ = .;
+		*(.bss*)
+		*(COMMON*)
+		__bss_end__ = .;
+	} >ram
+
+	PROVIDE(__stack__ = ORIGIN(ram) + LENGTH(ram));
+	PROVIDE(end = .);
+}
+
+ENTRY(start);
diff --git a/src/lpc/altos-standalone.ld b/src/lpc/altos-standalone.ld
new file mode 100644
index 00000000..032406f8
--- /dev/null
+++ b/src/lpc/altos-standalone.ld
@@ -0,0 +1,85 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+MEMORY {
+	rom (rx) : ORIGIN = 0x00000000, LENGTH = 32K
+	ram (!w) : ORIGIN = 0x10000000, LENGTH = 4K - 128 - 32
+	usb (!x) : ORIGIN = 0x20004000 + 2K - 256, LENGTH = 256
+	stack (!w) : ORIGIN = 0x10000000 + 4K - 128 - 32, LENGTH = 128
+}
+
+INCLUDE registers.ld
+
+EXTERN (lpc_interrupt_vector)
+
+SECTIONS {
+	/*
+	 * Rom contents
+	 */
+
+	.text ORIGIN(rom) : {
+		__text_start__ = .;
+		*(.interrupt)	/* Interrupt vectors */
+
+		. = ORIGIN(rom) + 0x100;
+
+		ao_romconfig.o(.romconfig*)
+		ao_product.o(.romconfig*)
+
+		*(.text*)	/* Executable code */
+		*(.rodata*)	/* Constants */
+
+	} > rom
+
+	.ARM.exidx : {
+		*(.ARM.exidx* .gnu.linkonce.armexidx.*)
+		__text_end__ = .;
+	} > rom
+
+	/* Boot data which must live at the start of ram so that
+	 * the application and bootloader share the same addresses.
+	 * This must be all uninitialized data
+	 */
+	.boot (NOLOAD) : {
+		__boot_start__ = .;
+		*(.boot)
+		. = ALIGN(4);
+		__boot_end__ = .;
+	} >ram
+
+	/* Data -- relocated to RAM, but written to ROM
+	 */
+	.data ORIGIN(ram) : AT (ADDR(.ARM.exidx) + SIZEOF (.ARM.exidx)) {
+		__data_start__ = .;
+		*(.data)	/* initialized data */
+		__data_end__ = .;
+		__bss_start__ = .;
+	} >ram
+
+	.bss : {
+		*(.bss)
+		*(COMMON)
+		__bss_end__ = .;
+	} >ram
+	PROVIDE(end = .);
+
+	PROVIDE(__stack__ = ORIGIN(stack) + LENGTH(stack));
+}
+
+ENTRY(start);
+
+
diff --git a/src/lpc/altos.ld b/src/lpc/altos.ld
new file mode 100644
index 00000000..00d4f18a
--- /dev/null
+++ b/src/lpc/altos.ld
@@ -0,0 +1,90 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+MEMORY {
+	rom (rx) : ORIGIN = 0x00001000, LENGTH = 28K
+	ram (!w) : ORIGIN = 0x10000000, LENGTH = 4K - 128
+	usb (!x) : ORIGIN = 0x20004000 + 2K - 256, LENGTH = 256
+	stack (!w) : ORIGIN = 0x10000000 + 4K - 128, LENGTH = 128
+}
+
+INCLUDE registers.ld
+
+EXTERN (lpc_interrupt_vector)
+
+SECTIONS {
+	/*
+	 * Rom contents
+	 */
+
+	.interrupt ORIGIN(ram) : AT (ORIGIN(rom)) {
+		__interrupt_start__ = .;
+		__interrupt_rom__ = ORIGIN(rom);
+		*(.interrupt)	/* Interrupt vectors */
+		__interrupt_end__ = .;
+	} > ram
+
+	.text ORIGIN(rom) + 0x100 : {
+		__text_start__ = .;
+
+		ao_romconfig.o(.romconfig*)
+		ao_product.o(.romconfig*)
+
+		*(.text*)	/* Executable code */
+		*(.rodata*)	/* Constants */
+
+	} > rom
+
+	.ARM.exidx : {
+		*(.ARM.exidx* .gnu.linkonce.armexidx.*)
+		__text_end__ = .;
+	} > rom
+
+	/* Boot data which must live at the start of ram so that
+	 * the application and bootloader share the same addresses.
+	 * This must be all uninitialized data
+	 */
+	.boot : {
+		__boot_start__ = .;
+		*(.boot)
+		. = ALIGN(4);
+		__boot_end__ = .;
+	} >ram
+
+	/* Data -- relocated to RAM, but written to ROM
+	 */
+	.data : AT (ADDR(.ARM.exidx) + SIZEOF (.ARM.exidx)) {
+		__data_start__ = .;
+		*(.data)	/* initialized data */
+		__data_end__ = .;
+		__bss_start__ = .;
+	} >ram
+
+	.bss : {
+		__bss_start__ = .;
+		*(.bss)
+		*(COMMON)
+		__bss_end__ = .;
+	} >ram
+	PROVIDE(end = .);
+
+	PROVIDE(__stack__ = ORIGIN(stack) + LENGTH(stack));
+}
+
+ENTRY(start);
+
+
diff --git a/src/lpc/ao_adc_lpc.c b/src/lpc/ao_adc_lpc.c
new file mode 100644
index 00000000..7005f86e
--- /dev/null
+++ b/src/lpc/ao_adc_lpc.c
@@ -0,0 +1,216 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+#include <ao_data.h>
+
+#ifndef AO_ADC_0
+#define AO_ADC_0	0
+#endif
+
+#ifndef AO_ADC_1
+#define AO_ADC_1	0
+#endif
+
+#ifndef AO_ADC_2
+#define AO_ADC_2	0
+#endif
+
+#ifndef AO_ADC_3
+#define AO_ADC_3	0
+#endif
+
+#ifndef AO_ADC_4
+#define AO_ADC_4	0
+#endif
+
+#ifndef AO_ADC_5
+#define AO_ADC_5	0
+#endif
+
+#ifndef AO_ADC_6
+#define AO_ADC_6	0
+#endif
+
+#ifndef AO_ADC_7
+#define AO_ADC_7	0
+#endif
+
+#define AO_ADC_NUM	(AO_ADC_0 + AO_ADC_1 + AO_ADC_2 + AO_ADC_3 + \
+			 AO_ADC_4 + AO_ADC_5 + AO_ADC_6 + AO_ADC_7)
+
+/* ADC clock is divided by this value + 1, which ensures that
+ * the ADC clock will be strictly less than 4.5MHz as required
+ */
+#define AO_ADC_CLKDIV	(AO_LPC_SYSCLK / 450000)
+
+static uint8_t		ao_adc_ready;
+static uint8_t		ao_adc_sequence;
+
+static const uint8_t	ao_adc_mask_seq[AO_ADC_NUM] = {
+#if AO_ADC_0
+	1 << 0,
+#endif
+#if AO_ADC_1
+	1 << 1,
+#endif
+#if AO_ADC_2
+	1 << 2,
+#endif
+#if AO_ADC_3
+	1 << 3,
+#endif
+#if AO_ADC_4
+	1 << 4,
+#endif
+#if AO_ADC_5
+	1 << 6,
+#endif
+#if AO_ADC_6
+	1 << 6,
+#endif
+#if AO_ADC_7
+	1 << 7,
+#endif
+};
+
+#define sample(id)	(*out++ = (uint16_t) lpc_adc.dr[id] >> 1)
+
+static inline void lpc_adc_start(void) {
+	lpc_adc.cr = ((ao_adc_mask_seq[ao_adc_sequence] << LPC_ADC_CR_SEL) |
+		      (AO_ADC_CLKDIV << LPC_ADC_CR_CLKDIV) |
+		      (0 << LPC_ADC_CR_BURST) |
+		      (LPC_ADC_CR_CLKS_11 << LPC_ADC_CR_CLKS) |
+		      (LPC_ADC_CR_START_NOW << LPC_ADC_CR_START));
+}
+
+void  lpc_adc_isr(void)
+{
+	uint16_t	*out;
+
+	/* Store converted value in packet */
+	out = (uint16_t *) &ao_data_ring[ao_data_head].adc;
+	out[ao_adc_sequence] = (uint16_t) lpc_adc.gdr >> 1;
+	if (++ao_adc_sequence < AO_ADC_NUM) {
+		lpc_adc_start();
+		return;
+	}
+
+	AO_DATA_PRESENT(AO_DATA_ADC);
+	if (ao_data_present == AO_DATA_ALL) {
+#if HAS_MS5607
+		ao_data_ring[ao_data_head].ms5607_raw = ao_ms5607_current;
+#endif
+#if HAS_MMA655X
+		ao_data_ring[ao_data_head].mma655x = ao_mma655x_current;
+#endif
+#if HAS_HMC5883
+		ao_data_ring[ao_data_head].hmc5883 = ao_hmc5883_current;
+#endif
+#if HAS_MPU6000
+		ao_data_ring[ao_data_head].mpu6000 = ao_mpu6000_current;
+#endif
+		ao_data_ring[ao_data_head].tick = ao_tick_count;
+		ao_data_head = ao_data_ring_next(ao_data_head);
+		ao_wakeup((void *) &ao_data_head);
+	}
+	ao_adc_ready = 1;
+}
+
+
+/*
+ * Start the ADC sequence using burst mode
+ */
+void
+ao_adc_poll(void)
+{
+	if (!ao_adc_ready)
+		return;
+	ao_adc_ready = 0;
+	ao_adc_sequence = 0;
+	lpc_adc_start();
+}
+
+static void
+ao_adc_dump(void) __reentrant
+{
+	struct ao_data	packet;
+	int16_t *d;
+	uint8_t i;
+
+	ao_data_get(&packet);
+#ifdef AO_ADC_DUMP
+	AO_ADC_DUMP(&packet);
+#else
+	printf("tick: %5u",  packet.tick);
+	d = (int16_t *) (&packet.adc);
+	for (i = 0; i < AO_NUM_ADC; i++)
+		printf (" %2d: %5d", i, d[i]);
+	printf("\n");
+#endif
+}
+
+__code struct ao_cmds ao_adc_cmds[] = {
+	{ ao_adc_dump,	"a\0Display current ADC values" },
+	{ 0, NULL },
+};
+
+void
+ao_adc_init(void)
+{
+	lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_ADC);
+	lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_ADC_PD);
+
+	/* Enable interrupt when channel is complete */
+	lpc_adc.inten = (1 << LPC_ADC_INTEN_ADGINTEN);
+
+	lpc_nvic_set_enable(LPC_ISR_ADC_POS);
+	lpc_nvic_set_priority(LPC_ISR_ADC_POS, AO_LPC_NVIC_CLOCK_PRIORITY);
+#if AO_ADC_0
+	ao_enable_analog(0, 11, 0);
+#endif
+#if AO_ADC_1
+	ao_enable_analog(0, 12, 1);
+#endif
+#if AO_ADC_2
+	ao_enable_analog(0, 13, 2);
+#endif
+#if AO_ADC_3
+	ao_enable_analog(0, 14, 3);
+#endif
+#if AO_ADC_4
+	ao_enable_analog(0, 15, 4);
+#endif
+#if AO_ADC_5
+	ao_enable_analog(0, 16, 5);
+#endif
+#if AO_ADC_6
+	ao_enable_analog(0, 22, 6);
+#endif
+#if AO_ADC_7
+	ao_enable_analog(0, 23, 7);
+#endif
+
+	lpc_adc.cr = ((0 << LPC_ADC_CR_SEL) |
+		      (AO_ADC_CLKDIV << LPC_ADC_CR_CLKDIV) |
+		      (0 << LPC_ADC_CR_BURST) |
+		      (LPC_ADC_CR_CLKS_11 << LPC_ADC_CR_CLKS));
+
+	ao_cmd_register(&ao_adc_cmds[0]);
+
+	ao_adc_ready = 1;
+}
diff --git a/src/lpc/ao_arch.h b/src/lpc/ao_arch.h
new file mode 100644
index 00000000..d04bf2c8
--- /dev/null
+++ b/src/lpc/ao_arch.h
@@ -0,0 +1,146 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_ARCH_H_
+#define _AO_ARCH_H_
+
+#include <lpc.h>
+
+/*
+ * LPC11U14 definitions and code fragments for AltOS
+ */
+
+#ifndef AO_STACK_SIZE
+#define AO_STACK_SIZE	512
+#endif
+
+#define AO_LED_TYPE	uint16_t
+
+#define AO_PORT_TYPE	uint32_t
+
+#ifndef AO_TICK_TYPE
+#define AO_TICK_TYPE	uint16_t
+#define AO_TICK_SIGNED	int16_t
+#endif
+
+/* Various definitions to make GCC look more like SDCC */
+
+#define ao_arch_naked_declare	__attribute__((naked))
+#define ao_arch_naked_define
+#define __pdata
+#define __data
+#define __xdata
+#define __code const
+#define __reentrant
+#define __interrupt(n)
+#define __at(n)
+
+#define ao_arch_reboot() arm_scb.aircr = ((0x05fa << 16) |	\
+					  (0 << 15) |		\
+					  (1 << 2) |		\
+					  (0 << 1))
+
+#define ao_arch_nop()		asm("nop")
+
+#define ao_arch_interrupt(n)	/* nothing */
+
+#undef putchar
+#undef getchar
+#define putchar(c)	ao_putchar(c)
+#define getchar		ao_getchar
+
+extern void putchar(char c);
+extern char getchar(void);
+
+/*
+ * ao_romconfig.c
+ */
+
+#define AO_ROMCONFIG_VERSION	2
+
+#define AO_ROMCONFIG_SYMBOL(a) __attribute__((section(".romconfig"))) const
+
+extern const uint16_t ao_romconfig_version;
+extern const uint16_t ao_romconfig_check;
+extern const uint16_t ao_serial_number;
+extern const uint32_t ao_radio_cal;
+
+#define ao_arch_task_members\
+	uint32_t *sp;			/* saved stack pointer */
+
+#define ao_arch_block_interrupts()	asm("cpsid i")
+#define ao_arch_release_interrupts()	asm("cpsie i")
+
+/*
+ * For now, we're running at a weird frequency
+ */
+
+#if AO_HSE
+#define AO_PLLSRC	AO_HSE
+#else
+#define AO_PLLSRC	STM_HSI_FREQ
+#endif
+
+#define AO_PLLVCO	(AO_PLLSRC * AO_PLLMUL)
+#define AO_SYSCLK	(AO_PLLVCO / AO_PLLDIV)
+#define AO_HCLK		(AO_SYSCLK / AO_AHB_PRESCALER)
+#define AO_PCLK1	(AO_HCLK / AO_APB1_PRESCALER)
+#define AO_PCLK2	(AO_HCLK / AO_APB2_PRESCALER)
+
+#if AO_APB1_PRESCALER == 1
+#define AO_TIM23467_CLK		AO_PCLK1
+#else
+#define AO_TIM23467_CLK		(2 * AO_PCLK1)
+#endif
+
+#if AO_APB2_PRESCALER == 1
+#define AO_TIM91011_CLK		AO_PCLK2
+#else
+#define AO_TIM91011_CLK		(2 * AO_PCLK2)
+#endif
+
+#define AO_LPC_NVIC_HIGH_PRIORITY	0
+#define AO_LPC_NVIC_CLOCK_PRIORITY	1
+#define AO_LPC_NVIC_MED_PRIORITY	2
+#define AO_LPC_NVIC_LOW_PRIORITY	3
+
+void
+ao_adc_init(void);
+
+#define AO_USB_OUT_EP	2
+#define AO_USB_IN_EP	3
+
+void
+ao_serial_init(void);
+
+/* SPI definitions */
+
+#define AO_SPI_SPEED_12MHz		4
+#define AO_SPI_SPEED_6MHz		8
+#define AO_SPI_SPEED_4MHz		12
+#define AO_SPI_SPEED_2MHz		24
+#define AO_SPI_SPEED_1MHz		48
+#define AO_SPI_SPEED_500kHz		96
+#define AO_SPI_SPEED_250kHz		192
+
+#define AO_SPI_SPEED_FAST	AO_SPI_SPEED_12MHz
+
+#define AO_BOOT_APPLICATION_BASE	((uint32_t *) 0x00001000)
+#define AO_BOOT_LOADER_BASE		((uint32_t *) 0x00000000)
+#define HAS_BOOT_LOADER			1
+
+#endif /* _AO_ARCH_H_ */
diff --git a/src/lpc/ao_arch_funcs.h b/src/lpc/ao_arch_funcs.h
new file mode 100644
index 00000000..0891903e
--- /dev/null
+++ b/src/lpc/ao_arch_funcs.h
@@ -0,0 +1,241 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_ARCH_FUNCS_H_
+#define _AO_ARCH_FUNCS_H_
+
+#define ao_spi_get_bit(reg,bit,pin,bus,speed) ao_spi_get_mask(reg,(1<<bit),bus,speed)
+#define ao_spi_put_bit(reg,bit,pin,bus) ao_spi_put_mask(reg,(1<<bit),bus)
+
+#define ao_enable_port(port) (lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_GPIO))
+#define ao_disable_port(port) (lpc_scb.sysahbclkctrl &= ~(1 << LPC_SCB_SYSAHBCLKCTRL_GPIO))
+
+#define lpc_all_bit(port,bit)	(((port) << 5) | (bit))
+
+#define ao_gpio_set(port, bit, pin, v)	(lpc_gpio.byte[lpc_all_bit(port,bit)] = (v))
+
+#define ao_gpio_get(port, bit, pin) 	(lpc_gpio.byte[lpc_all_bit(port,bit)])
+
+#define ao_enable_output(port,bit,pin,v) do {			\
+		ao_enable_port(port);				\
+		ao_gpio_set(port, bit, pin, v);			\
+		lpc_gpio.dir[port] |= (1 << bit);		\
+	} while (0)
+
+#define ao_gpio_set_mode(port,bit,mode) do {				\
+		vuint32_t *_ioconf = &lpc_ioconf.pio0_0 + ((port)*24+(bit)); \
+		vuint32_t _mode;					\
+		if (mode == AO_EXTI_MODE_PULL_UP)			\
+			_mode = LPC_IOCONF_MODE_PULL_UP << LPC_IOCONF_MODE; \
+		else if (mode == AO_EXTI_MODE_PULL_DOWN)		\
+			_mode = LPC_IOCONF_MODE_PULL_UP << LPC_IOCONF_MODE; \
+		else							\
+			_mode = LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE; \
+		*_ioconf = ((*_ioconf & ~(LPC_IOCONF_MODE_MASK << LPC_IOCONF_MODE)) | \
+			    _mode |					\
+			    (1 << LPC_IOCONF_ADMODE));			\
+	} while (0)
+
+#define ao_enable_input(port,bit,mode) do {				\
+		ao_enable_port(port);					\
+		lpc_gpio.dir[port] &= ~(1 << bit);			\
+		ao_gpio_set_mode(port,bit,mode);			\
+	} while (0)
+
+#define lpc_token_paster_2(x,y)		x ## y
+#define lpc_token_evaluator_2(x,y)	lpc_token_paster_2(x,y)
+#define lpc_token_paster_3(x,y,z)	x ## y ## z
+#define lpc_token_evaluator_3(x,y,z)	lpc_token_paster_3(x,y,z)
+#define lpc_token_paster_4(w,x,y,z)	w ## x ## y ## z
+#define lpc_token_evaluator_4(w,x,y,z)	lpc_token_paster_4(w,x,y,z)
+#define analog_reg(port,bit)		lpc_token_evaluator_4(pio,port,_,bit)
+#define analog_func(id)			lpc_token_evaluator_2(LPC_IOCONF_FUNC_AD,id)
+
+#define ao_enable_analog(port,bit,id) do {				\
+		ao_enable_port(port);					\
+		lpc_gpio.dir[port] &= ~(1 << bit);			\
+		lpc_ioconf.analog_reg(port,bit) = ((analog_func(id) << LPC_IOCONF_FUNC) | \
+						   (0 << LPC_IOCONF_ADMODE)); \
+	} while (0)
+	
+#define ARM_PUSH32(stack, val)	(*(--(stack)) = (val))
+
+static inline uint32_t
+ao_arch_irqsave(void) {
+	uint32_t	primask;
+	asm("mrs %0,primask" : "=&r" (primask));
+	ao_arch_block_interrupts();
+	return primask;
+}
+
+static inline void
+ao_arch_irqrestore(uint32_t primask) {
+	asm("msr primask,%0" : : "r" (primask));
+}
+
+static inline void
+ao_arch_memory_barrier() {
+	asm volatile("" ::: "memory");
+}
+
+#if HAS_TASK
+static inline void
+ao_arch_init_stack(struct ao_task *task, void *start)
+{
+	uint32_t	*sp = (uint32_t *) (task->stack + AO_STACK_SIZE);
+	uint32_t	a = (uint32_t) start;
+	int		i;
+
+	/* Return address (goes into LR) */
+	ARM_PUSH32(sp, a);
+
+	/* Clear register values r0-r7 */
+	i = 8;
+	while (i--)
+		ARM_PUSH32(sp, 0);
+
+	/* APSR */
+	ARM_PUSH32(sp, 0);
+
+	/* PRIMASK with interrupts enabled */
+	ARM_PUSH32(sp, 0);
+
+	task->sp = sp;
+}
+
+static inline void ao_arch_save_regs(void) {
+	/* Save general registers */
+	asm("push {r0-r7,lr}\n");
+
+	/* Save APSR */
+	asm("mrs r0,apsr");
+	asm("push {r0}");
+
+	/* Save PRIMASK */
+	asm("mrs r0,primask");
+	asm("push {r0}");
+}
+
+static inline void ao_arch_save_stack(void) {
+	uint32_t	*sp;
+	asm("mov %0,sp" : "=&r" (sp) );
+	ao_cur_task->sp = (sp);
+	if ((uint8_t *) sp < &ao_cur_task->stack[0])
+		ao_panic (AO_PANIC_STACK);
+}
+
+static inline void ao_arch_restore_stack(void) {
+	uint32_t	sp;
+	sp = (uint32_t) ao_cur_task->sp;
+
+	/* Switch stacks */
+	asm("mov sp, %0" : : "r" (sp) );
+
+	/* Restore PRIMASK */
+	asm("pop {r0}");
+	asm("msr primask,r0");
+
+	/* Restore APSR */
+	asm("pop {r0}");
+	asm("msr apsr_nczvq,r0");
+
+	/* Restore general registers and return */
+	asm("pop {r0-r7,pc}\n");
+}
+
+#define ao_arch_isr_stack()
+
+#endif /* HAS_TASK */
+
+#define ao_arch_wait_interrupt() do {			\
+		asm(".global ao_idle_loc\n\twfi\nao_idle_loc:");	\
+		ao_arch_release_interrupts();				\
+		ao_arch_block_interrupts();				\
+	} while (0)
+
+#define ao_arch_critical(b) do {				\
+		ao_arch_block_interrupts();			\
+		do { b } while (0);				\
+		ao_arch_release_interrupts();			\
+	} while (0)
+
+/*
+ * SPI
+ */
+
+#define ao_spi_set_cs(port,mask) (lpc_gpio.clr[port] = (mask))
+#define ao_spi_clr_cs(port,mask) (lpc_gpio.set[port] = (mask))
+
+#define ao_spi_get_mask(port,mask,bus,speed) do {	\
+		ao_spi_get(bus, speed);			\
+		ao_spi_set_cs(port, mask);		\
+	} while (0)
+
+#define ao_spi_put_mask(reg,mask,bus) do {	\
+		ao_spi_clr_cs(reg,mask);	\
+		ao_spi_put(bus);		\
+	} while (0)
+
+#define ao_spi_get_bit(reg,bit,pin,bus,speed) ao_spi_get_mask(reg,(1<<bit),bus,speed)
+#define ao_spi_put_bit(reg,bit,pin,bus) ao_spi_put_mask(reg,(1<<bit),bus)
+
+void
+ao_spi_get(uint8_t spi_index, uint32_t speed);
+
+void
+ao_spi_put(uint8_t spi_index);
+
+void
+ao_spi_send(void *block, uint16_t len, uint8_t spi_index);
+
+void
+ao_spi_send_fixed(uint8_t value, uint16_t len, uint8_t spi_index);
+
+void
+ao_spi_recv(void *block, uint16_t len, uint8_t spi_index);
+
+void
+ao_spi_duplex(void *out, void *in, uint16_t len, uint8_t spi_index);
+
+extern uint16_t	ao_spi_speed[LPC_NUM_SPI];
+
+void
+ao_spi_init(void);
+
+#define ao_spi_init_cs(port, mask) do {					\
+		uint8_t __bit__;					\
+		for (__bit__ = 0; __bit__ < 32; __bit__++) {		\
+			if (mask & (1 << __bit__))			\
+				ao_enable_output(port, __bit__, PIN, 1); \
+		}							\
+	} while (0)
+
+#define HAS_ARCH_START_SCHEDULER	1
+
+static inline void ao_arch_start_scheduler(void) {
+	uint32_t	sp;
+	uint32_t	control;
+
+	asm("mrs %0,msp" : "=&r" (sp));
+	asm("msr psp,%0" : : "r" (sp));
+	asm("mrs %0,control" : "=&r" (control));
+	control |= (1 << 1);
+	asm("msr control,%0" : : "r" (control));
+	asm("isb");
+}
+
+#endif /* _AO_ARCH_FUNCS_H_ */
diff --git a/src/lpc/ao_beep_lpc.c b/src/lpc/ao_beep_lpc.c
new file mode 100644
index 00000000..eb9132b8
--- /dev/null
+++ b/src/lpc/ao_beep_lpc.c
@@ -0,0 +1,87 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "ao.h"
+
+void
+ao_beep(uint8_t beep)
+{
+	if (beep == 0) {
+		lpc_ct32b1.tcr = ((0 << LPC_CT32B_TCR_CEN) |
+				  (1 << LPC_CT32B_TCR_CRST));
+		lpc_scb.sysahbclkctrl &= ~(1 << LPC_SCB_SYSAHBCLKCTRL_CT32B1);
+	} else {
+		lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_CT32B1);
+
+		/* Set prescaler to match cc1111 clocks
+		 */
+		lpc_ct32b1.pr = AO_LPC_SYSCLK / 750000 - 1;
+
+		/* Write the desired data in the match registers */
+
+		/* Reset after two time units */
+		lpc_ct32b1.mr[0] = beep << 1;
+
+		/* PWM width is half of that */
+		lpc_ct32b1.mr[1] = beep;
+
+		/* Flip output 1 on PWM match */
+		lpc_ct32b1.emr = (LPC_CT32B_EMR_EMC_TOGGLE << LPC_CT32B_EMR_EMC1);
+
+		/* Reset on match 0 */
+		lpc_ct32b1.mcr = (1 << LPC_CT32B_MCR_MR0R);
+
+		/* PWM on match 1 */
+		lpc_ct32b1.pwmc = (1 << LPC_CT32B_PWMC_PWMEN1);
+		
+		/* timer mode */
+		lpc_ct32b1.ctcr = 0;
+
+		/* And turn the timer on */
+		lpc_ct32b1.tcr = ((1 << LPC_CT32B_TCR_CEN) |
+				  (0 << LPC_CT32B_TCR_CRST));
+	}
+}
+
+void
+ao_beep_for(uint8_t beep, uint16_t ticks) __reentrant
+{
+	ao_beep(beep);
+	ao_delay(ticks);
+	ao_beep(0);
+}
+
+void
+ao_beep_init(void)
+{
+	/* Our beeper is on c32b1_mat1
+	 * which is on pin pio0_14
+	 */
+
+	lpc_ioconf.pio0_14 = ((LPC_IOCONF_FUNC_PIO0_14_CT32B1_MAT1 << LPC_IOCONF_FUNC) |
+			      (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) |
+			      (0 << LPC_IOCONF_HYS) |
+			      (0 << LPC_IOCONF_INV) |
+			      (1 << LPC_IOCONF_ADMODE) |
+			      (0 << LPC_IOCONF_OD));
+
+	lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_CT32B1);
+
+	/* Disable the counter and reset the value */
+	lpc_ct32b1.tcr = ((0 << LPC_CT32B_TCR_CEN) |
+			  (1 << LPC_CT32B_TCR_CRST));
+}
diff --git a/src/lpc/ao_boot.h b/src/lpc/ao_boot.h
new file mode 100644
index 00000000..e0ed4de7
--- /dev/null
+++ b/src/lpc/ao_boot.h
@@ -0,0 +1,39 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_BOOT_H_
+#define _AO_BOOT_H_
+
+void
+ao_boot_chain(uint32_t *base);
+
+void
+ao_boot_check_pin(void);
+
+/* Return true to switch to application (if present) */
+int
+ao_boot_check_chain(void);
+
+void
+ao_boot_reboot(uint32_t *base);
+
+static inline void
+ao_boot_loader(void) {
+	ao_boot_reboot(AO_BOOT_LOADER_BASE);
+}
+
+#endif /* _AO_BOOT_H_ */
diff --git a/src/lpc/ao_boot_chain.c b/src/lpc/ao_boot_chain.c
new file mode 100644
index 00000000..a08d1f2c
--- /dev/null
+++ b/src/lpc/ao_boot_chain.c
@@ -0,0 +1,67 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+#include <ao_boot.h>
+
+void
+ao_boot_chain(uint32_t *base)
+{
+	uint32_t	sp;
+	uint32_t	pc;
+
+	sp = base[0];
+	pc = base[1];
+	if (0x00000100 <= pc && pc <= 0x00008000 && (pc & 1) == 1) {
+		asm ("mov sp, %0" : : "r" (sp));
+		asm ("mov lr, %0" : : "r" (pc));
+		asm ("bx lr");
+	}
+}
+
+#define AO_BOOT_SIGNAL	0x5a5aa5a5
+#define AO_BOOT_CHECK	0xc3c33c3c
+
+struct ao_boot {
+	uint32_t	*base;
+	uint32_t	signal;
+	uint32_t	check;
+};
+
+static struct ao_boot __attribute__ ((section(".boot"))) ao_boot;
+	
+int
+ao_boot_check_chain(void)
+{
+	if (ao_boot.signal == AO_BOOT_SIGNAL && ao_boot.check == AO_BOOT_CHECK) {
+		ao_boot.signal = 0;
+		ao_boot.check = 0;
+		if (ao_boot.base == 0)
+			return 0;
+		ao_boot_chain(ao_boot.base);
+	}
+	return 1;
+}
+
+void
+ao_boot_reboot(uint32_t *base)
+{
+	ao_boot.base = base;
+	ao_boot.signal = AO_BOOT_SIGNAL;
+	ao_boot.check = AO_BOOT_CHECK;
+	ao_arch_reboot();
+}
diff --git a/src/lpc/ao_boot_pin.c b/src/lpc/ao_boot_pin.c
new file mode 100644
index 00000000..51ecc0a9
--- /dev/null
+++ b/src/lpc/ao_boot_pin.c
@@ -0,0 +1,46 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+#include <ao_boot.h>
+#include <ao_exti.h>
+
+void
+ao_boot_check_pin(void)
+{
+	uint16_t v;
+
+	/* Enable power interface clock */
+//	stm_rcc.apb1enr |= (1 << STM_RCC_APB1ENR_PWREN);
+	
+	/* Enable the input pin */
+	ao_enable_input(AO_BOOT_APPLICATION_GPIO, AO_BOOT_APPLICATION_PIN,
+			AO_BOOT_APPLICATION_MODE);
+
+	for (v = 0; v < 100; v++)
+		ao_arch_nop();
+
+	/* Read the value */
+	v = ao_gpio_get(AO_BOOT_APPLICATION_GPIO, AO_BOOT_APPLICATION_PIN, AO_BOOT_APPLICATION);
+
+	/* Reset the chip to turn off the port and the power interface clock */
+	ao_gpio_set_mode(AO_BOOT_APPLICATION_GPIO, AO_BOOT_APPLICATION_PIN, 0);
+	ao_disable_port(AO_BOOT_APPLICATION_GPIO);
+//	stm_rcc.apb1enr &= ~(1 << STM_RCC_APB1ENR_PWREN);
+	if (v == AO_BOOT_APPLICATION_VALUE)
+		ao_boot_chain(AO_BOOT_APPLICATION_BASE);
+}
diff --git a/src/lpc/ao_exti.h b/src/lpc/ao_exti.h
new file mode 100644
index 00000000..e8599eb4
--- /dev/null
+++ b/src/lpc/ao_exti.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_EXTI_H_
+#define _AO_EXTI_H_
+
+#define AO_EXTI_MODE_RISING	1
+#define AO_EXTI_MODE_FALLING	2
+#define AO_EXTI_MODE_PULL_UP	4
+#define AO_EXTI_MODE_PULL_DOWN	8
+#define AO_EXTI_PRIORITY_LOW	16
+#define AO_EXTI_PRIORITY_MED	0
+#define AO_EXTI_PRIORITY_HIGH	32
+#define AO_EXTI_PIN_NOCONFIGURE	64
+
+void
+ao_exti_setup(uint8_t gpio, uint8_t pin, uint8_t mode, void (*callback)());
+
+void
+ao_exti_set_mode(uint8_t gpio, uint8_t pin, uint8_t mode);
+
+void
+ao_exti_set_callback(uint8_t gpio, uint8_t pin, void (*callback)());
+
+void
+ao_exti_enable(uint8_t gpio, uint8_t pin);
+
+void
+ao_exti_disable(uint8_t gpio, uint8_t pin);
+
+void
+ao_exti_init(void);
+
+#endif /* _AO_EXTI_H_ */
diff --git a/src/lpc/ao_exti_lpc.c b/src/lpc/ao_exti_lpc.c
new file mode 100644
index 00000000..588cf58c
--- /dev/null
+++ b/src/lpc/ao_exti_lpc.c
@@ -0,0 +1,178 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+#include <ao_exti.h>
+
+#define LPC_NUM_PINS	56
+#define LPC_NUM_PINT	8
+
+static void	(*ao_exti_callback[LPC_NUM_PINT])(void);
+
+static uint8_t	ao_pint_map[LPC_NUM_PINS];
+static uint8_t	ao_pint_mode[LPC_NUM_PINS];
+static uint8_t	ao_pint_inuse;
+static uint8_t	ao_pint_enabled;
+
+static void
+ao_exti_isr(uint8_t pint)
+{
+	uint8_t	mask = 1 << pint;
+
+	if (lpc_gpio_pin.ist & mask) {
+		lpc_gpio_pin.ist = mask;
+		lpc_gpio_pin.rise = mask;
+		lpc_gpio_pin.fall = mask;
+
+		(*ao_exti_callback[pint]) ();
+	}
+}
+
+#define pin_isr(n)	void lpc_pin_int ## n ## _isr(void) { ao_exti_isr(n); }
+pin_isr(0)
+pin_isr(1)
+pin_isr(2)
+pin_isr(3)
+pin_isr(4)
+pin_isr(5)
+pin_isr(6)
+pin_isr(7)
+
+#define pin_id(port,pin)	((port) * 24 + (pin));
+
+static void
+_ao_exti_set_enable(uint8_t pint)
+{
+	uint8_t		mask = 1 << pint;
+	uint8_t		mode;
+
+	if (ao_pint_enabled & mask)
+		mode = ao_pint_mode[pint];
+	else
+		mode = 0;
+
+	if (mode & AO_EXTI_MODE_RISING)
+		lpc_gpio_pin.sienr = mask;
+	else
+		lpc_gpio_pin.cienr = mask;
+		
+	if (mode & AO_EXTI_MODE_FALLING)
+		lpc_gpio_pin.sienf = mask;
+	else
+		lpc_gpio_pin.cienf = mask;
+	lpc_gpio_pin.rise = mask;
+	lpc_gpio_pin.fall = mask;
+}
+
+void
+ao_exti_setup (uint8_t port, uint8_t pin, uint8_t mode, void (*callback)(void)) {
+	uint8_t		id = pin_id(port,pin);
+	uint8_t	       	pint;
+	uint32_t	mask;
+	uint8_t		prio;
+
+	for (pint = 0; pint < LPC_NUM_PINT; pint++)
+		if ((ao_pint_inuse & (1 << pint)) == 0)
+			break;
+	if (pint == LPC_NUM_PINT)
+		ao_panic(AO_PANIC_EXTI);
+
+	if (!mode & AO_EXTI_PIN_NOCONFIGURE)
+		ao_enable_input(port, pin, mode);
+
+	ao_arch_block_interrupts();
+	mask = (1 << pint);
+	ao_pint_inuse |= mask;
+	ao_pint_enabled &= ~mask;
+	
+	ao_pint_map[id] = pint;
+	ao_exti_callback[pint] = callback;
+
+	/* configure gpio to interrupt routing */
+	lpc_scb.pintsel[pint] = id;
+
+	/* Set edge triggered */
+	lpc_gpio_pin.isel &= ~mask;
+
+	ao_pint_enabled &= ~mask;
+	ao_pint_mode[pint] = mode;
+	_ao_exti_set_enable(pint);
+
+	/* Set interrupt mask and rising/falling mode */
+
+	prio = AO_LPC_NVIC_MED_PRIORITY;
+	if (mode & AO_EXTI_PRIORITY_LOW)
+		prio = AO_LPC_NVIC_LOW_PRIORITY;
+	else if (mode & AO_EXTI_PRIORITY_HIGH)
+		prio = AO_LPC_NVIC_HIGH_PRIORITY;
+
+	/* Set priority and enable */
+	lpc_nvic_set_priority(LPC_ISR_PIN_INT0_POS + pint, prio);
+	lpc_nvic_set_enable(LPC_ISR_PIN_INT0_POS + pint);
+	ao_arch_release_interrupts();
+}
+
+void
+ao_exti_set_mode(uint8_t port, uint8_t pin, uint8_t mode)
+{
+	uint8_t		id = pin_id(port,pin);
+	uint8_t		pint = ao_pint_map[id];
+
+	ao_arch_block_interrupts();
+	ao_pint_mode[pint] = mode;
+	_ao_exti_set_enable(pint);
+	ao_arch_release_interrupts();
+}
+
+void
+ao_exti_set_callback(uint8_t port, uint8_t pin, void (*callback)()) {
+	uint8_t		id = pin_id(port,pin);
+	uint8_t		pint = ao_pint_map[id];
+
+	ao_exti_callback[pint] = callback;
+}
+
+void
+ao_exti_enable(uint8_t port, uint8_t pin)
+{
+	uint8_t		id = pin_id(port,pin);
+	uint8_t		pint = ao_pint_map[id];
+	uint8_t		mask = 1 << pint;
+
+	ao_arch_block_interrupts();
+	ao_pint_enabled |= mask;
+	_ao_exti_set_enable(pint);
+	ao_arch_release_interrupts();
+}
+
+void
+ao_exti_disable(uint8_t port, uint8_t pin) {
+	uint8_t		id = pin_id(port,pin);
+	uint8_t		pint = ao_pint_map[id];
+	uint8_t		mask = 1 << pint;
+
+	ao_arch_block_interrupts();
+	ao_pint_enabled &= ~mask;
+	_ao_exti_set_enable(pint);
+	ao_arch_release_interrupts();
+}
+
+void
+ao_exti_init(void)
+{
+	lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_PINT);
+}
diff --git a/src/lpc/ao_flash.h b/src/lpc/ao_flash.h
new file mode 100644
index 00000000..aaf66b39
--- /dev/null
+++ b/src/lpc/ao_flash.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_FLASH_H_
+#define _AO_FLASH_H_
+
+uint32_t
+ao_flash_erase_page(uint8_t *page);
+
+uint32_t
+ao_flash_page(uint8_t *page, uint8_t *src);
+
+uint32_t
+ao_lpc_read_part_id(void);
+
+#endif /* _AO_FLASH_H_ */
diff --git a/src/lpc/ao_flash_loader_lpc.c b/src/lpc/ao_flash_loader_lpc.c
new file mode 100644
index 00000000..2ab548cf
--- /dev/null
+++ b/src/lpc/ao_flash_loader_lpc.c
@@ -0,0 +1,32 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "ao.h"
+#include <ao_exti.h>
+#include <ao_boot.h>
+#include <ao_flash_task.h>
+
+int
+main(void)
+{
+	ao_clock_init();
+
+	ao_usb_init();
+
+	ao_flash_task();
+	return 0;
+}
diff --git a/src/lpc/ao_flash_lpc.c b/src/lpc/ao_flash_lpc.c
new file mode 100644
index 00000000..5a31f39f
--- /dev/null
+++ b/src/lpc/ao_flash_lpc.c
@@ -0,0 +1,158 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+#include <ao_flash.h>
+
+#define IAP_LOCATION 0x1fff1ff1
+
+typedef void (*iap_func)(uint32_t *in, uint32_t *out);
+
+static void
+iap(uint32_t *in, uint32_t *out)
+{
+	ao_arch_block_interrupts();
+	lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_FLASHREG);
+	((iap_func) IAP_LOCATION)(in, out);
+	ao_arch_release_interrupts();
+}
+
+#define LPC_IAP_PREPARE_WRITE		50
+#define LPC_IAP_COPY_RAM_TO_FLASH	51
+#define LPC_IAP_ERASE_SECTOR		52
+#define LPC_IAP_BLANK_CHECK		53
+#define LPC_IAP_READ_PART_ID		54
+#define LPC_IAP_READ_BOOT_CODE_VERSION	55
+#define LPC_IAP_COMPARE			56
+#define LPC_IAP_REINVOKE_ISP		57
+#define LPC_IAP_READ_UID		58
+#define LPC_IAP_ERASE_PAGE		59
+#define LPC_IAP_EEPROM_WRITE		61
+#define LPC_IAP_EEPROM_READ		62
+
+#define LPC_IAP_CMD_SUCCESS		0
+#define LPC_IAP_INVALID_COMMAND		1
+#define LPC_IAP_SRC_ADDR_ERROR		2
+#define LPC_IAP_DST_ADDR_ERROR		3
+#define LPC_IAP_SRC_ADDR_NOT_MAPPED	4
+#define LPC_IAP_DST_ADDR_NOT_MAPPED	5
+#define LPC_IAP_COUNT_ERROR		6
+#define LPC_IAP_INVALID_SECTOR		7
+#define LPC_IAP_SECTOR_NOT_BLANK	8
+#define LPC_IAP_SECTOR_NOT_PREPARED_FOR_WRITE_OPERATION	9
+#define LPC_IAP_COMPARE_ERROR		10
+#define LPC_IAP_BUSY			11
+#define LPC_IAP_PARAM_ERROR		12
+#define LPC_IAP_ADDR_ERROR		13
+#define LPC_IAP_ADDR_NOT_MAPPED		14
+#define LPC_IAP_CMD_LOCKED		15
+#define LPC_IAP_INVALID_CODE		16
+#define LPC_IAP_INVALID_BAUD_RATE	17
+#define LPC_IAP_INVALID_STOP_BIT	18
+#define LPC_IAP_CODE_READ_PROTECTION_ENABLED	19
+
+#define LPC_FLASH_BASE			((uint8_t *) 0x0)
+#define LPC_FLASH_SECTOR		4096
+#define LPC_FLASH_SECTOR_MASK	        (LPC_FLASH_SECTOR - 1)
+#define LPC_FLASH_SECTOR_SHIFT		12
+
+static uint32_t	iap_in[5], iap_out[5];
+
+static uint32_t
+ao_lpc_addr_to_sector(uint8_t *addr)
+{
+	uint32_t	off = addr - LPC_FLASH_BASE;
+
+	return off >> LPC_FLASH_SECTOR_SHIFT;
+}
+
+static uint8_t
+ao_lpc_addr_is_sector_aligned(uint8_t *addr)
+{
+	uint32_t	off = addr - LPC_FLASH_BASE;
+	return		(off & LPC_FLASH_SECTOR_MASK) == 0;
+}
+
+static uint32_t
+ao_lpc_prepare_write(uint32_t start_sector, uint32_t end_sector)
+{
+	iap_in[0] = LPC_IAP_PREPARE_WRITE;
+	iap_in[1] = start_sector;
+	iap_in[2] = end_sector;
+	iap(iap_in,iap_out);
+	return iap_out[0];
+}
+
+static uint32_t
+ao_lpc_copy_ram_to_flash(uint8_t *dst, uint8_t *src, uint32_t len, uint32_t freq)
+{
+	iap_in[0] = LPC_IAP_COPY_RAM_TO_FLASH;
+	iap_in[1] = (uint32_t) dst;
+	iap_in[2] = (uint32_t) src;
+	iap_in[3] = len;
+	iap_in[4] = freq;
+	iap(iap_in,iap_out);
+	return iap_out[0];
+}
+
+static uint32_t
+ao_lpc_erase_sector(uint32_t start_sector, uint32_t end_sector, uint32_t freq)
+{
+	iap_in[0] = LPC_IAP_ERASE_SECTOR;
+	iap_in[1] = start_sector;
+	iap_in[2] = end_sector;
+	iap_in[3] = freq;
+	iap(iap_in,iap_out);
+	return iap_out[0];
+}
+
+uint32_t
+ao_lpc_read_part_id(void)
+{
+	iap_in[0] = LPC_IAP_READ_PART_ID;
+	iap(iap_in,iap_out);
+	return iap_out[1];
+}
+
+uint32_t
+ao_flash_erase_page(uint8_t *page)
+{
+	uint32_t	ret = LPC_IAP_CMD_SUCCESS;
+	if (ao_lpc_addr_is_sector_aligned(page)) {
+		uint32_t	sector = ao_lpc_addr_to_sector(page);
+		ret = ao_lpc_prepare_write(sector, sector);
+		if (ret == LPC_IAP_CMD_SUCCESS)
+			ret = ao_lpc_erase_sector(sector, sector, AO_LPC_SYSCLK / 1000);
+	}
+	return ret;
+}
+
+uint32_t
+ao_flash_page(uint8_t *page, uint8_t *src)
+{
+	uint32_t	sector = ao_lpc_addr_to_sector(page);
+	uint32_t	ret;
+
+	ret = ao_flash_erase_page(page);
+	if (ret != LPC_IAP_CMD_SUCCESS)
+		return ret;
+	ret = ao_lpc_prepare_write(sector, sector);
+	if (ret != LPC_IAP_CMD_SUCCESS)
+		return ret;
+	ret = ao_lpc_copy_ram_to_flash(page, src, 256, AO_LPC_SYSCLK / 1000);
+	return ret;
+}
diff --git a/src/lpc/ao_flash_lpc_pins.h b/src/lpc/ao_flash_lpc_pins.h
new file mode 100644
index 00000000..e2243d5c
--- /dev/null
+++ b/src/lpc/ao_flash_lpc_pins.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_FLASH_LPC_PINS_H_
+#define _AO_FLASH_LPC_PINS_H_
+
+#include <ao_flash_pins.h>
+
+/* Crystal on the board */
+#define AO_LPC_CLKIN	12000000
+
+/* Main clock frequency. 48MHz for USB so we don't use the USB PLL */
+#define AO_LPC_CLKOUT	48000000
+
+/* System clock frequency */
+#define AO_LPC_SYSCLK	24000000
+
+#endif /* _AO_FLASH_STM_PINS_H_ */
diff --git a/src/lpc/ao_interrupt.c b/src/lpc/ao_interrupt.c
new file mode 100644
index 00000000..c4dc7867
--- /dev/null
+++ b/src/lpc/ao_interrupt.c
@@ -0,0 +1,176 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+#include <string.h>
+#include <ao_boot.h>
+
+#ifndef IS_FLASH_LOADER
+#error Should define IS_FLASH_LOADER
+#define IS_FLASH_LOADER	0
+#endif
+
+#if !IS_FLASH_LOADER
+#define RELOCATE_INTERRUPT	1
+#endif
+
+extern void main(void);
+extern char __stack__;
+extern char __text_start__, __text_end__;
+extern char __data_start__, __data_end__;
+extern char __bss_start__, __bss_end__;
+#if RELOCATE_INTERRUPT
+extern char __interrupt_rom__, __interrupt_start__, __interrupt_end__;
+#endif
+
+/* Interrupt functions */
+
+void lpc_halt_isr(void)
+{
+	ao_panic(AO_PANIC_CRASH);
+}
+
+void lpc_ignore_isr(void)
+{
+}
+
+void start(void) {
+#ifdef AO_BOOT_CHAIN
+	if (ao_boot_check_chain()) {
+#ifdef AO_BOOT_PIN
+		ao_boot_check_pin();
+#endif
+	}
+#endif
+#if RELOCATE_INTERRUPT
+	memcpy(&__interrupt_start__, &__interrupt_rom__, &__interrupt_end__ - &__interrupt_start__);
+	lpc_scb.sysmemremap = LPC_SCB_SYSMEMREMAP_MAP_RAM << LPC_SCB_SYSMEMREMAP_MAP;
+#endif
+	memcpy(&__data_start__, &__text_end__, &__data_end__ - &__data_start__);
+	memset(&__bss_start__, '\0', &__bss_end__ - &__bss_start__);
+	main();
+}
+
+#define STRINGIFY(x) #x
+
+#define isr(name) \
+	void __attribute__ ((weak)) lpc_ ## name ## _isr(void); \
+	_Pragma(STRINGIFY(weak lpc_ ## name ## _isr = lpc_ignore_isr))
+
+#define isr_halt(name) \
+	void __attribute__ ((weak)) lpc_ ## name ## _isr(void); \
+	_Pragma(STRINGIFY(weak lpc_ ## name ## _isr = lpc_halt_isr))
+
+isr(nmi)
+isr_halt(hardfault)
+isr_halt(memmanage)
+isr_halt(busfault)
+isr_halt(usagefault)
+isr(svc)
+isr(debugmon)
+isr(pendsv)
+isr(systick)
+
+isr(pin_int0)	/* IRQ0 */
+isr(pin_int1)
+isr(pin_int2)
+isr(pin_int3)
+isr(pin_int4)	/* IRQ4 */
+isr(pin_int5)
+isr(pin_int6)
+isr(pin_int7)
+
+isr(gint0)	/* IRQ8 */
+isr(gint1)
+isr(ssp1)
+isr(i2c)
+
+isr(ct16b0)	/* IRQ16 */
+isr(ct16b1)
+isr(ct32b0)
+isr(ct32b1)
+isr(ssp0)	/* IRQ20 */
+isr(usart)
+isr(usb_irq)
+isr(usb_fiq)
+
+isr(adc)	/* IRQ24 */
+isr(wwdt)
+isr(bod)
+isr(flash)
+
+isr(usb_wakeup)
+
+#define i(addr,name)	[(addr)/4] = lpc_ ## name ## _isr
+#define c(addr,value)	[(addr)/4] = (value)
+
+__attribute__ ((section(".interrupt")))
+const void *lpc_interrupt_vector[] = {
+	[0] = &__stack__,
+	[1] = start,
+	i(0x08, nmi),
+	i(0x0c, hardfault),
+	c(0x10, 0),
+	c(0x14, 0),
+	c(0x18, 0),
+	c(0x1c, 0),
+	c(0x20, 0),
+	c(0x24, 0),
+	c(0x28, 0),
+	i(0x2c, svc),
+	i(0x30, hardfault),
+	i(0x34, hardfault),
+	i(0x38, pendsv),
+	i(0x3c, systick),
+
+	i(0x40, pin_int0),	/* IRQ0 */
+	i(0x44, pin_int1),
+	i(0x48, pin_int2),
+	i(0x4c, pin_int3),
+	i(0x50, pin_int4),	/* IRQ4 */
+	i(0x54, pin_int5),
+	i(0x58, pin_int6),
+	i(0x5c, pin_int7),
+
+	i(0x60, gint0),		/* IRQ8 */
+	i(0x64, gint1),
+	i(0x68, hardfault),
+	i(0x6c, hardfault),
+	i(0x70, hardfault),	/* IRQ12 */
+	i(0x74, hardfault),
+	i(0x78, ssp1),
+	i(0x7c, i2c),
+
+	i(0x80, ct16b0),	/* IRQ16 */
+	i(0x84, ct16b1),
+	i(0x88, ct32b0),
+	i(0x8c, ct32b1),
+	i(0x90, ssp0),		/* IRQ20 */
+	i(0x94, usart),
+	i(0x98, usb_irq),
+	i(0x9c, usb_fiq),
+
+	i(0xa0, adc),		/* IRQ24 */
+	i(0xa4, wwdt),
+	i(0xa8, bod),
+	i(0xac, flash),
+
+	i(0xb0, hardfault),	/* IRQ28 */
+	i(0xb4, hardfault),
+	i(0xb8, usb_wakeup),
+	i(0xbc, hardfault),	
+};
diff --git a/src/lpc/ao_led_lpc.c b/src/lpc/ao_led_lpc.c
new file mode 100644
index 00000000..7bef51ba
--- /dev/null
+++ b/src/lpc/ao_led_lpc.c
@@ -0,0 +1,66 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+
+__pdata uint16_t ao_led_enable;
+
+void
+ao_led_on(uint16_t colors)
+{
+	lpc_gpio.pin[LED_PORT] |= colors;
+}
+
+void
+ao_led_off(uint16_t colors)
+{
+	lpc_gpio.pin[LED_PORT] &= ~colors;
+}
+
+void
+ao_led_set(uint16_t colors)
+{
+	uint16_t	on = colors & ao_led_enable;
+	uint16_t	off = ~colors & ao_led_enable;
+
+	ao_led_off(off);
+	ao_led_on(on);
+}
+
+void
+ao_led_toggle(uint16_t colors)
+{
+	lpc_gpio.pin[LED_PORT] ^= colors;
+}
+
+void
+ao_led_for(uint16_t colors, uint16_t ticks) __reentrant
+{
+	ao_led_on(colors);
+	ao_delay(ticks);
+	ao_led_off(colors);
+}
+
+void
+ao_led_init(uint16_t enable)
+{
+	int	bit;
+
+	ao_led_enable = enable;
+	lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_GPIO);
+	lpc_gpio.dir[LED_PORT] |= enable;
+}
diff --git a/src/lpc/ao_romconfig.c b/src/lpc/ao_romconfig.c
new file mode 100644
index 00000000..cbb922ec
--- /dev/null
+++ b/src/lpc/ao_romconfig.c
@@ -0,0 +1,25 @@
+/*
+ * Copyright © 2011 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "ao.h"
+
+AO_ROMCONFIG_SYMBOL (0) uint16_t ao_romconfig_version = AO_ROMCONFIG_VERSION;
+AO_ROMCONFIG_SYMBOL (0) uint16_t ao_romconfig_check = ~AO_ROMCONFIG_VERSION;
+AO_ROMCONFIG_SYMBOL (0) uint16_t ao_serial_number = 0;
+#ifdef AO_RADIO_CAL_DEFAULT
+AO_ROMCONFIG_SYMBOL (0) uint32_t ao_radio_cal = AO_RADIO_CAL_DEFAULT;
+#endif
diff --git a/src/lpc/ao_serial_lpc.c b/src/lpc/ao_serial_lpc.c
new file mode 100644
index 00000000..431ae98a
--- /dev/null
+++ b/src/lpc/ao_serial_lpc.c
@@ -0,0 +1,213 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+#include <ao_serial.h>
+
+struct ao_fifo	ao_usart_rx_fifo;
+struct ao_fifo	ao_usart_tx_fifo;
+uint8_t		ao_usart_tx_avail;
+uint8_t		ao_usart_tx_avail_min;
+
+#define LPC_USART_TX_FIFO_SIZE	16
+
+void
+ao_debug_out(char c)
+{
+	if (c == '\n')
+		ao_debug_out('\r');
+	while (!(lpc_usart.lsr & (1 << LPC_USART_LSR_TEMT)))
+		;
+	lpc_usart.rbr_thr = c;
+}
+
+static void
+_ao_serial_tx_start(void)
+{
+	if (!ao_fifo_empty(ao_usart_tx_fifo) && ao_usart_tx_avail) {
+		ao_usart_tx_avail--;
+		if (ao_usart_tx_avail < ao_usart_tx_avail_min)
+			ao_usart_tx_avail_min = ao_usart_tx_avail;
+		ao_fifo_remove(ao_usart_tx_fifo, lpc_usart.rbr_thr);
+	}
+}
+
+void
+lpc_usart_isr(void)
+{
+	uint8_t wake_input = 0;
+	(void) lpc_usart.iir_fcr;
+
+	while (lpc_usart.lsr & (1 << LPC_USART_LSR_RDR)) {
+		char c = lpc_usart.rbr_thr;
+		if (!ao_fifo_full(ao_usart_rx_fifo))
+			ao_fifo_insert(ao_usart_rx_fifo, c);
+		wake_input = 1;
+	}
+	if (lpc_usart.lsr & (1 << LPC_USART_LSR_THRE)) {
+		ao_usart_tx_avail = LPC_USART_TX_FIFO_SIZE;
+		_ao_serial_tx_start();
+		ao_wakeup(&ao_usart_tx_fifo);
+	}
+	if (wake_input) {
+		ao_wakeup(&ao_usart_rx_fifo);
+		if (stdin)
+			ao_wakeup(&ao_stdin_ready);
+	}
+}
+
+int
+_ao_serial0_pollchar(void)
+{
+	int	c;
+	
+	if (ao_fifo_empty(ao_usart_rx_fifo))
+		c = AO_READ_AGAIN;
+	else {
+		uint8_t	u;
+		ao_fifo_remove(ao_usart_rx_fifo,u);
+		c = u;
+	}
+	return c;
+}
+
+char
+ao_serial0_getchar(void)
+{
+	int c;
+	ao_arch_block_interrupts();
+	while ((c = _ao_serial0_pollchar()) == AO_READ_AGAIN)
+		ao_sleep(&ao_usart_rx_fifo);
+	ao_arch_release_interrupts();
+	return (char) c;
+}
+
+void
+ao_serial0_putchar(char c)
+{
+	ao_arch_block_interrupts();
+	while (ao_fifo_full(ao_usart_tx_fifo))
+		ao_sleep(&ao_usart_tx_fifo);
+	ao_fifo_insert(ao_usart_tx_fifo, c);
+	_ao_serial_tx_start();
+	ao_arch_release_interrupts();
+}
+
+void
+ao_serial0_drain(void)
+{
+	ao_arch_block_interrupts();
+	while (!ao_fifo_empty(ao_usart_tx_fifo))
+		ao_sleep(&ao_usart_tx_fifo);
+	ao_arch_release_interrupts();
+}
+
+#include "ao_serial_lpc.h"
+
+void
+ao_serial0_set_speed(uint8_t speed)
+{
+	if (speed > AO_SERIAL_SPEED_115200)
+		return;
+
+	/* Flip to allow access to divisor latches */
+	lpc_usart.lcr |= (1 << LPC_USART_LCR_DLAB);
+
+	/* DL LSB */
+	lpc_usart.rbr_thr = ao_usart_speeds[speed].dl & 0xff;
+	
+	/* DL MSB */
+	lpc_usart.ier = (ao_usart_speeds[speed].dl >> 8) & 0xff;
+
+	lpc_usart.fdr = ((ao_usart_speeds[speed].divaddval << LPC_USART_FDR_DIVADDVAL) |
+			 (ao_usart_speeds[speed].mulval << LPC_USART_FDR_MULVAL));
+
+	/* Turn access to divisor latches back off */
+	lpc_usart.lcr &= ~(1 << LPC_USART_LCR_DLAB);
+}
+
+void
+ao_serial_init(void)
+{
+#if SERIAL_0_18_19
+	lpc_ioconf.pio0_18 = ((LPC_IOCONF_FUNC_PIO0_18_RXD << LPC_IOCONF_FUNC) |
+			      (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) |
+			      (0 << LPC_IOCONF_HYS) |
+			      (0 << LPC_IOCONF_INV) |
+			      (0 << LPC_IOCONF_OD));
+	lpc_ioconf.pio0_19 = ((LPC_IOCONF_FUNC_PIO0_19_TXD << LPC_IOCONF_FUNC) |
+			      (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) |
+			      (0 << LPC_IOCONF_HYS) |
+			      (0 << LPC_IOCONF_INV) |
+			      (0 << LPC_IOCONF_OD));
+#endif
+
+	/* Turn on the USART */
+	lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_USART);
+
+	/* Turn on the USART clock */
+	lpc_scb.uartclkdiv = AO_LPC_CLKOUT / AO_LPC_USARTCLK;
+
+	/* Configure USART */
+
+	/* Enable FIFOs, reset fifo contents, interrupt on 1 received char */
+	lpc_usart.iir_fcr = ((1 << LPC_USART_FCR_FIFOEN) |
+			 (1 << LPC_USART_FCR_RXFIFORES) |
+			 (1 << LPC_USART_FCR_TXFIFORES) |
+			 (LPC_USART_FCR_RXTL_1 << LPC_USART_FCR_RXTL));
+
+	ao_usart_tx_avail = LPC_USART_TX_FIFO_SIZE;
+	ao_usart_tx_avail_min = LPC_USART_TX_FIFO_SIZE;
+
+	/* 8 n 1 */
+	lpc_usart.lcr = ((LPC_USART_LCR_WLS_8 << LPC_USART_LCR_WLS) |
+			 (LPC_USART_LCR_SBS_1 << LPC_USART_LCR_SBS) |
+			 (0 << LPC_USART_LCR_PE) |
+			 (LPC_USART_LCR_PS_ODD << LPC_USART_LCR_PS) |
+			 (0 << LPC_USART_LCR_BC) |
+			 (0 << LPC_USART_LCR_DLAB));
+
+	/* Disable flow control */
+	lpc_usart.mcr = ((0 << LPC_USART_MCR_DTRCTRL) |
+			 (0 << LPC_USART_MCR_RTSCTRL) |
+			 (0 << LPC_USART_MCR_LMS) |
+			 (0 << LPC_USART_MCR_RTSEN) |
+			 (0 << LPC_USART_MCR_CTSEN));
+
+	/* 16x oversampling */
+	lpc_usart.osr = ((0 << LPC_USART_OSR_OSFRAC) |
+			 ((16 - 1) << LPC_USART_OSR_OSINT) |
+			 (0 << LPC_USART_OSR_FDINT));
+
+	/* Full duplex */
+	lpc_usart.hden = ((0 << LPC_USART_HDEN_HDEN));
+
+	/* Set baud rate */
+	ao_serial0_set_speed(AO_SERIAL_SPEED_9600);
+
+	/* Enable interrupts */
+	lpc_usart.ier = ((1 << LPC_USART_IER_RBRINTEN) |
+			 (1 << LPC_USART_IER_THREINTEN));
+
+	lpc_nvic_set_enable(LPC_ISR_USART_POS);
+	lpc_nvic_set_priority(LPC_ISR_USART_POS, 0);
+#if USE_SERIAL_0_STDIN
+	ao_add_stdio(_ao_serial_pollchar,
+		     ao_serial_putchar,
+		     NULL);
+#endif
+}
diff --git a/src/lpc/ao_spi_lpc.c b/src/lpc/ao_spi_lpc.c
new file mode 100644
index 00000000..a889137c
--- /dev/null
+++ b/src/lpc/ao_spi_lpc.c
@@ -0,0 +1,207 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+
+static uint8_t		ao_spi_mutex[LPC_NUM_SPI];
+
+static struct lpc_ssp * const ao_lpc_ssp[LPC_NUM_SPI] = { &lpc_ssp0, &lpc_ssp1 };
+
+static uint8_t	spi_dev_null;
+
+#define tx_busy(lpc_ssp) (lpc_ssp->sr & ((1 << LPC_SSP_SR_BSY) | (1 << LPC_SSP_SR_TNF))) != (1 << LPC_SSP_SR_TNF)
+#define rx_busy(lpc_ssp) (lpc_ssp->sr & ((1 << LPC_SSP_SR_BSY) | (1 << LPC_SSP_SR_RNE))) != (1 << LPC_SSP_SR_RNE)
+
+#define spi_loop(len, put, get) do {					\
+		while (len--) {						\
+			/* Wait for space in the fifo */		\
+			while (tx_busy(lpc_ssp))			\
+				;					\
+									\
+			/* send a byte */				\
+			lpc_ssp->dr = put;				\
+									\
+			/* Wait for byte to appear in the fifo */	\
+			while (rx_busy(lpc_ssp))			\
+				;					\
+									\
+			/* recv a byte */				\
+			get lpc_ssp->dr;				\
+		}							\
+	} while (0)
+
+void
+ao_spi_send(void *block, uint16_t len, uint8_t id)
+{
+	uint8_t	*b = block;
+	struct lpc_ssp *lpc_ssp = ao_lpc_ssp[id];
+
+	spi_loop(len, *b++, (void));
+}
+
+void
+ao_spi_send_fixed(uint8_t value, uint16_t len, uint8_t id)
+{
+	struct lpc_ssp *lpc_ssp = ao_lpc_ssp[id];
+
+	spi_loop(len, value, (void));
+}
+
+void
+ao_spi_recv(void *block, uint16_t len, uint8_t id)
+{
+	uint8_t	*b = block;
+	struct lpc_ssp *lpc_ssp = ao_lpc_ssp[id];
+
+	spi_loop(len, 0xff, *b++ =);
+}
+
+void
+ao_spi_duplex(void *out, void *in, uint16_t len, uint8_t id)
+{
+	uint8_t	*o = out;
+	uint8_t	*i = in;
+	struct lpc_ssp *lpc_ssp = ao_lpc_ssp[id];
+
+	spi_loop(len, *o++, *i++ =);
+}
+
+void
+ao_spi_get(uint8_t id, uint32_t speed)
+{
+	struct lpc_ssp	*lpc_ssp = ao_lpc_ssp[id];
+
+	ao_mutex_get(&ao_spi_mutex[id]);
+	
+	/* Set the clock prescale */
+	lpc_ssp->cpsr = speed;
+}
+
+void
+ao_spi_put(uint8_t id)
+{
+	ao_mutex_put(&ao_spi_mutex[id]);
+}
+
+static void
+ao_spi_channel_init(uint8_t id)
+{
+	struct lpc_ssp	*lpc_ssp = ao_lpc_ssp[id];
+	uint8_t	d;
+
+	lpc_ssp->cr0 = ((LPC_SSP_CR0_DSS_8 << LPC_SSP_CR0_DSS) |
+			(LPC_SSP_CR0_FRF_SPI << LPC_SSP_CR0_FRF) |
+			(0 << LPC_SSP_CR0_CPOL) |
+			(0 << LPC_SSP_CR0_CPHA) |
+			(0 << LPC_SSP_CR0_SCR));
+
+	/* Enable the device */
+	lpc_ssp->cr1 = ((0 << LPC_SSP_CR1_LBM) |
+			(1 << LPC_SSP_CR1_SSE) |
+			(LPC_SSP_CR1_MS_MASTER << LPC_SSP_CR1_MS) |
+			(0 << LPC_SSP_CR1_SOD));
+
+	/* Drain the receive fifo */
+	for (d = 0; d < LPC_SSP_FIFOSIZE; d++)
+		(void) lpc_ssp->dr;
+}
+
+void
+ao_spi_init(void)
+{
+#if HAS_SPI_0
+	/* Configure pins */
+#if SPI_SCK0_P0_6
+	lpc_ioconf.pio0_6 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO0_6_SCK0);
+#define HAS_SCK0
+#endif
+#if SPI_SCK0_P0_10
+	lpc_ioconf.pio0_10 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO0_10_SCK0);
+#define HAS_SCK0
+#endif
+#if SPI_SCK0_P1_29
+	lpc_ioconf.pio1_29 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO1_29_SCK0);
+#define HAS_SCK0
+#endif
+#ifndef HAS_SCK0
+#error "No pin specified for SCK0"
+#endif
+	lpc_ioconf.pio0_8 = ao_lpc_alternate(LPC_IOCONF_FUNC_MISO0);
+	lpc_ioconf.pio0_9 = ao_lpc_alternate(LPC_IOCONF_FUNC_MOSI0);
+
+	/* Enable the device */
+	lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_SSP0);
+
+	/* Turn on the clock */
+	lpc_scb.ssp0clkdiv = 1;
+
+	/* Reset the device */
+	lpc_scb.presetctrl &= ~(1 << LPC_SCB_PRESETCTRL_SSP0_RST_N);
+	lpc_scb.presetctrl |= (1 << LPC_SCB_PRESETCTRL_SSP0_RST_N);
+	ao_spi_channel_init(0);
+#endif			   
+
+#if HAS_SPI_1
+
+#if SPI_SCK1_P1_15
+	lpc_ioconf.pio1_15 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO1_15_SCK1);
+#define HAS_SCK1
+#endif
+#if SPI_SCK1_P1_20
+	lpc_ioconf.pio1_20 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO1_20_SCK1);
+#define HAS_SCK1
+#endif
+#ifndef HAS_SCK1
+#error "No pin specified for SCK1"
+#endif
+
+#if SPI_MISO1_P0_22
+	lpc_ioconf.pio0_22 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO0_22_MISO1);
+#define HAS_MISO1
+#endif
+#if SPI_MISO1_P1_21
+	lpc_ioconf.pio1_21 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO1_21_MISO1);
+#define HAS_MISO1
+#endif
+#ifndef HAS_MISO1
+#error "No pin specified for MISO1"
+#endif
+
+#if SPI_MOSI1_P0_21
+	lpc_ioconf.pio0_21 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO0_21_MOSI1);
+#define HAS_MOSI1
+#endif
+#if SPI_MOSI1_P1_22
+	lpc_ioconf.pio1_22 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO1_22_MOSI1);
+#define HAS_MOSI1
+#endif
+#ifndef HAS_MOSI1
+#error "No pin specified for MOSI1"
+#endif
+		
+	/* Enable the device */
+	lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_SSP1);
+
+	/* Turn on the clock */
+	lpc_scb.ssp1clkdiv = 1;
+
+	/* Reset the device */
+	lpc_scb.presetctrl &= ~(1 << LPC_SCB_PRESETCTRL_SSP1_RST_N);
+	lpc_scb.presetctrl |= (1 << LPC_SCB_PRESETCTRL_SSP1_RST_N);
+	ao_spi_channel_init(1);
+#endif /* HAS_SPI_1 */
+}
diff --git a/src/lpc/ao_timer_lpc.c b/src/lpc/ao_timer_lpc.c
new file mode 100644
index 00000000..44fb410e
--- /dev/null
+++ b/src/lpc/ao_timer_lpc.c
@@ -0,0 +1,213 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+
+volatile __data AO_TICK_TYPE ao_tick_count;
+
+uint16_t
+ao_time(void)
+{
+	return ao_tick_count;
+}
+
+#if AO_DATA_ALL
+volatile __data uint8_t	ao_data_interval = 1;
+volatile __data uint8_t	ao_data_count;
+#endif
+
+void lpc_systick_isr(void)
+{
+	if (lpc_systick.csr & (1 << LPC_SYSTICK_CSR_COUNTFLAG)) {
+		++ao_tick_count;
+#if HAS_TASK_QUEUE
+		if (ao_task_alarm_tick && (int16_t) (ao_tick_count - ao_task_alarm_tick) >= 0)
+			ao_task_check_alarm((uint16_t) ao_tick_count);
+#endif
+#if AO_DATA_ALL
+		if (++ao_data_count == ao_data_interval) {
+			ao_data_count = 0;
+			ao_adc_poll();
+#if (AO_DATA_ALL & ~(AO_DATA_ADC))
+			ao_wakeup((void *) &ao_data_count);
+#endif
+		}
+#endif
+	}
+}
+
+#if HAS_ADC
+void
+ao_timer_set_adc_interval(uint8_t interval)
+{
+	ao_arch_critical(
+		ao_data_interval = interval;
+		ao_data_count = 0;
+		);
+}
+#endif
+
+#define SYSTICK_RELOAD ((AO_LPC_SYSCLK / 2) / 100 - 1)
+
+/* Initialize our 100Hz clock */
+void
+ao_timer_init(void)
+{
+	lpc_systick.rvr = SYSTICK_RELOAD;
+	lpc_systick.cvr = 0;
+	lpc_systick.csr = ((1 << LPC_SYSTICK_CSR_ENABLE) |
+			   (1 << LPC_SYSTICK_CSR_TICKINT) |
+			   (LPC_SYSTICK_CSR_CLKSOURCE_CPU_OVER_2 << LPC_SYSTICK_CSR_CLKSOURCE));
+}
+
+#define AO_LPC_M	((AO_LPC_CLKOUT / AO_LPC_CLKIN) - 1)
+
+#define AO_LPC_FCCO_MIN	156000000
+
+static void
+ao_clock_delay(void)
+{
+	uint32_t	i;
+	for (i = 0; i < 200; i++)
+		ao_arch_nop();
+}
+
+void
+ao_clock_init(void)
+{
+	uint8_t		p;
+	uint32_t	i;
+
+	/* Turn off all perhipherals except for GPIO configuration */
+	lpc_scb.sysahbclkctrl = ((1 << LPC_SCB_SYSAHBCLKCTRL_SYS) |
+				 (1 << LPC_SCB_SYSAHBCLKCTRL_ROM) |
+				 (1 << LPC_SCB_SYSAHBCLKCTRL_RAM0) |
+				 (1 << LPC_SCB_SYSAHBCLKCTRL_FLASHARRAY) |
+				 (1 << LPC_SCB_SYSAHBCLKCTRL_GPIO) |
+				 (1 << LPC_SCB_SYSAHBCLKCTRL_IOCON));
+				 
+	/* Enable the brown-out detection at the highest voltage to
+	 * make sure the flash part remains happy
+	 */
+
+	lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_BOD_PD);
+	lpc_scb.bodctrl = ((LPC_SCB_BOD_BODRSTLEV_2_63 << LPC_SCB_BOD_BODRSTLEV) |
+			   (LPC_SCB_BOD_BODINTVAL_RESERVED << LPC_SCB_BOD_BODINTVAL) |
+			   (1 << LPC_SCB_BOD_BODRSTENA));
+
+	/* Turn the IRC clock back on */
+	lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_IRC_PD);
+	ao_clock_delay();
+	
+	/* Switch to the IRC clock */
+	lpc_scb.mainclksel = LPC_SCB_MAINCLKSEL_SEL_IRC << LPC_SCB_MAINCLKSEL_SEL;
+	lpc_scb.mainclkuen = (0 << LPC_SCB_MAINCLKUEN_ENA);
+	lpc_scb.mainclkuen = (1 << LPC_SCB_MAINCLKUEN_ENA);
+	while (!(lpc_scb.mainclkuen & (1 << LPC_SCB_MAINCLKUEN_ENA)))
+		;
+	
+	/* Switch USB to the main clock */
+	lpc_scb.usbclksel = (LPC_SCB_USBCLKSEL_SEL_MAIN_CLOCK << LPC_SCB_USBCLKSEL_SEL);
+	lpc_scb.usbclkuen = (0 << LPC_SCB_USBCLKUEN_ENA);
+	lpc_scb.usbclkuen = (1 << LPC_SCB_USBCLKUEN_ENA);
+	while (!(lpc_scb.usbclkuen & (1 << LPC_SCB_USBCLKUEN_ENA)))
+		;
+	
+	/* Find a PLL post divider ratio that gets the FCCO in range */
+	for (p = 0; p < 4; p++)
+		if (AO_LPC_CLKOUT << (1 + p) >= AO_LPC_FCCO_MIN)
+			break;
+
+	if (p == 4)
+		ao_panic(AO_PANIC_CRASH);
+
+	/* Power down the PLL before touching the registers */
+	lpc_scb.pdruncfg |= (1 << LPC_SCB_PDRUNCFG_SYSPLL_PD);
+	ao_clock_delay();
+
+	/* Set PLL divider values */
+	lpc_scb.syspllctrl = ((AO_LPC_M << LPC_SCB_SYSPLLCTRL_MSEL) |
+			      (p << LPC_SCB_SYSPLLCTRL_PSEL));
+
+	/* Turn off the external crystal clock */
+	lpc_scb.pdruncfg |= (1 << LPC_SCB_PDRUNCFG_SYSOSC_PD);
+	ao_clock_delay();
+
+	/* Configure the crystal clock */
+	lpc_scb.sysoscctrl = ((0 << LPC_SCB_SYSOSCCTRL_BYPASS) |			   /* using a crystal */
+			      ((AO_LPC_CLKIN > 15000000) << LPC_SCB_SYSOSCCTRL_FREQRANGE));/* set range */
+
+	/* Turn on the external crystal clock */
+	lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_SYSOSC_PD);
+	ao_clock_delay();
+
+	/* Select crystal as PLL input */
+
+	lpc_scb.syspllclksel = (LPC_SCB_SYSPLLCLKSEL_SEL_SYSOSC << LPC_SCB_SYSPLLCLKSEL_SEL);
+	lpc_scb.syspllclkuen = (1 << LPC_SCB_SYSPLLCLKUEN_ENA);
+	lpc_scb.syspllclkuen = (0 << LPC_SCB_SYSPLLCLKUEN_ENA);
+	lpc_scb.syspllclkuen = (1 << LPC_SCB_SYSPLLCLKUEN_ENA);
+	while (!(lpc_scb.syspllclkuen & (1 << LPC_SCB_SYSPLLCLKUEN_ENA)))
+		;
+	
+	/* Turn on the PLL */
+	lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_SYSPLL_PD);
+
+	/* Wait for it to lock */
+	
+	for (i = 0; i < 20000; i++)
+		if (lpc_scb.syspllstat & (1 << LPC_SCB_SYSPLLSTAT_LOCK))
+			break;
+	if (i == 20000)
+		ao_panic(AO_PANIC_CRASH);
+
+	/* Switch to the PLL */
+	lpc_scb.mainclksel = LPC_SCB_MAINCLKSEL_SEL_PLL_OUTPUT << LPC_SCB_MAINCLKSEL_SEL;
+	lpc_scb.mainclkuen = (1 << LPC_SCB_MAINCLKUEN_ENA);
+	lpc_scb.mainclkuen = (0 << LPC_SCB_MAINCLKUEN_ENA);
+	lpc_scb.mainclkuen = (1 << LPC_SCB_MAINCLKUEN_ENA);
+	while (!(lpc_scb.mainclkuen & (1 << LPC_SCB_MAINCLKUEN_ENA)))
+		;
+
+	/* Set system clock divider */
+	lpc_scb.sysahbclkdiv = AO_LPC_CLKOUT / AO_LPC_SYSCLK;
+
+	/* Shut down perhipheral clocks (enabled as needed) */
+	lpc_scb.ssp0clkdiv = 0;
+	lpc_scb.uartclkdiv = 0;
+	lpc_scb.ssp1clkdiv = 0;
+	lpc_scb.usbclkdiv = 0;
+	lpc_scb.clkoutdiv = 0;
+
+	/* Switch USB PLL source to system osc so we can power down the IRC */
+	lpc_scb.usbpllclksel = (LPC_SCB_USBPLLCLKSEL_SEL_SYSOSC << LPC_SCB_USBPLLCLKSEL_SEL);
+	lpc_scb.usbpllclkuen = (0 << LPC_SCB_USBPLLCLKUEN_ENA);
+	lpc_scb.usbpllclkuen = (1 << LPC_SCB_USBPLLCLKUEN_ENA);
+	while (!(lpc_scb.usbpllclkuen & (1 << LPC_SCB_USBPLLCLKUEN_ENA)))
+		;
+	
+	/* Power down everything we don't need */
+	lpc_scb.pdruncfg = ((1 << LPC_SCB_PDRUNCFG_IRCOUT_PD) |
+			    (1 << LPC_SCB_PDRUNCFG_IRC_PD) |
+			    (0 << LPC_SCB_PDRUNCFG_BOD_PD) |
+			    (1 << LPC_SCB_PDRUNCFG_ADC_PD) |
+			    (1 << LPC_SCB_PDRUNCFG_WDTOSC_PD) |
+			    (1 << LPC_SCB_PDRUNCFG_USBPLL_PD) |
+			    (1 << LPC_SCB_PDRUNCFG_USBPAD_PD) |
+			    (1 << 11) |
+			    (7 << 13));
+}
diff --git a/src/lpc/ao_usb_lpc.c b/src/lpc/ao_usb_lpc.c
new file mode 100644
index 00000000..686dc3a4
--- /dev/null
+++ b/src/lpc/ao_usb_lpc.c
@@ -0,0 +1,1055 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "ao.h"
+#include "ao_usb.h"
+#include "ao_product.h"
+
+#ifndef USE_USB_STDIO
+#define USE_USB_STDIO	1
+#endif
+
+#if USE_USB_STDIO
+#define AO_USB_OUT_SLEEP_ADDR	(&ao_stdin_ready)
+#else
+#define AO_USB_OUT_SLEEP_ADDR	(&ao_usb_out_avail)
+#endif
+
+#define USB_DEBUG 	0
+#define USB_DEBUG_DATA	0
+#define USB_ECHO	0
+
+#if USB_DEBUG
+#define debug(format, args...)	printf(format, ## args);
+#else
+#define debug(format, args...)
+#endif
+
+#if USB_DEBUG_DATA
+#define debug_data(format, args...)	printf(format, ## args);
+#else
+#define debug_data(format, args...)
+#endif
+
+struct ao_usb_setup {
+	uint8_t		dir_type_recip;
+	uint8_t		request;
+	uint16_t	value;
+	uint16_t	index;
+	uint16_t	length;
+} ao_usb_setup;
+
+static uint8_t 	ao_usb_ep0_state;
+
+/* Pending EP0 IN data */
+static const uint8_t	*ao_usb_ep0_in_data;	/* Remaining data */
+static uint8_t 		ao_usb_ep0_in_len;	/* Remaining amount */
+static uint16_t		ao_usb_ep0_in_max;	/* Requested amount from host */
+
+/* Temp buffer for smaller EP0 in data */
+static uint8_t	ao_usb_ep0_in_buf[2];
+
+/* Pending EP0 OUT data */
+static uint8_t *ao_usb_ep0_out_data;
+static uint8_t 	ao_usb_ep0_out_len;
+
+/*
+ * Objects allocated in special USB memory
+ */
+
+/* USB address of end of allocated storage */
+static uint8_t	*ao_usb_sram;
+
+/* Pointer to ep0 tx/rx buffers in USB memory */
+static uint8_t	*ao_usb_ep0_tx_buffer;
+static uint8_t	*ao_usb_ep0_setup_buffer;
+static uint8_t	*ao_usb_ep0_rx_buffer;
+
+/* Pointer to bulk data tx/rx buffers in USB memory */
+static uint8_t	*ao_usb_in_tx_buffer;
+static uint8_t	*ao_usb_out_rx_buffer;
+
+/* Our data buffers */
+static uint8_t	ao_usb_tx_buffer[AO_USB_IN_SIZE];
+static uint8_t	ao_usb_tx_count;
+
+static uint8_t	ao_usb_rx_buffer[AO_USB_OUT_SIZE];
+static uint8_t	ao_usb_rx_count, ao_usb_rx_pos;
+
+extern struct lpc_usb_endpoint lpc_usb_endpoint;
+
+/* Marks when we don't need to send an IN packet.
+ * This happens only when the last IN packet is not full,
+ * otherwise the host will expect to keep seeing packets.
+ * Send a zero-length packet as required
+ */
+static uint8_t	ao_usb_in_flushed;
+
+/* Marks when we have delivered an IN packet to the hardware
+ * and it has not been received yet. ao_sleep on this address
+ * to wait for it to be delivered.
+ */
+static uint8_t	ao_usb_in_pending;
+
+/* Marks when an OUT packet has been received by the hardware
+ * but not pulled to the shadow buffer.
+ */
+static uint8_t	ao_usb_out_avail;
+static uint8_t	ao_usb_running;
+static uint8_t	ao_usb_configuration;
+static uint8_t	ueienx_0;
+
+#define AO_USB_EP0_GOT_RESET	1
+#define AO_USB_EP0_GOT_SETUP	2
+#define AO_USB_EP0_GOT_RX_DATA	4
+#define AO_USB_EP0_GOT_TX_ACK	8
+
+static uint8_t	ao_usb_ep0_receive;
+static uint8_t	ao_usb_address;
+static uint8_t	ao_usb_address_pending;
+
+static inline uint32_t set_toggle(uint32_t 	current_value,
+				   uint32_t	mask,
+				   uint32_t	desired_value)
+{
+	return (current_value ^ desired_value) & mask;
+}
+
+/*
+ * Set current device address and mark the
+ * interface as active
+ */
+void
+ao_usb_set_address(uint8_t address)
+{
+	debug("ao_usb_set_address %02x\n", address);
+	lpc_usb.devcmdstat = ((address << LPC_USB_DEVCMDSTAT_DEV_ADDR) |
+			      (1 << LPC_USB_DEVCMDSTAT_DEV_EN) |
+			      (0 << LPC_USB_DEVCMDSTAT_SETUP) |
+			      (0 << LPC_USB_DEVCMDSTAT_PLL_ON) |
+			      (0 << LPC_USB_DEVCMDSTAT_LPM_SUP) |
+			      (0 << LPC_USB_DEVCMDSTAT_INTONNAK_AO) |
+			      (0 << LPC_USB_DEVCMDSTAT_INTONNAK_AI) |
+			      (0 << LPC_USB_DEVCMDSTAT_INTONNAK_CO) |
+			      (0 << LPC_USB_DEVCMDSTAT_INTONNAK_CI) |
+			      (1 << LPC_USB_DEVCMDSTAT_DCON) |
+			      (0 << LPC_USB_DEVCMDSTAT_DSUS) |
+			      (0 << LPC_USB_DEVCMDSTAT_DCON_C) |
+			      (0 << LPC_USB_DEVCMDSTAT_DSUS_C) |
+			      (0 << LPC_USB_DEVCMDSTAT_DRES_C) |
+			      (0 << LPC_USB_DEVCMDSTAT_VBUSDEBOUNCED));
+	ao_usb_address_pending = 0;
+}
+
+#define TX_DBG 0
+#define RX_DBG 0
+
+#if TX_DBG
+#define _tx_dbg0(msg) _dbg(__LINE__,msg,0)
+#define _tx_dbg1(msg,value) _dbg(__LINE__,msg,value)
+#else
+#define _tx_dbg0(msg)
+#define _tx_dbg1(msg,value)
+#endif
+
+#if RX_DBG
+#define _rx_dbg0(msg) _dbg(__LINE__,msg,0)
+#define _rx_dbg1(msg,value) _dbg(__LINE__,msg,value)
+#else
+#define _rx_dbg0(msg)
+#define _rx_dbg1(msg,value)
+#endif
+
+#if TX_DBG || RX_DBG
+static void _dbg(int line, char *msg, uint32_t value);
+#endif
+
+/*
+ * Set just endpoint 0, for use during startup
+ */
+
+static uint8_t *
+ao_usb_alloc_sram(uint16_t size)
+{
+	uint8_t	*addr = ao_usb_sram;
+
+	ao_usb_sram += (size + 63) & ~63;
+	return addr;
+}
+
+static uint16_t
+ao_usb_sram_offset(uint8_t *addr)
+{
+	return (uint16_t) ((intptr_t) addr >> 6);
+}
+
+static void
+ao_usb_set_ep(vuint32_t *ep, uint8_t *addr, uint16_t nbytes)
+{
+	*ep = ((ao_usb_sram_offset(addr) << LPC_USB_EP_OFFSET) |
+	       (nbytes << LPC_USB_EP_NBYTES) |
+	       (0 << LPC_USB_EP_ENDPOINT_ISO) |
+	       (0 << LPC_USB_EP_RATE_FEEDBACK) |
+	       (0 << LPC_USB_EP_TOGGLE_RESET) |
+	       (0 << LPC_USB_EP_STALL) |
+	       (0 << LPC_USB_EP_DISABLED) |
+	       (1 << LPC_USB_EP_ACTIVE));
+}
+
+static inline uint16_t
+ao_usb_ep_count(vuint32_t *ep)
+{
+	return (*ep >> LPC_USB_EP_NBYTES) & LPC_USB_EP_NBYTES_MASK;
+}
+
+static inline uint8_t
+ao_usb_ep_stall(vuint32_t *ep)
+{
+	return (*ep >> LPC_USB_EP_STALL) & 1;
+}
+
+static inline vuint32_t *
+ao_usb_ep0_out(void)
+{
+	return &lpc_usb_endpoint.ep0_out;
+}
+
+static inline vuint32_t *
+ao_usb_ep0_in(void)
+{
+	return &lpc_usb_endpoint.ep0_in;
+}
+
+static inline vuint32_t *
+ao_usb_epn_out(uint8_t n)
+{
+	return &lpc_usb_endpoint.epn[n-1].out[0];
+}
+
+static inline vuint32_t *
+ao_usb_epn_in(uint8_t n)
+{
+	return &lpc_usb_endpoint.epn[n-1].in[0];
+}
+
+static void
+ao_usb_set_epn_in(uint8_t n, uint8_t *addr, uint16_t nbytes)
+{
+	ao_usb_set_ep(ao_usb_epn_in(n), addr, nbytes);
+}
+
+static void
+ao_usb_set_epn_out(uint8_t n, uint8_t *addr, uint16_t nbytes)
+{
+	ao_usb_set_ep(ao_usb_epn_out(n), addr, nbytes);
+}
+
+static inline uint16_t
+ao_usb_epn_out_count(uint8_t n)
+{
+	return ao_usb_ep_count(ao_usb_epn_out(n));
+}
+
+static inline uint16_t
+ao_usb_epn_in_count(uint8_t n)
+{
+	return ao_usb_ep_count(ao_usb_epn_in(n));
+}
+
+static uint8_t *
+ao_usb_enable_ep(vuint32_t *ep, uint16_t nbytes, uint16_t set_nbytes)
+{
+	uint8_t	*addr = ao_usb_alloc_sram(nbytes);
+	
+	ao_usb_set_ep(ep, addr, set_nbytes);
+	return addr;
+}
+
+static void
+ao_usb_disable_ep(vuint32_t *ep)
+{
+	*ep = ((0 << LPC_USB_EP_OFFSET) |
+	       (0 << LPC_USB_EP_NBYTES) |
+	       (0 << LPC_USB_EP_ENDPOINT_ISO) |
+	       (0 << LPC_USB_EP_RATE_FEEDBACK) |
+	       (0 << LPC_USB_EP_TOGGLE_RESET) |
+	       (0 << LPC_USB_EP_STALL) |
+	       (1 << LPC_USB_EP_DISABLED) |
+	       (0 << LPC_USB_EP_ACTIVE));
+}
+
+static void
+ao_usb_enable_epn(uint8_t n, uint16_t out_bytes, uint8_t **out_addr, uint16_t in_bytes, uint8_t **in_addr)
+{
+	uint8_t	*addr;
+
+	addr = ao_usb_enable_ep(ao_usb_epn_out(n), out_bytes, out_bytes);
+	if (out_addr)
+		*out_addr = addr;
+	ao_usb_disable_ep(&lpc_usb_endpoint.epn[n-1].out[1]);
+
+	addr = ao_usb_enable_ep(ao_usb_epn_in(n), in_bytes, 0);
+	if (in_addr)
+		*in_addr = addr;
+	ao_usb_disable_ep(&lpc_usb_endpoint.epn[n-1].in[1]);
+}
+
+static void
+ao_usb_disable_epn(uint8_t n)
+{
+	ao_usb_disable_ep(ao_usb_epn_out(n));
+	ao_usb_disable_ep(&lpc_usb_endpoint.epn[n-1].out[1]);
+	ao_usb_disable_ep(ao_usb_epn_in(n));
+	ao_usb_disable_ep(&lpc_usb_endpoint.epn[n-1].in[1]);
+}
+
+static void
+ao_usb_reset(void)
+{
+	ao_usb_set_address(0);
+	ao_usb_configuration = 0;
+}
+
+static void
+ao_usb_set_ep0(void)
+{
+	int			e;
+
+	/* Everything is single buffered for now */
+	lpc_usb.epbufcfg = 0;
+	lpc_usb.epinuse = 0;
+	lpc_usb.epskip = 0xffffffff;
+
+	lpc_usb.intstat = 0xc00003ff;
+
+	ao_usb_sram = lpc_usb_sram;
+
+	lpc_usb.epliststart = (uint32_t) (intptr_t) &lpc_usb_endpoint;
+	lpc_usb.databufstart = ((uint32_t) (intptr_t) ao_usb_sram) & 0xffc00000;
+
+	/* Set up EP 0 - a Control end point with 32 bytes of in and out buffers */
+
+	ao_usb_ep0_rx_buffer = ao_usb_enable_ep(ao_usb_ep0_out(), AO_USB_CONTROL_SIZE, AO_USB_CONTROL_SIZE);
+	ao_usb_ep0_setup_buffer = ao_usb_alloc_sram(AO_USB_CONTROL_SIZE);
+	lpc_usb_endpoint.setup = ao_usb_sram_offset(ao_usb_ep0_setup_buffer);
+	ao_usb_ep0_tx_buffer = ao_usb_enable_ep(ao_usb_ep0_in(), AO_USB_CONTROL_SIZE, 0);
+
+	/* Clear all of the other endpoints */
+	for (e = 1; e <= 4; e++)
+		ao_usb_disable_epn(e);
+	ao_usb_reset();
+}
+
+static void
+ao_usb_set_configuration(void)
+{
+	debug ("ao_usb_set_configuration\n");
+
+	/* Set up the INT end point */
+	ao_usb_enable_epn(AO_USB_INT_EP, 0, NULL, 0, NULL);
+	
+	/* Set up the OUT end point */
+	ao_usb_enable_epn(AO_USB_OUT_EP, AO_USB_OUT_SIZE, &ao_usb_out_rx_buffer, 0, NULL);
+
+	/* Set up the IN end point */
+	ao_usb_enable_epn(AO_USB_IN_EP, 0, NULL, AO_USB_IN_SIZE, &ao_usb_in_tx_buffer);
+
+	ao_usb_running = 1;
+}
+
+/* Send an IN data packet */
+static void
+ao_usb_ep0_flush(void)
+{
+	uint8_t this_len;
+
+	this_len = ao_usb_ep0_in_len;
+	if (this_len > AO_USB_CONTROL_SIZE)
+		this_len = AO_USB_CONTROL_SIZE;
+
+	ao_usb_ep0_in_len -= this_len;
+	ao_usb_ep0_in_max -= this_len;
+
+	if (this_len < AO_USB_CONTROL_SIZE || ao_usb_ep0_in_max == 0)
+		ao_usb_ep0_state = AO_USB_EP0_IDLE;
+
+	debug_data ("Flush EP0 len %d:", this_len);
+	memcpy(ao_usb_ep0_tx_buffer, ao_usb_ep0_in_data, this_len);
+	debug_data ("\n");
+	ao_usb_ep0_in_data += this_len;
+
+	/* Mark the endpoint as TX valid to send the packet */
+	ao_usb_set_ep(ao_usb_ep0_in(), ao_usb_ep0_tx_buffer, this_len);
+	debug ("queue tx.  0 now %08x\n", *ao_usb_ep0_in());
+}
+
+/* Read data from the ep0 OUT fifo */
+static void
+ao_usb_ep0_fill(void)
+{
+	uint16_t	len;
+	uint8_t		*rx_buffer;
+
+	/* Pull all of the data out of the packet */
+	if (lpc_usb.devcmdstat & (1 << LPC_USB_DEVCMDSTAT_SETUP)) {
+		rx_buffer = ao_usb_ep0_setup_buffer;
+		len = 8;
+	} else {
+		rx_buffer = ao_usb_ep0_rx_buffer;
+		len = AO_USB_CONTROL_SIZE - ao_usb_ep_count(ao_usb_ep0_out());
+	}
+
+	if (len > ao_usb_ep0_out_len)
+		len = ao_usb_ep0_out_len;
+	ao_usb_ep0_out_len -= len;
+
+	debug_data ("Fill EP0 len %d:", len);
+	memcpy(ao_usb_ep0_out_data, rx_buffer, len);
+	debug_data ("\n");
+	ao_usb_ep0_out_data += len;
+
+	/* ACK the packet */
+	ao_usb_set_ep(ao_usb_ep0_out(), ao_usb_ep0_rx_buffer, AO_USB_CONTROL_SIZE);
+	lpc_usb.devcmdstat |= (1 << LPC_USB_DEVCMDSTAT_SETUP);
+}
+
+static void
+ao_usb_ep0_in_reset(void)
+{
+	ao_usb_ep0_in_data = ao_usb_ep0_in_buf;
+	ao_usb_ep0_in_len = 0;
+}
+
+static void
+ao_usb_ep0_in_queue_byte(uint8_t a)
+{
+	if (ao_usb_ep0_in_len < sizeof (ao_usb_ep0_in_buf))
+		ao_usb_ep0_in_buf[ao_usb_ep0_in_len++] = a;
+}
+
+static void
+ao_usb_ep0_in_set(const uint8_t *data, uint8_t len)
+{
+	ao_usb_ep0_in_data = data;
+	ao_usb_ep0_in_len = len;
+}
+
+static void
+ao_usb_ep0_out_set(uint8_t *data, uint8_t len)
+{
+	ao_usb_ep0_out_data = data;
+	ao_usb_ep0_out_len = len;
+}
+
+static void
+ao_usb_ep0_in_start(uint16_t max)
+{
+	ao_usb_ep0_in_max = max;
+	/* Don't send more than asked for */
+	if (ao_usb_ep0_in_len > max)
+		ao_usb_ep0_in_len = max;
+	ao_usb_ep0_flush();
+}
+
+static struct ao_usb_line_coding ao_usb_line_coding = {115200, 0, 0, 8};
+
+/* Walk through the list of descriptors and find a match
+ */
+static void
+ao_usb_get_descriptor(uint16_t value)
+{
+	const uint8_t		*descriptor;
+	uint8_t		type = value >> 8;
+	uint8_t		index = value;
+
+	descriptor = ao_usb_descriptors;
+	while (descriptor[0] != 0) {
+		if (descriptor[1] == type && index-- == 0) {
+			uint8_t	len;
+			if (type == AO_USB_DESC_CONFIGURATION)
+				len = descriptor[2];
+			else
+				len = descriptor[0];
+			ao_usb_ep0_in_set(descriptor, len);
+			break;
+		}
+		descriptor += descriptor[0];
+	}
+}
+
+static void
+ao_usb_ep0_setup(void)
+{
+	/* Pull the setup packet out of the fifo */
+	ao_usb_ep0_out_set((uint8_t *) &ao_usb_setup, 8);
+	ao_usb_ep0_fill();
+	if (ao_usb_ep0_out_len != 0) {
+		debug ("invalid setup packet length\n");
+		return;
+	}
+
+	if ((ao_usb_setup.dir_type_recip & AO_USB_DIR_IN) || ao_usb_setup.length == 0)
+		ao_usb_ep0_state = AO_USB_EP0_DATA_IN;
+	else
+		ao_usb_ep0_state = AO_USB_EP0_DATA_OUT;
+
+	ao_usb_ep0_in_reset();
+
+	switch(ao_usb_setup.dir_type_recip & AO_USB_SETUP_TYPE_MASK) {
+	case AO_USB_TYPE_STANDARD:
+		debug ("Standard setup packet\n");
+		switch(ao_usb_setup.dir_type_recip & AO_USB_SETUP_RECIP_MASK) {
+		case AO_USB_RECIP_DEVICE:
+			debug ("Device setup packet\n");
+			switch(ao_usb_setup.request) {
+			case AO_USB_REQ_GET_STATUS:
+				debug ("get status\n");
+				ao_usb_ep0_in_queue_byte(0);
+				ao_usb_ep0_in_queue_byte(0);
+				break;
+			case AO_USB_REQ_SET_ADDRESS:
+				debug ("set address %d\n", ao_usb_setup.value);
+				ao_usb_address = ao_usb_setup.value;
+				ao_usb_address_pending = 1;
+				break;
+			case AO_USB_REQ_GET_DESCRIPTOR:
+				debug ("get descriptor %d\n", ao_usb_setup.value);
+				ao_usb_get_descriptor(ao_usb_setup.value);
+				break;
+			case AO_USB_REQ_GET_CONFIGURATION:
+				debug ("get configuration %d\n", ao_usb_configuration);
+				ao_usb_ep0_in_queue_byte(ao_usb_configuration);
+				break;
+			case AO_USB_REQ_SET_CONFIGURATION:
+				ao_usb_configuration = ao_usb_setup.value;
+				debug ("set configuration %d\n", ao_usb_configuration);
+				ao_usb_set_configuration();
+				break;
+			}
+			break;
+		case AO_USB_RECIP_INTERFACE:
+			debug ("Interface setup packet\n");
+			switch(ao_usb_setup.request) {
+			case AO_USB_REQ_GET_STATUS:
+				ao_usb_ep0_in_queue_byte(0);
+				ao_usb_ep0_in_queue_byte(0);
+				break;
+			case AO_USB_REQ_GET_INTERFACE:
+				ao_usb_ep0_in_queue_byte(0);
+				break;
+			case AO_USB_REQ_SET_INTERFACE:
+				break;
+			}
+			break;
+		case AO_USB_RECIP_ENDPOINT:
+			debug ("Endpoint setup packet\n");
+			switch(ao_usb_setup.request) {
+			case AO_USB_REQ_GET_STATUS:
+				ao_usb_ep0_in_queue_byte(0);
+				ao_usb_ep0_in_queue_byte(0);
+				break;
+			}
+			break;
+		}
+		break;
+	case AO_USB_TYPE_CLASS:
+		debug ("Class setup packet\n");
+		switch (ao_usb_setup.request) {
+		case AO_USB_SET_LINE_CODING:
+			debug ("set line coding\n");
+			ao_usb_ep0_out_set((uint8_t *) &ao_usb_line_coding, 7);
+			break;
+		case AO_USB_GET_LINE_CODING:
+			debug ("get line coding\n");
+			ao_usb_ep0_in_set((const uint8_t *) &ao_usb_line_coding, 7);
+			break;
+		case AO_USB_SET_CONTROL_LINE_STATE:
+			break;
+		}
+		break;
+	}
+
+	/* If we're not waiting to receive data from the host,
+	 * queue an IN response
+	 */
+	if (ao_usb_ep0_state == AO_USB_EP0_DATA_IN)
+		ao_usb_ep0_in_start(ao_usb_setup.length);
+}
+
+static void
+ao_usb_ep0_handle(uint8_t receive)
+{
+	ao_usb_ep0_receive = 0;
+
+	if (receive & AO_USB_EP0_GOT_RESET) {
+		debug ("\treset\n");
+		ao_usb_reset();
+		return;
+	}
+	if (receive & AO_USB_EP0_GOT_SETUP) {
+		debug ("\tsetup\n");
+		ao_usb_ep0_setup();
+	}
+	if (receive & AO_USB_EP0_GOT_RX_DATA) {
+		debug ("\tgot rx data\n");
+		if (ao_usb_ep0_state == AO_USB_EP0_DATA_OUT) {
+			ao_usb_ep0_fill();
+			if (ao_usb_ep0_out_len == 0) {
+				ao_usb_ep0_state = AO_USB_EP0_DATA_IN;
+				ao_usb_ep0_in_start(0);
+			}
+		}
+	}
+	if (receive & AO_USB_EP0_GOT_TX_ACK) {
+		debug ("\tgot tx ack\n");
+
+		/* Wait until the IN packet is received from addr 0
+		 * before assigning our local address
+		 */
+		if (ao_usb_address_pending) {
+#if HAS_FLIGHT
+			/* Go to idle mode if USB is connected
+			 */
+			ao_flight_force_idle = 1;
+#endif
+			ao_usb_set_address(ao_usb_address);
+		}
+		if (ao_usb_ep0_state == AO_USB_EP0_DATA_IN)
+			ao_usb_ep0_flush();
+	}
+}
+
+static uint16_t	control_count;
+static uint16_t int_count;
+static uint16_t	in_count;
+static uint16_t	out_count;
+static uint16_t	reset_count;
+
+void
+lpc_usb_irq_isr(void)
+{
+	uint32_t	intstat = lpc_usb.intstat & lpc_usb.inten;
+
+	lpc_usb.intstat = intstat;
+	/* Handle EP0 OUT packets */
+	if (intstat & (1 << LPC_USB_INT_EPOUT(0))) {
+		if (lpc_usb.devcmdstat & (1 << LPC_USB_DEVCMDSTAT_SETUP))
+			ao_usb_ep0_receive |= AO_USB_EP0_GOT_SETUP;
+		else
+			ao_usb_ep0_receive |= AO_USB_EP0_GOT_RX_DATA;
+
+		ao_usb_ep0_handle(ao_usb_ep0_receive);
+	}
+
+	/* Handle EP0 IN packets */
+	if (intstat & (1 << LPC_USB_INT_EPIN(0))) {
+		ao_usb_ep0_receive |= AO_USB_EP0_GOT_TX_ACK;
+
+		ao_usb_ep0_handle(ao_usb_ep0_receive);
+	}
+
+
+	/* Handle OUT packets */
+	if (intstat & (1 << LPC_USB_INT_EPOUT(AO_USB_OUT_EP))) {
+		++out_count;
+		_rx_dbg1("RX ISR", *ao_usb_epn_out(AO_USB_OUT_EP));
+		ao_usb_out_avail = 1;
+		_rx_dbg0("out avail set");
+		ao_wakeup(AO_USB_OUT_SLEEP_ADDR)
+		_rx_dbg0("stdin awoken");
+	}
+
+	/* Handle IN packets */
+	if (intstat & (1 << LPC_USB_INT_EPIN(AO_USB_IN_EP))) {
+		++in_count;
+		_tx_dbg1("TX ISR", *ao_usb_epn_in(AO_USB_IN_EP));
+		ao_usb_in_pending = 0;
+		ao_wakeup(&ao_usb_in_pending);
+	}
+
+	/* NAK all INT EP IN packets */
+	if (intstat & (1 << LPC_USB_INT_EPIN(AO_USB_INT_EP))) {
+		;
+	}
+
+	/* Check for reset */
+	if (intstat & (1 << LPC_USB_INT_DEV)) {
+		if (lpc_usb.devcmdstat & (1 << LPC_USB_DEVCMDSTAT_DRES_C))
+		{
+			lpc_usb.devcmdstat |= (1 << LPC_USB_DEVCMDSTAT_DRES_C);
+			ao_usb_ep0_receive |= AO_USB_EP0_GOT_RESET;
+			ao_usb_ep0_handle(ao_usb_ep0_receive);
+		}
+	}
+}
+
+
+
+/* Queue the current IN buffer for transmission */
+static void
+_ao_usb_in_send(void)
+{
+	_tx_dbg0("in_send start");
+	debug ("send %d\n", ao_usb_tx_count);
+	while (ao_usb_in_pending)
+		ao_sleep(&ao_usb_in_pending);
+	ao_usb_in_pending = 1;
+	if (ao_usb_tx_count != AO_USB_IN_SIZE)
+		ao_usb_in_flushed = 1;
+	memcpy(ao_usb_in_tx_buffer, ao_usb_tx_buffer, ao_usb_tx_count);
+	ao_usb_set_ep(ao_usb_epn_in(AO_USB_IN_EP), ao_usb_in_tx_buffer, ao_usb_tx_count);
+	ao_usb_tx_count = 0;
+	_tx_dbg0("in_send end");
+}
+
+/* Wait for a free IN buffer. Interrupts are blocked */
+static void
+_ao_usb_in_wait(void)
+{
+	for (;;) {
+		/* Check if the current buffer is writable */
+		if (ao_usb_tx_count < AO_USB_IN_SIZE)
+			break;
+
+		_tx_dbg0("in_wait top");
+		/* Wait for an IN buffer to be ready */
+		while (ao_usb_in_pending)
+			ao_sleep(&ao_usb_in_pending);
+		_tx_dbg0("in_wait bottom");
+	}
+}
+
+void
+ao_usb_flush(void)
+{
+	if (!ao_usb_running)
+		return;
+
+	/* Anytime we've sent a character since
+	 * the last time we flushed, we'll need
+	 * to send a packet -- the only other time
+	 * we would send a packet is when that
+	 * packet was full, in which case we now
+	 * want to send an empty packet
+	 */
+	ao_arch_block_interrupts();
+	while (!ao_usb_in_flushed) {
+		_tx_dbg0("flush top");
+		_ao_usb_in_send();
+		_tx_dbg0("flush end");
+	}
+	ao_arch_release_interrupts();
+}
+
+void
+ao_usb_putchar(char c)
+{
+	if (!ao_usb_running)
+		return;
+
+	ao_arch_block_interrupts();
+	_ao_usb_in_wait();
+
+	ao_usb_in_flushed = 0;
+	ao_usb_tx_buffer[ao_usb_tx_count++] = (uint8_t) c;
+
+	/* Send the packet when full */
+	if (ao_usb_tx_count == AO_USB_IN_SIZE) {
+		_tx_dbg0("putchar full");
+		_ao_usb_in_send();
+		_tx_dbg0("putchar flushed");
+	}
+	ao_arch_release_interrupts();
+}
+
+static void
+_ao_usb_out_recv(void)
+{
+	_rx_dbg0("out_recv top");
+	ao_usb_out_avail = 0;
+
+	ao_usb_rx_count = AO_USB_OUT_SIZE - ao_usb_epn_out_count(AO_USB_OUT_EP);
+
+	_rx_dbg1("out_recv count", ao_usb_rx_count);
+	debug ("recv %d\n", ao_usb_rx_count);
+	debug_data("Fill OUT len %d:", ao_usb_rx_count);
+	memcpy(ao_usb_rx_buffer, ao_usb_out_rx_buffer, ao_usb_rx_count);
+	debug_data("\n");
+	ao_usb_rx_pos = 0;
+
+	/* ACK the packet */
+	ao_usb_set_epn_out(AO_USB_OUT_EP, ao_usb_out_rx_buffer, AO_USB_OUT_SIZE);
+}
+
+int
+_ao_usb_pollchar(void)
+{
+	uint8_t c;
+
+	if (!ao_usb_running)
+		return AO_READ_AGAIN;
+
+	for (;;) {
+		if (ao_usb_rx_pos != ao_usb_rx_count)
+			break;
+
+		_rx_dbg0("poll check");
+		/* Check to see if a packet has arrived */
+		if (!ao_usb_out_avail) {
+			_rx_dbg0("poll none");
+			return AO_READ_AGAIN;
+		}
+		_ao_usb_out_recv();
+	}
+
+	/* Pull a character out of the fifo */
+	c = ao_usb_rx_buffer[ao_usb_rx_pos++];
+	return c;
+}
+
+char
+ao_usb_getchar(void)
+{
+	int	c;
+
+	ao_arch_block_interrupts();
+	while ((c = _ao_usb_pollchar()) == AO_READ_AGAIN)
+		ao_sleep(AO_USB_OUT_SLEEP_ADDR);
+	ao_arch_release_interrupts();
+	return c;
+}
+
+void
+ao_usb_disable(void)
+{
+	ao_arch_block_interrupts();
+
+#if HAS_USB_PULLUP
+	ao_gpio_set(AO_USB_PULLUP_PORT, AO_USB_PULLUP_PIN, AO_USB_PULLUP, 0);
+#endif
+	/* Disable interrupts */
+	lpc_usb.inten = 0;
+
+	lpc_nvic_clear_enable(LPC_ISR_USB_IRQ_POS);
+
+	/* Disable the device */
+	lpc_usb.devcmdstat = 0;
+
+	/* Turn off USB clock */
+	lpc_scb.usbclkdiv = 0;
+
+	/* Disable USB PHY and PLL */
+	lpc_scb.pdruncfg |= ((1 << LPC_SCB_PDRUNCFG_USBPAD_PD) |
+			     (1 << LPC_SCB_PDRUNCFG_USBPLL_PD));
+
+	/* Disable USB registers and RAM */
+	lpc_scb.sysahbclkctrl &= ~((1 << LPC_SCB_SYSAHBCLKCTRL_USB) |
+				   (1 << LPC_SCB_SYSAHBCLKCTRL_USBRAM));
+
+	ao_arch_release_interrupts();
+}
+
+void
+ao_usb_enable(void)
+{
+	int	t;
+
+	/* Enable USB pins */
+#if HAS_USB_CONNECT
+	lpc_ioconf.pio0_6 = ((LPC_IOCONF_FUNC_USB_CONNECT << LPC_IOCONF_FUNC) |
+			     (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) |
+			     (0 << LPC_IOCONF_HYS) |
+			     (0 << LPC_IOCONF_INV) |
+			     (0 << LPC_IOCONF_OD) |
+			     0x80);
+#endif
+#if HAS_USB_VBUS
+	lpc_ioconf.pio0_3 = ((LPC_IOCONF_FUNC_USB_VBUS << LPC_IOCONF_FUNC) |
+			     (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) |
+			     (0 << LPC_IOCONF_HYS) |
+			     (0 << LPC_IOCONF_INV) |
+			     (0 << LPC_IOCONF_OD) |
+			     0x80);
+#endif
+	/* Enable USB registers and RAM */
+	lpc_scb.sysahbclkctrl |= ((1 << LPC_SCB_SYSAHBCLKCTRL_USB) |
+				  (1 << LPC_SCB_SYSAHBCLKCTRL_USBRAM));
+
+	/* Enable USB PHY */
+	lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_USBPAD_PD);
+	
+	/* Turn on USB PLL */
+	lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_USBPLL_PD);
+
+	lpc_scb.usbpllclksel = (LPC_SCB_SYSPLLCLKSEL_SEL_SYSOSC << LPC_SCB_SYSPLLCLKSEL_SEL);
+	lpc_scb.usbpllclkuen = (0 << LPC_SCB_USBPLLCLKUEN_ENA);
+	lpc_scb.usbpllclkuen = (1 << LPC_SCB_USBPLLCLKUEN_ENA);
+	while (!(lpc_scb.usbpllclkuen & (1 << LPC_SCB_USBPLLCLKUEN_ENA)))
+		;
+	lpc_scb.usbpllctrl = 0x23;
+	while (!(lpc_scb.usbpllstat & 1))
+		;
+
+	lpc_scb.usbclksel = 0;
+	lpc_scb.usbclkuen = (0 << LPC_SCB_USBCLKUEN_ENA);
+	lpc_scb.usbclkuen = (1 << LPC_SCB_USBCLKUEN_ENA);
+	while (!(lpc_scb.usbclkuen & (1 << LPC_SCB_USBCLKUEN_ENA)))
+		;
+
+	/* Turn on USB clock, use 48MHz clock unchanged */
+	lpc_scb.usbclkdiv = 1;
+
+	/* Configure interrupts */
+	ao_arch_block_interrupts();
+
+	/* Route all interrupts to the main isr */
+	lpc_usb.introuting = 0;
+
+	/* Configure NVIC */
+
+	lpc_nvic_set_enable(LPC_ISR_USB_IRQ_POS);
+	lpc_nvic_set_priority(LPC_ISR_USB_IRQ_POS, 0);
+
+	/* Clear any spurious interrupts */
+	lpc_usb.intstat = 0xffffffff;
+
+	debug ("ao_usb_enable\n");
+
+	/* Enable interrupts */
+	lpc_usb.inten = ((1 << LPC_USB_INT_EPOUT(0)) |
+			 (1 << LPC_USB_INT_EPIN(0)) |
+			 (1 << LPC_USB_INT_EPIN(AO_USB_INT_EP)) |
+			 (1 << LPC_USB_INT_EPOUT(AO_USB_OUT_EP)) |
+			 (1 << LPC_USB_INT_EPIN(AO_USB_IN_EP)) |
+			 (1 << LPC_USB_INT_DEV));
+
+	ao_arch_release_interrupts();
+
+	lpc_usb.devcmdstat = 0;
+	for (t = 0; t < 1000; t++)
+		ao_arch_nop();
+
+	ao_usb_set_ep0();
+
+#if HAS_USB_PULLUP
+	ao_gpio_set(AO_USB_PULLUP_PORT, AO_USB_PULLUP_PIN, AO_USB_PULLUP, 1);
+#endif
+}
+
+#if USB_ECHO
+struct ao_task ao_usb_echo_task;
+
+static void
+ao_usb_echo(void)
+{
+	char	c;
+
+	for (;;) {
+		c = ao_usb_getchar();
+		ao_usb_putchar(c);
+		ao_usb_flush();
+	}
+}
+#endif
+
+#if USB_DEBUG
+static void
+ao_usb_irq(void)
+{
+	printf ("control: %d out: %d in: %d int: %d reset: %d\n",
+		control_count, out_count, in_count, int_count, reset_count);
+}
+
+__code struct ao_cmds ao_usb_cmds[] = {
+	{ ao_usb_irq, "I\0Show USB interrupt counts" },
+	{ 0, NULL }
+};
+#endif
+
+void
+ao_usb_init(void)
+{
+#if HAS_USB_PULLUP
+	ao_enable_output(AO_USB_PULLUP_PORT, AO_USB_PULLUP_PIN, AO_USB_PULLUP, 0);
+#endif
+
+	ao_usb_enable();
+
+	debug ("ao_usb_init\n");
+#if USB_ECHO
+	ao_add_task(&ao_usb_echo_task, ao_usb_echo, "usb echo");
+#endif
+#if USB_DEBUG
+	ao_cmd_register(&ao_usb_cmds[0]);
+#endif
+#if USE_USB_STDIO
+	ao_add_stdio(_ao_usb_pollchar, ao_usb_putchar, ao_usb_flush);
+#endif
+}
+
+#if TX_DBG || RX_DBG
+
+struct ao_usb_dbg {
+	int		line;
+	char		*msg;
+	uint32_t	value;
+	uint32_t	primask;
+#if TX_DBG
+	uint16_t	in_count;
+	uint32_t	in_ep;
+	uint32_t	in_pending;
+	uint32_t	tx_count;
+	uint32_t	in_flushed;
+#endif
+#if RX_DBG
+	uint8_t		rx_count;
+	uint8_t		rx_pos;
+	uint8_t		out_avail;
+	uint32_t	out_ep;
+#endif
+};
+
+#define NUM_USB_DBG	8
+
+static struct ao_usb_dbg dbg[NUM_USB_DBG];
+static int dbg_i;
+
+static void _dbg(int line, char *msg, uint32_t value)
+{
+	uint32_t	primask;
+	dbg[dbg_i].line = line;
+	dbg[dbg_i].msg = msg;
+	dbg[dbg_i].value = value;
+	asm("mrs %0,primask" : "=&r" (primask));
+	dbg[dbg_i].primask = primask;
+#if TX_DBG
+	dbg[dbg_i].in_count = in_count;
+	dbg[dbg_i].in_ep = *ao_usb_epn_in(AO_USB_IN_EP);
+	dbg[dbg_i].in_pending = ao_usb_in_pending;
+	dbg[dbg_i].tx_count = ao_usb_tx_count;
+	dbg[dbg_i].in_flushed = ao_usb_in_flushed;
+#endif
+#if RX_DBG
+	dbg[dbg_i].rx_count = ao_usb_rx_count;
+	dbg[dbg_i].rx_pos = ao_usb_rx_pos;
+	dbg[dbg_i].out_avail = ao_usb_out_avail;
+	dbg[dbg_i].out_ep = *ao_usb_epn_out(AO_USB_OUT_EP);
+#endif
+	if (++dbg_i == NUM_USB_DBG)
+		dbg_i = 0;
+}
+#endif
diff --git a/src/lpc/baud_rate b/src/lpc/baud_rate
new file mode 100644
index 00000000..2bfbf309
--- /dev/null
+++ b/src/lpc/baud_rate
@@ -0,0 +1,131 @@
+#!/usr/bin/env nickle
+
+/*
+ * Given a main clock frequency,
+ * compute USART clock freq and a table
+ * of USART config parameters for our target baud rates
+ */
+
+real main_clock = 0;
+real usart_clock = 0;
+
+real[] baud_rates = { 4800, 9600, 19200, 57600, 115200 };
+
+void
+compute_baud_rate(real rate) {
+	int	divaddval;
+	int	mulval;
+
+	real 	dl_est = usart_clock / (16 * rate);
+
+	if (dl_est == floor(dl_est)) {
+		divaddval = 0;
+		mulval = 1;
+	} else {
+		if (false) {
+
+			/* This is how the docs suggest doing it; this
+			 * generates a rate which is reasonably close
+			 */
+
+			real fr_est = 1.5;
+
+			/* Compute fractional estimate */
+			do {
+				dl_est = floor(usart_clock / (16 * rate * fr_est) + 0.5);
+				fr_est = usart_clock / (16 * rate * dl_est);
+			} while (fr_est <= 1.1 || 1.9 <= fr_est);
+
+			/* Given fractional estimate, compute divaddval/mulvals that work best */
+
+			real best_dist = 1000;
+			for (int tmp_divaddval = 1; tmp_divaddval < 15; tmp_divaddval++) {
+				for (int tmp_mulval = 1; tmp_mulval < 16; tmp_mulval++) {
+					real fr = 1 + tmp_divaddval / tmp_mulval;
+					real dist = abs(fr - fr_est);
+					if (dist < best_dist) {
+						divaddval = tmp_divaddval;
+						mulval = tmp_mulval;
+						best_dist = dist;
+					}
+				}
+			}
+		} else {
+
+			/* This exhaustively searches for the best match */
+
+			real my_best_dist = 1e20;
+			int my_best_dl;
+			int my_best_divaddval;
+			int my_best_mulval;
+			for (int my_dl = 1; my_dl < 1024; my_dl++) {
+				for (int my_mulval = 1; my_mulval < 16; my_mulval++) {
+					for (int my_divaddval = 0; my_divaddval < my_mulval; my_divaddval++) {
+						real my_rate = usart_clock / ((16 * my_dl) * (1 + my_divaddval/my_mulval));
+
+						real my_dist = abs(rate - my_rate);
+
+						if (my_dist == 0 && my_divaddval == 0) {
+							my_dist = -1;
+						}
+
+						if (my_dist < my_best_dist) {
+							my_best_dl = my_dl;
+							my_best_divaddval = my_divaddval;
+							my_best_mulval = my_mulval;
+							my_best_dist = my_dist;
+						}
+					}
+				}
+			}
+
+			dl_est = my_best_dl;
+			divaddval = my_best_divaddval;
+			mulval = my_best_mulval;
+		}
+	}
+
+	int dl = floor (dl_est);	
+
+	real actual = usart_clock / ((16 * dl) * (1 + divaddval/mulval));
+
+	printf("\t[AO_SERIAL_SPEED_%d] = { /* actual = %8.2f */\n", floor(rate), actual);
+	printf("\t\t.dl = %d,\n", dl);
+	printf("\t\t.divaddval = %d,\n", divaddval);
+	printf("\t\t.mulval = %d\n", mulval);
+	printf("\t},\n");
+}
+
+void
+main() {
+	if (dim(argv) < 2) {
+		printf ("usage: %s <main-clock>\n", argv[0]);
+		exit(1);
+	}
+	main_clock = string_to_real(argv[1]);
+
+	for (int div = 0; div < 4; div++) {
+		if (main_clock / (1 << div) <= 12000000) {
+			usart_clock = main_clock / (1 << div);
+			break;
+		}
+	}
+
+	if (usart_clock == 0) {
+		printf ("can't get usart clock in range\n");
+		exit(1);
+	}
+
+	printf ("#define AO_LPC_USARTCLK %d\n\n", floor(usart_clock));
+	printf("static const struct {\n");
+	printf("\tuint16_t dl;\n");
+	printf("\tuint8_t divaddval;\n");
+	printf("\tuint8_t mulval;\n");
+	printf("} ao_usart_speeds[] = {\n");
+	for (int i = 0; i < dim(baud_rates); i++) {
+		compute_baud_rate(baud_rates[i]);
+	}
+	printf ("};\n");
+}
+
+main();
diff --git a/src/lpc/figure-checksum b/src/lpc/figure-checksum
new file mode 100755
index 00000000..0b1de578
--- /dev/null
+++ b/src/lpc/figure-checksum
@@ -0,0 +1,39 @@
+#!/usr/bin/env nickle
+
+autoimport Process;
+
+int byteflip(int x) {
+	return ((x >> 24) & 0xff) | ((x >> 8) & 0xff00) | ((x << 8) & 0xff0000) | ((x << 24) & 0xff000000);
+}
+
+void main () {
+	file	input = popen(popen_direction.read, true, "objdump",
+			      "objdump", "-j", ".text",
+			      "--start-address=0",
+			      "--stop-address=0x20",
+			      "-s", argv[1]);
+	int sum = 0;
+
+	void add_in(int addr, int value) {
+		if (addr < 0x1c) {
+			sum += value;
+		} else if (addr == 0x1c) {
+			printf ("-DCKSUM=0x%08x\n", -sum & 0xffffffff);
+			exit(0);
+		}
+	}
+	while (!File::end(input)) {
+		string line = File::fgets(input);
+		string[] words = String::wordsplit(line, " ");
+
+		if (dim(words) < 5)
+		    continue;
+		if (words[0] == "0000" || words[0] == "0010") {
+			int addr = string_to_integer(words[0], 16);
+			for (int i = 0; i < 4; i++)
+				add_in(addr + i * 4, byteflip(string_to_integer(words[i+1], 16)));
+		}
+	}
+}
+
+main();
diff --git a/src/lpc/lpc.h b/src/lpc/lpc.h
new file mode 100644
index 00000000..3300c86f
--- /dev/null
+++ b/src/lpc/lpc.h
@@ -0,0 +1,1249 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _LPC_H_
+#define _LPC_H_
+
+#include <stdint.h>
+
+typedef volatile uint32_t	vuint32_t;
+typedef volatile uint16_t	vuint16_t;
+typedef volatile uint8_t	vuint8_t;
+typedef volatile void *		vvoid_t;
+
+struct lpc_ioconf {
+	vuint32_t	pio0_0;
+	vuint32_t	pio0_1;
+	vuint32_t	pio0_2;
+	vuint32_t	pio0_3;
+
+	vuint32_t	pio0_4;
+	vuint32_t	pio0_5;
+	vuint32_t	pio0_6;
+	vuint32_t	pio0_7;
+
+	vuint32_t	pio0_8;
+	vuint32_t	pio0_9;
+	vuint32_t	pio0_10;
+	vuint32_t	pio0_11;
+
+	vuint32_t	pio0_12;
+	vuint32_t	pio0_13;
+	vuint32_t	pio0_14;
+	vuint32_t	pio0_15;
+
+	vuint32_t	pio0_16;
+	vuint32_t	pio0_17;
+	vuint32_t	pio0_18;
+	vuint32_t	pio0_19;
+
+	vuint32_t	pio0_20;
+	vuint32_t	pio0_21;
+	vuint32_t	pio0_22;
+	vuint32_t	pio0_23;
+
+	vuint32_t	pio1_0;		/* 0x60 */
+	vuint32_t	pio1_1;
+	vuint32_t	pio1_2;
+	vuint32_t	pio1_3;
+
+	vuint32_t	pio1_4;
+	vuint32_t	pio1_5;
+	vuint32_t	pio1_6;
+	vuint32_t	pio1_7;
+
+	vuint32_t	pio1_8;		/* 0x80 */
+	vuint32_t	pio1_9;
+	vuint32_t	pio1_10;
+	vuint32_t	pio1_11;
+
+	vuint32_t	pio1_12;
+	vuint32_t	pio1_13;
+	vuint32_t	pio1_14;
+	vuint32_t	pio1_15;
+
+	vuint32_t	pio1_16;	/* 0xa0 */
+	vuint32_t	pio1_17;
+	vuint32_t	pio1_18;
+	vuint32_t	pio1_19;
+
+	vuint32_t	pio1_20;
+	vuint32_t	pio1_21;
+	vuint32_t	pio1_22;
+	vuint32_t	pio1_23;
+
+	vuint32_t	pio1_24;	/* 0xc0 */
+	vuint32_t	pio1_25;
+	vuint32_t	pio1_26;
+	vuint32_t	pio1_27;
+
+	vuint32_t	pio1_28;
+	vuint32_t	pio1_29;
+	vuint32_t	pio1_30;
+	vuint32_t	pio1_31;
+};
+
+extern struct lpc_ioconf lpc_ioconf;
+
+#define LPC_IOCONF_FUNC		0
+
+/* PIO0_0 */
+#define  LPC_IOCONF_FUNC_RESET		0
+#define  LPC_IOCONF_FUNC_PIO0_0		1
+
+/* PIO0_1 */
+#define  LPC_IOCONF_FUNC_PIO0_1		0
+#define  LPC_IOCONF_FUNC_CLKOUT		1
+#define  LPC_IOCONF_FUNC_CT32B0_MAT2	2
+#define  LPC_IOCONF_FUNC_USB_FTOGGLE	3
+
+/* PIO0_2 */
+#define  LPC_IOCONF_FUNC_PIO0_2		0
+#define  LPC_IOCONF_FUNC_SSEL0		1
+#define  LPC_IOCONF_FUNC_CT16B0_CAP0	2
+
+/* PIO0_3 */
+#define  LPC_IOCONF_FUNC_PIO0_3		0
+#define  LPC_IOCONF_FUNC_USB_VBUS	1
+
+/* PIO0_4
+#define  LPC_IOCONF_FUNC_PIO0_4		0
+#define  LPC_IOCONF_FUNC_I2C_SCL	1
+
+/* PIO0_5 */
+#define  LPC_IOCONF_FUNC_PIO0_5		0
+#define  LPC_IOCONF_FUNC_I2C_SDA	1
+
+/* PIO0_6 */
+#define  LPC_IOCONF_FUNC_PIO0_6		0
+#define  LPC_IOCONF_FUNC_USB_CONNECT	1
+#define  LPC_IOCONF_FUNC_PIO0_6_SCK0	2
+
+/* PIO0_7 */
+#define  LPC_IOCONF_FUNC_PIO0_7		0
+#define  LPC_IOCONF_FUNC_CTS		1
+
+/* PIO0_8 */
+#define  LPC_IOCONF_FUNC_PIO0_8		0
+#define  LPC_IOCONF_FUNC_MISO0		1
+#define  LPC_IOCONF_FUNC_CT16B0_MAT0	2
+
+/* PIO0_9 */
+#define  LPC_IOCONF_FUNC_PIO0_9		0
+#define  LPC_IOCONF_FUNC_MOSI0		1
+#define  LPC_IOCONF_FUNC_CT16B0_MAT1	2
+
+/* PIO0_10 */
+#define  LPC_IOCONF_FUNC_SWCLK		0
+#define  LPC_IOCONF_FUNC_PIO0_10	1
+#define  LPC_IOCONF_FUNC_PIO0_10_SCK0	2
+#define  LPC_IOCONF_FUNC_CT16B0_MAT2	3
+
+/* PIO0_11 */
+#define  LPC_IOCONF_FUNC_TDI		0
+#define  LPC_IOCONF_FUNC_PIO0_11	1
+#define  LPC_IOCONF_FUNC_AD0		2
+#define  LPC_IOCONF_FUNC_CT32B0_MAT3	3
+
+/* PIO0_12 */
+#define  LPC_IOCONF_FUNC_TMS		0
+#define  LPC_IOCONF_FUNC_PIO0_12	1
+#define  LPC_IOCONF_FUNC_AD1		2
+#define  LPC_IOCONF_FUNC_CT32B1_CAP0	3
+
+/* PIO0_13 */
+#define  LPC_IOCONF_FUNC_TD0		0
+#define  LPC_IOCONF_FUNC_PIO0_13	1
+#define  LPC_IOCONF_FUNC_AD2		2
+#define  LPC_IOCONF_FUNC_CT32B1_MAT0	3
+
+/* PIO0_14 */
+#define  LPC_IOCONF_FUNC_TRST		0
+#define  LPC_IOCONF_FUNC_PIO0_14	1
+#define  LPC_IOCONF_FUNC_AD3		2
+#define  LPC_IOCONF_FUNC_PIO0_14_CT32B1_MAT1	3
+
+/* PIO0_15 */
+#define  LPC_IOCONF_FUNC_SWDIO		0
+#define  LPC_IOCONF_FUNC_PIO0_15	1
+#define  LPC_IOCONF_FUNC_AD4		2
+#define  LPC_IOCONF_FUNC_CT32B1_MAT2	3
+
+/* PIO0_16 */
+#define  LPC_IOCONF_FUNC_PIO0_16	0
+#define  LPC_IOCONF_FUNC_AD5		1
+#define  LPC_IOCONF_FUNC_CT32B1_MAT3	2
+
+/* PIO0_17 */
+#define  LPC_IOCONF_FUNC_PIO0_17	0
+#define  LPC_IOCONF_FUNC_RTS		1
+#define  LPC_IOCONF_FUNC_CT32B0_CAP0	2
+#define  LPC_IOCONF_FUNC_SCLK		3
+
+/* PIO0_18 */
+#define  LPC_IOCONF_FUNC_PIO0_18		0
+#define  LPC_IOCONF_FUNC_PIO0_18_RXD		1
+#define  LPC_IOCONF_FUNC_PIO0_18_CT32B0_MAT0	2
+
+/* PIO0_19 */
+#define  LPC_IOCONF_FUNC_PIO0_19		0
+#define  LPC_IOCONF_FUNC_PIO0_19_TXD		1
+#define  LPC_IOCONF_FUNC_PIO0_19_CT32B0_MAT1	2
+
+/* PIO0_20 */
+#define  LPC_IOCONF_FUNC_PIO0_20	0
+#define  LPC_IOCONF_FUNC_CT16B1_CAP0	1
+
+/* PIO0_21 */
+#define  LPC_IOCONF_FUNC_PIO0_21	0
+#define  LPC_IOCONF_FUNC_CT16B1_MAT0	1
+#define  LPC_IOCONF_FUNC_PIO0_21_MOSI1	2
+
+/* PIO0_22 */
+#define  LPC_IOCONF_FUNC_PIO0_22	0
+#define  LPC_IOCONF_FUNC_AD6		1
+#define  LPC_IOCONF_FUNC_CT16B1_MAT1	2
+#define  LPC_IOCONF_FUNC_PIO0_22_MISO1	3
+
+/* PIO0_23 */
+#define  LPC_IOCONF_FUNC_PIO0_23	0
+#define  LPC_IOCONF_FUNC_AD7		1
+
+/* PIO1_0 */
+#define  LPC_IOCONF_FUNC_PIO1_0		0
+#define  LPC_IOCONF_FUNC_CT32B1_MAT1	1
+
+/* PIO1_1 */
+#define  LPC_IOCONF_FUNC_PIO1_1		0
+#define  LPC_IOCONF_FUNC_CT32B1_MAT1	1
+
+/* PIO1_2 */
+#define  LPC_IOCONF_FUNC_PIO1_2		0
+#define  LPC_IOCONF_FUNC_PIO1_2_CT32B1_MAT2	1
+
+/* PIO1_3*/
+#define  LPC_IOCONF_FUNC_PIO1_3		0
+#define  LPC_IOCONF_FUNC_PIO1_3_CT32B1_MAT3	1
+
+/* PIO1_4 */
+#define  LPC_IOCONF_FUNC_PIO1_4		0
+#define  LPC_IOCONF_FUNC_PIO1_4_CT32B1_CAP0	1
+
+/* PIO1_5 */
+#define  LPC_IOCONF_FUNC_PIO1_5		0
+#define  LPC_IOCONF_FUNC_CT32B1_CAP1	1
+
+/* PIO1_6 */
+#define  LPC_IOCONF_FUNC_PIO1_6		0
+
+/* PIO1_7 */
+#define  LPC_IOCONF_FUNC_PIO1_7		0
+
+/* PIO1_8 */
+#define  LPC_IOCONF_FUNC_PIO1_8		0
+
+/* PIO1_9 */
+#define  LPC_IOCONF_FUNC_PIO1_9		0
+
+/* PIO1_10 */
+#define  LPC_IOCONF_FUNC_PIO1_10	0
+
+/* PIO1_11 */
+#define  LPC_IOCONF_FUNC_PIO1_11	0
+
+/* PIO1_12 */
+#define  LPC_IOCONF_FUNC_PIO1_12	0
+
+/* PIO1_13 */
+#define  LPC_IOCONF_FUNC_PIO1_13	0
+#define  LPC_IOCONF_FUNC_DTR		1
+#define  LPC_IOCONF_FUNC_CT16B0_MAT0	2
+#define  LPC_IOCONF_FUNC_PIO1_13_TXD		3
+
+/* PIO1_14 */
+#define  LPC_IOCONF_FUNC_PIO1_14	0
+#define  LPC_IOCONF_FUNC_DSR		1
+#define  LPC_IOCONF_FUNC_CT16B0_MAT1	2
+#define  LPC_IOCONF_FUNC_PIO1_13_RXD		3
+
+/* PIO1_15 */
+#define  LPC_IOCONF_FUNC_PIO1_15	0
+#define  LPC_IOCONF_FUNC_DCD		1
+#define  LPC_IOCONF_FUNC_PIO1_15_CT16B0_MAT2	2
+#define  LPC_IOCONF_FUNC_PIO1_15_SCK1	3
+
+/* PIO1_16 */
+#define  LPC_IOCONF_FUNC_PIO1_16	0
+#define  LPC_IOCONF_FUNC_RI		1
+#define  LPC_IOCONF_FUNC_CT16B0_CAP0	2
+
+/* PIO1_17 */
+#define  LPC_IOCONF_FUNC_PIO1_17	0
+#define  LPC_IOCONF_FUNC_CT16B0_CAP1	1
+#define  LPC_IOCONF_FUNC_PIO1_17_RXD		2
+
+/* PIO1_18 */
+#define  LPC_IOCONF_FUNC_PIO1_18	0
+#define  LPC_IOCONF_FUNC_CT16B1_CAP1	1
+#define  LPC_IOCONF_FUNC_PIO1_18_TXD		2
+
+/* PIO1_19 */
+#define  LPC_IOCONF_FUNC_PIO1_19	0
+#define  LPC_IOCONF_FUNC_DTR		1
+#define  LPC_IOCONF_FUNC_SSEL1		2
+
+/* PIO1_20 */
+#define  LPC_IOCONF_FUNC_PIO1_20	0
+#define  LPC_IOCONF_FUNC_DSR		1
+#define  LPC_IOCONF_FUNC_PIO1_20_SCK1		2
+
+/* PIO1_21 */
+#define  LPC_IOCONF_FUNC_PIO1_21	0
+#define  LPC_IOCONF_FUNC_DCD		1
+#define  LPC_IOCONF_FUNC_PIO1_21_MISO1		2
+
+/* PIO1_22 */
+#define  LPC_IOCONF_FUNC_PIO1_22	0
+#define  LPC_IOCONF_FUNC_RI		1
+#define  LPC_IOCONF_FUNC_PIO1_22_MOSI1	2
+
+/* PIO1_23 */
+#define  LPC_IOCONF_FUNC_PIO1_23	0
+#define  LPC_IOCONF_FUNC_PIO1_23_CT16B1_MAT1	1
+#define  LPC_IOCONF_FUNC_SSEL1		2
+
+/* PIO1_24 */
+#define  LPC_IOCONF_FUNC_PIO1_24	0
+#define  LPC_IOCONF_FUNC_PIO1_24_CT32B0_MAT0	1
+
+/* PIO1_25 */
+#define  LPC_IOCONF_FUNC_PIO1_25	0
+#define  LPC_IOCONF_FUNC_PIO1_25_CT32B0_MAT1	1
+
+/* PIO1_26 */
+#define  LPC_IOCONF_FUNC_PIO1_26	0
+#define  LPC_IOCONF_FUNC_PIO1_26_CT32B0_MAT2	1
+#define  LPC_IOCONF_FUNC_PIO1_26_RXD		2
+
+/* PIO1_27 */
+#define  LPC_IOCONF_FUNC_PIO1_27	0
+#define  LPC_IOCONF_FUNC_PIO1_27_CT32B0_MAT3	1
+#define  LPC_IOCONF_FUNC_PIO1_27_TXD		2
+
+/* PIO1_28 */
+#define  LPC_IOCONF_FUNC_PIO1_28	0
+#define  LPC_IOCONF_FUNC_PIO1_28_CT32B0_CAP0	1
+#define  LPC_IOCONF_FUNC_PIO1_28_SCLK		2
+
+/* PIO1_29 */
+#define  LPC_IOCONF_FUNC_PIO1_29		0
+#define  LPC_IOCONF_FUNC_PIO1_29_SCK0		1
+#define  LPC_IOCONF_FUNC_PIO1_29_CT32B0_CAP1	2
+
+/* PIO1_31 */
+#define  LPC_IOCONF_FUNC_PIO1_31	0
+
+#define  LPC_IOCONF_FUNC_MASK		0x7
+
+#define ao_lpc_alternate(func) (((func) << LPC_IOCONF_FUNC) | \
+				(LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) | \
+				(0 << LPC_IOCONF_HYS) |			\
+				(0 << LPC_IOCONF_INV) |			\
+				(0 << LPC_IOCONF_OD) |			\
+				0x80)
+
+#define LPC_IOCONF_MODE			3
+#define  LPC_IOCONF_MODE_INACTIVE		0
+#define  LPC_IOCONF_MODE_PULL_DOWN		1
+#define  LPC_IOCONF_MODE_PULL_UP		2
+#define  LPC_IOCONF_MODE_REPEATER		3
+#define  LPC_IOCONF_MODE_MASK			3
+
+#define LPC_IOCONF_HYS			5
+
+#define LPC_IOCONF_INV			6
+#define LPC_IOCONF_ADMODE		7
+#define LPC_IOCONF_FILTR		8
+#define LPC_IOCONF_OD			10
+
+struct lpc_scb {
+	vuint32_t	sysmemremap;	/* 0x00 */
+	vuint32_t	presetctrl;
+	vuint32_t	syspllctrl;
+	vuint32_t	syspllstat;
+
+	vuint32_t	usbpllctrl;	/* 0x10 */
+	vuint32_t	usbpllstat;
+	uint32_t	r18;
+	uint32_t	r1c;
+
+	vuint32_t	sysoscctrl;	/* 0x20 */
+	vuint32_t	wdtoscctrl;
+	uint32_t	r28;
+	uint32_t	r2c;
+
+	vuint32_t	sysrststat;	/* 0x30 */
+	uint32_t	r34;
+	uint32_t	r38;
+	uint32_t	r3c;
+
+	vuint32_t	syspllclksel;	/* 0x40 */
+	vuint32_t	syspllclkuen;
+	vuint32_t	usbpllclksel;
+	vuint32_t	usbpllclkuen;
+
+	uint32_t	r50[8];
+
+	vuint32_t	mainclksel;	/* 0x70 */
+	vuint32_t	mainclkuen;
+	vuint32_t	sysahbclkdiv;
+	uint32_t	r7c;		
+
+	vuint32_t	sysahbclkctrl;	/* 0x80 */
+	uint32_t	r84[3];
+
+	uint32_t	r90;		/* 0x90 */
+	vuint32_t	ssp0clkdiv;
+	vuint32_t	uartclkdiv;
+	vuint32_t	ssp1clkdiv;
+
+	uint32_t	ra0[8];
+
+	vuint32_t	usbclksel;	/* 0xc0 */
+	vuint32_t	usbclkuen;
+	vuint32_t	usbclkdiv;
+	uint32_t	rcc;
+
+	uint32_t	rd0[4];
+	
+	vuint32_t	clkoutsel;	/* 0xe0 */
+	vuint32_t	clkoutuen;
+	vuint32_t	clkoutdiv;
+	uint32_t	rec;
+	
+	uint32_t	rf0[4];		/* 0xf0 */
+	
+	vuint32_t	pioporcap0;	/* 0x100 */
+	vuint32_t	pioporcap1;
+	uint32_t	r102[2];
+
+	uint32_t	r110[4];	/* 0x110 */
+	uint32_t	r120[4];	/* 0x120 */
+	uint32_t	r130[4];	/* 0x130 */
+	uint32_t	r140[4];	/* 0x140 */
+	
+	vuint32_t	bodctrl;	/* 0x150 */
+	vuint32_t	systckcal;
+	uint32_t	r158[2];
+
+	uint32_t	r160[4];	/* 0x160 */
+
+	vuint32_t	irqlatency;	/* 0x170 */
+	vuint32_t	nmisrc;
+	vuint32_t	pintsel[8];
+
+	vuint32_t	usbclkctrl;	/* 0x198 */
+	vuint32_t	usbclkst;
+
+	uint32_t	r1a0[6*4];	/* 0x1a0 */
+
+	uint32_t	r200;		/* 0x200 */
+	vuint32_t	starterp0;
+	uint32_t	r208[2];
+
+	uint32_t	r210;		/* 0x210 */
+	vuint32_t	starterp1;
+	uint32_t	r218[2];
+
+	uint32_t	r220[4];	/* 0x220 */
+
+	vuint32_t	pdsleepcfg;	/* 0x230 */
+	vuint32_t	pdawakecfg;
+	vuint32_t	pdruncfg;
+	uint32_t	r23c;
+
+	uint32_t	r240[12 * 4];	/* 0x240 */
+
+	uint32_t	r300[15 * 4];	/* 0x300 */
+			     
+	uint32_t	r3f0;		/* 0x3f0 */
+	vuint32_t	device_id;
+};
+
+extern struct lpc_scb lpc_scb;
+
+#define LPC_SCB_SYSMEMREMAP_MAP		0
+# define LPC_SCB_SYSMEMREMAP_MAP_BOOT_LOADER	0
+# define LPC_SCB_SYSMEMREMAP_MAP_RAM		1
+# define LPC_SCB_SYSMEMREMAP_MAP_FLASH		2
+
+#define LPC_SCB_PRESETCTRL_SSP0_RST_N	0
+#define LPC_SCB_PRESETCTRL_I2C_RST_N	1
+#define LPC_SCB_PRESETCTRL_SSP1_RST_N	2
+
+#define LPC_SCB_SYSPLLCTRL_MSEL		0
+#define LPC_SCB_SYSPLLCTRL_PSEL		5
+#define  LPC_SCB_SYSPLLCTRL_PSEL_1		0
+#define  LPC_SCB_SYSPLLCTRL_PSEL_2		1
+#define  LPC_SCB_SYSPLLCTRL_PSEL_4		2
+#define  LPC_SCB_SYSPLLCTRL_PSEL_8		3
+#define  LPC_SCB_SYSPLLCTRL_PSEL_MASK		3
+
+#define LPC_SCB_SYSPLLSTAT_LOCK		0
+
+#define LPC_SCB_USBPLLCTRL_MSEL		0
+#define LPC_SCB_USBPLLCTRL_PSEL		5
+#define  LPC_SCB_USBPLLCTRL_PSEL_1		0
+#define  LPC_SCB_USBPLLCTRL_PSEL_2		1
+#define  LPC_SCB_USBPLLCTRL_PSEL_4		2
+#define  LPC_SCB_USBPLLCTRL_PSEL_8		3
+#define  LPC_SCB_USBPLLCTRL_PSEL_MASK		3
+
+#define LPC_SCB_USBPLLSTAT_LOCK		0
+
+#define LPC_SCB_SYSOSCCTRL_BYPASS	0
+#define LPC_SCB_SYSOSCCTRL_FREQRANGE	1
+#define  LPC_SCB_SYSOSCCTRL_FREQRANGE_1_20	0
+#define  LPC_SCB_SYSOSCCTRL_FREQRANGE_15_25	1
+
+#define LPC_SCB_WDTOSCCTRL_DIVSEL		0
+#define  LPC_SCB_WDTOSCCTRL_DIVSEL_MASK			0x1f
+#define LPC_SCB_WDTOSCCTRL_FREQSEL		5
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_0_6			1
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_1_05		2
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_1_4			3
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_1_75		4
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_2_1			5
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_2_4			6
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_2_7			7
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_3_0			8
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_3_25		9
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_3_5			0x0a
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_3_75		0x0b
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_4_0			0x0c
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_4_2			0x0d
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_4_4			0x0e
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_4_6			0x0f
+#define  LPC_SCB_WDTOSCCTRL_FREQSEL_MASK		0x0f
+
+#define LPC_SCB_SYSRSTSTAT_POR		0
+#define LPC_SCB_SYSRSTSTAT_EXTRST	1
+#define LPC_SCB_SYSRSTSTAT_WDT		2
+#define LPC_SCB_SYSRSTSTAT_BOD		3
+#define LPC_SCB_SYSRSTSTAT_SYSRST	4
+
+#define LPC_SCB_SYSPLLCLKSEL_SEL	0
+#define  LPC_SCB_SYSPLLCLKSEL_SEL_IRC		0
+#define  LPC_SCB_SYSPLLCLKSEL_SEL_SYSOSC	1
+#define  LPC_SCB_SYSPLLCLKSEL_SEL_MASK		3
+
+#define LPC_SCB_SYSPLLCLKUEN_ENA	0
+
+#define LPC_SCB_USBPLLCLKSEL_SEL	0
+#define  LPC_SCB_USBPLLCLKSEL_SEL_IRC		0
+#define  LPC_SCB_USBPLLCLKSEL_SEL_SYSOSC	1
+#define  LPC_SCB_USBPLLCLKSEL_SEL_MASK		3
+
+#define LPC_SCB_USBPLLCLKUEN_ENA	0
+
+#define LPC_SCB_MAINCLKSEL_SEL		0
+#define  LPC_SCB_MAINCLKSEL_SEL_IRC		0
+#define  LPC_SCB_MAINCLKSEL_SEL_PLL_INPUT	1
+#define  LPC_SCB_MAINCLKSEL_SEL_WATCHDOG	2
+#define  LPC_SCB_MAINCLKSEL_SEL_PLL_OUTPUT	3
+#define  LPC_SCB_MAINCLKSEL_SEL_MASK		3
+
+#define LPC_SCB_MAINCLKUEN_ENA		0
+
+#define LPC_SCB_SYSAHBCLKDIV_DIV	0
+
+#define LPC_SCB_SYSAHBCLKCTRL_SYS	0
+#define LPC_SCB_SYSAHBCLKCTRL_ROM	1
+#define LPC_SCB_SYSAHBCLKCTRL_RAM0	2
+#define LPC_SCB_SYSAHBCLKCTRL_FLASHREG	3
+#define LPC_SCB_SYSAHBCLKCTRL_FLASHARRAY	4
+#define LPC_SCB_SYSAHBCLKCTRL_I2C	5
+#define LPC_SCB_SYSAHBCLKCTRL_GPIO	6
+#define LPC_SCB_SYSAHBCLKCTRL_CT16B0	7
+#define LPC_SCB_SYSAHBCLKCTRL_CT16B1	8
+#define LPC_SCB_SYSAHBCLKCTRL_CT32B0	9
+#define LPC_SCB_SYSAHBCLKCTRL_CT32B1	10
+#define LPC_SCB_SYSAHBCLKCTRL_SSP0	11
+#define LPC_SCB_SYSAHBCLKCTRL_USART	12
+#define LPC_SCB_SYSAHBCLKCTRL_ADC	13
+#define LPC_SCB_SYSAHBCLKCTRL_USB	14
+#define LPC_SCB_SYSAHBCLKCTRL_WWDT	15
+#define LPC_SCB_SYSAHBCLKCTRL_IOCON	16
+#define LPC_SCB_SYSAHBCLKCTRL_SSP1	18
+#define LPC_SCB_SYSAHBCLKCTRL_PINT	19
+#define LPC_SCB_SYSAHBCLKCTRL_GROUP0INT	23
+#define LPC_SCB_SYSAHBCLKCTRL_GROUP1INT	24
+#define LPC_SCB_SYSAHBCLKCTRL_RAM1	26
+#define LPC_SCB_SYSAHBCLKCTRL_USBRAM	27
+
+#define LPC_SCB_SSP0CLKDIV_
+#define LPC_SCB_UARTCLKDIV_
+#define LPC_SCB_SSP1CLKDIV_
+
+#define LPC_SCB_USBCLKSEL_SEL		0
+#define LPC_SCB_USBCLKSEL_SEL_USB_PLL		0
+#define LPC_SCB_USBCLKSEL_SEL_MAIN_CLOCK	1
+
+#define LPC_SCB_USBCLKUEN_ENA		0
+#define LPC_SCB_USBCLKDIV_DIV		0
+
+#define LPC_SCB_CLKOUTSEL_SEL		0
+#define  LPC_SCB_CLKOUTSEL_SEL_IRC		0
+#define  LPC_SCB_CLKOUTSEL_SEL_SYSOSC		1
+#define  LPC_SCB_CLKOUTSEL_SEL_LF		2
+#define  LPC_SCB_CLKOUTSEL_SEL_MAIN_CLOCK	3
+
+#define LPC_SCB_CLKOUTUEN_ENA		0
+
+#define LPC_SCB_BOD_BODRSTLEV		0
+# define LPC_SCB_BOD_BODRSTLEV_1_46		0
+# define LPC_SCB_BOD_BODRSTLEV_2_06		1
+# define LPC_SCB_BOD_BODRSTLEV_2_35		2
+# define LPC_SCB_BOD_BODRSTLEV_2_63		3
+#define LPC_SCB_BOD_BODINTVAL		2
+# define LPC_SCB_BOD_BODINTVAL_RESERVED		0
+# define LPC_SCB_BOD_BODINTVAL_2_22		1
+# define LPC_SCB_BOD_BODINTVAL_2_52	       	2
+# define LPC_SCB_BOD_BODINTVAL_2_80		3
+#define LPC_SCB_BOD_BODRSTENA		4
+
+#define LPC_SCB_PDRUNCFG_IRCOUT_PD	0
+#define LPC_SCB_PDRUNCFG_IRC_PD		1
+#define LPC_SCB_PDRUNCFG_FLASH_PD	2
+#define LPC_SCB_PDRUNCFG_BOD_PD		3
+#define LPC_SCB_PDRUNCFG_ADC_PD		4
+#define LPC_SCB_PDRUNCFG_SYSOSC_PD	5
+#define LPC_SCB_PDRUNCFG_WDTOSC_PD	6
+#define LPC_SCB_PDRUNCFG_SYSPLL_PD	7
+#define LPC_SCB_PDRUNCFG_USBPLL_PD	8
+#define LPC_SCB_PDRUNCFG_USBPAD_PD	10
+
+struct lpc_flash {
+	uint32_t	r0[4];		/* 0x0 */
+
+	vuint32_t	flashcfg;	/* 0x10 */
+};
+
+extern struct lpc_flash lpc_flash;
+
+struct lpc_gpio_pin {
+	vuint32_t	isel;		/* 0x00 */
+	vuint32_t	ienr;
+	vuint32_t	sienr;
+	vuint32_t	cienr;
+
+	vuint32_t	ienf;		/* 0x10 */
+	vuint32_t	sienf;
+	vuint32_t	cienf;
+	vuint32_t	rise;
+
+	vuint32_t	fall;		/* 0x20 */
+	vuint32_t	ist;
+};
+
+extern struct lpc_gpio_pin lpc_gpio_pin;
+
+struct lpc_gpio_group0 {
+};
+
+extern struct lpc_gpio_group0 lpc_gpio_group0;
+
+struct lpc_gpio_group1 {
+};
+
+extern struct lpc_gpio_group1 lpc_gpio_group1;
+
+struct lpc_gpio {
+	vuint8_t	byte[0x40];	/* 0x0000 */
+
+	uint8_t		r0030[0x1000 - 0x40];
+
+	vuint32_t	word[0x40];	/* 0x1000 */
+
+	uint8_t		r1100[0x2000 - 0x1100];
+	
+	vuint32_t	dir[2];		/* 0x2000 */
+
+	uint8_t		r2008[0x2080 - 0x2008];
+
+	vuint32_t	mask[2];	/* 0x2080 */
+
+	uint8_t		r2088[0x2100 - 0x2088];
+
+	vuint32_t	pin[2];		/* 0x2100 */
+
+	uint8_t		r2108[0x2200 - 0x2108];
+
+	vuint32_t	set[2];		/* 0x2200 */
+
+	uint8_t		r2208[0x2280 - 0x2208];
+
+	vuint32_t	clr[2];		/* 0x2280 */
+
+	uint8_t		r2288[0x2300 - 0x2288];
+
+	vuint32_t	not[2];		/* 0x2300 */
+};
+
+extern struct lpc_gpio lpc_gpio;
+
+struct lpc_systick {
+	uint8_t		r0000[0x10];	/* 0x0000 */
+
+	vuint32_t	csr;		/* 0x0010 */
+	vuint32_t	rvr;
+	vuint32_t	cvr;
+	vuint32_t	calib;
+};
+
+extern struct lpc_systick lpc_systick;
+
+#define LPC_SYSTICK_CSR_ENABLE		0
+#define LPC_SYSTICK_CSR_TICKINT		1
+#define LPC_SYSTICK_CSR_CLKSOURCE	2
+#define  LPC_SYSTICK_CSR_CLKSOURCE_CPU_OVER_2		0
+#define  LPC_SYSTICK_CSR_CLKSOURCE_CPU			1
+#define LPC_SYSTICK_CSR_COUNTFLAG	16
+
+struct lpc_usart {
+	vuint32_t	rbr_thr;	/* 0x0000 */
+	vuint32_t	ier;
+	vuint32_t	iir_fcr;
+	vuint32_t	lcr;
+
+	vuint32_t	mcr;		/* 0x0010 */
+	vuint32_t	lsr;
+	vuint32_t	msr;
+	vuint32_t	scr;
+
+	vuint32_t	acr;		/* 0x0020 */
+	vuint32_t	icr;
+	vuint32_t	fdr;
+	vuint32_t	osr;
+
+	vuint32_t	ter;		/* 0x0030 */
+	uint32_t	r34[3];
+
+	vuint32_t	hden;		/* 0x0040 */
+	uint32_t	r44;
+	vuint32_t	scictrl;
+	vuint32_t	rs485ctrl;
+
+	vuint32_t	rs485addrmatch;	/* 0x0050 */
+	vuint32_t	rs485dly;
+	vuint32_t	syncctrl;
+};
+
+extern struct lpc_usart lpc_usart;
+
+#define LPC_USART_IER_RBRINTEN	0
+#define LPC_USART_IER_THREINTEN	1
+#define LPC_USART_IER_RSLINTEN	2
+#define LPC_USART_IER_MSINTEN	3
+#define LPC_USART_IER_ABEOINTEN	8
+#define LPC_USART_IER_ABTOINTEN	9
+
+#define LPC_USART_IIR_INTSTATUS		0
+#define LPC_USART_IIR_INTID		1
+#define LPC_USART_IIR_INTID_RLS			3
+#define LPC_USART_IIR_INTID_RDA			2
+#define LPC_USART_IIR_INTID_CTI			6
+#define LPC_USART_IIR_INTID_THRE		1
+#define LPC_USART_IIR_INTID_MS			0
+#define LPC_USART_IIR_INTID_MASK		7
+#define LPC_USART_IIR_FIFOEN		6
+#define LPC_USART_IIR_ABEOINT		8
+#define LPC_USART_IIR_ABTOINT		9
+
+#define LPC_USART_FCR_FIFOEN		0
+#define LPC_USART_FCR_RXFIFORES		1
+#define LPC_USART_FCR_TXFIFORES		2
+#define LPC_USART_FCR_RXTL		6
+#define LPC_USART_FCR_RXTL_1			0
+#define LPC_USART_FCR_RXTL_4			1
+#define LPC_USART_FCR_RXTL_8			2
+#define LPC_USART_FCR_RXTL_14			3
+
+#define LPC_USART_LCR_WLS	0
+#define LPC_USART_LCR_WLS_5		0
+#define LPC_USART_LCR_WLS_6		1
+#define LPC_USART_LCR_WLS_7		2
+#define LPC_USART_LCR_WLS_8		3
+#define LPC_USART_LCR_WLS_MASK		3
+#define LPC_USART_LCR_SBS	2
+#define LPC_USART_LCR_SBS_1		0
+#define LPC_USART_LCR_SBS_2		1
+#define LPC_USART_LCR_SBS_MASK		1
+#define LPC_USART_LCR_PE	3
+#define LPC_USART_LCR_PS	4
+#define LPC_USART_LCR_PS_ODD		0
+#define LPC_USART_LCR_PS_EVEN		1
+#define LPC_USART_LCR_PS_ONE		2
+#define LPC_USART_LCR_PS_ZERO		3
+#define LPC_USART_LCR_PS_MASK		3
+#define LPC_USART_LCR_BC	6
+#define LPC_USART_LCR_DLAB	7
+
+#define LPC_USART_MCR_DTRCTRL	0
+#define LPC_USART_MCR_RTSCTRL	1
+#define LPC_USART_MCR_LMS	4
+#define LPC_USART_MCR_RTSEN	6
+#define LPC_USART_MCR_CTSEN	7
+
+#define LPC_USART_LSR_RDR	0
+#define LPC_USART_LSR_OE	1
+#define LPC_USART_LSR_PE	2
+#define LPC_USART_LSR_FE	3
+#define LPC_USART_LSR_BI	4
+#define LPC_USART_LSR_THRE	5
+#define LPC_USART_LSR_TEMT	6
+#define LPC_USART_LSR_RXFE	7
+#define LPC_USART_LSR_TXERR	8
+
+#define LPC_USART_MSR_DCTS	0
+#define LPC_USART_MSR_DDSR	1
+#define LPC_USART_MSR_TERI	2
+#define LPC_USART_MSR_DDCD	3
+#define LPC_USART_MSR_CTS	4
+#define LPC_USART_MSR_DSR	5
+#define LPC_USART_MSR_RI	6
+#define LPC_USART_MSR_DCD	7
+
+#define LPC_USART_ACR_START	0
+#define LPC_USART_ACR_MODE	1
+#define LPC_USART_ACR_AUTORESTART	2
+#define LPC_USART_ACR_ABEOINTCLR	8
+#define LPC_USART_ACR_ABTOINTCLR	9
+
+#define LPC_USART_FDR_DIVADDVAL	0
+#define LPC_USART_FDR_MULVAL	4
+
+#define LPC_USART_OSR_OSFRAC	1
+#define LPC_USART_OSR_OSINT	4
+#define LPC_USART_OSR_FDINT	8
+
+#define LPC_USART_TER_TXEN	7
+
+#define LPC_USART_HDEN_HDEN	0
+
+struct lpc_usb {
+	vuint32_t	devcmdstat;
+	vuint32_t	info;
+	vuint32_t	epliststart;
+	vuint32_t	databufstart;
+	vuint32_t	lpm;
+	vuint32_t	epskip;
+	vuint32_t	epinuse;
+	vuint32_t	epbufcfg;
+	vuint32_t	intstat;
+	vuint32_t	inten;
+	vuint32_t	intsetstat;
+	vuint32_t	introuting;
+	uint32_t	r30;
+	vuint32_t	eptoggle;
+} lpc_usb;
+
+extern struct lpc_usb lpc_usb;
+
+#define LPC_USB_DEVCMDSTAT_DEV_ADDR	0
+#define LPC_USB_DEVCMDSTAT_DEV_ADDR_MASK	0x7f
+#define LPC_USB_DEVCMDSTAT_DEV_EN	7
+#define LPC_USB_DEVCMDSTAT_SETUP	8
+#define LPC_USB_DEVCMDSTAT_PLL_ON	9
+#define LPC_USB_DEVCMDSTAT_LPM_SUP	11
+#define LPC_USB_DEVCMDSTAT_INTONNAK_AO	12
+#define LPC_USB_DEVCMDSTAT_INTONNAK_AI	13
+#define LPC_USB_DEVCMDSTAT_INTONNAK_CO	14
+#define LPC_USB_DEVCMDSTAT_INTONNAK_CI	15
+#define LPC_USB_DEVCMDSTAT_DCON		16
+#define LPC_USB_DEVCMDSTAT_DSUS		17
+#define LPC_USB_DEVCMDSTAT_LPM_SUS	19
+#define LPC_USB_DEVCMDSTAT_LPM_REWP	20
+#define LPC_USB_DEVCMDSTAT_DCON_C	24
+#define LPC_USB_DEVCMDSTAT_DSUS_C	25
+#define LPC_USB_DEVCMDSTAT_DRES_C	26
+#define LPC_USB_DEVCMDSTAT_VBUSDEBOUNCED	28
+
+#define LPC_USB_INFO_FRAME_NR		0
+#define LPC_USB_INFO_FRAME_NR_MASK	0x3ff
+#define LPC_USB_INFO_ERR_CODE		11
+#define LPC_USB_INFO_ERR_CODE_NO_ERROR			0
+#define LPC_USB_INFO_ERR_CODE_PID_ENCODING_ERROR	1
+#define LPC_USB_INFO_ERR_CODE_PID_UNKNOWN		2
+#define LPC_USB_INFO_ERR_CODE_PACKET_UNEXPECTED		3
+#define LPC_USB_INFO_ERR_CODE_TOKEN_CRC_ERROR		4
+#define LPC_USB_INFO_ERR_CODE_DATA_CRC_ERROR		5
+#define LPC_USB_INFO_ERR_CODE_TIME_OUT			6
+#define LPC_USB_INFO_ERR_CODE_BABBLE			7
+#define LPC_USB_INFO_ERR_CODE_TRUNCATED_EOP		8
+#define LPC_USB_INFO_ERR_CODE_SENT_RECEIVED_NAK		9
+#define LPC_USB_INFO_ERR_CODE_SENT_STALL		0xa
+#define LPC_USB_INFO_ERR_CODE_OVERRUN			0xb
+#define LPC_USB_INFO_ERR_CODE_SENT_EMPTY_PACKET		0xc
+#define LPC_USB_INFO_ERR_CODE_BITSTUFF_ERROR		0xd
+#define LPC_USB_INFO_ERR_CODE_SYNC_ERROR		0xe
+#define LPC_USB_INFO_ERR_CODE_WRONG_DATA_TOGGLE		0xf
+#define LPC_USB_INFO_ERR_CODE_MASK			0xf
+
+#define LPC_USB_EPLISTSTART_EP_LIST			0
+
+#define LPC_USB_DATABUFSTART_DA_BUF			0
+
+#define LPC_USB_LPM_HIRD_HW		0
+#define LPC_USB_LPM_HIRD_HW_MASK		0xf
+#define LPC_USB_LPM_HIRD_SW		4
+#define LPC_USB_LPM_HIRD_SW_MASK		0xf
+#define LPC_USB_LPM_DATA_PENDING	8
+
+#define LPC_USB_EPSKIP_SKIP		0
+
+#define LPC_USB_EPINUSE_BUF(ep)		(ep)
+
+#define LPC_USB_EPBUFCFG_BUF_SB(ep)	(ep)
+
+#define LPC_USB_INT_EPOUT(ep)		((ep) << 1)
+#define LPC_USB_INT_EPIN(ep)		(((ep) << 1) + 1)
+
+#define LPC_USB_INT_FRAME		30
+#define LPC_USB_INT_DEV			31
+
+#define LPC_USB_INTIN_EP_INT_EN(ep)	(ep)
+#define LPC_USB_INTIN_FRAME_INT_EN	30
+#define LPC_USB_INTIN_DEV_INT_EN	31
+
+#define LPC_USB_INTSETSTAT_EP_SET_INT(ep)	(ep)
+#define LPC_USB_INTSETSTAT_FRAME_SET_INT	30
+#define LPC_USB_INTSETSTAT_DEV_SET_INT		31
+
+#define LPC_USB_INTROUTING_ROUTE_INT(ep)	(ep)
+#define LPC_USB_INTROUTING_INT30		30
+#define LPC_USB_INTROUTING_INT31		31
+
+#define LPC_USB_EPTOGGLE_TOGGLE(ep)		(ep)
+
+struct lpc_usb_epn {
+	vuint32_t		out[2];
+	vuint32_t		in[2];
+};
+
+struct lpc_usb_endpoint {
+	vuint32_t		ep0_out;
+	vuint32_t		setup;
+	vuint32_t		ep0_in;
+	vuint32_t		reserved_0c;
+	struct lpc_usb_epn	epn[4];
+};
+
+/* Assigned in registers.ld to point at the base
+ * of USB ram
+ */
+
+extern uint8_t lpc_usb_sram[];
+
+#define LPC_USB_EP_ACTIVE		31
+#define LPC_USB_EP_DISABLED		30
+#define LPC_USB_EP_STALL		29
+#define LPC_USB_EP_TOGGLE_RESET		28
+#define LPC_USB_EP_RATE_FEEDBACK	27
+#define LPC_USB_EP_ENDPOINT_ISO		26
+#define LPC_USB_EP_NBYTES		16
+#define  LPC_USB_EP_NBYTES_MASK			0x3ff
+#define LPC_USB_EP_OFFSET		0
+
+#define LPC_ISR_PIN_INT0_POS	0
+#define LPC_ISR_PIN_INT1_POS	1
+#define LPC_ISR_PIN_INT2_POS	2
+#define LPC_ISR_PIN_INT3_POS	3
+#define LPC_ISR_PIN_INT4_POS	4
+#define LPC_ISR_PIN_INT5_POS	5
+#define LPC_ISR_PIN_INT6_POS	6
+#define LPC_ISR_PIN_INT7_POS	7
+#define LPC_ISR_GINT0_POS	8
+#define LPC_ISR_GINT1_POS	9
+#define LPC_ISR_SSP1_POS	14
+#define LPC_ISR_I2C_POS		15
+#define LPC_ISR_CT16B0_POS	16
+#define LPC_ISR_CT16B1_POS	17
+#define LPC_ISR_CT32B0_POS	18
+#define LPC_ISR_CT32B1_POS	19
+#define LPC_ISR_SSP0_POS	20
+#define LPC_ISR_USART_POS	21
+#define LPC_ISR_USB_IRQ_POS	22
+#define LPC_ISR_USB_FIQ_POS	23
+#define LPC_ISR_ADC_POS		24
+#define LPC_ISR_WWDT_POS	25
+#define LPC_ISR_BOD_POS		26
+#define LPC_ISR_FLASH_POS	27
+#define LPC_ISR_USB_WAKEUP_POS	30
+
+struct lpc_nvic {
+	vuint32_t	iser;		/* 0x000 0xe000e100 Set Enable Register */
+
+	uint8_t		_unused020[0x080 - 0x004];
+
+	vuint32_t	icer;		/* 0x080 0xe000e180 Clear Enable Register */
+
+	uint8_t		_unused0a0[0x100 - 0x084];
+
+	vuint32_t	ispr;		/* 0x100 0xe000e200 Set Pending Register */
+
+	uint8_t		_unused120[0x180 - 0x104];
+
+	vuint32_t	icpr;		/* 0x180 0xe000e280 Clear Pending Register */
+
+	uint8_t		_unused1a0[0x300 - 0x184];
+
+	vuint32_t	ipr[8];		/* 0x300 0xe000e400 Priority Register */
+};
+
+extern struct lpc_nvic lpc_nvic;
+
+static inline void
+lpc_nvic_set_enable(int irq) {
+	lpc_nvic.iser = (1 << irq);
+}
+
+static inline void
+lpc_nvic_clear_enable(int irq) {
+	lpc_nvic.icer = (1 << irq);
+}
+
+static inline int
+lpc_nvic_enabled(int irq) {
+	return (lpc_nvic.iser >> irq) & 1;
+}
+
+	
+static inline void
+lpc_nvic_set_pending(int irq) {
+	lpc_nvic.ispr = (1 << irq);
+}
+
+static inline void
+lpc_nvic_clear_pending(int irq) {
+	lpc_nvic.icpr = (1 << irq);
+}
+
+static inline int
+lpc_nvic_pending(int irq) {
+	return (lpc_nvic.ispr >> irq) & 1;
+}
+
+#define IRQ_PRIO_REG(irq)	((irq) >> 2)
+#define IRQ_PRIO_BIT(irq)	(((irq) & 3) << 3)
+#define IRQ_PRIO_MASK(irq)	(0xff << IRQ_PRIO_BIT(irq))
+
+static inline void
+lpc_nvic_set_priority(int irq, uint8_t prio) {
+	int		n = IRQ_PRIO_REG(irq);
+	uint32_t	v;
+
+	v = lpc_nvic.ipr[n];
+	v &= ~IRQ_PRIO_MASK(irq);
+	v |= (prio) << IRQ_PRIO_BIT(irq);
+	lpc_nvic.ipr[n] = v;
+}
+
+static inline uint8_t
+lpc_nvic_get_priority(int irq) {
+	return (lpc_nvic.ipr[IRQ_PRIO_REG(irq)] >> IRQ_PRIO_BIT(irq)) & IRQ_PRIO_MASK(0);
+}
+
+struct arm_scb {
+	vuint32_t	cpuid;
+	vuint32_t	icsr;
+	uint32_t	reserved08;
+	vuint32_t	aircr;
+
+	vuint32_t	scr;
+	vuint32_t	ccr;
+	uint32_t	reserved18;
+	vuint32_t	shpr2;
+
+	vuint32_t	shpr3;
+};
+
+extern struct arm_scb arm_scb;
+
+struct lpc_ssp {
+	vuint32_t	cr0;	/* 0x00 */
+	vuint32_t	cr1;
+	vuint32_t	dr;
+	vuint32_t	sr;
+
+	vuint32_t	cpsr;	/* 0x10 */
+	vuint32_t	imsc;
+	vuint32_t	ris;
+	vuint32_t	mis;
+
+	vuint32_t	icr;	/* 0x20 */
+};
+
+extern struct lpc_ssp lpc_ssp0, lpc_ssp1;
+
+#define LPC_NUM_SPI		2
+
+#define LPC_SSP_FIFOSIZE	8
+
+#define LPC_SSP_CR0_DSS		0
+#define  LPC_SSP_CR0_DSS_4		0x3
+#define  LPC_SSP_CR0_DSS_5		0x4
+#define  LPC_SSP_CR0_DSS_6		0x5
+#define  LPC_SSP_CR0_DSS_7		0x6
+#define  LPC_SSP_CR0_DSS_8		0x7
+#define  LPC_SSP_CR0_DSS_9		0x8
+#define  LPC_SSP_CR0_DSS_10		0x9
+#define  LPC_SSP_CR0_DSS_11		0xa
+#define  LPC_SSP_CR0_DSS_12		0xb
+#define  LPC_SSP_CR0_DSS_13		0xc
+#define  LPC_SSP_CR0_DSS_14		0xd
+#define  LPC_SSP_CR0_DSS_15		0xe
+#define  LPC_SSP_CR0_DSS_16		0xf
+#define LPC_SSP_CR0_FRF		4
+#define  LPC_SSP_CR0_FRF_SPI		0
+#define  LPC_SSP_CR0_FRF_TI		1
+#define  LPC_SSP_CR0_FRF_MICROWIRE	2
+#define LPC_SSP_CR0_CPOL	6
+#define  LPC_SSP_CR0_CPOL_LOW		0
+#define  LPC_SSP_CR0_CPOL_HIGH		1
+#define LPC_SSP_CR0_CPHA	7
+#define  LPC_SSP_CR0_CPHA_FIRST		0
+#define  LPC_SSP_CR0_CPHA_SECOND	1
+#define LPC_SSP_CR0_SCR		8
+
+#define LPC_SSP_CR1_LBM		0
+#define LPC_SSP_CR1_SSE		1
+#define LPC_SSP_CR1_MS		2
+#define  LPC_SSP_CR1_MS_MASTER		0
+#define  LPC_SSP_CR1_MS_SLAVE		1
+#define LPC_SSP_CR1_SOD		3
+
+#define LPC_SSP_SR_TFE		0
+#define LPC_SSP_SR_TNF		1
+#define LPC_SSP_SR_RNE		2
+#define LPC_SSP_SR_RFF		3
+#define LPC_SSP_SR_BSY		4
+
+#define LPC_SSP_IMSC_RORIM	0
+#define LPC_SSP_IMSC_RTIM	1
+#define LPC_SSP_IMSC_RXIM	2
+#define LPC_SSP_IMSC_TXIM	3
+
+#define LPC_SSP_RIS_RORRIS	0
+#define LPC_SSP_RIS_RTRIS	1
+#define LPC_SSP_RIS_RXRIS	2
+#define LPC_SSP_RIS_TXRIS	3
+
+#define LPC_SSP_MIS_RORMIS	0
+#define LPC_SSP_MIS_RTMIS	1
+#define LPC_SSP_MIS_RXMIS	2
+#define LPC_SSP_MIS_TXMIS	3
+
+#define LPC_SSP_ICR_RORIC	0
+#define LPC_SSP_ICR_RTIC	1
+
+struct lpc_adc {
+	vuint32_t	cr;	/* 0x00 */
+	vuint32_t	gdr;
+	uint32_t	r08;
+	vuint32_t	inten;
+
+	vuint32_t	dr[8];	/* 0x10 */
+
+	vuint32_t	stat;	/* 0x30 */
+};
+
+extern struct lpc_adc lpc_adc;
+
+#define LPC_ADC_CR_SEL		0
+#define LPC_ADC_CR_CLKDIV	8
+#define LPC_ADC_CR_BURST	16
+#define LPC_ADC_CR_CLKS		17
+#define  LPC_ADC_CR_CLKS_11		0
+#define  LPC_ADC_CR_CLKS_10		1
+#define  LPC_ADC_CR_CLKS_9		2
+#define  LPC_ADC_CR_CLKS_8		3
+#define  LPC_ADC_CR_CLKS_7		4
+#define  LPC_ADC_CR_CLKS_6		5
+#define  LPC_ADC_CR_CLKS_5		6
+#define  LPC_ADC_CR_CLKS_4		7
+#define LPC_ADC_CR_START	24
+#define  LPC_ADC_CR_START_NONE		0
+#define  LPC_ADC_CR_START_NOW		1
+
+#define LPC_ADC_GDR_CHN			24
+#define LPC_ADC_GDR_OVERRUN		30
+#define LPC_ADC_GDR_DONE		31
+
+#define LPC_ADC_INTEN_ADINTEN	0
+#define LPC_ADC_INTEN_ADGINTEN	8
+
+#define LPC_ADC_STAT_DONE	0
+#define LPC_ADC_STAT_OVERRUN	8
+#define LPC_ADC_STAT_ADINT	16
+
+struct lpc_ct32b {
+	vuint32_t	ir;	/* 0x00 */
+	vuint32_t	tcr;
+	vuint32_t	tc;
+	vuint32_t	pr;
+	
+	vuint32_t	pc;	/* 0x10 */
+	vuint32_t	mcr;
+	vuint32_t	mr[4];	/* 0x18 */
+	vuint32_t	ccr;	/* 0x28 */
+	vuint32_t	cr0;
+	
+	vuint32_t	cr1_0;	/* 0x30 (only for ct32b0 */
+	vuint32_t	cr1_1;	/* 0x34 (only for ct32b1 */
+	uint32_t	r38;
+	vuint32_t	emr;
+
+	uint32_t	r40[12];
+
+	vuint32_t	ctcr;	/* 0x70 */
+	vuint32_t	pwmc;
+};
+
+extern struct lpc_ct32b lpc_ct32b0, lpc_ct32b1;
+
+#define LPC_CT32B_TCR_CEN	0
+#define LPC_CT32B_TCR_CRST	1
+
+#define LPC_CT32B_MCR_MR0R	1
+
+#define LPC_CT32B_PWMC_PWMEN0	0
+#define LPC_CT32B_PWMC_PWMEN1	1
+#define LPC_CT32B_PWMC_PWMEN2	2
+#define LPC_CT32B_PWMC_PWMEN3	3
+
+#define LPC_CT32B_EMR_EMC0	4
+#define LPC_CT32B_EMR_EMC1	6
+#define LPC_CT32B_EMR_EMC2	8
+#define LPC_CT32B_EMR_EMC3	10
+
+#define LPC_CT32B_EMR_EMC_NOTHING	0
+#define LPC_CT32B_EMR_EMC_CLEAR		1
+#define LPC_CT32B_EMR_EMC_SET		2
+#define LPC_CT32B_EMR_EMC_TOGGLE	3
+
+#endif /* _LPC_H_ */
diff --git a/src/lpc/registers.ld b/src/lpc/registers.ld
new file mode 100644
index 00000000..a523c39c
--- /dev/null
+++ b/src/lpc/registers.ld
@@ -0,0 +1,19 @@
+lpc_usb_sram	= 0x20004000;
+lpc_usb_endpoint = 0x20004700;
+lpc_usart	= 0x40008000;
+lpc_ct32b0	= 0x40014000;
+lpc_ct32b1	= 0x40018000;
+lpc_adc		= 0x4001c000;
+lpc_flash	= 0x4003c000;
+lpc_ssp0	= 0x40040000;
+lpc_ioconf	= 0x40044000;
+lpc_scb		= 0x40048000;
+lpc_gpio_pin	= 0x4004c000;
+lpc_ssp1	= 0x40058000;
+lpc_gpio_group0 = 0x4005c000;
+lpc_gpio_group1 = 0x40060000;
+lpc_usb		= 0x40080000;
+lpc_gpio	= 0x50000000;
+lpc_systick	= 0xe000e000;
+lpc_nvic	= 0xe000e100;
+arm_scb		= 0xe000ed00;
diff --git a/src/lpcxpresso/.gitignore b/src/lpcxpresso/.gitignore
new file mode 100644
index 00000000..892c3acc
--- /dev/null
+++ b/src/lpcxpresso/.gitignore
@@ -0,0 +1,3 @@
+ao_product.h
+ao_serial_lpc.h
+*.elf
diff --git a/src/lpcxpresso/Makefile b/src/lpcxpresso/Makefile
new file mode 100644
index 00000000..374c052f
--- /dev/null
+++ b/src/lpcxpresso/Makefile
@@ -0,0 +1,66 @@
+#
+# AltOS build
+#
+#
+
+include ../lpc/Makefile.defs
+
+INC = \
+	ao.h \
+	ao_arch.h \
+	ao_arch_funcs.h \
+	ao_pins.h \
+	ao_product.h \
+	lpc.h
+
+#
+# Common AltOS sources
+#
+ALTOS_SRC = \
+	ao_interrupt.c \
+	ao_romconfig.c \
+	ao_product.c \
+	ao_panic.c \
+	ao_led_lpc.c \
+	ao_task.c \
+	ao_cmd.c \
+	ao_timer_lpc.c \
+	ao_serial_lpc.c \
+	ao_usb_lpc.c \
+	ao_stdio.c
+
+PRODUCT=LpcDemo-v0.0
+PRODUCT_DEF=-DLPC_DEMO
+IDPRODUCT=0x000a
+
+CFLAGS = $(PRODUCT_DEF) $(LPC_CFLAGS) -g -Os
+
+PROG=lpc-demo.elf
+
+SRC=$(ALTOS_SRC) ao_demo.c
+OBJ=$(SRC:.c=.o)
+
+all: $(PROG)
+
+LDFLAGS=-L../lpc -Wl,-Taltos.ld
+
+$(PROG): Makefile $(OBJ)
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS)
+
+ao_product.h: ao-make-product.5c ../Version
+	$(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@
+
+$(OBJ): $(INC)
+
+load: $(PROG)
+	lpc-load $(PROG)
+
+distclean:	clean
+
+clean:
+	rm -f *.o $(PROG)
+	rm -f ao_product.h
+
+install:
+
+uninstall:
diff --git a/src/lpcxpresso/ao_demo.c b/src/lpcxpresso/ao_demo.c
new file mode 100644
index 00000000..0c931611
--- /dev/null
+++ b/src/lpcxpresso/ao_demo.c
@@ -0,0 +1,47 @@
+/*
+ * Copyright © 2011 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+#include <ao_usb.h>
+
+int
+main(void)
+{
+	int	i;
+	ao_led_init(LEDS_AVAILABLE);
+	ao_led_on(AO_LED_RED);
+	ao_clock_init();
+	ao_timer_init();
+	
+	ao_serial_init();
+	ao_usb_init();
+	ao_cmd_init();
+	ao_task_init();
+
+	ao_start_scheduler();
+
+	for (;;) {
+		ao_led_off(AO_LED_RED);
+		for (;;)
+			if (ao_tick_count & 1)
+				break;
+		ao_led_on(AO_LED_RED);
+		for (;;)
+			if (!(ao_tick_count & 1))
+				break;
+	}
+}
diff --git a/src/lpcxpresso/ao_pins.h b/src/lpcxpresso/ao_pins.h
new file mode 100644
index 00000000..c0074ce2
--- /dev/null
+++ b/src/lpcxpresso/ao_pins.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#define HAS_BEEP	0
+#define	HAS_LED		1
+
+/* Crystal on the board */
+#define AO_LPC_CLKIN	12000000
+
+/* Main clock frequency. 48MHz for USB so we don't use the USB PLL */
+#define AO_LPC_CLKOUT	48000000
+
+/* System clock frequency */
+#define AO_LPC_SYSCLK	24000000
+
+#define LED_PORT	0
+#define LED_PIN_RED	7
+
+#define AO_LED_RED	(1 << LED_PIN_RED)
+
+#define LEDS_AVAILABLE	AO_LED_RED
+
+#define HAS_USB		1
+
+#define HAS_USB_CONNECT	1
+#define HAS_USB_VBUS	1
+
+#define PACKET_HAS_SLAVE	0
+
+/* USART */
+
+#define HAS_SERIAL		1
+#define USE_SERIAL_0_STDIN	1
+#define SERIAL_0_18_19		1
+#define SERIAL_0_14_15		0
+#define SERIAL_0_17_18		0
+#define SERIAL_0_26_27		0
diff --git a/src/math/ef_acos.c b/src/math/ef_acos.c
new file mode 100644
index 00000000..f73f97de
--- /dev/null
+++ b/src/math/ef_acos.c
@@ -0,0 +1,84 @@
+/* ef_acos.c -- float version of e_acos.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice 
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+static const float 
+#else
+static float 
+#endif
+one =  1.0000000000e+00, /* 0x3F800000 */
+pi =  3.1415925026e+00, /* 0x40490fda */
+pio2_hi =  1.5707962513e+00, /* 0x3fc90fda */
+pio2_lo =  7.5497894159e-08, /* 0x33a22168 */
+pS0 =  1.6666667163e-01, /* 0x3e2aaaab */
+pS1 = -3.2556581497e-01, /* 0xbea6b090 */
+pS2 =  2.0121252537e-01, /* 0x3e4e0aa8 */
+pS3 = -4.0055535734e-02, /* 0xbd241146 */
+pS4 =  7.9153501429e-04, /* 0x3a4f7f04 */
+pS5 =  3.4793309169e-05, /* 0x3811ef08 */
+qS1 = -2.4033949375e+00, /* 0xc019d139 */
+qS2 =  2.0209457874e+00, /* 0x4001572d */
+qS3 = -6.8828397989e-01, /* 0xbf303361 */
+qS4 =  7.7038154006e-02; /* 0x3d9dc62e */
+
+#ifdef __STDC__
+	float __ieee754_acosf(float x)
+#else
+	float __ieee754_acosf(x)
+	float x;
+#endif
+{
+	float z,p,q,r,w,s,c,df;
+	__int32_t hx,ix;
+	GET_FLOAT_WORD(hx,x);
+	ix = hx&0x7fffffff;
+	if(ix==0x3f800000) {		/* |x|==1 */
+	    if(hx>0) return 0.0;	/* acos(1) = 0  */
+	    else return pi+(float)2.0*pio2_lo;	/* acos(-1)= pi */
+	} else if(ix>0x3f800000) {	/* |x| >= 1 */
+	    return (x-x)/(x-x);		/* acos(|x|>1) is NaN */
+	}
+	if(ix<0x3f000000) {	/* |x| < 0.5 */
+	    if(ix<=0x23000000) return pio2_hi+pio2_lo;/*if|x|<2**-57*/
+	    z = x*x;
+	    p = z*(pS0+z*(pS1+z*(pS2+z*(pS3+z*(pS4+z*pS5)))));
+	    q = one+z*(qS1+z*(qS2+z*(qS3+z*qS4)));
+	    r = p/q;
+	    return pio2_hi - (x - (pio2_lo-x*r));
+	} else  if (hx<0) {		/* x < -0.5 */
+	    z = (one+x)*(float)0.5;
+	    p = z*(pS0+z*(pS1+z*(pS2+z*(pS3+z*(pS4+z*pS5)))));
+	    q = one+z*(qS1+z*(qS2+z*(qS3+z*qS4)));
+	    s = __ieee754_sqrtf(z);
+	    r = p/q;
+	    w = r*s-pio2_lo;
+	    return pi - (float)2.0*(s+w);
+	} else {			/* x > 0.5 */
+	    __int32_t idf;
+	    z = (one-x)*(float)0.5;
+	    s = __ieee754_sqrtf(z);
+	    df = s;
+	    GET_FLOAT_WORD(idf,df);
+	    SET_FLOAT_WORD(df,idf&0xfffff000);
+	    c  = (z-df*df)/(s+df);
+	    p = z*(pS0+z*(pS1+z*(pS2+z*(pS3+z*(pS4+z*pS5)))));
+	    q = one+z*(qS1+z*(qS2+z*(qS3+z*qS4)));
+	    r = p/q;
+	    w = r*s+c;
+	    return (float)2.0*(df+w);
+	}
+}
diff --git a/src/math/ef_rem_pio2.c b/src/math/ef_rem_pio2.c
new file mode 100644
index 00000000..f1191d09
--- /dev/null
+++ b/src/math/ef_rem_pio2.c
@@ -0,0 +1,193 @@
+/* ef_rem_pio2.c -- float version of e_rem_pio2.c
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice 
+ * is preserved.
+ * ====================================================
+ *
+ */
+
+/* __ieee754_rem_pio2f(x,y)
+ * 
+ * return the remainder of x rem pi/2 in y[0]+y[1] 
+ * use __kernel_rem_pio2f()
+ */
+
+#include "fdlibm.h"
+
+/*
+ * Table of constants for 2/pi, 396 Hex digits (476 decimal) of 2/pi 
+ */
+#ifdef __STDC__
+static const __int32_t two_over_pi[] = {
+#else
+static __int32_t two_over_pi[] = {
+#endif
+0xA2, 0xF9, 0x83, 0x6E, 0x4E, 0x44, 0x15, 0x29, 0xFC,
+0x27, 0x57, 0xD1, 0xF5, 0x34, 0xDD, 0xC0, 0xDB, 0x62, 
+0x95, 0x99, 0x3C, 0x43, 0x90, 0x41, 0xFE, 0x51, 0x63,
+0xAB, 0xDE, 0xBB, 0xC5, 0x61, 0xB7, 0x24, 0x6E, 0x3A, 
+0x42, 0x4D, 0xD2, 0xE0, 0x06, 0x49, 0x2E, 0xEA, 0x09,
+0xD1, 0x92, 0x1C, 0xFE, 0x1D, 0xEB, 0x1C, 0xB1, 0x29, 
+0xA7, 0x3E, 0xE8, 0x82, 0x35, 0xF5, 0x2E, 0xBB, 0x44,
+0x84, 0xE9, 0x9C, 0x70, 0x26, 0xB4, 0x5F, 0x7E, 0x41, 
+0x39, 0x91, 0xD6, 0x39, 0x83, 0x53, 0x39, 0xF4, 0x9C,
+0x84, 0x5F, 0x8B, 0xBD, 0xF9, 0x28, 0x3B, 0x1F, 0xF8, 
+0x97, 0xFF, 0xDE, 0x05, 0x98, 0x0F, 0xEF, 0x2F, 0x11,
+0x8B, 0x5A, 0x0A, 0x6D, 0x1F, 0x6D, 0x36, 0x7E, 0xCF, 
+0x27, 0xCB, 0x09, 0xB7, 0x4F, 0x46, 0x3F, 0x66, 0x9E,
+0x5F, 0xEA, 0x2D, 0x75, 0x27, 0xBA, 0xC7, 0xEB, 0xE5, 
+0xF1, 0x7B, 0x3D, 0x07, 0x39, 0xF7, 0x8A, 0x52, 0x92,
+0xEA, 0x6B, 0xFB, 0x5F, 0xB1, 0x1F, 0x8D, 0x5D, 0x08, 
+0x56, 0x03, 0x30, 0x46, 0xFC, 0x7B, 0x6B, 0xAB, 0xF0,
+0xCF, 0xBC, 0x20, 0x9A, 0xF4, 0x36, 0x1D, 0xA9, 0xE3, 
+0x91, 0x61, 0x5E, 0xE6, 0x1B, 0x08, 0x65, 0x99, 0x85,
+0x5F, 0x14, 0xA0, 0x68, 0x40, 0x8D, 0xFF, 0xD8, 0x80, 
+0x4D, 0x73, 0x27, 0x31, 0x06, 0x06, 0x15, 0x56, 0xCA,
+0x73, 0xA8, 0xC9, 0x60, 0xE2, 0x7B, 0xC0, 0x8C, 0x6B, 
+};
+
+/* This array is like the one in e_rem_pio2.c, but the numbers are
+   single precision and the last 8 bits are forced to 0.  */
+#ifdef __STDC__
+static const __int32_t npio2_hw[] = {
+#else
+static __int32_t npio2_hw[] = {
+#endif
+0x3fc90f00, 0x40490f00, 0x4096cb00, 0x40c90f00, 0x40fb5300, 0x4116cb00,
+0x412fed00, 0x41490f00, 0x41623100, 0x417b5300, 0x418a3a00, 0x4196cb00,
+0x41a35c00, 0x41afed00, 0x41bc7e00, 0x41c90f00, 0x41d5a000, 0x41e23100,
+0x41eec200, 0x41fb5300, 0x4203f200, 0x420a3a00, 0x42108300, 0x4216cb00,
+0x421d1400, 0x42235c00, 0x4229a500, 0x422fed00, 0x42363600, 0x423c7e00,
+0x4242c700, 0x42490f00
+};
+
+/*
+ * invpio2:  24 bits of 2/pi
+ * pio2_1:   first  17 bit of pi/2
+ * pio2_1t:  pi/2 - pio2_1
+ * pio2_2:   second 17 bit of pi/2
+ * pio2_2t:  pi/2 - (pio2_1+pio2_2)
+ * pio2_3:   third  17 bit of pi/2
+ * pio2_3t:  pi/2 - (pio2_1+pio2_2+pio2_3)
+ */
+
+#ifdef __STDC__
+static const float 
+#else
+static float 
+#endif
+zero =  0.0000000000e+00, /* 0x00000000 */
+half =  5.0000000000e-01, /* 0x3f000000 */
+two8 =  2.5600000000e+02, /* 0x43800000 */
+invpio2 =  6.3661980629e-01, /* 0x3f22f984 */
+pio2_1  =  1.5707855225e+00, /* 0x3fc90f80 */
+pio2_1t =  1.0804334124e-05, /* 0x37354443 */
+pio2_2  =  1.0804273188e-05, /* 0x37354400 */
+pio2_2t =  6.0770999344e-11, /* 0x2e85a308 */
+pio2_3  =  6.0770943833e-11, /* 0x2e85a300 */
+pio2_3t =  6.1232342629e-17; /* 0x248d3132 */
+
+#ifdef __STDC__
+	__int32_t __ieee754_rem_pio2f(float x, float *y)
+#else
+	__int32_t __ieee754_rem_pio2f(x,y)
+	float x,y[];
+#endif
+{
+	float z,w,t,r,fn;
+	float tx[3];
+	__int32_t i,j,n,ix,hx;
+	int e0,nx;
+
+	GET_FLOAT_WORD(hx,x);
+	ix = hx&0x7fffffff;
+	if(ix<=0x3f490fd8)   /* |x| ~<= pi/4 , no need for reduction */
+	    {y[0] = x; y[1] = 0; return 0;}
+	if(ix<0x4016cbe4) {  /* |x| < 3pi/4, special case with n=+-1 */
+	    if(hx>0) { 
+		z = x - pio2_1;
+		if((ix&0xfffffff0)!=0x3fc90fd0) { /* 24+24 bit pi OK */
+		    y[0] = z - pio2_1t;
+		    y[1] = (z-y[0])-pio2_1t;
+		} else {		/* near pi/2, use 24+24+24 bit pi */
+		    z -= pio2_2;
+		    y[0] = z - pio2_2t;
+		    y[1] = (z-y[0])-pio2_2t;
+		}
+		return 1;
+	    } else {	/* negative x */
+		z = x + pio2_1;
+		if((ix&0xfffffff0)!=0x3fc90fd0) { /* 24+24 bit pi OK */
+		    y[0] = z + pio2_1t;
+		    y[1] = (z-y[0])+pio2_1t;
+		} else {		/* near pi/2, use 24+24+24 bit pi */
+		    z += pio2_2;
+		    y[0] = z + pio2_2t;
+		    y[1] = (z-y[0])+pio2_2t;
+		}
+		return -1;
+	    }
+	}
+	if(ix<=0x43490f80) { /* |x| ~<= 2^7*(pi/2), medium size */
+	    t  = fabsf(x);
+	    n  = (__int32_t) (t*invpio2+half);
+	    fn = (float)n;
+	    r  = t-fn*pio2_1;
+	    w  = fn*pio2_1t;	/* 1st round good to 40 bit */
+	    if(n<32&&(ix&0xffffff00)!=npio2_hw[n-1]) {	
+		y[0] = r-w;	/* quick check no cancellation */
+	    } else {
+	        __uint32_t high;
+	        j  = ix>>23;
+	        y[0] = r-w; 
+		GET_FLOAT_WORD(high,y[0]);
+	        i = j-((high>>23)&0xff);
+	        if(i>8) {  /* 2nd iteration needed, good to 57 */
+		    t  = r;
+		    w  = fn*pio2_2;	
+		    r  = t-w;
+		    w  = fn*pio2_2t-((t-r)-w);	
+		    y[0] = r-w;
+		    GET_FLOAT_WORD(high,y[0]);
+		    i = j-((high>>23)&0xff);
+		    if(i>25)  {	/* 3rd iteration need, 74 bits acc */
+		    	t  = r;	/* will cover all possible cases */
+		    	w  = fn*pio2_3;	
+		    	r  = t-w;
+		    	w  = fn*pio2_3t-((t-r)-w);	
+		    	y[0] = r-w;
+		    }
+		}
+	    }
+	    y[1] = (r-y[0])-w;
+	    if(hx<0) 	{y[0] = -y[0]; y[1] = -y[1]; return -n;}
+	    else	 return n;
+	}
+    /* 
+     * all other (large) arguments
+     */
+	if(!FLT_UWORD_IS_FINITE(ix)) {
+	    y[0]=y[1]=x-x; return 0;
+	}
+    /* set z = scalbn(|x|,ilogb(x)-7) */
+	e0 	= (int)((ix>>23)-134);	/* e0 = ilogb(z)-7; */
+	SET_FLOAT_WORD(z, ix - ((__int32_t)e0<<23));
+	for(i=0;i<2;i++) {
+		tx[i] = (float)((__int32_t)(z));
+		z     = (z-tx[i])*two8;
+	}
+	tx[2] = z;
+	nx = 3;
+	while(tx[nx-1]==zero) nx--;	/* skip zero term */
+	n  =  __kernel_rem_pio2f(tx,y,e0,nx,2,two_over_pi);
+	if(hx<0) {y[0] = -y[0]; y[1] = -y[1]; return -n;}
+	return n;
+}
diff --git a/src/math/ef_sqrt.c b/src/math/ef_sqrt.c
new file mode 100644
index 00000000..80e7f360
--- /dev/null
+++ b/src/math/ef_sqrt.c
@@ -0,0 +1,89 @@
+/* ef_sqrtf.c -- float version of e_sqrt.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice 
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+static	const float	one	= 1.0, tiny=1.0e-30;
+#else
+static	float	one	= 1.0, tiny=1.0e-30;
+#endif
+
+#ifdef __STDC__
+	float __ieee754_sqrtf(float x)
+#else
+	float __ieee754_sqrtf(x)
+	float x;
+#endif
+{
+	float z;
+	__uint32_t r,hx;
+	__int32_t ix,s,q,m,t,i;
+
+	GET_FLOAT_WORD(ix,x);
+	hx = ix&0x7fffffff;
+
+    /* take care of Inf and NaN */
+	if(!FLT_UWORD_IS_FINITE(hx))
+	    return x*x+x;		/* sqrt(NaN)=NaN, sqrt(+inf)=+inf
+					   sqrt(-inf)=sNaN */
+    /* take care of zero and -ves */
+	if(FLT_UWORD_IS_ZERO(hx)) return x;/* sqrt(+-0) = +-0 */
+	if(ix<0) return (x-x)/(x-x);		/* sqrt(-ve) = sNaN */
+
+    /* normalize x */
+	m = (ix>>23);
+	if(FLT_UWORD_IS_SUBNORMAL(hx)) {		/* subnormal x */
+	    for(i=0;(ix&0x00800000L)==0;i++) ix<<=1;
+	    m -= i-1;
+	}
+	m -= 127;	/* unbias exponent */
+	ix = (ix&0x007fffffL)|0x00800000L;
+	if(m&1)	/* odd m, double x to make it even */
+	    ix += ix;
+	m >>= 1;	/* m = [m/2] */
+
+    /* generate sqrt(x) bit by bit */
+	ix += ix;
+	q = s = 0;		/* q = sqrt(x) */
+	r = 0x01000000L;		/* r = moving bit from right to left */
+
+	while(r!=0) {
+	    t = s+r; 
+	    if(t<=ix) { 
+		s    = t+r; 
+		ix  -= t; 
+		q   += r; 
+	    } 
+	    ix += ix;
+	    r>>=1;
+	}
+
+    /* use floating add to find out rounding direction */
+	if(ix!=0) {
+	    z = one-tiny; /* trigger inexact flag */
+	    if (z>=one) {
+	        z = one+tiny;
+		if (z>one)
+		    q += 2;
+		else
+		    q += (q&1);
+	    }
+	}
+	ix = (q>>1)+0x3f000000L;
+	ix += (m <<23);
+	SET_FLOAT_WORD(z,ix);
+	return z;
+}
diff --git a/src/math/fdlibm.h b/src/math/fdlibm.h
new file mode 100644
index 00000000..821619ad
--- /dev/null
+++ b/src/math/fdlibm.h
@@ -0,0 +1,413 @@
+
+/* @(#)fdlibm.h 5.1 93/09/24 */
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice 
+ * is preserved.
+ * ====================================================
+ */
+
+/* AltOS local */
+#include <math.h>
+#include <stdint.h>
+#define __int32_t int32_t
+#define __uint32_t uint32_t
+
+#define __ieee754_acosf acosf
+#define __ieee754_sqrtf sqrtf
+
+/* REDHAT LOCAL: Include files.  */
+#include <math.h>
+/* #include <sys/types.h> */
+#include <machine/ieeefp.h>
+
+/* REDHAT LOCAL: Default to XOPEN_MODE.  */
+#define _XOPEN_MODE
+
+/* Most routines need to check whether a float is finite, infinite, or not a
+   number, and many need to know whether the result of an operation will
+   overflow.  These conditions depend on whether the largest exponent is
+   used for NaNs & infinities, or whether it's used for finite numbers.  The
+   macros below wrap up that kind of information:
+
+   FLT_UWORD_IS_FINITE(X)
+	True if a positive float with bitmask X is finite.
+
+   FLT_UWORD_IS_NAN(X)
+	True if a positive float with bitmask X is not a number.
+
+   FLT_UWORD_IS_INFINITE(X)
+	True if a positive float with bitmask X is +infinity.
+
+   FLT_UWORD_MAX
+	The bitmask of FLT_MAX.
+
+   FLT_UWORD_HALF_MAX
+	The bitmask of FLT_MAX/2.
+
+   FLT_UWORD_EXP_MAX
+	The bitmask of the largest finite exponent (129 if the largest
+	exponent is used for finite numbers, 128 otherwise).
+
+   FLT_UWORD_LOG_MAX
+	The bitmask of log(FLT_MAX), rounded down.  This value is the largest
+	input that can be passed to exp() without producing overflow.
+
+   FLT_UWORD_LOG_2MAX
+	The bitmask of log(2*FLT_MAX), rounded down.  This value is the
+	largest input than can be passed to cosh() without producing
+	overflow.
+
+   FLT_LARGEST_EXP
+	The largest biased exponent that can be used for finite numbers
+	(255 if the largest exponent is used for finite numbers, 254
+	otherwise) */
+
+#ifdef _FLT_LARGEST_EXPONENT_IS_NORMAL
+#define FLT_UWORD_IS_FINITE(x) 1
+#define FLT_UWORD_IS_NAN(x) 0
+#define FLT_UWORD_IS_INFINITE(x) 0
+#define FLT_UWORD_MAX 0x7fffffff
+#define FLT_UWORD_EXP_MAX 0x43010000
+#define FLT_UWORD_LOG_MAX 0x42b2d4fc
+#define FLT_UWORD_LOG_2MAX 0x42b437e0
+#define HUGE ((float)0X1.FFFFFEP128)
+#else
+#define FLT_UWORD_IS_FINITE(x) ((x)<0x7f800000L)
+#define FLT_UWORD_IS_NAN(x) ((x)>0x7f800000L)
+#define FLT_UWORD_IS_INFINITE(x) ((x)==0x7f800000L)
+#define FLT_UWORD_MAX 0x7f7fffffL
+#define FLT_UWORD_EXP_MAX 0x43000000
+#define FLT_UWORD_LOG_MAX 0x42b17217
+#define FLT_UWORD_LOG_2MAX 0x42b2d4fc
+#define HUGE ((float)3.40282346638528860e+38)
+#endif
+#define FLT_UWORD_HALF_MAX (FLT_UWORD_MAX-(1L<<23))
+#define FLT_LARGEST_EXP (FLT_UWORD_MAX>>23)
+
+/* Many routines check for zero and subnormal numbers.  Such things depend
+   on whether the target supports denormals or not:
+
+   FLT_UWORD_IS_ZERO(X)
+	True if a positive float with bitmask X is +0.	Without denormals,
+	any float with a zero exponent is a +0 representation.	With
+	denormals, the only +0 representation is a 0 bitmask.
+
+   FLT_UWORD_IS_SUBNORMAL(X)
+	True if a non-zero positive float with bitmask X is subnormal.
+	(Routines should check for zeros first.)
+
+   FLT_UWORD_MIN
+	The bitmask of the smallest float above +0.  Call this number
+	REAL_FLT_MIN...
+
+   FLT_UWORD_EXP_MIN
+	The bitmask of the float representation of REAL_FLT_MIN's exponent.
+
+   FLT_UWORD_LOG_MIN
+	The bitmask of |log(REAL_FLT_MIN)|, rounding down.
+
+   FLT_SMALLEST_EXP
+	REAL_FLT_MIN's exponent - EXP_BIAS (1 if denormals are not supported,
+	-22 if they are).
+*/
+
+#ifdef _FLT_NO_DENORMALS
+#define FLT_UWORD_IS_ZERO(x) ((x)<0x00800000L)
+#define FLT_UWORD_IS_SUBNORMAL(x) 0
+#define FLT_UWORD_MIN 0x00800000
+#define FLT_UWORD_EXP_MIN 0x42fc0000
+#define FLT_UWORD_LOG_MIN 0x42aeac50
+#define FLT_SMALLEST_EXP 1
+#else
+#define FLT_UWORD_IS_ZERO(x) ((x)==0)
+#define FLT_UWORD_IS_SUBNORMAL(x) ((x)<0x00800000L)
+#define FLT_UWORD_MIN 0x00000001
+#define FLT_UWORD_EXP_MIN 0x43160000
+#define FLT_UWORD_LOG_MIN 0x42cff1b5
+#define FLT_SMALLEST_EXP -22
+#endif
+
+#ifdef __STDC__
+#undef __P
+#define	__P(p)	p
+#else
+#define	__P(p)	()
+#endif
+
+/* 
+ * set X_TLOSS = pi*2**52, which is possibly defined in <values.h>
+ * (one may replace the following line by "#include <values.h>")
+ */
+
+#define X_TLOSS		1.41484755040568800000e+16 
+
+/* Functions that are not documented, and are not in <math.h>.  */
+
+#ifdef _SCALB_INT
+extern double scalb __P((double, int));
+#else
+extern double scalb __P((double, double));
+#endif
+extern double significand __P((double));
+
+/* ieee style elementary functions */
+extern double __ieee754_sqrt __P((double));			
+extern double __ieee754_acos __P((double));			
+extern double __ieee754_acosh __P((double));			
+extern double __ieee754_log __P((double));			
+extern double __ieee754_atanh __P((double));			
+extern double __ieee754_asin __P((double));			
+extern double __ieee754_atan2 __P((double,double));			
+extern double __ieee754_exp __P((double));
+extern double __ieee754_cosh __P((double));
+extern double __ieee754_fmod __P((double,double));
+extern double __ieee754_pow __P((double,double));
+extern double __ieee754_lgamma_r __P((double,int *));
+extern double __ieee754_gamma_r __P((double,int *));
+extern double __ieee754_log10 __P((double));
+extern double __ieee754_sinh __P((double));
+extern double __ieee754_hypot __P((double,double));
+extern double __ieee754_j0 __P((double));
+extern double __ieee754_j1 __P((double));
+extern double __ieee754_y0 __P((double));
+extern double __ieee754_y1 __P((double));
+extern double __ieee754_jn __P((int,double));
+extern double __ieee754_yn __P((int,double));
+extern double __ieee754_remainder __P((double,double));
+extern __int32_t __ieee754_rem_pio2 __P((double,double*));
+#ifdef _SCALB_INT
+extern double __ieee754_scalb __P((double,int));
+#else
+extern double __ieee754_scalb __P((double,double));
+#endif
+
+/* fdlibm kernel function */
+extern double __kernel_standard __P((double,double,int));
+extern double __kernel_sin __P((double,double,int));
+extern double __kernel_cos __P((double,double));
+extern double __kernel_tan __P((double,double,int));
+extern int    __kernel_rem_pio2 __P((double*,double*,int,int,int,const __int32_t*));
+
+/* Undocumented float functions.  */
+#ifdef _SCALB_INT
+extern float scalbf __P((float, int));
+#else
+extern float scalbf __P((float, float));
+#endif
+extern float significandf __P((float));
+
+/* ieee style elementary float functions */
+extern float __ieee754_sqrtf __P((float));			
+extern float __ieee754_acosf __P((float));			
+extern float __ieee754_acoshf __P((float));			
+extern float __ieee754_logf __P((float));			
+extern float __ieee754_atanhf __P((float));			
+extern float __ieee754_asinf __P((float));			
+extern float __ieee754_atan2f __P((float,float));			
+extern float __ieee754_expf __P((float));
+extern float __ieee754_coshf __P((float));
+extern float __ieee754_fmodf __P((float,float));
+extern float __ieee754_powf __P((float,float));
+extern float __ieee754_lgammaf_r __P((float,int *));
+extern float __ieee754_gammaf_r __P((float,int *));
+extern float __ieee754_log10f __P((float));
+extern float __ieee754_sinhf __P((float));
+extern float __ieee754_hypotf __P((float,float));
+extern float __ieee754_j0f __P((float));
+extern float __ieee754_j1f __P((float));
+extern float __ieee754_y0f __P((float));
+extern float __ieee754_y1f __P((float));
+extern float __ieee754_jnf __P((int,float));
+extern float __ieee754_ynf __P((int,float));
+extern float __ieee754_remainderf __P((float,float));
+extern __int32_t __ieee754_rem_pio2f __P((float,float*));
+#ifdef _SCALB_INT
+extern float __ieee754_scalbf __P((float,int));
+#else
+extern float __ieee754_scalbf __P((float,float));
+#endif
+
+/* float versions of fdlibm kernel functions */
+extern float __kernel_sinf __P((float,float,int));
+extern float __kernel_cosf __P((float,float));
+extern float __kernel_tanf __P((float,float,int));
+extern int   __kernel_rem_pio2f __P((float*,float*,int,int,int,const __int32_t*));
+
+/* The original code used statements like
+	n0 = ((*(int*)&one)>>29)^1;		* index of high word *
+	ix0 = *(n0+(int*)&x);			* high word of x *
+	ix1 = *((1-n0)+(int*)&x);		* low word of x *
+   to dig two 32 bit words out of the 64 bit IEEE floating point
+   value.  That is non-ANSI, and, moreover, the gcc instruction
+   scheduler gets it wrong.  We instead use the following macros.
+   Unlike the original code, we determine the endianness at compile
+   time, not at run time; I don't see much benefit to selecting
+   endianness at run time.  */
+
+#ifndef __IEEE_BIG_ENDIAN
+#ifndef __IEEE_LITTLE_ENDIAN
+ #error Must define endianness
+#endif
+#endif
+
+/* A union which permits us to convert between a double and two 32 bit
+   ints.  */
+
+#ifdef __IEEE_BIG_ENDIAN
+
+typedef union 
+{
+  double value;
+  struct 
+  {
+    __uint32_t msw;
+    __uint32_t lsw;
+  } parts;
+} ieee_double_shape_type;
+
+#endif
+
+#ifdef __IEEE_LITTLE_ENDIAN
+
+typedef union 
+{
+  double value;
+  struct 
+  {
+    __uint32_t lsw;
+    __uint32_t msw;
+  } parts;
+} ieee_double_shape_type;
+
+#endif
+
+/* Get two 32 bit ints from a double.  */
+
+#define EXTRACT_WORDS(ix0,ix1,d)				\
+do {								\
+  ieee_double_shape_type ew_u;					\
+  ew_u.value = (d);						\
+  (ix0) = ew_u.parts.msw;					\
+  (ix1) = ew_u.parts.lsw;					\
+} while (0)
+
+/* Get the more significant 32 bit int from a double.  */
+
+#define GET_HIGH_WORD(i,d)					\
+do {								\
+  ieee_double_shape_type gh_u;					\
+  gh_u.value = (d);						\
+  (i) = gh_u.parts.msw;						\
+} while (0)
+
+/* Get the less significant 32 bit int from a double.  */
+
+#define GET_LOW_WORD(i,d)					\
+do {								\
+  ieee_double_shape_type gl_u;					\
+  gl_u.value = (d);						\
+  (i) = gl_u.parts.lsw;						\
+} while (0)
+
+/* Set a double from two 32 bit ints.  */
+
+#define INSERT_WORDS(d,ix0,ix1)					\
+do {								\
+  ieee_double_shape_type iw_u;					\
+  iw_u.parts.msw = (ix0);					\
+  iw_u.parts.lsw = (ix1);					\
+  (d) = iw_u.value;						\
+} while (0)
+
+/* Set the more significant 32 bits of a double from an int.  */
+
+#define SET_HIGH_WORD(d,v)					\
+do {								\
+  ieee_double_shape_type sh_u;					\
+  sh_u.value = (d);						\
+  sh_u.parts.msw = (v);						\
+  (d) = sh_u.value;						\
+} while (0)
+
+/* Set the less significant 32 bits of a double from an int.  */
+
+#define SET_LOW_WORD(d,v)					\
+do {								\
+  ieee_double_shape_type sl_u;					\
+  sl_u.value = (d);						\
+  sl_u.parts.lsw = (v);						\
+  (d) = sl_u.value;						\
+} while (0)
+
+/* A union which permits us to convert between a float and a 32 bit
+   int.  */
+
+typedef union
+{
+  float value;
+  __uint32_t word;
+} ieee_float_shape_type;
+
+/* Get a 32 bit int from a float.  */
+
+#define GET_FLOAT_WORD(i,d)					\
+do {								\
+  ieee_float_shape_type gf_u;					\
+  gf_u.value = (d);						\
+  (i) = gf_u.word;						\
+} while (0)
+
+/* Set a float from a 32 bit int.  */
+
+#define SET_FLOAT_WORD(d,i)					\
+do {								\
+  ieee_float_shape_type sf_u;					\
+  sf_u.word = (i);						\
+  (d) = sf_u.value;						\
+} while (0)
+
+/* Macros to avoid undefined behaviour that can arise if the amount
+   of a shift is exactly equal to the size of the shifted operand.  */
+
+#define SAFE_LEFT_SHIFT(op,amt)					\
+  (((amt) < 8 * sizeof(op)) ? ((op) << (amt)) : 0)
+
+#define SAFE_RIGHT_SHIFT(op,amt)				\
+  (((amt) < 8 * sizeof(op)) ? ((op) >> (amt)) : 0)
+
+#ifdef  _COMPLEX_H
+
+/*
+ * Quoting from ISO/IEC 9899:TC2:
+ *
+ * 6.2.5.13 Types
+ * Each complex type has the same representation and alignment requirements as
+ * an array type containing exactly two elements of the corresponding real type;
+ * the first element is equal to the real part, and the second element to the
+ * imaginary part, of the complex number.
+ */
+typedef union {
+        float complex z;
+        float parts[2];
+} float_complex;
+
+typedef union {
+        double complex z;
+        double parts[2];
+} double_complex;
+
+typedef union {
+        long double complex z;
+        long double parts[2];
+} long_double_complex;
+
+#define REAL_PART(z)    ((z).parts[0])
+#define IMAG_PART(z)    ((z).parts[1])
+
+#endif  /* _COMPLEX_H */
+
diff --git a/src/math/ieeefp.h b/src/math/ieeefp.h
new file mode 100644
index 00000000..0b06fb78
--- /dev/null
+++ b/src/math/ieeefp.h
@@ -0,0 +1,256 @@
+#ifndef _IEEE_FP_H_
+#define _IEEE_FP_H_
+
+#include "_ansi.h"
+
+#include <machine/ieeefp.h>
+
+_BEGIN_STD_C
+
+/* FIXME FIXME FIXME:
+   Neither of __ieee_{float,double}_shape_tape seem to be used anywhere
+   except in libm/test.  If that is the case, please delete these from here.
+   If that is not the case, please insert documentation here describing why
+   they're needed.  */
+
+#ifdef __IEEE_BIG_ENDIAN
+
+typedef union 
+{
+  double value;
+  struct 
+  {
+    unsigned int sign : 1;
+    unsigned int exponent: 11;
+    unsigned int fraction0:4;
+    unsigned int fraction1:16;
+    unsigned int fraction2:16;
+    unsigned int fraction3:16;
+    
+  } number;
+  struct 
+  {
+    unsigned int sign : 1;
+    unsigned int exponent: 11;
+    unsigned int quiet:1;
+    unsigned int function0:3;
+    unsigned int function1:16;
+    unsigned int function2:16;
+    unsigned int function3:16;
+  } nan;
+  struct 
+  {
+    unsigned long msw;
+    unsigned long lsw;
+  } parts;
+    long aslong[2];
+} __ieee_double_shape_type;
+
+#endif
+
+#ifdef __IEEE_LITTLE_ENDIAN
+
+typedef union 
+{
+  double value;
+  struct 
+  {
+#ifdef __SMALL_BITFIELDS
+    unsigned int fraction3:16;
+    unsigned int fraction2:16;
+    unsigned int fraction1:16;
+    unsigned int fraction0: 4;
+#else
+    unsigned int fraction1:32;
+    unsigned int fraction0:20;
+#endif
+    unsigned int exponent :11;
+    unsigned int sign     : 1;
+  } number;
+  struct 
+  {
+#ifdef __SMALL_BITFIELDS
+    unsigned int function3:16;
+    unsigned int function2:16;
+    unsigned int function1:16;
+    unsigned int function0:3;
+#else
+    unsigned int function1:32;
+    unsigned int function0:19;
+#endif
+    unsigned int quiet:1;
+    unsigned int exponent: 11;
+    unsigned int sign : 1;
+  } nan;
+  struct 
+  {
+    unsigned long lsw;
+    unsigned long msw;
+  } parts;
+
+  long aslong[2];
+
+} __ieee_double_shape_type;
+
+#endif
+
+#ifdef __IEEE_BIG_ENDIAN
+
+typedef union
+{
+  float value;
+  struct 
+  {
+    unsigned int sign : 1;
+    unsigned int exponent: 8;
+    unsigned int fraction0: 7;
+    unsigned int fraction1: 16;
+  } number;
+  struct 
+  {
+    unsigned int sign:1;
+    unsigned int exponent:8;
+    unsigned int quiet:1;
+    unsigned int function0:6;
+    unsigned int function1:16;
+  } nan;
+  long p1;
+  
+} __ieee_float_shape_type;
+
+#endif
+
+#ifdef __IEEE_LITTLE_ENDIAN
+
+typedef union
+{
+  float value;
+  struct 
+  {
+    unsigned int fraction0: 7;
+    unsigned int fraction1: 16;
+    unsigned int exponent: 8;
+    unsigned int sign : 1;
+  } number;
+  struct 
+  {
+    unsigned int function1:16;
+    unsigned int function0:6;
+    unsigned int quiet:1;
+    unsigned int exponent:8;
+    unsigned int sign:1;
+  } nan;
+  long p1;
+  
+} __ieee_float_shape_type;
+
+#endif
+
+
+
+
+
+/* FLOATING ROUNDING */
+
+typedef int fp_rnd;
+#define FP_RN 0 	/* Round to nearest 		*/
+#define FP_RM 1		/* Round down 			*/
+#define FP_RP 2		/* Round up 			*/
+#define FP_RZ 3		/* Round to zero (trunate) 	*/
+
+fp_rnd _EXFUN(fpgetround,(void));
+fp_rnd _EXFUN(fpsetround, (fp_rnd));
+
+/* EXCEPTIONS */
+
+typedef int fp_except;
+#define FP_X_INV 0x10	/* Invalid operation 		*/
+#define FP_X_DX  0x80	/* Divide by zero		*/
+#define FP_X_OFL 0x04	/* Overflow exception		*/
+#define FP_X_UFL 0x02	/* Underflow exception		*/
+#define FP_X_IMP 0x01	/* imprecise exception		*/
+
+fp_except _EXFUN(fpgetmask,(void));
+fp_except _EXFUN(fpsetmask,(fp_except));
+fp_except _EXFUN(fpgetsticky,(void));
+fp_except _EXFUN(fpsetsticky, (fp_except));
+
+/* INTEGER ROUNDING */
+
+typedef int fp_rdi;
+#define FP_RDI_TOZ 0	/* Round to Zero 		*/
+#define FP_RDI_RD  1	/* Follow float mode		*/
+
+fp_rdi _EXFUN(fpgetroundtoi,(void));
+fp_rdi _EXFUN(fpsetroundtoi,(fp_rdi));
+
+#undef isnan
+#undef isinf
+
+int _EXFUN(isnan, (double));
+int _EXFUN(isinf, (double));
+int _EXFUN(finite, (double));
+
+
+
+int _EXFUN(isnanf, (float));
+int _EXFUN(isinff, (float));
+int _EXFUN(finitef, (float));
+
+#define __IEEE_DBL_EXPBIAS 1023
+#define __IEEE_FLT_EXPBIAS 127
+
+#define __IEEE_DBL_EXPLEN 11
+#define __IEEE_FLT_EXPLEN 8
+
+
+#define __IEEE_DBL_FRACLEN (64 - (__IEEE_DBL_EXPLEN + 1))
+#define __IEEE_FLT_FRACLEN (32 - (__IEEE_FLT_EXPLEN + 1))
+
+#define __IEEE_DBL_MAXPOWTWO	((double)(1L << 32 - 2) * (1L << (32-11) - 32 + 1))
+#define __IEEE_FLT_MAXPOWTWO	((float)(1L << (32-8) - 1))
+
+#define __IEEE_DBL_NAN_EXP 0x7ff
+#define __IEEE_FLT_NAN_EXP 0xff
+
+#ifndef __ieeefp_isnanf
+#define __ieeefp_isnanf(x) (((*(long *)&(x) & 0x7f800000L)==0x7f800000L) && \
+			    ((*(long *)&(x) & 0x007fffffL)!=0000000000L))
+#endif
+#define isnanf(x)	__ieeefp_isnanf(x)
+
+#ifndef __ieeefp_isinff
+#define __ieeefp_isinff(x) (((*(long *)&(x) & 0x7f800000L)==0x7f800000L) && \
+			    ((*(long *)&(x) & 0x007fffffL)==0000000000L))
+#endif
+#define isinff(x)	__ieeefp_isinff(x)
+
+#ifndef __ieeefp_finitef
+#define __ieeefp_finitef(x) (((*(long *)&(x) & 0x7f800000L)!=0x7f800000L))
+#endif
+#define finitef(x)	__ieeefp_finitef(x)
+
+#ifdef _DOUBLE_IS_32BITS
+#undef __IEEE_DBL_EXPBIAS
+#define __IEEE_DBL_EXPBIAS __IEEE_FLT_EXPBIAS
+
+#undef __IEEE_DBL_EXPLEN
+#define __IEEE_DBL_EXPLEN __IEEE_FLT_EXPLEN
+
+#undef __IEEE_DBL_FRACLEN
+#define __IEEE_DBL_FRACLEN __IEEE_FLT_FRACLEN
+
+#undef __IEEE_DBL_MAXPOWTWO
+#define __IEEE_DBL_MAXPOWTWO __IEEE_FLT_MAXPOWTWO
+
+#undef __IEEE_DBL_NAN_EXP
+#define __IEEE_DBL_NAN_EXP __IEEE_FLT_NAN_EXP
+
+#undef __ieee_double_shape_type
+#define __ieee_double_shape_type __ieee_float_shape_type
+
+#endif /* _DOUBLE_IS_32BITS */
+
+_END_STD_C
+
+#endif /* _IEEE_FP_H_ */
diff --git a/src/math/kf_cos.c b/src/math/kf_cos.c
new file mode 100644
index 00000000..4f71af23
--- /dev/null
+++ b/src/math/kf_cos.c
@@ -0,0 +1,59 @@
+/* kf_cos.c -- float version of k_cos.c
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice 
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+static const float 
+#else
+static float 
+#endif
+one =  1.0000000000e+00, /* 0x3f800000 */
+C1  =  4.1666667908e-02, /* 0x3d2aaaab */
+C2  = -1.3888889225e-03, /* 0xbab60b61 */
+C3  =  2.4801587642e-05, /* 0x37d00d01 */
+C4  = -2.7557314297e-07, /* 0xb493f27c */
+C5  =  2.0875723372e-09, /* 0x310f74f6 */
+C6  = -1.1359647598e-11; /* 0xad47d74e */
+
+#ifdef __STDC__
+	float __kernel_cosf(float x, float y)
+#else
+	float __kernel_cosf(x, y)
+	float x,y;
+#endif
+{
+	float a,hz,z,r,qx;
+	__int32_t ix;
+	GET_FLOAT_WORD(ix,x);
+	ix &= 0x7fffffff;			/* ix = |x|'s high word*/
+	if(ix<0x32000000) {			/* if x < 2**27 */
+	    if(((int)x)==0) return one;		/* generate inexact */
+	}
+	z  = x*x;
+	r  = z*(C1+z*(C2+z*(C3+z*(C4+z*(C5+z*C6)))));
+	if(ix < 0x3e99999a) 			/* if |x| < 0.3 */ 
+	    return one - ((float)0.5*z - (z*r - x*y));
+	else {
+	    if(ix > 0x3f480000) {		/* x > 0.78125 */
+		qx = (float)0.28125;
+	    } else {
+	        SET_FLOAT_WORD(qx,ix-0x01000000);	/* x/4 */
+	    }
+	    hz = (float)0.5*z-qx;
+	    a  = one-qx;
+	    return a - (hz - (z*r-x*y));
+	}
+}
diff --git a/src/math/kf_rem_pio2.c b/src/math/kf_rem_pio2.c
new file mode 100644
index 00000000..261c4812
--- /dev/null
+++ b/src/math/kf_rem_pio2.c
@@ -0,0 +1,208 @@
+/* kf_rem_pio2.c -- float version of k_rem_pio2.c
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice 
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+
+/* In the float version, the input parameter x contains 8 bit
+   integers, not 24 bit integers.  113 bit precision is not supported.  */
+
+#ifdef __STDC__
+static const int init_jk[] = {4,7,9}; /* initial value for jk */
+#else
+static int init_jk[] = {4,7,9}; 
+#endif
+
+#ifdef __STDC__
+static const float PIo2[] = {
+#else
+static float PIo2[] = {
+#endif
+  1.5703125000e+00, /* 0x3fc90000 */
+  4.5776367188e-04, /* 0x39f00000 */
+  2.5987625122e-05, /* 0x37da0000 */
+  7.5437128544e-08, /* 0x33a20000 */
+  6.0026650317e-11, /* 0x2e840000 */
+  7.3896444519e-13, /* 0x2b500000 */
+  5.3845816694e-15, /* 0x27c20000 */
+  5.6378512969e-18, /* 0x22d00000 */
+  8.3009228831e-20, /* 0x1fc40000 */
+  3.2756352257e-22, /* 0x1bc60000 */
+  6.3331015649e-25, /* 0x17440000 */
+};
+
+#ifdef __STDC__
+static const float			
+#else
+static float			
+#endif
+zero   = 0.0,
+one    = 1.0,
+two8   =  2.5600000000e+02, /* 0x43800000 */
+twon8  =  3.9062500000e-03; /* 0x3b800000 */
+
+#ifdef __STDC__
+	int __kernel_rem_pio2f(float *x, float *y, int e0, int nx, int prec, const __int32_t *ipio2) 
+#else
+	int __kernel_rem_pio2f(x,y,e0,nx,prec,ipio2) 	
+	float x[], y[]; int e0,nx,prec; __int32_t ipio2[];
+#endif
+{
+	__int32_t jz,jx,jv,jp,jk,carry,n,iq[20],i,j,k,m,q0,ih;
+	float z,fw,f[20],fq[20],q[20];
+
+    /* initialize jk*/
+	jk = init_jk[prec];
+	jp = jk;
+
+    /* determine jx,jv,q0, note that 3>q0 */
+	jx =  nx-1;
+	jv = (e0-3)/8; if(jv<0) jv=0;
+	q0 =  e0-8*(jv+1);
+
+    /* set up f[0] to f[jx+jk] where f[jx+jk] = ipio2[jv+jk] */
+	j = jv-jx; m = jx+jk;
+	for(i=0;i<=m;i++,j++) f[i] = (j<0)? zero : (float) ipio2[j];
+
+    /* compute q[0],q[1],...q[jk] */
+	for (i=0;i<=jk;i++) {
+	    for(j=0,fw=0.0;j<=jx;j++) fw += x[j]*f[jx+i-j]; q[i] = fw;
+	}
+
+	jz = jk;
+recompute:
+    /* distill q[] into iq[] reversingly */
+	for(i=0,j=jz,z=q[jz];j>0;i++,j--) {
+	    fw    =  (float)((__int32_t)(twon8* z));
+	    iq[i] =  (__int32_t)(z-two8*fw);
+	    z     =  q[j-1]+fw;
+	}
+
+    /* compute n */
+	z  = scalbnf(z,(int)q0);	/* actual value of z */
+	z -= (float)8.0*floorf(z*(float)0.125);	/* trim off integer >= 8 */
+	n  = (__int32_t) z;
+	z -= (float)n;
+	ih = 0;
+	if(q0>0) {	/* need iq[jz-1] to determine n */
+	    i  = (iq[jz-1]>>(8-q0)); n += i;
+	    iq[jz-1] -= i<<(8-q0);
+	    ih = iq[jz-1]>>(7-q0);
+	} 
+	else if(q0==0) ih = iq[jz-1]>>8;
+	else if(z>=(float)0.5) ih=2;
+
+	if(ih>0) {	/* q > 0.5 */
+	    n += 1; carry = 0;
+	    for(i=0;i<jz ;i++) {	/* compute 1-q */
+		j = iq[i];
+		if(carry==0) {
+		    if(j!=0) {
+			carry = 1; iq[i] = 0x100- j;
+		    }
+		} else  iq[i] = 0xff - j;
+	    }
+	    if(q0>0) {		/* rare case: chance is 1 in 12 */
+	        switch(q0) {
+	        case 1:
+	    	   iq[jz-1] &= 0x7f; break;
+	    	case 2:
+	    	   iq[jz-1] &= 0x3f; break;
+	        }
+	    }
+	    if(ih==2) {
+		z = one - z;
+		if(carry!=0) z -= scalbnf(one,(int)q0);
+	    }
+	}
+
+    /* check if recomputation is needed */
+	if(z==zero) {
+	    j = 0;
+	    for (i=jz-1;i>=jk;i--) j |= iq[i];
+	    if(j==0) { /* need recomputation */
+		for(k=1;iq[jk-k]==0;k++);   /* k = no. of terms needed */
+
+		for(i=jz+1;i<=jz+k;i++) {   /* add q[jz+1] to q[jz+k] */
+		    f[jx+i] = (float) ipio2[jv+i];
+		    for(j=0,fw=0.0;j<=jx;j++) fw += x[j]*f[jx+i-j];
+		    q[i] = fw;
+		}
+		jz += k;
+		goto recompute;
+	    }
+	}
+
+    /* chop off zero terms */
+	if(z==(float)0.0) {
+	    jz -= 1; q0 -= 8;
+	    while(iq[jz]==0) { jz--; q0-=8;}
+	} else { /* break z into 8-bit if necessary */
+	    z = scalbnf(z,-(int)q0);
+	    if(z>=two8) { 
+		fw = (float)((__int32_t)(twon8*z));
+		iq[jz] = (__int32_t)(z-two8*fw);
+		jz += 1; q0 += 8;
+		iq[jz] = (__int32_t) fw;
+	    } else iq[jz] = (__int32_t) z ;
+	}
+
+    /* convert integer "bit" chunk to floating-point value */
+	fw = scalbnf(one,(int)q0);
+	for(i=jz;i>=0;i--) {
+	    q[i] = fw*(float)iq[i]; fw*=twon8;
+	}
+
+    /* compute PIo2[0,...,jp]*q[jz,...,0] */
+	for(i=jz;i>=0;i--) {
+	    for(fw=0.0,k=0;k<=jp&&k<=jz-i;k++) fw += PIo2[k]*q[i+k];
+	    fq[jz-i] = fw;
+	}
+
+    /* compress fq[] into y[] */
+	switch(prec) {
+	    case 0:
+		fw = 0.0;
+		for (i=jz;i>=0;i--) fw += fq[i];
+		y[0] = (ih==0)? fw: -fw; 
+		break;
+	    case 1:
+	    case 2:
+		fw = 0.0;
+		for (i=jz;i>=0;i--) fw += fq[i]; 
+		y[0] = (ih==0)? fw: -fw; 
+		fw = fq[0]-fw;
+		for (i=1;i<=jz;i++) fw += fq[i];
+		y[1] = (ih==0)? fw: -fw; 
+		break;
+	    case 3:	/* painful */
+		for (i=jz;i>0;i--) {
+		    fw      = fq[i-1]+fq[i]; 
+		    fq[i]  += fq[i-1]-fw;
+		    fq[i-1] = fw;
+		}
+		for (i=jz;i>1;i--) {
+		    fw      = fq[i-1]+fq[i]; 
+		    fq[i]  += fq[i-1]-fw;
+		    fq[i-1] = fw;
+		}
+		for (fw=0.0,i=jz;i>=2;i--) fw += fq[i]; 
+		if(ih==0) {
+		    y[0] =  fq[0]; y[1] =  fq[1]; y[2] =  fw;
+		} else {
+		    y[0] = -fq[0]; y[1] = -fq[1]; y[2] = -fw;
+		}
+	}
+	return n&7;
+}
diff --git a/src/math/kf_sin.c b/src/math/kf_sin.c
new file mode 100644
index 00000000..e81fa0bd
--- /dev/null
+++ b/src/math/kf_sin.c
@@ -0,0 +1,49 @@
+/* kf_sin.c -- float version of k_sin.c
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice 
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+static const float 
+#else
+static float 
+#endif
+half =  5.0000000000e-01,/* 0x3f000000 */
+S1  = -1.6666667163e-01, /* 0xbe2aaaab */
+S2  =  8.3333337680e-03, /* 0x3c088889 */
+S3  = -1.9841270114e-04, /* 0xb9500d01 */
+S4  =  2.7557314297e-06, /* 0x3638ef1b */
+S5  = -2.5050759689e-08, /* 0xb2d72f34 */
+S6  =  1.5896910177e-10; /* 0x2f2ec9d3 */
+
+#ifdef __STDC__
+	float __kernel_sinf(float x, float y, int iy)
+#else
+	float __kernel_sinf(x, y, iy)
+	float x,y; int iy;		/* iy=0 if y is zero */
+#endif
+{
+	float z,r,v;
+	__int32_t ix;
+	GET_FLOAT_WORD(ix,x);
+	ix &= 0x7fffffff;			/* high word of x */
+	if(ix<0x32000000)			/* |x| < 2**-27 */
+	   {if((int)x==0) return x;}		/* generate inexact */
+	z	=  x*x;
+	v	=  z*x;
+	r	=  S2+z*(S3+z*(S4+z*(S5+z*S6)));
+	if(iy==0) return x+v*(S1+z*r);
+	else      return x-((z*(half*y-v*r)-y)-v*S1);
+}
diff --git a/src/math/machine/ieeefp.h b/src/math/machine/ieeefp.h
new file mode 100644
index 00000000..fffa3804
--- /dev/null
+++ b/src/math/machine/ieeefp.h
@@ -0,0 +1,382 @@
+#ifndef __IEEE_BIG_ENDIAN
+#ifndef __IEEE_LITTLE_ENDIAN
+
+/* This file can define macros to choose variations of the IEEE float
+   format:
+
+   _FLT_LARGEST_EXPONENT_IS_NORMAL
+
+	Defined if the float format uses the largest exponent for finite
+	numbers rather than NaN and infinity representations.  Such a
+	format cannot represent NaNs or infinities at all, but it's FLT_MAX
+	is twice the IEEE value.
+
+   _FLT_NO_DENORMALS
+
+	Defined if the float format does not support IEEE denormals.  Every
+	float with a zero exponent is taken to be a zero representation.
+ 
+   ??? At the moment, there are no equivalent macros above for doubles and
+   the macros are not fully supported by --enable-newlib-hw-fp.
+
+   __IEEE_BIG_ENDIAN
+
+        Defined if the float format is big endian.  This is mutually exclusive
+        with __IEEE_LITTLE_ENDIAN.
+
+   __IEEE_LITTLE_ENDIAN
+ 
+        Defined if the float format is little endian.  This is mutually exclusive
+        with __IEEE_BIG_ENDIAN.
+
+   Note that one of __IEEE_BIG_ENDIAN or __IEEE_LITTLE_ENDIAN must be specified for a
+   platform or error will occur.
+
+   __IEEE_BYTES_LITTLE_ENDIAN
+
+        This flag is used in conjunction with __IEEE_BIG_ENDIAN to describe a situation 
+	whereby multiple words of an IEEE floating point are in big endian order, but the
+	words themselves are little endian with respect to the bytes.
+
+   _DOUBLE_IS_32BITS 
+
+        This is used on platforms that support double by using the 32-bit IEEE
+        float type.
+
+   _FLOAT_ARG
+
+        This represents what type a float arg is passed as.  It is used when the type is
+        not promoted to double.
+	
+*/
+
+#if (defined(__arm__) || defined(__thumb__)) && !defined(__MAVERICK__)
+/* ARM traditionally used big-endian words; and within those words the
+   byte ordering was big or little endian depending upon the target.
+   Modern floating-point formats are naturally ordered; in this case
+   __VFP_FP__ will be defined, even if soft-float.  */
+#ifdef __VFP_FP__
+# ifdef __ARMEL__
+#  define __IEEE_LITTLE_ENDIAN
+# else
+#  define __IEEE_BIG_ENDIAN
+# endif
+#else
+# define __IEEE_BIG_ENDIAN
+# ifdef __ARMEL__
+#  define __IEEE_BYTES_LITTLE_ENDIAN
+# endif
+#endif
+#endif
+
+#ifdef __hppa__
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#ifdef __SPU__
+#define __IEEE_BIG_ENDIAN
+
+#define isfinite(__y) \
+	(__extension__ ({int __cy; \
+		(sizeof (__y) == sizeof (float))  ? (1) : \
+		(__cy = fpclassify(__y)) != FP_INFINITE && __cy != FP_NAN;}))
+
+#define isinf(__x) ((sizeof (__x) == sizeof (float))  ?  (0) : __isinfd(__x))
+#define isnan(__x) ((sizeof (__x) == sizeof (float))  ?  (0) : __isnand(__x))
+
+/*
+ * Macros for use in ieeefp.h. We can't just define the real ones here
+ * (like those above) as we have name space issues when this is *not*
+ * included via generic the ieeefp.h.
+ */
+#define __ieeefp_isnanf(x)	0
+#define __ieeefp_isinff(x)	0
+#define __ieeefp_finitef(x)	1
+#endif
+
+#ifdef __sparc__
+#ifdef __LITTLE_ENDIAN_DATA__
+#define __IEEE_LITTLE_ENDIAN
+#else
+#define __IEEE_BIG_ENDIAN
+#endif
+#endif
+
+#if defined(__m68k__) || defined(__mc68000__)
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#if defined(__mc68hc11__) || defined(__mc68hc12__) || defined(__mc68hc1x__)
+#define __IEEE_BIG_ENDIAN
+#ifdef __HAVE_SHORT_DOUBLE__
+# define _DOUBLE_IS_32BITS
+#endif
+#endif
+
+#if defined (__H8300__) || defined (__H8300H__) || defined (__H8300S__) || defined (__H8500__) || defined (__H8300SX__)
+#define __IEEE_BIG_ENDIAN
+#define _FLOAT_ARG float
+#define _DOUBLE_IS_32BITS
+#endif
+
+#if defined (__xc16x__) || defined (__xc16xL__) || defined (__xc16xS__)
+#define __IEEE_LITTLE_ENDIAN
+#define _FLOAT_ARG float
+#define _DOUBLE_IS_32BITS
+#endif
+
+
+#ifdef __sh__
+#ifdef __LITTLE_ENDIAN__
+#define __IEEE_LITTLE_ENDIAN
+#else
+#define __IEEE_BIG_ENDIAN
+#endif
+#if defined(__SH2E__) || defined(__SH3E__) || defined(__SH4_SINGLE_ONLY__) || defined(__SH2A_SINGLE_ONLY__)
+#define _DOUBLE_IS_32BITS
+#endif
+#endif
+
+#ifdef _AM29K
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#ifdef _WIN32
+#define __IEEE_LITTLE_ENDIAN
+#endif
+
+#ifdef __i386__
+#define __IEEE_LITTLE_ENDIAN
+#endif
+
+#ifdef __i960__
+#define __IEEE_LITTLE_ENDIAN
+#endif
+
+#ifdef __lm32__
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#ifdef __M32R__
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#if defined(_C4x) || defined(_C3x)
+#define __IEEE_BIG_ENDIAN
+#define _DOUBLE_IS_32BITS
+#endif
+
+#ifdef __TMS320C6X__
+#ifdef _BIG_ENDIAN
+#define __IEEE_BIG_ENDIAN
+#else
+#define __IEEE_LITTLE_ENDIAN
+#endif
+#endif
+
+#ifdef __TIC80__
+#define __IEEE_LITTLE_ENDIAN
+#endif
+
+#ifdef __MIPSEL__
+#define __IEEE_LITTLE_ENDIAN
+#endif
+#ifdef __MIPSEB__
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#ifdef __MMIX__
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#ifdef __D30V__
+#define __IEEE_BIG_ENDIAN
+#endif
+
+/* necv70 was __IEEE_LITTLE_ENDIAN. */
+
+#ifdef __W65__
+#define __IEEE_LITTLE_ENDIAN
+#define _DOUBLE_IS_32BITS
+#endif
+
+#if defined(__Z8001__) || defined(__Z8002__)
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#ifdef __m88k__
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#ifdef __mn10300__
+#define __IEEE_LITTLE_ENDIAN
+#endif
+
+#ifdef __mn10200__
+#define __IEEE_LITTLE_ENDIAN
+#define _DOUBLE_IS_32BITS
+#endif
+
+#ifdef __v800
+#define __IEEE_LITTLE_ENDIAN
+#endif
+
+#ifdef __v850
+#define __IEEE_LITTLE_ENDIAN
+#endif
+
+#ifdef __D10V__
+#define __IEEE_BIG_ENDIAN
+#if __DOUBLE__ == 32
+#define _DOUBLE_IS_32BITS
+#endif
+#endif
+
+#ifdef __PPC__
+#if (defined(_BIG_ENDIAN) && _BIG_ENDIAN) || (defined(_AIX) && _AIX)
+#define __IEEE_BIG_ENDIAN
+#else
+#if (defined(_LITTLE_ENDIAN) && _LITTLE_ENDIAN) || (defined(__sun__) && __sun__) || (defined(_WIN32) && _WIN32)
+#define __IEEE_LITTLE_ENDIAN
+#endif
+#endif
+#endif
+
+#ifdef __xstormy16__
+#define __IEEE_LITTLE_ENDIAN
+#endif
+
+#ifdef __arc__
+#ifdef __big_endian__
+#define __IEEE_BIG_ENDIAN
+#else
+#define __IEEE_LITTLE_ENDIAN
+#endif
+#endif
+
+#ifdef __CRX__
+#define __IEEE_LITTLE_ENDIAN
+#endif
+
+#ifdef __fr30__
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#ifdef __mcore__
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#ifdef __mt__
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#ifdef __frv__
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#ifdef __moxie__
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#ifdef __ia64__
+#ifdef __BIG_ENDIAN__
+#define __IEEE_BIG_ENDIAN
+#else
+#define __IEEE_LITTLE_ENDIAN
+#endif
+#endif
+
+#ifdef __AVR__
+#define __IEEE_LITTLE_ENDIAN
+#define _DOUBLE_IS_32BITS
+#endif
+
+#if defined(__or32__) || defined(__or1k__) || defined(__or16__)
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#ifdef __IP2K__
+#define __IEEE_BIG_ENDIAN
+#define __SMALL_BITFIELDS
+#define _DOUBLE_IS_32BITS
+#endif
+
+#ifdef __iq2000__
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#ifdef __MAVERICK__
+#ifdef __ARMEL__
+#  define __IEEE_LITTLE_ENDIAN
+#else  /* must be __ARMEB__ */
+#  define __IEEE_BIG_ENDIAN
+#endif /* __ARMEL__ */
+#endif /* __MAVERICK__ */
+
+#ifdef __m32c__
+#define __IEEE_LITTLE_ENDIAN
+#define __SMALL_BITFIELDS
+#endif
+
+#ifdef __CRIS__
+#define __IEEE_LITTLE_ENDIAN
+#endif
+
+#ifdef __BFIN__
+#define __IEEE_LITTLE_ENDIAN
+#endif
+
+#ifdef __x86_64__
+#define __IEEE_LITTLE_ENDIAN
+#endif
+
+#ifdef __mep__
+#ifdef __LITTLE_ENDIAN__
+#define __IEEE_LITTLE_ENDIAN
+#else
+#define __IEEE_BIG_ENDIAN
+#endif
+#endif
+
+#ifdef __MICROBLAZE__
+#define __IEEE_BIG_ENDIAN
+#endif
+
+#ifdef __RL78__
+#define __IEEE_LITTLE_ENDIAN
+#define __SMALL_BITFIELDS	/* 16 Bit INT */
+#define _DOUBLE_IS_32BITS
+#endif
+
+#ifdef __RX__
+
+#ifdef __RX_BIG_ENDIAN__
+#define __IEEE_BIG_ENDIAN
+#else
+#define __IEEE_LITTLE_ENDIAN
+#endif
+
+#ifndef __RX_64BIT_DOUBLES__
+#define _DOUBLE_IS_32BITS
+#endif
+
+#ifdef __RX_16BIT_INTS__
+#define __SMALL_BITFIELDS
+#endif
+
+#endif
+
+#if (defined(__CR16__) || defined(__CR16C__) ||defined(__CR16CP__))
+#define __IEEE_LITTLE_ENDIAN
+#define __SMALL_BITFIELDS	/* 16 Bit INT */
+#endif
+
+#ifndef __IEEE_BIG_ENDIAN
+#ifndef __IEEE_LITTLE_ENDIAN
+#error Endianess not declared!!
+#endif /* not __IEEE_LITTLE_ENDIAN */
+#endif /* not __IEEE_BIG_ENDIAN */
+
+#endif /* not __IEEE_LITTLE_ENDIAN */
+#endif /* not __IEEE_BIG_ENDIAN */
+
diff --git a/src/math/math.h b/src/math/math.h
new file mode 100644
index 00000000..fd543bc2
--- /dev/null
+++ b/src/math/math.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _MATH_H_
+#define _MATH_H_
+
+float acosf(float x);
+
+float cosf(float x);
+
+float sinf(float x);
+
+float sqrtf(float x);
+
+float fabsf(float x);
+
+float floorf(float x);
+
+float scalbnf(float x, int n);
+
+float copysignf(float x, float y);
+
+#endif
diff --git a/src/math/sf_copysign.c b/src/math/sf_copysign.c
new file mode 100644
index 00000000..f547c82e
--- /dev/null
+++ b/src/math/sf_copysign.c
@@ -0,0 +1,50 @@
+/* sf_copysign.c -- float version of s_copysign.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice 
+ * is preserved.
+ * ====================================================
+ */
+
+/*
+ * copysignf(float x, float y)
+ * copysignf(x,y) returns a value with the magnitude of x and
+ * with the sign bit of y.
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+	float copysignf(float x, float y)
+#else
+	float copysignf(x,y)
+	float x,y;
+#endif
+{
+	__uint32_t ix,iy;
+	GET_FLOAT_WORD(ix,x);
+	GET_FLOAT_WORD(iy,y);
+	SET_FLOAT_WORD(x,(ix&0x7fffffff)|(iy&0x80000000));
+        return x;
+}
+
+#ifdef _DOUBLE_IS_32BITS
+
+#ifdef __STDC__
+	double copysign(double x, double y)
+#else
+	double copysign(x,y)
+	double x,y;
+#endif
+{
+	return (double) copysignf((float) x, (float) y);
+}
+
+#endif /* defined(_DOUBLE_IS_32BITS) */
diff --git a/src/math/sf_cos.c b/src/math/sf_cos.c
new file mode 100644
index 00000000..4c0a9a53
--- /dev/null
+++ b/src/math/sf_cos.c
@@ -0,0 +1,68 @@
+/* sf_cos.c -- float version of s_cos.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice 
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+static const float one=1.0;
+#else
+static float one=1.0;
+#endif
+
+#ifdef __STDC__
+	float cosf(float x)
+#else
+	float cosf(x)
+	float x;
+#endif
+{
+	float y[2],z=0.0;
+	__int32_t n,ix;
+
+	GET_FLOAT_WORD(ix,x);
+
+    /* |x| ~< pi/4 */
+	ix &= 0x7fffffff;
+	if(ix <= 0x3f490fd8) return __kernel_cosf(x,z);
+
+    /* cos(Inf or NaN) is NaN */
+	else if (!FLT_UWORD_IS_FINITE(ix)) return x-x;
+
+    /* argument reduction needed */
+	else {
+	    n = __ieee754_rem_pio2f(x,y);
+	    switch(n&3) {
+		case 0: return  __kernel_cosf(y[0],y[1]);
+		case 1: return -__kernel_sinf(y[0],y[1],1);
+		case 2: return -__kernel_cosf(y[0],y[1]);
+		default:
+		        return  __kernel_sinf(y[0],y[1],1);
+	    }
+	}
+}
+
+#ifdef _DOUBLE_IS_32BITS
+
+#ifdef __STDC__
+	double cos(double x)
+#else
+	double cos(x)
+	double x;
+#endif
+{
+	return (double) cosf((float) x);
+}
+
+#endif /* defined(_DOUBLE_IS_32BITS) */
diff --git a/src/math/sf_fabs.c b/src/math/sf_fabs.c
new file mode 100644
index 00000000..2aaed326
--- /dev/null
+++ b/src/math/sf_fabs.c
@@ -0,0 +1,47 @@
+/* sf_fabs.c -- float version of s_fabs.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice 
+ * is preserved.
+ * ====================================================
+ */
+
+/*
+ * fabsf(x) returns the absolute value of x.
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+	float fabsf(float x)
+#else
+	float fabsf(x)
+	float x;
+#endif
+{
+	__uint32_t ix;
+	GET_FLOAT_WORD(ix,x);
+	SET_FLOAT_WORD(x,ix&0x7fffffff);
+        return x;
+}
+
+#ifdef _DOUBLE_IS_32BITS
+
+#ifdef __STDC__
+	double fabs(double x)
+#else
+	double fabs(x)
+	double x;
+#endif
+{
+	return (double) fabsf((float) x);
+}
+
+#endif /* defined(_DOUBLE_IS_32BITS) */
diff --git a/src/math/sf_floor.c b/src/math/sf_floor.c
new file mode 100644
index 00000000..9264d81e
--- /dev/null
+++ b/src/math/sf_floor.c
@@ -0,0 +1,80 @@
+/* sf_floor.c -- float version of s_floor.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice 
+ * is preserved.
+ * ====================================================
+ */
+
+/*
+ * floorf(x)
+ * Return x rounded toward -inf to integral value
+ * Method:
+ *	Bit twiddling.
+ * Exception:
+ *	Inexact flag raised if x not equal to floorf(x).
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+static const float huge = 1.0e30;
+#else
+static float huge = 1.0e30;
+#endif
+
+#ifdef __STDC__
+	float floorf(float x)
+#else
+	float floorf(x)
+	float x;
+#endif
+{
+	__int32_t i0,j0;
+	__uint32_t i,ix;
+	GET_FLOAT_WORD(i0,x);
+	ix = (i0&0x7fffffff);
+	j0 = (ix>>23)-0x7f;
+	if(j0<23) {
+	    if(j0<0) { 	/* raise inexact if x != 0 */
+		if(huge+x>(float)0.0) {/* return 0*sign(x) if |x|<1 */
+		    if(i0>=0) {i0=0;} 
+		    else if(!FLT_UWORD_IS_ZERO(ix))
+			{ i0=0xbf800000;}
+		}
+	    } else {
+		i = (0x007fffff)>>j0;
+		if((i0&i)==0) return x; /* x is integral */
+		if(huge+x>(float)0.0) {	/* raise inexact flag */
+		    if(i0<0) i0 += (0x00800000)>>j0;
+		    i0 &= (~i);
+		}
+	    }
+	} else {
+	    if(!FLT_UWORD_IS_FINITE(ix)) return x+x;	/* inf or NaN */
+	    else return x;		/* x is integral */
+	}
+	SET_FLOAT_WORD(x,i0);
+	return x;
+}
+
+#ifdef _DOUBLE_IS_32BITS
+
+#ifdef __STDC__
+	double floor(double x)
+#else
+	double floor(x)
+	double x;
+#endif
+{
+	return (double) floorf((float) x);
+}
+
+#endif /* defined(_DOUBLE_IS_32BITS) */
diff --git a/src/math/sf_scalbn.c b/src/math/sf_scalbn.c
new file mode 100644
index 00000000..70006001
--- /dev/null
+++ b/src/math/sf_scalbn.c
@@ -0,0 +1,86 @@
+/* sf_scalbn.c -- float version of s_scalbn.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice 
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+#include <limits.h>
+#include <float.h>
+
+#if INT_MAX > 50000
+#define OVERFLOW_INT 50000
+#else
+#define OVERFLOW_INT 30000
+#endif
+
+#ifdef __STDC__
+static const float
+#else
+static float
+#endif
+two25   =  3.355443200e+07,	/* 0x4c000000 */
+twom25  =  2.9802322388e-08,	/* 0x33000000 */
+huge   = 1.0e+30,
+tiny   = 1.0e-30;
+
+#ifdef __STDC__
+	float scalbnf (float x, int n)
+#else
+	float scalbnf (x,n)
+	float x; int n;
+#endif
+{
+	__int32_t  k,ix;
+	__uint32_t hx;
+
+	GET_FLOAT_WORD(ix,x);
+	hx = ix&0x7fffffff;
+        k = hx>>23;		/* extract exponent */
+	if (FLT_UWORD_IS_ZERO(hx))
+	    return x;
+        if (!FLT_UWORD_IS_FINITE(hx))
+	    return x+x;		/* NaN or Inf */
+        if (FLT_UWORD_IS_SUBNORMAL(hx)) {
+	    x *= two25;
+	    GET_FLOAT_WORD(ix,x);
+	    k = ((ix&0x7f800000)>>23) - 25; 
+            if (n< -50000) return tiny*x; 	/*underflow*/
+        }
+        k = k+n; 
+        if (k > FLT_LARGEST_EXP) return huge*copysignf(huge,x); /* overflow  */
+        if (k > 0) 				/* normal result */
+	    {SET_FLOAT_WORD(x,(ix&0x807fffff)|(k<<23)); return x;}
+        if (k < FLT_SMALLEST_EXP) {
+            if (n > OVERFLOW_INT) 	/* in case integer overflow in n+k */
+		return huge*copysignf(huge,x);	/*overflow*/
+	    else return tiny*copysignf(tiny,x);	/*underflow*/
+        }
+        k += 25;				/* subnormal result */
+	SET_FLOAT_WORD(x,(ix&0x807fffff)|(k<<23));
+        return x*twom25;
+}
+
+#ifdef _DOUBLE_IS_32BITS
+
+#ifdef __STDC__
+	double scalbn(double x, int n)
+#else
+	double scalbn(x,n)
+	double x;
+	int n;
+#endif
+{
+	return (double) scalbnf((float) x, n);
+}
+
+#endif /* defined(_DOUBLE_IS_32BITS) */
diff --git a/src/math/sf_sin.c b/src/math/sf_sin.c
new file mode 100644
index 00000000..da81845d
--- /dev/null
+++ b/src/math/sf_sin.c
@@ -0,0 +1,62 @@
+/* sf_sin.c -- float version of s_sin.c.
+ * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com.
+ */
+
+/*
+ * ====================================================
+ * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
+ *
+ * Developed at SunPro, a Sun Microsystems, Inc. business.
+ * Permission to use, copy, modify, and distribute this
+ * software is freely granted, provided that this notice 
+ * is preserved.
+ * ====================================================
+ */
+
+#include "fdlibm.h"
+
+#ifdef __STDC__
+	float sinf(float x)
+#else
+	float sinf(x)
+	float x;
+#endif
+{
+	float y[2],z=0.0;
+	__int32_t n,ix;
+
+	GET_FLOAT_WORD(ix,x);
+
+    /* |x| ~< pi/4 */
+	ix &= 0x7fffffff;
+	if(ix <= 0x3f490fd8) return __kernel_sinf(x,z,0);
+
+    /* sin(Inf or NaN) is NaN */
+	else if (!FLT_UWORD_IS_FINITE(ix)) return x-x;
+
+    /* argument reduction needed */
+	else {
+	    n = __ieee754_rem_pio2f(x,y);
+	    switch(n&3) {
+		case 0: return  __kernel_sinf(y[0],y[1],1);
+		case 1: return  __kernel_cosf(y[0],y[1]);
+		case 2: return -__kernel_sinf(y[0],y[1],1);
+		default:
+			return -__kernel_cosf(y[0],y[1]);
+	    }
+	}
+}
+
+#ifdef _DOUBLE_IS_32BITS
+
+#ifdef __STDC__
+	double sin(double x)
+#else
+	double sin(x)
+	double x;
+#endif
+{
+	return (double) sinf((float) x);
+}
+
+#endif /* defined(_DOUBLE_IS_32BITS) */
diff --git a/src/megadongle-v0.1/Makefile b/src/megadongle-v0.1/Makefile
index 7f12963f..6035c348 100644
--- a/src/megadongle-v0.1/Makefile
+++ b/src/megadongle-v0.1/Makefile
@@ -62,14 +62,15 @@ CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STA
 
 PROGNAME=megadongle-v0.1
 PROG=$(PROGNAME)-$(VERSION).elf
+HEX=$(PROGNAME)-$(VERSION).ihx
 
 SRC=$(ALTOS_SRC) ao_megadongle.c
 OBJ=$(SRC:.c=.o)
 
-all: $(PROG)
+all: $(PROG) $(HEX)
 
 $(PROG): Makefile $(OBJ) altos.ld
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS)
 
 $(OBJ): $(INC)
 
@@ -79,7 +80,7 @@ ao_product.h: ao-make-product.5c ../Version
 distclean:	clean
 
 clean:
-	rm -f *.o $(PROGNAME)-*.elf
+	rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx
 	rm -f ao_product.h
 
 install:
diff --git a/src/micropeak/Makefile b/src/micropeak/Makefile
index 315b93f6..dcc32874 100644
--- a/src/micropeak/Makefile
+++ b/src/micropeak/Makefile
@@ -2,23 +2,19 @@
 # Tiny AltOS build
 #
 #
-vpath % ../attiny:../drivers:../core:..
+vpath % ../attiny:../drivers:../core:../product:..
 vpath ao-make-product.5c ../util
 vpath make-altitude-pa ../util
 
+include ../avr/Makefile.defs
+
 MCU=attiny85
 DUDECPUTYPE=t85
 #PROGRAMMER=stk500v2 -P usb
-PROGRAMMER=usbtiny
-LOADCMD=avrdude
 LOADSLOW=-i 32 -B 32
 LOADARG=-p $(DUDECPUTYPE) -c $(PROGRAMMER) -e -U flash:w:
-CC=avr-gcc
-OBJCOPY=avr-objcopy
 
-ifndef VERSION
-include ../Version
-endif
+#LDFLAGS=-L$(LDSCRIPTS) -Tavr25.x
 
 ALTOS_SRC = \
 	ao_micropeak.c \
@@ -28,7 +24,7 @@ ALTOS_SRC = \
 	ao_ms5607.c \
 	ao_exti.c \
 	ao_convert_pa.c \
-	ao_report_tiny.c \
+	ao_report_micro.c \
 	ao_notask.c \
 	ao_eeprom_tiny.c \
 	ao_panic.c \
@@ -51,7 +47,7 @@ INC=\
 IDPRODUCT=0
 PRODUCT=MicroPeak-v0.1
 PRODUCT_DEF=-DMICROPEAK
-CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../core -I.. -I../drivers
+CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../core -I.. -I../drivers -I../product
 CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -O2 -mcall-prologues -DATTINY
 
 NICKLE=nickle
diff --git a/src/nanopeak-v0.1/.gitignore b/src/nanopeak-v0.1/.gitignore
new file mode 100644
index 00000000..27cd0a7c
--- /dev/null
+++ b/src/nanopeak-v0.1/.gitignore
@@ -0,0 +1,2 @@
+ao_product.h
+nanopeak-*
diff --git a/src/nanopeak-v0.1/Makefile b/src/nanopeak-v0.1/Makefile
new file mode 100644
index 00000000..04eea902
--- /dev/null
+++ b/src/nanopeak-v0.1/Makefile
@@ -0,0 +1,108 @@
+#
+# Tiny AltOS build
+#
+#
+vpath % ../attiny:../drivers:../core:../product:..
+vpath ao-make-product.5c ../util
+vpath make-altitude-pa ../util
+
+include ../avr/Makefile.defs
+
+MCU=attiny85
+DUDECPUTYPE=t85
+#PROGRAMMER=stk500v2 -P usb
+LOADSLOW=-i 32 -B 32
+LOADARG=-p $(DUDECPUTYPE) -c $(PROGRAMMER) -e -U flash:w:
+
+#LDFLAGS=-L$(LDSCRIPTS) -Tavr25.x
+
+ALTOS_SRC = \
+	ao_micropeak.c \
+	ao_spi_attiny.c \
+	ao_led.c \
+	ao_clock.c \
+	ao_ms5607.c \
+	ao_exti.c \
+	ao_convert_pa.c \
+	ao_report_micro.c \
+	ao_notask.c \
+	ao_eeprom_tiny.c \
+	ao_panic.c \
+	ao_log_micro.c \
+	ao_async.c \
+	ao_microflight.c \
+	ao_microkalman.c
+
+INC=\
+	ao.h \
+	ao_pins.h \
+	ao_arch.h \
+	ao_arch_funcs.h \
+	ao_exti.h \
+	ao_ms5607.h \
+	ao_log_micro.h \
+	ao_micropeak.h \
+	altitude-pa.h
+
+IDPRODUCT=0
+PRODUCT=NanoPeak-v0.1
+PRODUCT_DEF=-DNANOPEAK
+CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../core -I.. -I../drivers -I../product
+CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -O2 -mcall-prologues -DATTINY
+
+NICKLE=nickle
+
+PROG=nanopeak-v0.1
+
+SRC=$(ALTOS_SRC)
+OBJ=$(SRC:.c=.o)
+
+V=0
+# The user has explicitly enabled quiet compilation.
+ifeq ($(V),0)
+quiet = @printf "  $1 $2 $@\n"; $($1)
+endif
+# Otherwise, print the full command line.
+quiet ?= $($1)
+
+all: $(PROG) $(PROG).hex
+
+CHECK=sh ../util/check-avr-mem
+
+$(PROG): Makefile $(OBJ)
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ)
+	$(call quiet,CHECK) $(PROG) || ($(RM) -f $(PROG); exit 1)
+
+$(PROG).hex: $(PROG)
+	avr-size $(PROG)
+	$(OBJCOPY) -R .eeprom -O ihex $(PROG) $@
+
+
+load: $(PROG).hex
+	$(LOADCMD) $(LOADARG)$(PROG).hex
+
+load-slow: $(PROG).hex
+	$(LOADCMD) $(LOADSLOW) $(LOADARG)$(PROG).hex
+
+ao_product.h: ao-make-product.5c ../Version
+	$(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@
+
+ao_product.o: ao_product.c ao_product.h
+
+%.o : %.c $(INC)
+	$(call quiet,CC) -c $(CFLAGS) $<
+
+distclean:	clean
+
+clean:
+	rm -f *.o $(PROG) $(PROG).hex
+	rm -f ao_product.h
+
+../altitude-pa.h: make-altitude-pa
+	nickle $< > $@
+
+install:
+
+uninstall:
+
+$(OBJ): ao_product.h $(INC)
diff --git a/src/nanopeak-v0.1/ao_pins.h b/src/nanopeak-v0.1/ao_pins.h
new file mode 100644
index 00000000..bd4a06d1
--- /dev/null
+++ b/src/nanopeak-v0.1/ao_pins.h
@@ -0,0 +1,65 @@
+/*
+ * Copyright © 2011 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_PINS_H_
+#define _AO_PINS_H_
+#include <avr/pgmspace.h>
+
+#define AO_LED_ORANGE		(1<<3)
+#define AO_LED_SERIAL		3
+#define AO_LED_PANIC		AO_LED_ORANGE
+#define AO_LED_REPORT		AO_LED_ORANGE
+#define LEDS_AVAILABLE		(AO_LED_ORANGE)
+#define USE_SERIAL_1_STDIN	0
+#define HAS_USB			0
+#define PACKET_HAS_SLAVE	0
+#define HAS_SERIAL_1		0
+#define HAS_TASK		0
+#define HAS_MS5607		1
+#define HAS_MS5611		0
+#define HAS_EEPROM		0
+#define HAS_BEEP		0
+#define AVR_CLOCK		250000UL
+
+/* SPI */
+#define SPI_PORT		PORTB
+#define SPI_PIN			PINB
+#define SPI_DIR			DDRB
+#define AO_MS5607_CS_PORT	PORTB
+#define AO_MS5607_CS_PIN	4
+
+/* MS5607 */
+#define AO_MS5607_SPI_INDEX	0
+#define AO_MS5607_MISO_PORT	PORTB
+#define AO_MS5607_MISO_PIN	0
+#define AO_MS5607_BARO_OVERSAMPLE	4096
+#define AO_MS5607_TEMP_OVERSAMPLE	1024
+
+/* I2C */
+#define I2C_PORT		PORTB
+#define I2C_PIN			PINB
+#define I2C_DIR			DDRB
+#define I2C_PIN_SCL		PINB2
+#define I2C_PIN_SDA		PINB0
+
+#define AO_CONST_ATTRIB		PROGMEM
+typedef int32_t alt_t;
+#define FETCH_ALT(o)		((alt_t) pgm_read_dword(&altitude_table[o]))
+
+#define AO_ALT_VALUE(x)		((x) * (alt_t) 10)
+
+#endif /* _AO_PINS_H_ */
diff --git a/src/product/Makefile.teledongle b/src/product/Makefile.teledongle
index 3101b777..da9bcba0 100644
--- a/src/product/Makefile.teledongle
+++ b/src/product/Makefile.teledongle
@@ -80,9 +80,9 @@ endif
 # Otherwise, print the full command line.
 quiet ?= $($1)
 
-all: ../$(PROG)
+all: $(PROG)
 
-../$(PROG): $(REL) Makefile
+$(PROG): $(REL) Makefile
 	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) ..
 	$(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@
 
diff --git a/src/product/Makefile.telelaunch b/src/product/Makefile.telelaunch
index 1e55989c..a5e2eb7f 100644
--- a/src/product/Makefile.telelaunch
+++ b/src/product/Makefile.telelaunch
@@ -82,9 +82,9 @@ endif
 # Otherwise, print the full command line.
 quiet ?= $($1)
 
-all: ../$(PROG)
+all: $(PROG)
 
-../$(PROG): $(REL) Makefile
+$(PROG): $(REL) Makefile
 	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) ..
 	$(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM)  || rm $@
 
diff --git a/src/product/Makefile.telemetrum b/src/product/Makefile.telemetrum
index 5e3eed7f..c740a483 100644
--- a/src/product/Makefile.telemetrum
+++ b/src/product/Makefile.telemetrum
@@ -94,10 +94,10 @@ endif
 # Otherwise, print the full command line.
 quiet ?= $($1)
 
-all: ../$(PROG)
+all: $(PROG)
 
-../$(PROG): $(REL) Makefile
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) ..
+$(PROG): $(REL) Makefile
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL)
 	$(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@
 
 ao_product.h: ao-make-product.5c ../Version
diff --git a/src/product/Makefile.telemini b/src/product/Makefile.telemini
index ef8906ba..0884079e 100644
--- a/src/product/Makefile.telemini
+++ b/src/product/Makefile.telemini
@@ -83,10 +83,10 @@ endif
 # Otherwise, print the full command line.
 quiet ?= $($1)
 
-all: ../$(PROG)
+all: $(PROG)
 
-../$(PROG): $(REL) Makefile
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) ..
+$(PROG): $(REL) Makefile
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL)
 	$(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@
 
 ao_product.h: ao-make-product.5c ../Version
diff --git a/src/product/Makefile.telenano b/src/product/Makefile.telenano
index 67410ae0..c31989ee 100644
--- a/src/product/Makefile.telenano
+++ b/src/product/Makefile.telenano
@@ -82,9 +82,9 @@ endif
 # Otherwise, print the full command line.
 quiet ?= $($1)
 
-all: ../$(PROG)
+all: $(PROG)
 
-../$(PROG): $(REL) Makefile
+$(PROG): $(REL) Makefile
 	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) ..
 	$(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@
 
diff --git a/src/product/ao_flash_pins.h b/src/product/ao_flash_pins.h
index b774df6d..dd67d820 100644
--- a/src/product/ao_flash_pins.h
+++ b/src/product/ao_flash_pins.h
@@ -35,6 +35,7 @@
 #define HAS_VERSION		0
 
 #define AO_BOOT_CHAIN		1
-#define AO_BOOT_PIN		1
+
+#define IS_FLASH_LOADER		1
 
 #endif /* _AO_FLASH_PINS_H_ */
diff --git a/src/product/ao_flash_task.c b/src/product/ao_flash_task.c
index fdc4d0aa..4cfbf75f 100644
--- a/src/product/ao_flash_task.c
+++ b/src/product/ao_flash_task.c
@@ -73,7 +73,7 @@ static void
 ao_block_erase(void)
 {
 	uint32_t	addr = ao_get_hex32();
-	uint32_t	*p = (uint32_t *) addr;
+	void		*p = (void *) addr;
 
 	ao_flash_erase_page(p);
 }
@@ -82,11 +82,8 @@ static void
 ao_block_write(void)
 {
 	uint32_t	addr = ao_get_hex32();
-	uint32_t	*p = (uint32_t *) addr;
-	union {
-		uint8_t		data8[256];
-		uint32_t	data32[64];
-	} u;
+	void		*p = (void *) addr;
+	uint8_t		data[256];
 	uint16_t	i;
 
 	if (addr < (uint32_t) AO_BOOT_APPLICATION_BASE) {
@@ -94,8 +91,8 @@ ao_block_write(void)
 		return;
 	}
 	for (i = 0; i < 256; i++)
-		u.data8[i] = ao_usb_getchar();
-	ao_flash_page(p, u.data32);
+		data[i] = ao_usb_getchar();
+	ao_flash_page(p, (void *) data);
 }
 
 static void
diff --git a/src/micropeak/ao_micropeak.c b/src/product/ao_micropeak.c
index 10f0d192..10f0d192 100644
--- a/src/micropeak/ao_micropeak.c
+++ b/src/product/ao_micropeak.c
diff --git a/src/micropeak/ao_micropeak.h b/src/product/ao_micropeak.h
index 382b98d9..0cefca6f 100644
--- a/src/micropeak/ao_micropeak.h
+++ b/src/product/ao_micropeak.h
@@ -20,15 +20,15 @@
 
 #define SAMPLE_SLEEP		AO_MS_TO_TICKS(96)
 
-/* 16 sample, or about two seconds worth */
-#define GROUND_AVG_SHIFT	4
+/* 64 sample, or about six seconds worth */
+#define GROUND_AVG_SHIFT	6
 #define GROUND_AVG		(1 << GROUND_AVG_SHIFT)
 
 /* Pressure change (in Pa) to detect boost */
-#define BOOST_DETECT		120	/* 10m at sea level, 12m at 2000m */
+#define BOOST_DETECT		360	/* 30m at sea level, 36m at 2000m */
 
 /* Wait after power on before doing anything to give the user time to assemble the rocket */
-#define BOOST_DELAY		AO_SEC_TO_TICKS(30)
+#define BOOST_DELAY		AO_SEC_TO_TICKS(60)
 
 /* Pressure change (in Pa) to detect landing */
 #define LAND_DETECT		24	/* 2m at sea level, 2.4m at 2000m */
diff --git a/src/product/ao_terraui.c b/src/product/ao_terraui.c
index 1866eb0c..8fd97033 100644
--- a/src/product/ao_terraui.c
+++ b/src/product/ao_terraui.c
@@ -629,7 +629,7 @@ ao_terragps(void)
 
 	for (;;) {
 		while (ao_gps_tick == gps_tick)
-			ao_sleep(&ao_gps_data);
+			ao_sleep(&ao_gps_new);
 		gps_tick = ao_gps_tick;
 		ao_gps_progress = (ao_gps_progress + 1) & 3;
 	}
diff --git a/src/spiradio-v0.1/Makefile b/src/spiradio-v0.1/Makefile
index a207d34f..e644bc49 100644
--- a/src/spiradio-v0.1/Makefile
+++ b/src/spiradio-v0.1/Makefile
@@ -73,10 +73,10 @@ endif
 # Otherwise, print the full command line.
 quiet ?= $($1)
 
-all: ../$(PROG)
+all: $(PROG)
 
-../$(PROG): $(REL) Makefile
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) ..
+$(PROG): $(REL) Makefile
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL)
 	$(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM)  || rm $@
 
 ao_product.h: ao-make-product.5c ../Version
diff --git a/src/stm-bringup/Makefile b/src/stm-bringup/Makefile
index 1bc5aaad..797df2d6 100644
--- a/src/stm-bringup/Makefile
+++ b/src/stm-bringup/Makefile
@@ -4,15 +4,15 @@ vpath ao-make-product.5c ../util
 ifndef VERSION
 include ../Version
 endif
+TOPDIR=..
+include $(TOPDIR)/Makedefs
 
 CC=arm-none-eabi-gcc
 OBJCOPY=arm-none-eabi-objcopy
 
-PDCLIB=/opt/cortex
-C_LIB=$(PDCLIB)/lib/pdclib-cortex-m3.a
-C_INC=-I$(PDCLIB)/include
+C_LIB=$(PDCLIB_LIBS_M3)
 
-DEF_CFLAGS=-g -std=gnu99 -Os -mlittle-endian -mthumb -ffreestanding -nostdlib -I. -I../../src/stm $(C_INC)
+DEF_CFLAGS=-g -std=gnu99 -Os -mlittle-endian -mthumb -ffreestanding -nostdlib -I. -I../stm $(PDCLIB_INCLUDES)
 
 # to run from SRAM
 LD_FLAGS_RAM=-L../stm -Wl,-Taltos-ram.ld
@@ -28,10 +28,10 @@ all: bringup-ram.elf bringup.elf
 %.bin: %.elf
 	$(OBJCOPY) -O binary $^ $@
 
-bringup.elf: $(OBJ) $(C_LIB) bringup.ld
+bringup.elf: $(OBJ) bringup.ld
 	$(CC) $(CFLAGS) $(LD_FLAGS) -o $@ $(OBJ) $(C_LIB) -lgcc
 
-bringup-ram.elf: $(OBJ) $(C_LIB) altos-ram.ld
+bringup-ram.elf: $(OBJ) altos-ram.ld
 	$(CC) $(CFLAGS) $(LD_FLAGS_RAM) -o $@ $(OBJ) $(C_LIB) -lgcc
 
 clean:
diff --git a/src/stm-demo/Makefile b/src/stm-demo/Makefile
index d1f825db..98fcd9e5 100644
--- a/src/stm-demo/Makefile
+++ b/src/stm-demo/Makefile
@@ -36,10 +36,7 @@ ALTOS_SRC = \
 	ao_data.c \
 	ao_i2c_stm.c \
 	ao_usb_stm.c \
-	ao_exti_stm.c \
-	ao_event.c \
-	ao_quadrature.c \
-	ao_button.c
+	ao_exti_stm.c
 
 PRODUCT=StmDemo-v0.0
 PRODUCT_DEF=-DSTM_DEMO
@@ -59,7 +56,7 @@ all: $(ELF) $(IHX)
 LDFLAGS=-L../stm -Wl,-Taltos.ld
 
 $(ELF): Makefile $(OBJ)
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $@ $(OBJ) $(SAT_CLIB) -lgcc
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $@ $(OBJ) $(LIBS)
 
 ao_product.h: ao-make-product.5c ../Version
 	$(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@
@@ -69,7 +66,7 @@ $(OBJ): $(INC)
 distclean:	clean
 
 clean:
-	rm -f *.o $(PROG)
+	rm -f *.o *.elf *.ihx
 	rm -f ao_product.h
 
 install:
diff --git a/src/stm-demo/ao_demo.c b/src/stm-demo/ao_demo.c
index 5677cdf4..58cf651b 100644
--- a/src/stm-demo/ao_demo.c
+++ b/src/stm-demo/ao_demo.c
@@ -153,6 +153,7 @@ ao_temp (void)
 	printf ("temp: %d\n", temp);
 }
 
+#if 0
 static void
 ao_event(void)
 {
@@ -168,6 +169,7 @@ ao_event(void)
 	}
 
 }
+#endif
 
 __code struct ao_cmds ao_demo_cmds[] = {
 	{ ao_dma_test,	"D\0DMA test" },
@@ -175,7 +177,7 @@ __code struct ao_cmds ao_demo_cmds[] = {
 	{ ao_spi_read, "R\0SPI read" },
 	{ ao_i2c_write, "i\0I2C write" },
 	{ ao_temp, "t\0Show temp" },
-	{ ao_event, "e\0Monitor event queue" },
+/*	{ ao_event, "e\0Monitor event queue" }, */
 	{ 0, NULL }
 };
 
diff --git a/src/stm-flash/Makefile b/src/stm-flash/Makefile
index 1ea35581..5c0699e1 100644
--- a/src/stm-flash/Makefile
+++ b/src/stm-flash/Makefile
@@ -41,7 +41,7 @@ all: $(PROG)
 LDFLAGS=-L../stm -Wl,-Taltos-loader.ld
 
 $(PROG): Makefile $(OBJ)
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS)
 
 ao_product.h: ao-make-product.5c ../Version
 	$(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@
diff --git a/src/stm/Makefile-flash.defs b/src/stm/Makefile-flash.defs
index 016bb7e7..3890eff1 100644
--- a/src/stm/Makefile-flash.defs
+++ b/src/stm/Makefile-flash.defs
@@ -6,16 +6,15 @@ vpath ao-make-product.5c $(TOPDIR)/util
 .elf.ihx:
 	objcopy -O ihex $*.elf $@
 
-CC=arm-none-eabi-gcc
-SAT=/opt/cortex
-SAT_CLIB=$(SAT)/lib/pdclib-cortex-m3.a
-SAT_CFLAGS=-I$(SAT)/include
-
 ifndef VERSION
 include $(TOPDIR)/Version
 endif
+include $(TOPDIR)/Makedefs
+
+CC=$(ARM_CC)
+LIBS=$(PDCLIB_LIBS_M3) -lgcc
 
-AO_CFLAGS=-I. -I$(TOPDIR)/stm -I$(TOPDIR)/core -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR)
+AO_CFLAGS=-I. -I$(TOPDIR)/stm -I$(TOPDIR)/core -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR) $(PDCLIB_INCLUDES)
 STM_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m3 -mthumb -ffreestanding -nostdlib $(AO_CFLAGS) $(SAT_CFLAGS)
 
 LDFLAGS=-L$(TOPDIR)/stm -Wl,-Taltos-loader.ld
@@ -62,7 +61,7 @@ SRC = \
 
 OBJ=$(SRC:.c=.o)
 
-PRODUCT=AltosFlash-$(VERSION)
+PRODUCT=AltosFlash
 PRODUCT_DEF=-DALTOS_FLASH
 IDPRODUCT=0x000a
 
@@ -72,7 +71,7 @@ PROGNAME=altos-flash
 PROG=$(HARDWARE)-$(PROGNAME)-$(VERSION).elf
 
 $(PROG): Makefile $(OBJ) altos-loader.ld
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS)
 
 ao_product.h: ao-make-product.5c $(TOPDIR)/Version
 	$(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@
@@ -84,7 +83,7 @@ all: $(PROG)
 distclean:	clean
 
 clean:
-	rm -f *.o $(PROG)
+	rm -f *.o $(HARDWARE)-$(PROGNAME)-*.elf
 	rm -f ao_product.h
 
 install:
diff --git a/src/stm/Makefile.defs b/src/stm/Makefile.defs
index c8bb7d70..9adcfeb3 100644
--- a/src/stm/Makefile.defs
+++ b/src/stm/Makefile.defs
@@ -1,4 +1,4 @@
-vpath % ../stm:../product:../drivers:../core:../util:../kalman:../aes:..
+vpath % ../stm:../product:../drivers:../core:../util:../kalman:../aes:../math:..
 vpath make-altitude ../util
 vpath make-kalman ../util
 vpath kalman.5c ../kalman
@@ -10,23 +10,27 @@ vpath ao-make-product.5c ../util
 .SUFFIXES: .elf .ihx
 
 .elf.ihx:
-	objcopy -O ihex $*.elf $@
+	$(ELFTOHEX) --output=$@ $*.elf
 
-CC=arm-none-eabi-gcc
-SAT=/opt/cortex
-SAT_CLIB=$(SAT)/lib/pdclib-cortex-m3.a
-SAT_CFLAGS=-I$(SAT)/include
+ifndef TOPDIR
+TOPDIR=..
+endif
 
 ifndef VERSION
-include ../Version
+include $(TOPDIR)/Version
 endif
+include $(TOPDIR)/Makedefs
+
+CC=$(ARM_CC)
+LIBS=$(PDCLIB_LIBS_M3) -lgcc
 
-AO_CFLAGS=-I. -I../stm -I../core -I../drivers -I..
-STM_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m3 -mthumb -ffreestanding -nostdlib $(AO_CFLAGS) $(SAT_CFLAGS)
+AO_CFLAGS=-I. -I../stm -I../core -I../drivers -I../math -I.. $(PDCLIB_INCLUDES)
+STM_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m3 -mthumb -ffreestanding -nostdlib $(AO_CFLAGS)
 
 LDFLAGS=-L../stm -Wl,-Taltos.ld
 
 NICKLE=nickle
+ELFTOHEX=$(TOPDIR)/../ao-tools/ao-elftohex/ao-elftohex
 
 V=0
 # The user has explicitly enabled quiet compilation.
diff --git a/src/stm/altos-loader.ld b/src/stm/altos-loader.ld
index 2be964f2..0753f5f7 100644
--- a/src/stm/altos-loader.ld
+++ b/src/stm/altos-loader.ld
@@ -37,11 +37,11 @@ SECTIONS {
 
 		ao_romconfig.o(.romconfig*)
 		ao_product.o(.romconfig*)
-		*(.text)	/* Executable code */
+		*(.text*)	/* Executable code */
 		*(.ARM.exidx* .gnu.linkonce.armexidx.*)
 		*(.rodata*)	/* Constants */
-		__text_end__ = .;
 	} > rom
+	__text_end__ = .;
 
 	/* Boot data which must live at the start of ram so that
 	 * the application and bootloader share the same addresses.
@@ -66,7 +66,7 @@ SECTIONS {
 	.textram BLOCK(8): {
 		__data_start__ = .;
 		__text_ram_start__ = .;
-		*(.text.ram)
+		*(.ramtext)
 		__text_ram_end = .;
 	} >ram AT>rom
 
diff --git a/src/stm/ao_adc_stm.c b/src/stm/ao_adc_stm.c
index 48fc4262..53f19b40 100644
--- a/src/stm/ao_adc_stm.c
+++ b/src/stm/ao_adc_stm.c
@@ -113,11 +113,15 @@ ao_adc_dump(void) __reentrant
 	uint8_t i;
 
 	ao_data_get(&packet);
+#ifdef AO_ADC_DUMP
+	AO_ADC_DUMP(&packet);
+#else
 	printf("tick: %5u",  packet.tick);
 	d = (int16_t *) (&packet.adc);
 	for (i = 0; i < AO_NUM_ADC; i++)
 		printf (" %2d: %5d", i, d[i]);
 	printf("\n");
+#endif
 }
 
 __code struct ao_cmds ao_adc_cmds[] = {
diff --git a/src/stm/ao_arch.h b/src/stm/ao_arch.h
index adc288c3..42fe727a 100644
--- a/src/stm/ao_arch.h
+++ b/src/stm/ao_arch.h
@@ -34,6 +34,8 @@
 #define AO_TICK_SIGNED	int16_t
 #endif
 
+#define AO_PORT_TYPE	uint16_t
+
 /* Various definitions to make GCC look more like SDCC */
 
 #define ao_arch_naked_declare	__attribute__((naked))
diff --git a/src/stm/ao_arch_funcs.h b/src/stm/ao_arch_funcs.h
index 1e78cabc..b461cd3f 100644
--- a/src/stm/ao_arch_funcs.h
+++ b/src/stm/ao_arch_funcs.h
@@ -87,13 +87,16 @@ extern uint16_t	ao_spi_speed[STM_NUM_SPI];
 void
 ao_spi_init(void);
 
+#define ao_spi_set_cs(reg,mask) ((reg)->bsrr = ((uint32_t) (mask)) << 16)
+#define ao_spi_clr_cs(reg,mask) ((reg)->bsrr = (mask))
+
 #define ao_spi_get_mask(reg,mask,bus, speed) do {		\
 		ao_spi_get(bus, speed);				\
-		(reg)->bsrr = ((uint32_t) mask) << 16;	\
+		ao_spi_set_cs(reg,mask);			\
 	} while (0)
 
 #define ao_spi_put_mask(reg,mask,bus) do {	\
-		(reg)->bsrr = mask;		\
+		ao_spi_clr_cs(reg,mask);	\
 		ao_spi_put(bus);		\
 	} while (0)
 
@@ -326,7 +329,7 @@ static inline void ao_arch_restore_stack(void) {
 
 	/* Restore APSR */
 	asm("pop {r0}");
-	asm("msr apsr,r0");
+	asm("msr apsr_nczvq,r0");
 
 	/* Restore general registers */
 	asm("pop {r0-r12,lr}\n");
@@ -335,6 +338,11 @@ static inline void ao_arch_restore_stack(void) {
 	asm("bx lr");
 }
 
+#ifndef HAS_SAMPLE_PROFILE
+#define HAS_SAMPLE_PROFILE 0
+#endif
+
+#if !HAS_SAMPLE_PROFILE
 #define HAS_ARCH_START_SCHEDULER	1
 
 static inline void ao_arch_start_scheduler(void) {
@@ -346,16 +354,19 @@ static inline void ao_arch_start_scheduler(void) {
 	asm("mrs %0,control" : "=&r" (control));
 	control |= (1 << 1);
 	asm("msr control,%0" : : "r" (control));
+	asm("isb");
 }
+#endif
 
 #define ao_arch_isr_stack()
 
 #endif
 
-#define ao_arch_wait_interrupt() do {			\
-		asm(".global ao_idle_loc\n\twfi\nao_idle_loc:");	\
-		ao_arch_release_interrupts();				\
-		ao_arch_block_interrupts();				\
+#define ao_arch_wait_interrupt() do {				\
+		asm("\twfi\n");					\
+		ao_arch_release_interrupts();			\
+		asm(".global ao_idle_loc\nao_idle_loc:");	\
+		ao_arch_block_interrupts();			\
 	} while (0)
 
 #define ao_arch_critical(b) do {				\
diff --git a/src/stm/ao_beep_stm.c b/src/stm/ao_beep_stm.c
index 4761fbfc..a95d869b 100644
--- a/src/stm/ao_beep_stm.c
+++ b/src/stm/ao_beep_stm.c
@@ -17,6 +17,10 @@
 
 #include "ao.h"
 
+#ifndef BEEPER_CHANNEL
+#define BEEPER_CHANNEL	1
+#endif
+
 void
 ao_beep(uint8_t beep)
 {
@@ -56,6 +60,7 @@ ao_beep(uint8_t beep)
 		 *  is enabled and active high.
 		 */
 
+#if BEEPER_CHANNEL == 1
 		stm_tim3.ccmr1 = ((0 << STM_TIM234_CCMR1_OC2CE) |
 				  (STM_TIM234_CCMR1_OC2M_FROZEN << STM_TIM234_CCMR1_OC2M) |
 				  (0 << STM_TIM234_CCMR1_OC2PE) |
@@ -68,7 +73,6 @@ ao_beep(uint8_t beep)
 				  (0 << STM_TIM234_CCMR1_OC1FE) |
 				  (STM_TIM234_CCMR1_CC1S_OUTPUT << STM_TIM234_CCMR1_CC1S));
 
-
 		stm_tim3.ccer = ((0 << STM_TIM234_CCER_CC4NP) |
 				 (0 << STM_TIM234_CCER_CC4P) |
 				 (0 << STM_TIM234_CCER_CC4E) |
@@ -81,6 +85,33 @@ ao_beep(uint8_t beep)
 				 (0 << STM_TIM234_CCER_CC1NP) |
 				 (0 << STM_TIM234_CCER_CC1P) |
 				 (1 << STM_TIM234_CCER_CC1E));
+#endif
+#if BEEPER_CHANNEL == 4
+		stm_tim3.ccmr2 = ((0 << STM_TIM234_CCMR2_OC4CE) |
+				  (STM_TIM234_CCMR2_OC4M_TOGGLE << STM_TIM234_CCMR2_OC4M) |
+				  (0 << STM_TIM234_CCMR2_OC4PE) |
+				  (0 << STM_TIM234_CCMR2_OC4FE) |
+				  (STM_TIM234_CCMR2_CC4S_OUTPUT << STM_TIM234_CCMR2_CC4S) |
+
+				  (0 << STM_TIM234_CCMR2_OC3CE) |
+				  (STM_TIM234_CCMR2_OC3M_FROZEN << STM_TIM234_CCMR2_OC3M) |
+				  (0 << STM_TIM234_CCMR2_OC3PE) |
+				  (0 << STM_TIM234_CCMR2_OC3FE) |
+				  (STM_TIM234_CCMR2_CC3S_OUTPUT << STM_TIM234_CCMR2_CC3S));
+
+		stm_tim3.ccer = ((0 << STM_TIM234_CCER_CC4NP) |
+				 (0 << STM_TIM234_CCER_CC4P) |
+				 (1 << STM_TIM234_CCER_CC4E) |
+				 (0 << STM_TIM234_CCER_CC3NP) |
+				 (0 << STM_TIM234_CCER_CC3P) |
+				 (0 << STM_TIM234_CCER_CC3E) |
+				 (0 << STM_TIM234_CCER_CC2NP) |
+				 (0 << STM_TIM234_CCER_CC2P) |
+				 (0 << STM_TIM234_CCER_CC2E) |
+				 (0 << STM_TIM234_CCER_CC1NP) |
+				 (0 << STM_TIM234_CCER_CC1P) |
+				 (0 << STM_TIM234_CCER_CC1E));
+#endif
 
 
 		/* 5. Enable the counter by setting the CEN bit in the TIMx_CR1 register. */
@@ -110,13 +141,22 @@ ao_beep_for(uint8_t beep, uint16_t ticks) __reentrant
 void
 ao_beep_init(void)
 {
-	/* Our beeper is on PC6, which is hooked to TIM3_CH1,
-	 * which is on PC6
-	 */
+#if BEEPER_CHANNEL == 1
 
+	/* Our beeper is on PC6, which is hooked to TIM3_CH1.
+	 */
 	stm_rcc.ahbenr |= (1 << STM_RCC_AHBENR_GPIOCEN);
 
 	stm_afr_set(&stm_gpioc, 6, STM_AFR_AF2);
+#endif
+#if BEEPER_CHANNEL == 4
+
+	/* Our beeper is on PB1, which is hooked to TIM3_CH4.
+	 */
+	stm_rcc.ahbenr |= (1 << STM_RCC_AHBENR_GPIOBEN);
+
+	stm_afr_set(&stm_gpiob, 1, STM_AFR_AF2);
+#endif
 
 	/* Leave the timer off until requested */
 	
diff --git a/src/stm/ao_eeprom_stm.c b/src/stm/ao_eeprom_stm.c
index 58783f1a..4207a860 100644
--- a/src/stm/ao_eeprom_stm.c
+++ b/src/stm/ao_eeprom_stm.c
@@ -16,19 +16,10 @@
  */
 
 #include <ao.h>
-#include <ao_storage.h>
+#include <ao_eeprom.h>
 
 /* Total bytes of available storage */
-ao_pos_t	ao_storage_total = 4096;
-
-/* Block size - device is erased in these units. */
-ao_pos_t	ao_storage_block = 1024;
-
-/* Byte offset of config block. Will be ao_storage_block bytes long */
-ao_pos_t	ao_storage_config = 0;
-
-/* Storage unit size - device reads and writes must be within blocks of this size. */
-uint16_t	ao_storage_unit = 1024;
+const ao_pos_t ao_eeprom_total = 4096;
 
 /* Location of eeprom in address space */
 #define stm_eeprom	((uint8_t *) 0x08080000)
@@ -42,16 +33,6 @@ uint16_t	ao_storage_unit = 1024;
  * the same contents, or append to an existing page easily enough
  */
 
-/*
- * Erase the specified sector
- */
-uint8_t
-ao_storage_erase(ao_pos_t pos) __reentrant
-{
-	/* Not necessary */
-	return 1;
-}
-
 static void
 ao_intflash_unlock(void)
 {
@@ -131,16 +112,16 @@ ao_intflash_read(uint16_t pos)
 }
 
 /*
- * Write to flash
+ * Write to eeprom
  */
 
 uint8_t
-ao_storage_device_write(ao_pos_t pos32, __xdata void *v, uint16_t len) __reentrant
+ao_eeprom_write(ao_pos_t pos32, __xdata void *v, uint16_t len)
 {
 	uint16_t pos = pos32;
 	__xdata uint8_t *d = v;
 
-	if (pos >= ao_storage_total || pos + len > ao_storage_total)
+	if (pos >= ao_eeprom_total || pos + len > ao_eeprom_total)
 		return 0;
 
 	ao_intflash_unlock();
@@ -166,38 +147,26 @@ ao_storage_device_write(ao_pos_t pos32, __xdata void *v, uint16_t len) __reentra
 }
 
 /*
- * Read from flash
+ * Read from eeprom
  */
 uint8_t
-ao_storage_device_read(ao_pos_t pos, __xdata void *v, uint16_t len) __reentrant
+ao_eeprom_read(ao_pos_t pos, __xdata void *v, uint16_t len)
 {
 	uint8_t	*d = v;
 	
-	if (pos >= ao_storage_total || pos + len > ao_storage_total)
+	if (pos >= ao_eeprom_total || pos + len > ao_eeprom_total)
 		return 0;
 	while (len--)
 		*d++ = ao_intflash_read(pos++);
 	return 1;
 }
 
-void
-ao_storage_flush(void) __reentrant
-{
-}
-
-void
-ao_storage_setup(void)
-{
-}
-
-void
-ao_storage_device_info(void) __reentrant
-{
-	uint8_t	i;
-	printf ("Using internal flash\n");
-}
+/*
+ * Initialize eeprom
+ */
 
 void
-ao_storage_device_init(void)
+ao_eeprom_init(void)
 {
+	/* Nothing to do here */
 }
diff --git a/src/stm/ao_exti.h b/src/stm/ao_exti.h
index 35b56b57..ebea224d 100644
--- a/src/stm/ao_exti.h
+++ b/src/stm/ao_exti.h
@@ -25,6 +25,7 @@
 #define AO_EXTI_PRIORITY_LOW	16
 #define AO_EXTI_PRIORITY_MED	0
 #define AO_EXTI_PRIORITY_HIGH	32
+#define AO_EXTI_PIN_NOCONFIGURE	64
 
 void
 ao_exti_setup(struct stm_gpio *gpio, uint8_t pin, uint8_t mode, void (*callback)());
diff --git a/src/stm/ao_exti_stm.c b/src/stm/ao_exti_stm.c
index 1361d0d4..c1dcdf85 100644
--- a/src/stm/ao_exti_stm.c
+++ b/src/stm/ao_exti_stm.c
@@ -70,21 +70,23 @@ ao_exti_setup (struct stm_gpio *gpio, uint8_t pin, uint8_t mode, void (*callback
 	/* configure gpio to interrupt routing */
 	stm_exticr_set(gpio, pin);
 
-	/* configure pin as input, setting selected pull-up/down mode */
-	stm_moder_set(gpio, pin, STM_MODER_INPUT);
-	switch (mode & (AO_EXTI_MODE_PULL_UP|AO_EXTI_MODE_PULL_DOWN)) {
-	case 0:
-	default:
-		pupdr  = STM_PUPDR_NONE;
-		break;
-	case AO_EXTI_MODE_PULL_UP:
-		pupdr = STM_PUPDR_PULL_UP;
-		break;
-	case AO_EXTI_MODE_PULL_DOWN:
-		pupdr = STM_PUPDR_PULL_DOWN;
-		break;
+	if (!(mode & AO_EXTI_PIN_NOCONFIGURE)) {
+		/* configure pin as input, setting selected pull-up/down mode */
+		stm_moder_set(gpio, pin, STM_MODER_INPUT);
+		switch (mode & (AO_EXTI_MODE_PULL_UP|AO_EXTI_MODE_PULL_DOWN)) {
+		case 0:
+		default:
+			pupdr  = STM_PUPDR_NONE;
+			break;
+		case AO_EXTI_MODE_PULL_UP:
+			pupdr = STM_PUPDR_PULL_UP;
+			break;
+		case AO_EXTI_MODE_PULL_DOWN:
+			pupdr = STM_PUPDR_PULL_DOWN;
+			break;
+		}
+		stm_pupdr_set(gpio, pin, pupdr);
 	}
-	stm_pupdr_set(gpio, pin, pupdr);
 
 	/* Set interrupt mask and rising/falling mode */
 	stm_exti.imr &= ~mask;
diff --git a/src/stm/ao_fast_timer.c b/src/stm/ao_fast_timer.c
new file mode 100644
index 00000000..d61b40c9
--- /dev/null
+++ b/src/stm/ao_fast_timer.c
@@ -0,0 +1,120 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+#include <ao.h>
+#include <ao_fast_timer.h>
+
+static void (*ao_fast_timer_callback[AO_FAST_TIMER_MAX])(void);
+static uint8_t ao_fast_timer_count;
+static uint8_t ao_fast_timer_users;
+
+static void
+ao_fast_timer_enable(void)
+{
+	stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) |
+			(0 << STM_TIM67_CR1_OPM) |
+			(1 << STM_TIM67_CR1_URS) |
+			(0 << STM_TIM67_CR1_UDIS) |
+			(1 << STM_TIM67_CR1_CEN));
+}
+
+static void
+ao_fast_timer_disable(void)
+{
+	stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) |
+			(0 << STM_TIM67_CR1_OPM) |
+			(1 << STM_TIM67_CR1_URS) |
+			(0 << STM_TIM67_CR1_UDIS) |
+			(0 << STM_TIM67_CR1_CEN));
+}
+
+void
+ao_fast_timer_on(void (*callback)(void))
+{
+	ao_fast_timer_callback[ao_fast_timer_count] = callback;
+	if (!ao_fast_timer_count++)
+		ao_fast_timer_enable();
+}
+
+void
+ao_fast_timer_off(void (*callback)(void))
+{
+	uint8_t	n;
+
+	for (n = 0; n < ao_fast_timer_count; n++)
+		if (ao_fast_timer_callback[n] == callback) {
+			for (; n < ao_fast_timer_count-1; n++) {
+				ao_fast_timer_callback[n] = ao_fast_timer_callback[n+1];
+			}
+			if (!--ao_fast_timer_count)
+				ao_fast_timer_disable();
+			break;
+		}
+}
+
+void stm_tim6_isr(void)
+{
+	uint8_t	i;
+	if (stm_tim6.sr & (1 << STM_TIM67_SR_UIF)) {
+		stm_tim6.sr = 0;
+
+		for (i = 0; i < ao_fast_timer_count; i++)
+			(*ao_fast_timer_callback[i])();
+	}
+}
+
+/*
+ * According to the STM clock-configuration, timers run
+ * twice as fast as the APB1 clock *if* the APB1 prescaler
+ * is greater than 1.
+ */
+
+#if AO_APB1_PRESCALER > 1
+#define TIMER_23467_SCALER 2
+#else
+#define TIMER_23467_SCALER 1
+#endif
+
+#define TIMER_10kHz	((AO_PCLK1 * TIMER_23467_SCALER) / 10000)
+
+void
+ao_fast_timer_init(void)
+{
+	if (!ao_fast_timer_users) {
+		stm_nvic_set_enable(STM_ISR_TIM6_POS);
+		stm_nvic_set_priority(STM_ISR_TIM6_POS, AO_STM_NVIC_CLOCK_PRIORITY);
+
+		/* Turn on timer 6 */
+		stm_rcc.apb1enr |= (1 << STM_RCC_APB1ENR_TIM6EN);
+
+		stm_tim6.psc = TIMER_10kHz;
+		stm_tim6.arr = 9;
+		stm_tim6.cnt = 0;
+
+		/* Enable update interrupt */
+		stm_tim6.dier = (1 << STM_TIM67_DIER_UIE);
+
+		/* Poke timer to reload values */
+		stm_tim6.egr |= (1 << STM_TIM67_EGR_UG);
+
+		stm_tim6.cr2 = (STM_TIM67_CR2_MMS_RESET << STM_TIM67_CR2_MMS);
+		ao_fast_timer_disable();
+	}
+	if (ao_fast_timer_users == AO_FAST_TIMER_MAX)
+		ao_panic(AO_PANIC_FAST_TIMER);
+	ao_fast_timer_users++;
+}
+
diff --git a/src/stm/ao_fast_timer.h b/src/stm/ao_fast_timer.h
new file mode 100644
index 00000000..90fb3930
--- /dev/null
+++ b/src/stm/ao_fast_timer.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_FAST_TIMER_H_
+#define _AO_FAST_TIMER_H_
+
+void
+ao_fast_timer_init(void);
+
+#ifndef AO_FAST_TIMER_MAX
+#define AO_FAST_TIMER_MAX	2
+#endif
+
+void
+ao_fast_timer_on(void (*callback)(void));
+
+void
+ao_fast_timer_off(void (*callback)(void));
+
+#endif /* _AO_FAST_TIMER_H_ */
diff --git a/src/stm/ao_flash_stm.c b/src/stm/ao_flash_stm.c
index d7a85582..38b1c2d8 100644
--- a/src/stm/ao_flash_stm.c
+++ b/src/stm/ao_flash_stm.c
@@ -69,7 +69,7 @@ ao_flash_wait_bsy(void)
 		;
 }
 
-static void __attribute__ ((section(".text.ram"),noinline))
+static void __attribute__ ((section(".ramtext"),noinline))
 _ao_flash_erase_page(uint32_t *page)
 {
 	stm_flash.pecr |= (1 << STM_FLASH_PECR_ERASE) | (1 << STM_FLASH_PECR_PROG);
@@ -91,7 +91,7 @@ ao_flash_erase_page(uint32_t *page)
 	ao_flash_lock();
 }
 
-static void __attribute__ ((section(".text.ram"), noinline))
+static void __attribute__ ((section(".ramtext"), noinline))
 _ao_flash_half_page(uint32_t *dst, uint32_t *src)
 {
 	uint8_t		i;
diff --git a/src/stm/ao_timer.c b/src/stm/ao_timer.c
index daf2f400..34f9edb9 100644
--- a/src/stm/ao_timer.c
+++ b/src/stm/ao_timer.c
@@ -67,20 +67,6 @@ ao_timer_set_adc_interval(uint8_t interval)
 }
 #endif
 
-/*
- * According to the STM clock-configuration, timers run
- * twice as fast as the APB1 clock *if* the APB1 prescaler
- * is greater than 1.
- */
-
-#if AO_APB1_PRESCALER > 1
-#define TIMER_23467_SCALER 2
-#else
-#define TIMER_23467_SCALER 1
-#endif
-
-#define TIMER_10kHz	((AO_PCLK1 * TIMER_23467_SCALER) / 10000)
-
 #define SYSTICK_RELOAD (AO_SYSTICK / 100 - 1)
 
 void
@@ -104,7 +90,15 @@ ao_clock_init(void)
 	/* Switch to MSI while messing about */
 	stm_rcc.cr |= (1 << STM_RCC_CR_MSION);
 	while (!(stm_rcc.cr & (1 << STM_RCC_CR_MSIRDY)))
-		asm("nop");
+		ao_arch_nop();
+
+	stm_rcc.cfgr = (stm_rcc.cfgr & ~(STM_RCC_CFGR_SW_MASK << STM_RCC_CFGR_SW)) |
+		(STM_RCC_CFGR_SW_MSI << STM_RCC_CFGR_SW);
+
+	/* wait for system to switch to MSI */
+	while ((stm_rcc.cfgr & (STM_RCC_CFGR_SWS_MASK << STM_RCC_CFGR_SWS)) !=
+	       (STM_RCC_CFGR_SWS_MSI << STM_RCC_CFGR_SWS))
+		ao_arch_nop();
 
 	/* reset SW, HPRE, PPRE1, PPRE2, MCOSEL and MCOPRE */
 	stm_rcc.cfgr &= (uint32_t)0x88FFC00C;
@@ -155,7 +149,6 @@ ao_clock_init(void)
 	stm_flash.acr |= (1 << STM_FLASH_ACR_PRFEN);
 
 	/* Enable 1 wait state so the CPU can run at 32MHz */
-	/* (haven't managed to run the CPU at 32MHz yet, it's at 16MHz) */
 	stm_flash.acr |= (1 << STM_FLASH_ACR_LATENCY);
 
 	/* Enable power interface clock */
diff --git a/src/stm/ao_usb_stm.c b/src/stm/ao_usb_stm.c
index 11dde92e..28a9f9f3 100644
--- a/src/stm/ao_usb_stm.c
+++ b/src/stm/ao_usb_stm.c
@@ -572,7 +572,7 @@ ao_usb_ep0_out_set(uint8_t *data, uint8_t len)
 }
 
 static void
-ao_usb_ep0_in_start(uint8_t max)
+ao_usb_ep0_in_start(uint16_t max)
 {
 	/* Don't send more than asked for */
 	if (ao_usb_ep0_in_len > max)
@@ -965,7 +965,7 @@ ao_usb_disable(void)
 	stm_usb.cntr = (1 << STM_USB_CNTR_PDWN) | (1 << STM_USB_CNTR_FRES);
 
 	/* Disable the interface */
-	stm_rcc.apb1enr &+ ~(1 << STM_RCC_APB1ENR_USBEN);
+	stm_rcc.apb1enr &= ~(1 << STM_RCC_APB1ENR_USBEN);
 	ao_arch_release_interrupts();
 }
 
diff --git a/src/stm/stm32l.h b/src/stm/stm32l.h
index 63bde0f8..ff3f5336 100644
--- a/src/stm/stm32l.h
+++ b/src/stm/stm32l.h
@@ -171,6 +171,11 @@ stm_gpio_get(struct stm_gpio *gpio, int pin) {
 	return (gpio->idr >> pin) & 1;
 }
 
+static inline uint16_t
+stm_gpio_get_all(struct stm_gpio *gpio) {
+	return gpio->idr;
+}
+
 extern struct stm_gpio stm_gpioa;
 extern struct stm_gpio stm_gpiob;
 extern struct stm_gpio stm_gpioc;
@@ -1734,7 +1739,7 @@ extern struct stm_tim234 stm_tim2, stm_tim3, stm_tim4;
 #define  STM_TIM234_CCMR1_CC1S_INPUT_TRC		3
 #define  STM_TIM234_CCMR1_CC1S_MASK			3
 
-#define STM_TIM234_CCMR2_OC2CE	15
+#define STM_TIM234_CCMR2_OC4CE	15
 #define STM_TIM234_CCMR2_OC4M	12
 #define  STM_TIM234_CCMR2_OC4M_FROZEN			0
 #define  STM_TIM234_CCMR2_OC4M_SET_HIGH_ON_MATCH	1
diff --git a/src/teleballoon-v1.1/Makefile b/src/teleballoon-v1.1/Makefile
index 2eea996e..6ff076a9 100644
--- a/src/teleballoon-v1.1/Makefile
+++ b/src/teleballoon-v1.1/Makefile
@@ -103,7 +103,7 @@ quiet ?= $($1)
 all: $(PROG)
 
 $(PROG): $(REL) Makefile
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) ..
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL)
 	$(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@
 
 ao_product.h: ao-make-product.5c ../Version
diff --git a/src/teleballoon-v1.1/ao_balloon.c b/src/teleballoon-v1.1/ao_balloon.c
index e8972655..12752d1f 100644
--- a/src/teleballoon-v1.1/ao_balloon.c
+++ b/src/teleballoon-v1.1/ao_balloon.c
@@ -105,8 +105,8 @@ ao_flight(void)
 
 #if HAS_GPS
 				/* Record current GPS position by waking up GPS log tasks */
-				ao_wakeup(&ao_gps_data);
-				ao_wakeup(&ao_gps_tracking_data);
+				ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
+				ao_wakeup(&ao_gps_new);
 #endif
 
 				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
diff --git a/src/telebt-v1.0/Makefile b/src/telebt-v1.0/Makefile
index 911a8b09..40853fc3 100644
--- a/src/telebt-v1.0/Makefile
+++ b/src/telebt-v1.0/Makefile
@@ -82,7 +82,7 @@ quiet ?= $($1)
 all: $(PROG)
 
 $(PROG): $(REL) Makefile
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) ..
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL)
 	$(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@
 
 ao_product.h: ao-make-product.5c ../Version
diff --git a/src/telefire-v0.1/Makefile b/src/telefire-v0.1/Makefile
index 712b2e8b..f9e11698 100644
--- a/src/telefire-v0.1/Makefile
+++ b/src/telefire-v0.1/Makefile
@@ -84,10 +84,10 @@ endif
 # Otherwise, print the full command line.
 quiet ?= $($1)
 
-all: ../$(PROG)
+all: $(PROG)
 
-../$(PROG): $(REL) Makefile
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) ..
+$(PROG): $(REL) Makefile
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL)
 	$(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM)  || rm $@
 
 ao_product.h: ao-make-product.5c ../Version
diff --git a/src/telefire-v0.1/ao_pins.h b/src/telefire-v0.1/ao_pins.h
index 774d59f4..f7a3ff2c 100644
--- a/src/telefire-v0.1/ao_pins.h
+++ b/src/telefire-v0.1/ao_pins.h
@@ -40,13 +40,10 @@
 
 #define AO_LED_CONTINUITY(c)	(1 << ((c) + 2))
 #define AO_LED_CONTINUITY_MASK	(0xc)
-#define AO_LED_RX		0x10
-#define AO_LED_TX		0x20
-#define AO_LED_ARMED		0x40
-#define AO_LED_POWER		0x80
-
-#define AO_LED_RED		AO_LED_TX
-#define AO_LED_GREEN		AO_LED_RX
+#define AO_LED_ARMED		0x10
+#define AO_LED_RED		0x20
+#define AO_LED_AMBER		0x40
+#define AO_LED_GREEN		0x80
 
 #define LEDS_AVAILABLE		(0xff)
 #define HAS_EXTERNAL_TEMP	0
diff --git a/src/telefire-v0.2/.gitignore b/src/telefire-v0.2/.gitignore
new file mode 100644
index 00000000..4d4f4200
--- /dev/null
+++ b/src/telefire-v0.2/.gitignore
@@ -0,0 +1,2 @@
+telefire-*
+ao_product.h
diff --git a/src/telefire-v0.2/.sdcdbrc b/src/telefire-v0.2/.sdcdbrc
new file mode 100644
index 00000000..b9f6129c
--- /dev/null
+++ b/src/telefire-v0.2/.sdcdbrc
@@ -0,0 +1,2 @@
+--directory=../cc1111:../product:../core:../drivers:.
+
diff --git a/src/telefire-v0.2/Makefile b/src/telefire-v0.2/Makefile
new file mode 100644
index 00000000..a820990a
--- /dev/null
+++ b/src/telefire-v0.2/Makefile
@@ -0,0 +1,103 @@
+#
+# TeleFire build file
+#
+
+TELEFIRE_VER=0.2
+TELEFIRE_DEF=0_2
+
+vpath %.c ..:../core:../cc1111:../drivers:../product
+vpath %.h ..:../core:../cc1111:../drivers:../product
+vpath ao-make-product.5c ../util
+
+ifndef VERSION
+include ../Version
+endif
+
+INC = \
+	ao.h \
+	ao_pins.h \
+	ao_arch.h \
+	ao_arch_funcs.h \
+	ao_pad.h \
+	cc1111.h \
+	ao_product.h
+
+CORE_SRC = \
+	ao_cmd.c \
+	ao_config.c \
+	ao_convert.c \
+	ao_mutex.c \
+	ao_panic.c \
+	ao_stdio.c \
+	ao_storage.c \
+	ao_task.c \
+	ao_freq.c
+
+CC1111_SRC = \
+	ao_adc.c \
+	ao_aes.c \
+	ao_beep.c \
+	ao_dma.c \
+	ao_intflash.c \
+	ao_radio.c \
+	ao_radio_cmac.c \
+	ao_romconfig.c \
+	ao_serial.c \
+	ao_spi.c \
+	ao_string.c \
+	ao_timer.c \
+	ao_usb.c \
+	_bp.c
+
+DRIVER_SRC = \
+	ao_pca9922.c \
+	ao_74hc165.c \
+	ao_pad.c \
+	ao_radio_cmac_cmd.c
+
+PRODUCT_SRC = \
+	ao_telefire.c
+
+SRC = \
+	$(CORE_SRC) \
+	$(CC1111_SRC) \
+	$(DRIVER_SRC) \
+	$(PRODUCT_SRC)
+
+PROGNAME = telefire-v$(TELEFIRE_VER)
+PROG = $(PROGNAME)-$(VERSION).ihx
+PRODUCT=TeleFire-v$(TELEFIRE_VER)
+PRODUCT_DEF=-DTELEFIRE_V_$(TELEFIRE_DEF)
+IDPRODUCT=0x000f
+CODESIZE=0x6700
+
+include ../cc1111/Makefile.cc1111
+
+NICKLE=nickle
+CHECK_STACK=sh ../util/check-stack
+
+V=0
+# The user has explicitly enabled quiet compilation.
+ifeq ($(V),0)
+quiet = @printf "  $1 $2 $@\n"; $($1)
+endif
+# Otherwise, print the full command line.
+quiet ?= $($1)
+
+all: $(PROG)
+
+$(PROG): $(REL) Makefile
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL)
+	$(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM)  || rm $@
+
+ao_product.h: ao-make-product.5c ../Version
+	$(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@
+
+distclean:	clean
+
+clean: clean-cc1111
+
+install:
+
+uninstall:
+
diff --git a/src/telefire-v0.2/ao_pins.h b/src/telefire-v0.2/ao_pins.h
new file mode 100644
index 00000000..96e6b066
--- /dev/null
+++ b/src/telefire-v0.2/ao_pins.h
@@ -0,0 +1,141 @@
+/*
+ * Copyright © 2010 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_PINS_H_
+#define _AO_PINS_H_
+
+#define HAS_RADIO	1
+
+#define HAS_FLIGHT		0
+#define HAS_USB			1
+#define HAS_BEEP		0
+#define HAS_GPS			0
+#define HAS_SERIAL_1		0
+#define HAS_ADC			1
+#define HAS_DBG			0
+#define HAS_EEPROM		1
+#define HAS_LOG			0
+#define HAS_PAD			1
+#define USE_INTERNAL_FLASH	1
+#define DBG_ON_P1 		0
+#define IGNITE_ON_P2		0
+#define IGNITE_ON_P1		1
+#define IGNITE_ON_P0		0
+#define PACKET_HAS_MASTER	0
+#define PACKET_HAS_SLAVE	0
+
+#define AO_LED_CONTINUITY(c)	(1 << (c))
+#define AO_LED_CONTINUITY_MASK	(0xf)
+#define AO_LED_ARMED		0x10
+#define AO_LED_RED		0x20
+#define AO_LED_AMBER		0x40
+#define AO_LED_GREEN		0x80
+
+#define LEDS_AVAILABLE		(0xff)
+#define HAS_EXTERNAL_TEMP	0
+#define HAS_ACCEL_REF		0
+#define SPI_CS_ON_P1		1
+#define HAS_AES			1
+#define DMA_SHARE_AES_RADIO	1
+
+#define SPI_CS_PORT	P1
+#define SPI_CS_SEL	P1SEL
+#define SPI_CS_DIR	P1DIR
+
+#define SPI_CONST	0x00
+
+#define HAS_SPI_0		0
+#define HAS_SPI_1		1
+#define SPI_1_ALT_1		0
+#define SPI_1_ALT_2		1
+
+#define HAS_74HC165		1
+#define AO_74HC165_CS_PORT	P1
+#define AO_74HC165_CS_PIN	4
+#define AO_74HC165_CS		P1_4
+
+#define AO_PCA9922_CS_PORT	P2
+#define AO_PCA9922_CS_PIN	0
+#define AO_PCA9922_CS		P2_0
+
+#define AO_PAD_NUM		4
+#define	AO_PAD_PORT		P1
+#define AO_PAD_DIR		P1DIR
+
+#define AO_PAD_PIN_0		0
+#define AO_PAD_0		P1_0
+#define AO_PAD_ADC_0		0
+
+#define AO_PAD_PIN_1		1
+#define AO_PAD_1		P1_1
+#define AO_PAD_ADC_1		1
+
+#define AO_PAD_PIN_2		2
+#define AO_PAD_2		P1_2
+#define AO_PAD_ADC_2		2
+
+#define AO_PAD_PIN_3		3
+#define AO_PAD_3		P1_3
+#define AO_PAD_ADC_3		3
+
+#define AO_PAD_ALL_PINS		((1 << AO_PAD_PIN_0) | (1 << AO_PAD_PIN_1) | (1 << AO_PAD_PIN_2) | (1 << AO_PAD_PIN_3))
+#define AO_PAD_ALL_CHANNELS	((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))
+
+#define AO_SIREN_PORT		P2
+#define AO_SIREN_DIR		P2DIR
+#define AO_SIREN_PIN		3
+#define AO_SIREN		P2_3
+
+#define AO_STROBE_PORT		P2
+#define AO_STROBE_DIR		P2DIR
+#define AO_STROBE_PIN		4
+#define AO_STROBE		P2_4
+
+/* test these values with real igniters */
+#define AO_PAD_RELAY_CLOSED	3524
+#define AO_PAD_NO_IGNITER	16904
+#define AO_PAD_GOOD_IGNITER	22514
+
+#define AO_PAD_ADC_PYRO		4
+#define AO_PAD_ADC_BATT		5
+
+#define AO_ADC_FIRST_PIN	0
+
+struct ao_adc {
+	int16_t		sense[AO_PAD_NUM];
+	int16_t		pyro;
+	int16_t		batt;
+};
+
+#define AO_ADC_DUMP(p)							\
+	printf ("tick: %5u 0: %5d 1: %5d 2: %5d 3: %5d pyro: %5d batt %5d\n", \
+		(p)->tick,						\
+		(p)->adc.sense[0],					\
+		(p)->adc.sense[1],					\
+		(p)->adc.sense[2],					\
+		(p)->adc.sense[3],					\
+		(p)->adc.pyro,						\
+		(p)->adc.batt)
+
+#define AO_ADC_PINS	((1 << AO_PAD_ADC_0) | \
+			 (1 << AO_PAD_ADC_1) | \
+			 (1 << AO_PAD_ADC_2) | \
+			 (1 << AO_PAD_ADC_3) | \
+			 (1 << AO_PAD_ADC_PYRO) | \
+			 (1 << AO_PAD_ADC_BATT))
+
+#endif /* _AO_PINS_H_ */
diff --git a/src/telefire-v0.2/ao_telefire.c b/src/telefire-v0.2/ao_telefire.c
new file mode 100644
index 00000000..f27ca5e8
--- /dev/null
+++ b/src/telefire-v0.2/ao_telefire.c
@@ -0,0 +1,45 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+#include <ao_pad.h>
+#include <ao_74hc165.h>
+#include <ao_radio_cmac_cmd.h>
+
+void
+main(void)
+{
+	ao_clock_init();
+
+	ao_led_init(LEDS_AVAILABLE);
+
+	ao_task_init();
+
+	ao_timer_init();
+	ao_adc_init();
+	ao_cmd_init();
+	ao_spi_init();
+	ao_74hc165_init();
+	ao_storage_init();
+	ao_usb_init();
+	ao_radio_init();
+	ao_aes_init();
+	ao_pad_init();
+//	ao_radio_cmac_cmd_init();
+	ao_config_init();
+	ao_start_scheduler();
+}
diff --git a/src/telegps-v0.1/Makefile b/src/telegps-v0.1/Makefile
index 2c41235b..49e325ac 100644
--- a/src/telegps-v0.1/Makefile
+++ b/src/telegps-v0.1/Makefile
@@ -52,11 +52,11 @@ ALTOS_SRC = \
 	ao_exti_stm.c \
 	ao_serial_stm.c \
 	ao_gps_skytraq.c \
+	ao_gps_show.c \
 	ao_cc115l.c \
 	ao_fec_tx.c \
 	ao_rfpa0133.c \
 	ao_aprs.c \
-	ao_storage.c \
 	ao_eeprom_stm.c \
 	ao_sdcard.c \
 	ao_bufio.c \
@@ -74,14 +74,15 @@ CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STA
 
 PROGNAME=telegps-v0.1
 PROG=$(PROGNAME)-$(VERSION).elf
+HEX=$(PROGNAME)-$(VERSION).ihx
 
 SRC=$(ALTOS_SRC) ao_telegps.c
 OBJ=$(SRC:.c=.o)
 
-all: $(PROG)
+all: $(PROG) $(HEX)
 
 $(PROG): Makefile $(OBJ) altos.ld
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS)
 
 $(OBJ): $(INC)
 
@@ -91,7 +92,7 @@ ao_product.h: ao-make-product.5c ../Version
 distclean:	clean
 
 clean:
-	rm -f *.o $(PROGNAME)-*.elf
+	rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx
 	rm -f ao_product.h
 
 install:
diff --git a/src/telegps-v0.1/ao_pins.h b/src/telegps-v0.1/ao_pins.h
index 5bea2681..7ff59956 100644
--- a/src/telegps-v0.1/ao_pins.h
+++ b/src/telegps-v0.1/ao_pins.h
@@ -65,7 +65,10 @@
 #define ao_gps_fifo		(ao_stm_usart2.rx_fifo)
 
 #define HAS_EEPROM		1
-#define USE_INTERNAL_FLASH	1
+#define USE_INTERNAL_FLASH	0
+#define USE_EEPROM_CONFIG	1
+#define USE_STORAGE_CONFIG	0
+
 #define HAS_USB			1
 #define HAS_BEEP		0
 #define HAS_RADIO		1
diff --git a/src/telegps-v0.1/ao_telegps.c b/src/telegps-v0.1/ao_telegps.c
index 68116bfb..bc37b504 100644
--- a/src/telegps-v0.1/ao_telegps.c
+++ b/src/telegps-v0.1/ao_telegps.c
@@ -18,6 +18,7 @@
 #include <ao.h>
 #include <ao_exti.h>
 #include <ao_fat.h>
+#include <ao_eeprom.h>
 
 uint16_t	ao_flight_number = 1;
 
@@ -40,7 +41,7 @@ main(void)
 	ao_dma_init();
 	ao_exti_init();
 
-	ao_storage_init();
+	ao_eeprom_init();
 
 	ao_serial_init();
 
diff --git a/src/telegps-v0.3/.gitignore b/src/telegps-v0.3/.gitignore
new file mode 100644
index 00000000..892c3acc
--- /dev/null
+++ b/src/telegps-v0.3/.gitignore
@@ -0,0 +1,3 @@
+ao_product.h
+ao_serial_lpc.h
+*.elf
diff --git a/src/telegps-v0.3/Makefile b/src/telegps-v0.3/Makefile
new file mode 100644
index 00000000..bb9c8c64
--- /dev/null
+++ b/src/telegps-v0.3/Makefile
@@ -0,0 +1,84 @@
+#
+# AltOS build
+#
+#
+
+include ../lpc/Makefile.defs
+
+INC = \
+	ao.h \
+	ao_arch.h \
+	ao_arch_funcs.h \
+	ao_pins.h \
+	ao_product.h \
+	ao_task.h \
+	ao_whiten.h \
+	ao_cc115l.h \
+	ao_fec.h \
+	lpc.h \
+	Makefile
+
+
+ALTOS_SRC = \
+	ao_interrupt.c \
+	ao_boot_chain.c \
+	ao_product.c \
+	ao_romconfig.c \
+	ao_cmd.c \
+	ao_config.c \
+	ao_task.c \
+	ao_stdio.c \
+	ao_panic.c \
+	ao_timer_lpc.c \
+	ao_mutex.c \
+	ao_freq.c \
+	ao_spi_lpc.c \
+	ao_usb_lpc.c \
+	ao_exti_lpc.c \
+	ao_serial_lpc.c \
+	ao_gps_ublox.c \
+	ao_gps_show.c \
+	ao_cc115l.c \
+	ao_fec_tx.c \
+	ao_aprs.c \
+	ao_telemetry.c \
+	ao_storage.c \
+	ao_m25.c \
+	ao_log.c \
+	ao_log_mega.c \
+	ao_gps_report_mega.c \
+	$(SAMPLE_PROFILE)
+
+PRODUCT=TeleGPS-v0.3
+PRODUCT_DEF=-DTELEGPS
+IDPRODUCT=0x0025
+
+CFLAGS = $(PRODUCT_DEF) $(LPC_CFLAGS) $(PROFILE_DEF) -Os -g
+
+PROGNAME=telegps-v0.3
+PROG=$(PROGNAME)-$(VERSION).elf
+
+SRC=$(ALTOS_SRC) ao_telegps.c
+OBJ=$(SRC:.c=.o)
+
+all: $(PROG)
+
+LDFLAGS=-L../lpc -Wl,-Taltos.ld
+
+$(PROG): Makefile $(OBJ) altos.ld
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS)
+
+$(OBJ): $(INC)
+
+ao_product.h: ao-make-product.5c ../Version
+	$(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@
+
+distclean:	clean
+
+clean:
+	rm -f *.o ao_serial_lpc.h $(PROGNAME)-*.elf $(PROGNAME)-*.ihx
+	rm -f ao_product.h
+
+install:
+
+uninstall:
diff --git a/src/telegps-v0.3/ao_pins.h b/src/telegps-v0.3/ao_pins.h
new file mode 100644
index 00000000..a4afaa54
--- /dev/null
+++ b/src/telegps-v0.3/ao_pins.h
@@ -0,0 +1,114 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_PINS_H_
+#define _AO_PINS_H_
+
+#define AO_STACK_SIZE	448
+
+#define IS_FLASH_LOADER		0
+
+/* Crystal on the board */
+#define AO_LPC_CLKIN	12000000
+
+/* Main clock frequency. 48MHz for USB so we don't use the USB PLL */
+#define AO_LPC_CLKOUT	48000000
+
+/* System clock frequency */
+#define AO_LPC_SYSCLK	24000000
+
+#define HAS_SERIAL_0		1
+#define SERIAL_0_18_19		1
+#define USE_SERIAL_0_STDIN	0
+
+#define ao_gps_getchar		ao_serial0_getchar
+#define ao_gps_putchar		ao_serial0_putchar
+#define ao_gps_set_speed	ao_serial0_set_speed
+#define ao_gps_fifo		(ao_usart_rx_fifo)
+
+#define HAS_EEPROM		1
+#define USE_INTERNAL_FLASH	0
+#define HAS_USB			1
+#define HAS_BEEP		0
+#define HAS_RADIO		1
+#define HAS_TELEMETRY		1
+#define HAS_RDF			0
+#define HAS_APRS		1
+#define HAS_RADIO_RECV		0
+
+#define HAS_USB_PULLUP		1
+#define AO_USB_PULLUP_PORT	0
+#define AO_USB_PULLUP_PIN	7
+
+/* Flash part */
+#define HAS_SPI_0		1
+#define SPI_SCK0_P0_6		1
+#define SPI_0_OSPEEDR		AO_SPI_OSPEED_12MHz
+
+/* Radio */
+#define HAS_SPI_1		1
+#define SPI_SCK1_P1_15		1
+#define SPI_MISO1_P0_22		1
+#define SPI_MOSI1_P0_21		1
+
+#define HAS_GPS			1
+#define HAS_FLIGHT		0
+#define HAS_ADC			0
+#define HAS_LOG			1
+
+#define AO_CONFIG_DEFAULT_APRS_INTERVAL	5
+#define AO_CONFIG_DEFAULT_RADIO_POWER		0xc0
+
+/*
+ * GPS
+ */
+
+#define AO_SERIAL_SPEED_UBLOX	AO_SERIAL_SPEED_9600
+
+/*
+ * Radio (cc115l)
+ */
+
+/* gets pretty close to 434.550 */
+
+#define AO_RADIO_CAL_DEFAULT 	0x10b6a5
+
+#define HAS_RADIO_POWER		1
+#define AO_FEC_DEBUG		0
+#define AO_CC115L_SPI_CS_PORT	0
+#define AO_CC115L_SPI_CS_PIN	3
+#define AO_CC115L_SPI_BUS	0
+
+#define AO_CC115L_FIFO_INT_GPIO_IOCFG	CC115L_IOCFG2
+#define AO_CC115L_FIFO_INT_PORT		0
+#define AO_CC115L_FIFO_INT_PIN		20
+
+#define AO_CC115L_DONE_INT_GPIO_IOCFG	CC115L_IOCFG0
+#define AO_CC115L_DONE_INT_PORT		0
+#define AO_CC115L_DONE_INT_PIN		2
+
+/*
+ * Flash (M25)
+ */
+#define M25_MAX_CHIPS		1
+#define AO_M25_SPI_CS_PORT	0
+#define AO_M25_SPI_CS_MASK	(1 << 23)
+#define AO_M25_SPI_BUS		1
+
+#define PACKET_HAS_SLAVE	0
+
+#endif /* _AO_PINS_H_ */
diff --git a/src/telegps-v0.3/ao_telegps.c b/src/telegps-v0.3/ao_telegps.c
new file mode 100644
index 00000000..608817e7
--- /dev/null
+++ b/src/telegps-v0.3/ao_telegps.c
@@ -0,0 +1,65 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+#include <ao_exti.h>
+#include <ao_fat.h>
+
+uint16_t	ao_flight_number = 1;
+
+int
+main(void)
+{
+	ao_clock_init();
+	
+#if HAS_STACK_GUARD
+	ao_mpu_init();
+#endif
+
+	ao_task_init();
+	ao_timer_init();
+
+
+	ao_spi_init();
+	ao_exti_init();
+
+	ao_storage_init();
+
+	ao_serial_init();
+
+	ao_cmd_init();
+
+	ao_usb_init();
+	ao_radio_init();
+
+	ao_gps_init();
+#if HAS_LOG
+	ao_gps_report_mega_init();
+	ao_log_init();
+#endif
+
+	ao_telemetry_init();
+	ao_telemetry_set_interval(AO_SEC_TO_TICKS(1));
+
+#if HAS_SAMPLE_PROFILE
+	ao_sample_profile_init();
+#endif
+	ao_config_init();
+	
+	ao_start_scheduler();
+	return 0;
+}
diff --git a/src/telegps-v0.3/flash-loader/Makefile b/src/telegps-v0.3/flash-loader/Makefile
new file mode 100644
index 00000000..6f4ce0d8
--- /dev/null
+++ b/src/telegps-v0.3/flash-loader/Makefile
@@ -0,0 +1,8 @@
+#
+# AltOS flash loader build
+#
+#
+
+TOPDIR=../..
+HARDWARE=telegps-v0.3
+include $(TOPDIR)/lpc/Makefile-flash.defs
diff --git a/src/telegps-v0.3/flash-loader/ao_pins.h b/src/telegps-v0.3/flash-loader/ao_pins.h
new file mode 100644
index 00000000..91097a25
--- /dev/null
+++ b/src/telegps-v0.3/flash-loader/ao_pins.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_PINS_H_
+#define _AO_PINS_H_
+
+#include <ao_flash_lpc_pins.h>
+
+#define AO_BOOT_PIN		1
+#define AO_BOOT_APPLICATION_GPIO	0
+#define AO_BOOT_APPLICATION_PIN		19
+#define AO_BOOT_APPLICATION_VALUE	1
+#define AO_BOOT_APPLICATION_MODE	AO_EXTI_MODE_PULL_UP
+
+#define HAS_USB_PULLUP		1
+#define AO_USB_PULLUP_PORT	0
+#define AO_USB_PULLUP_PIN	7
+
+#endif /* _AO_PINS_H_ */
diff --git a/src/telelco-v0.1/Makefile b/src/telelco-v0.1/Makefile
index 24083308..44d9237f 100644
--- a/src/telelco-v0.1/Makefile
+++ b/src/telelco-v0.1/Makefile
@@ -69,14 +69,15 @@ CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) -Os -g
 
 PROGNAME=telelco-v0.1
 PROG=$(PROGNAME)-$(VERSION).elf
+HEX=$(PROGNAME)-$(VERSION).ihx
 
 SRC=$(ALTOS_SRC) ao_telelco.c
 OBJ=$(SRC:.c=.o)
 
-all: $(PROG)
+all: $(PROG) $(HEX)
 
 $(PROG): Makefile $(OBJ) altos.ld
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS)
 
 ../altitude.h: make-altitude
 	nickle $< > $@
@@ -89,7 +90,7 @@ ao_product.h: ao-make-product.5c ../Version
 distclean:	clean
 
 clean:
-	rm -f *.o $(PROGNAME)-*.elf
+	rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx
 	rm -f ao_product.h
 
 install:
diff --git a/src/telelco-v0.2/Makefile b/src/telelco-v0.2/Makefile
index cc6e62c4..f28bdd32 100644
--- a/src/telelco-v0.2/Makefile
+++ b/src/telelco-v0.2/Makefile
@@ -21,6 +21,7 @@ INC = \
 	ao_radio_spi.h \
 	ao_radio_cmac.h \
 	ao_cc1120_CC1120.h \
+	ao_debounce.h \
 	stm32l.h
 
 #
@@ -47,8 +48,8 @@ ALTOS_SRC = \
 	ao_dma_stm.c \
 	ao_spi_stm.c \
 	ao_beep_stm.c \
-	ao_storage.c \
 	ao_eeprom_stm.c \
+	ao_fast_timer.c \
 	ao_lcd_stm.c \
 	ao_usb_stm.c \
 	ao_exti_stm.c \
@@ -59,6 +60,7 @@ ALTOS_SRC = \
 	ao_fec_tx.c \
 	ao_fec_rx.c \
 	ao_seven_segment.c \
+	ao_debounce.c \
 	ao_quadrature.c \
 	ao_button.c \
 	ao_event.c \
@@ -75,14 +77,15 @@ CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) -Os -g
 
 PROGNAME=telelco-v0.2
 PROG=$(PROGNAME)-$(VERSION).elf
+HEX=$(PROGNAME)-$(VERSION).ihx
 
 SRC=$(ALTOS_SRC) ao_telelco.c
 OBJ=$(SRC:.c=.o)
 
-all: $(PROG)
+all: $(PROG) $(HEX)
 
 $(PROG): Makefile $(OBJ) altos.ld
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS)
 
 ../altitude.h: make-altitude
 	nickle $< > $@
@@ -95,7 +98,7 @@ ao_product.h: ao-make-product.5c ../Version
 distclean:	clean
 
 clean:
-	rm -f *.o $(PROGNAME)-*.elf
+	rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx
 	rm -f ao_product.h
 
 install:
diff --git a/src/telelco-v0.2/ao_lco.c b/src/telelco-v0.2/ao_lco.c
index 418c0509..e8d16ca9 100644
--- a/src/telelco-v0.2/ao_lco.c
+++ b/src/telelco-v0.2/ao_lco.c
@@ -49,16 +49,16 @@ static uint16_t	ao_lco_tick_offset;
 static struct ao_pad_query	ao_pad_query;
 
 static void
-ao_lco_set_pad(void)
+ao_lco_set_pad(uint8_t pad)
 {
-	ao_seven_segment_set(AO_LCO_PAD_DIGIT, ao_lco_pad + 1);
+	ao_seven_segment_set(AO_LCO_PAD_DIGIT, pad + 1);
 }
 
 static void
-ao_lco_set_box(void)
+ao_lco_set_box(uint8_t box)
 {
-	ao_seven_segment_set(AO_LCO_BOX_DIGIT_1, ao_lco_box % 10);
-	ao_seven_segment_set(AO_LCO_BOX_DIGIT_10, ao_lco_box / 10);
+	ao_seven_segment_set(AO_LCO_BOX_DIGIT_1, box % 10);
+	ao_seven_segment_set(AO_LCO_BOX_DIGIT_10, box / 10);
 }
 
 #define MASK_SIZE(n)	(((n) + 7) >> 3)
@@ -103,8 +103,6 @@ ao_lco_input(void)
 	int8_t	dir, new_box, new_pad;
 
 	ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200));
-	ao_lco_set_pad();
-	ao_lco_set_box();
 	for (;;) {
 		ao_event_get(&event);
 		PRINTD("event type %d unit %d value %d\n",
@@ -114,11 +112,9 @@ ao_lco_input(void)
 			switch (event.unit) {
 			case AO_QUADRATURE_PAD:
 				if (!ao_lco_armed) {
-					if (event.value == ao_lco_pad)
-						break;
-					dir = ((int8_t) event.value - (int8_t) ao_lco_pad) > 0 ? 1 : -1;
-					new_pad = event.value;
-					while (!ao_lco_pad_present(new_pad)) {
+					dir = (int8_t) event.value;
+					new_pad = ao_lco_pad;
+					do {
 						new_pad += dir;
 						if (new_pad > AO_PAD_MAX_CHANNELS)
 							new_pad = 0;
@@ -126,34 +122,30 @@ ao_lco_input(void)
 							new_pad = AO_PAD_MAX_CHANNELS - 1;
 						if (new_pad == ao_lco_pad)
 							break;
-					}
+					} while (!ao_lco_pad_present(new_pad));
 					if (new_pad != ao_lco_pad) {
 						ao_lco_pad = new_pad;
-						ao_quadrature_count[AO_QUADRATURE_PAD] = ao_lco_pad;
-						ao_lco_set_pad();
+						ao_lco_set_pad(ao_lco_pad);
 					}
 				}
 				break;
 			case AO_QUADRATURE_BOX:
 				if (!ao_lco_armed) {
-					if (event.value == ao_lco_box)
-						break;
-					dir = ((int8_t) event.value - (int8_t) ao_lco_box) > 0 ? 1 : -1;
-					new_box = event.value;
-					while (!ao_lco_box_present(new_box)) {
+					dir = (int8_t) event.value;
+					new_box = ao_lco_box;
+					do {
 						new_box += dir;
 						if (new_box > ao_lco_max_box)
 							new_box = ao_lco_min_box;
 						else if (new_box < ao_lco_min_box)
-							new_box = ao_lco_min_box;
+							new_box = ao_lco_max_box;
 						if (new_box == ao_lco_box)
 							break;
-					}
-					ao_quadrature_count[AO_QUADRATURE_PAD] = new_box;
+					} while (!ao_lco_box_present(new_box));
 					if (ao_lco_box != new_box) {
 						ao_lco_box = new_box;
 						ao_lco_got_channels = 0;
-						ao_lco_set_box();
+						ao_lco_set_box(ao_lco_box);
 					}
 				}
 				break;
@@ -219,7 +211,7 @@ ao_lco_update(void)
 		ao_lco_valid = 1;
 		if (!c) {
 			ao_lco_pad = ao_lco_pad_first();
-			ao_lco_set_pad();
+			ao_lco_set_pad(ao_lco_pad);
 		}
 	} else
 		ao_lco_valid = 0;
@@ -232,13 +224,24 @@ ao_lco_update(void)
 	       query.igniter_status[2],
 	       query.igniter_status[3]);
 #endif
-
 	ao_wakeup(&ao_pad_query);
 }
 
 static void
+ao_lco_box_reset_present(void)
+{
+	ao_lco_min_box = 0xff;
+	ao_lco_max_box = 0x00;
+	memset(ao_lco_box_mask, 0, sizeof (ao_lco_box_mask));
+}
+
+static void
 ao_lco_box_set_present(uint8_t box)
 {
+	if (box < ao_lco_min_box)
+		ao_lco_min_box = box;
+	if (box > ao_lco_max_box)
+		ao_lco_max_box = box;
 	if (box >= AO_PAD_MAX_BOXES)
 		return;
 	ao_lco_box_mask[MASK_ID(box)] |= 1 << MASK_SHIFT(box);
@@ -249,19 +252,18 @@ ao_lco_search(void)
 {
 	uint16_t	tick_offset;
 	int8_t		r;
-
-	ao_lco_min_box = 0xff;
-	ao_lco_max_box = 0x00;
-	for (ao_lco_box = 0; ao_lco_box < AO_PAD_MAX_BOXES; ao_lco_box++) {
-		if ((ao_lco_box % 10) == 0)
-			ao_lco_set_box();
-		r = ao_lco_query(ao_lco_box, &ao_pad_query, &ao_lco_tick_offset);
+	uint8_t		box;
+
+	ao_lco_box_reset_present();
+	for (box = 0; box < AO_PAD_MAX_BOXES; box++) {
+		if ((box % 10) == 0)
+			ao_lco_set_box(box);
+		tick_offset = 0;
+		r = ao_lco_query(box, &ao_pad_query, &tick_offset);
+		PRINTD("box %d result %d\n", box, r);
 		if (r == AO_RADIO_CMAC_OK) {
-			if (ao_lco_box < ao_lco_min_box)
-				ao_lco_min_box = ao_lco_box;
-			if (ao_lco_box > ao_lco_max_box)
-				ao_lco_max_box = ao_lco_box;
-			ao_lco_box_set_present(ao_lco_box);
+			ao_lco_box_set_present(box);
+			ao_delay(AO_MS_TO_TICKS(30));
 		}
 	}
 	if (ao_lco_min_box <= ao_lco_max_box)
@@ -271,6 +273,8 @@ ao_lco_search(void)
 	ao_lco_valid = 0;
 	ao_lco_got_channels = 0;
 	ao_lco_pad = 0;
+	ao_lco_set_pad(ao_lco_pad);
+	ao_lco_set_box(ao_lco_box);
 }
 
 static void
@@ -382,6 +386,7 @@ ao_lco_set_debug(void)
 
 __code struct ao_cmds ao_lco_cmds[] = {
 	{ ao_lco_set_debug,	"D <0 off, 1 on>\0Debug" },
+	{ ao_lco_search,	"s\0Search for pad boxes" },
 	{ 0, NULL }
 };
 #endif
diff --git a/src/telelco-v0.2/ao_pins.h b/src/telelco-v0.2/ao_pins.h
index 07ea1b45..62f221a1 100644
--- a/src/telelco-v0.2/ao_pins.h
+++ b/src/telelco-v0.2/ao_pins.h
@@ -43,6 +43,8 @@
 
 #define HAS_EEPROM		1
 #define USE_INTERNAL_FLASH	1
+#define USE_EEPROM_CONFIG	1
+#define USE_STORAGE_CONFIG	0
 #define HAS_USB			1
 #define HAS_BEEP		1
 #define HAS_RADIO		1
@@ -235,7 +237,6 @@
  */
 
 #define AO_QUADRATURE_COUNT	2
-#define AO_QUADRATURE_MODE	0
 
 #define AO_QUADRATURE_0_PORT	&stm_gpioe
 #define AO_QUADRATURE_0_A	3
diff --git a/src/telelco-v0.2/ao_telelco.c b/src/telelco-v0.2/ao_telelco.c
index 66bf0ba1..d9f7c693 100644
--- a/src/telelco-v0.2/ao_telelco.c
+++ b/src/telelco-v0.2/ao_telelco.c
@@ -28,6 +28,7 @@
 #include <ao_lco.h>
 #include <ao_lco_cmd.h>
 #include <ao_radio_cmac_cmd.h>
+#include <ao_eeprom.h>
 
 int
 main(void)
@@ -52,7 +53,7 @@ main(void)
 	ao_quadrature_init();
 	ao_button_init();
 
-	ao_storage_init();
+	ao_eeprom_init();
 	
 	ao_radio_init();
 
diff --git a/src/telemega-v0.1/Makefile b/src/telemega-v0.1/Makefile
index a72d08f2..0145f49c 100644
--- a/src/telemega-v0.1/Makefile
+++ b/src/telemega-v0.1/Makefile
@@ -25,6 +25,8 @@ INC = \
 	ao_task.h \
 	ao_whiten.h \
 	ao_sample_profile.h \
+	ao_quaternion.h \
+	math.h \
 	ao_mpu.h \
 	stm32l.h \
 	Makefile
@@ -44,6 +46,20 @@ INC = \
 #STACK_GUARD=ao_mpu_stm.c
 #STACK_GUARD_DEF=-DHAS_STACK_GUARD=1
 
+MATH_SRC=\
+	ef_acos.c \
+	ef_sqrt.c \
+	ef_rem_pio2.c \
+	kf_cos.c \
+	kf_sin.c \
+	kf_rem_pio2.c \
+	sf_copysign.c \
+	sf_cos.c \
+	sf_sin.c \
+	sf_fabs.c \
+	sf_floor.c \
+	sf_scalbn.c
+
 ALTOS_SRC = \
 	ao_boot_chain.c \
 	ao_interrupt.c \
@@ -59,6 +75,7 @@ ALTOS_SRC = \
 	ao_mutex.c \
 	ao_serial_stm.c \
 	ao_gps_skytraq.c \
+	ao_gps_show.c \
 	ao_gps_report_mega.c \
 	ao_ignite.c \
 	ao_freq.c \
@@ -73,6 +90,7 @@ ALTOS_SRC = \
 	ao_hmc5883.c \
 	ao_adc_stm.c \
 	ao_beep_stm.c \
+	ao_eeprom_stm.c \
 	ao_storage.c \
 	ao_m25.c \
 	ao_usb_stm.c \
@@ -92,6 +110,7 @@ ALTOS_SRC = \
 	ao_companion.c \
 	ao_pyro.c \
 	ao_aprs.c \
+	$(MATH_SRC) \
 	$(PROFILE) \
 	$(SAMPLE_PROFILE) \
 	$(STACK_GUARD)
@@ -104,14 +123,15 @@ CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STA
 
 PROGNAME=telemega-v0.1
 PROG=$(PROGNAME)-$(VERSION).elf
+HEX=$(PROGNAME)-$(VERSION).ihx
 
 SRC=$(ALTOS_SRC) ao_telemega.c
 OBJ=$(SRC:.c=.o)
 
-all: $(PROG)
+all: $(PROG) $(HEX)
 
 $(PROG): Makefile $(OBJ) altos.ld
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS)
 
 ../altitude-pa.h: make-altitude-pa
 	nickle $< > $@
@@ -124,7 +144,7 @@ ao_product.h: ao-make-product.5c ../Version
 distclean:	clean
 
 clean:
-	rm -f *.o $(PROGNAME)-*.elf
+	rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx
 	rm -f ao_product.h
 
 install:
diff --git a/src/telemega-v0.1/ao_pins.h b/src/telemega-v0.1/ao_pins.h
index 4c645871..daeb9f17 100644
--- a/src/telemega-v0.1/ao_pins.h
+++ b/src/telemega-v0.1/ao_pins.h
@@ -64,8 +64,11 @@
 #define ao_gps_set_speed	ao_serial3_set_speed
 #define ao_gps_fifo		(ao_stm_usart3.rx_fifo)
 
+#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX	(1024 * 1024)
 #define HAS_EEPROM		1
 #define USE_INTERNAL_FLASH	0
+#define USE_EEPROM_CONFIG	1
+#define USE_STORAGE_CONFIG	0
 #define HAS_USB			1
 #define HAS_BEEP		1
 #define HAS_RADIO		1
@@ -111,6 +114,7 @@
 #define HAS_GPS			1
 #define HAS_FLIGHT		1
 #define HAS_ADC			1
+#define HAS_ADC_TEMP		1
 #define HAS_LOG			1
 
 /*
@@ -120,28 +124,35 @@
 #define HAS_IGNITE		1
 #define HAS_IGNITE_REPORT	1
 
-#define AO_SENSE_DROGUE(p)	((p)->adc.sense[0])
-#define AO_SENSE_MAIN(p)	((p)->adc.sense[1])
+#define AO_SENSE_PYRO(p,n)	((p)->adc.sense[n])
+#define AO_SENSE_DROGUE(p)	((p)->adc.sense[4])
+#define AO_SENSE_MAIN(p)	((p)->adc.sense[5])
 #define AO_IGNITER_CLOSED	400
 #define AO_IGNITER_OPEN		60
 
-#define AO_IGNITER_DROGUE_PORT	(&stm_gpiod)
-#define AO_IGNITER_DROGUE_PIN	6
+/* Pyro A */
+#define AO_PYRO_PORT_0	(&stm_gpiod)
+#define AO_PYRO_PIN_0	6
 
-#define AO_IGNITER_MAIN_PORT	(&stm_gpiod)
-#define AO_IGNITER_MAIN_PIN	7
+/* Pyro B */
+#define AO_PYRO_PORT_1	(&stm_gpiod)
+#define AO_PYRO_PIN_1	7
 
-#define AO_PYRO_PORT_0	(&stm_gpiob)
-#define AO_PYRO_PIN_0	5
+/* Pyro C */
+#define AO_PYRO_PORT_2	(&stm_gpiob)
+#define AO_PYRO_PIN_2	5
 
-#define AO_PYRO_PORT_1	(&stm_gpioe)
-#define AO_PYRO_PIN_1	4
+/* Pyro D */
+#define AO_PYRO_PORT_3	(&stm_gpioe)
+#define AO_PYRO_PIN_3	4
 
-#define AO_PYRO_PORT_2	(&stm_gpioe)
-#define AO_PYRO_PIN_2	6
+/* Drogue */
+#define AO_IGNITER_DROGUE_PORT	(&stm_gpioe)
+#define AO_IGNITER_DROGUE_PIN	6
 
-#define AO_PYRO_PORT_3	(&stm_gpioe)
-#define AO_PYRO_PIN_3	5
+/* Main */
+#define AO_IGNITER_MAIN_PORT	(&stm_gpioe)
+#define AO_IGNITER_MAIN_PIN	5
 
 /* Number of general purpose pyro channels available */
 #define AO_PYRO_NUM	4
@@ -159,11 +170,16 @@ struct ao_adc {
 	int16_t			sense[AO_ADC_NUM_SENSE];
 	int16_t			v_batt;
 	int16_t			v_pbatt;
-	int16_t			accel_ref;
-	int16_t			accel;
 	int16_t			temp;
 };
 
+#define AO_ADC_DUMP(p) \
+	printf("tick: %5u A: %5d B: %5d C: %5d D: %5d drogue: %5d main: %5d batt: %5d pbatt: %5d temp: %5d\n", \
+	       (p)->tick, \
+	       (p)->adc.sense[0], (p)->adc.sense[1], (p)->adc.sense[2], \
+	       (p)->adc.sense[3], (p)->adc.sense[4], (p)->adc.sense[5], \
+	       (p)->adc.v_batt, (p)->adc.v_pbatt, (p)->adc.temp)
+
 #define AO_ADC_SENSE_A		0
 #define AO_ADC_SENSE_A_PORT	(&stm_gpioa)
 #define AO_ADC_SENSE_A_PIN	0
@@ -180,13 +196,13 @@ struct ao_adc {
 #define AO_ADC_SENSE_D_PORT	(&stm_gpioa)
 #define AO_ADC_SENSE_D_PIN	3
 
-#define AO_ADC_SENSE_E		4
-#define AO_ADC_SENSE_E_PORT	(&stm_gpioa)
-#define AO_ADC_SENSE_E_PIN	4
+#define AO_ADC_SENSE_DROGUE	4
+#define AO_ADC_SENSE_DROGUE_PORT	(&stm_gpioa)
+#define AO_ADC_SENSE_DROGUE_PIN	4
 
-#define AO_ADC_SENSE_F		22
-#define AO_ADC_SENSE_F_PORT	(&stm_gpioe)
-#define AO_ADC_SENSE_F_PIN	7
+#define AO_ADC_SENSE_MAIN	22
+#define AO_ADC_SENSE_MAIN_PORT	(&stm_gpioe)
+#define AO_ADC_SENSE_MAIN_PIN	7
 
 #define AO_ADC_V_BATT		8
 #define AO_ADC_V_BATT_PORT	(&stm_gpiob)
@@ -196,22 +212,13 @@ struct ao_adc {
 #define AO_ADC_V_PBATT_PORT	(&stm_gpiob)
 #define AO_ADC_V_PBATT_PIN	1
 
-#define AO_ADC_ACCEL_REF	10
-#define AO_ADC_ACCEL_REF_PORT	(&stm_gpioc)
-#define AO_ADC_ACCEL_REF_PIN	0
-
-#define AO_ADC_ACCEL		11
-#define AO_ADC_ACCEL_PORT	(&stm_gpioc)
-#define AO_ADC_ACCEL_PIN	1
-
 #define AO_ADC_TEMP		16
 
 #define AO_ADC_RCC_AHBENR	((1 << STM_RCC_AHBENR_GPIOAEN) | \
 				 (1 << STM_RCC_AHBENR_GPIOEEN) | \
-				 (1 << STM_RCC_AHBENR_GPIOBEN) | \
-				 (1 << STM_RCC_AHBENR_GPIOCEN))
+				 (1 << STM_RCC_AHBENR_GPIOBEN))
 
-#define AO_NUM_ADC_PIN		(AO_ADC_NUM_SENSE + 4)
+#define AO_NUM_ADC_PIN		(AO_ADC_NUM_SENSE + 2)
 
 #define AO_ADC_PIN0_PORT	AO_ADC_SENSE_A_PORT
 #define AO_ADC_PIN0_PIN		AO_ADC_SENSE_A_PIN
@@ -221,32 +228,26 @@ struct ao_adc {
 #define AO_ADC_PIN2_PIN		AO_ADC_SENSE_C_PIN
 #define AO_ADC_PIN3_PORT	AO_ADC_SENSE_D_PORT
 #define AO_ADC_PIN3_PIN		AO_ADC_SENSE_D_PIN
-#define AO_ADC_PIN4_PORT	AO_ADC_SENSE_E_PORT
-#define AO_ADC_PIN4_PIN		AO_ADC_SENSE_E_PIN
-#define AO_ADC_PIN5_PORT	AO_ADC_SENSE_F_PORT
-#define AO_ADC_PIN5_PIN		AO_ADC_SENSE_F_PIN
+#define AO_ADC_PIN4_PORT	AO_ADC_SENSE_DROGUE_PORT
+#define AO_ADC_PIN4_PIN		AO_ADC_SENSE_DROGUE_PIN
+#define AO_ADC_PIN5_PORT	AO_ADC_SENSE_MAIN_PORT
+#define AO_ADC_PIN5_PIN		AO_ADC_SENSE_MAIN_PIN
 #define AO_ADC_PIN6_PORT	AO_ADC_V_BATT_PORT
 #define AO_ADC_PIN6_PIN		AO_ADC_V_BATT_PIN
 #define AO_ADC_PIN7_PORT	AO_ADC_V_PBATT_PORT
 #define AO_ADC_PIN7_PIN		AO_ADC_V_PBATT_PIN
-#define AO_ADC_PIN8_PORT	AO_ADC_ACCEL_REF_PORT
-#define AO_ADC_PIN8_PIN		AO_ADC_ACCEL_REF_PIN
-#define AO_ADC_PIN9_PORT	AO_ADC_ACCEL_PORT
-#define AO_ADC_PIN9_PIN		AO_ADC_ACCEL_PIN
 
-#define AO_NUM_ADC	       	(AO_ADC_NUM_SENSE + 5)
+#define AO_NUM_ADC	       	(AO_ADC_NUM_SENSE + 3)
 
 #define AO_ADC_SQ1		AO_ADC_SENSE_A
 #define AO_ADC_SQ2		AO_ADC_SENSE_B
 #define AO_ADC_SQ3		AO_ADC_SENSE_C
 #define AO_ADC_SQ4		AO_ADC_SENSE_D
-#define AO_ADC_SQ5		AO_ADC_SENSE_E
-#define AO_ADC_SQ6		AO_ADC_SENSE_F
+#define AO_ADC_SQ5		AO_ADC_SENSE_DROGUE
+#define AO_ADC_SQ6		AO_ADC_SENSE_MAIN
 #define AO_ADC_SQ7		AO_ADC_V_BATT
 #define AO_ADC_SQ8		AO_ADC_V_PBATT
-#define AO_ADC_SQ9		AO_ADC_ACCEL_REF
-#define AO_ADC_SQ10		AO_ADC_ACCEL
-#define AO_ADC_SQ11		AO_ADC_TEMP
+#define AO_ADC_SQ9		AO_ADC_TEMP
 
 /*
  * Pressure sensor settings
@@ -296,7 +297,6 @@ struct ao_adc {
 #define AO_CC1120_MARC_GPIO	3
 #define AO_CC1120_MARC_GPIO_IOCFG	CC1120_IOCFG3
 
-
 #define HAS_BOOT_RADIO		0
 
 /*
@@ -316,8 +316,7 @@ struct ao_adc {
 #define AO_MPU6000_INT_PORT	(&stm_gpioc)
 #define AO_MPU6000_INT_PIN	13
 #define AO_MPU6000_I2C_INDEX	STM_I2C_INDEX(1)
-
-#define HAS_HIGHG_ACCEL		0
+#define HAS_IMU			1
 
 /*
  * mma655x
diff --git a/src/telemega-v0.1/ao_telemega.c b/src/telemega-v0.1/ao_telemega.c
index fbdab64a..7b035269 100644
--- a/src/telemega-v0.1/ao_telemega.c
+++ b/src/telemega-v0.1/ao_telemega.c
@@ -24,6 +24,7 @@
 #include <ao_packet.h>
 #include <ao_companion.h>
 #include <ao_profile.h>
+#include <ao_eeprom.h>
 #if HAS_SAMPLE_PROFILE
 #include <ao_sample_profile.h>
 #endif
@@ -71,6 +72,7 @@ main(void)
 	ao_mma655x_init();
 #endif
 
+	ao_eeprom_init();
 	ao_storage_init();
 	
 	ao_flight_init();
diff --git a/src/telemega-v0.3/.gitignore b/src/telemega-v1.0/.gitignore
index e67759a2..e67759a2 100644
--- a/src/telemega-v0.3/.gitignore
+++ b/src/telemega-v1.0/.gitignore
diff --git a/src/telemega-v1.0/Makefile b/src/telemega-v1.0/Makefile
new file mode 100644
index 00000000..543f7e74
--- /dev/null
+++ b/src/telemega-v1.0/Makefile
@@ -0,0 +1,153 @@
+#
+# AltOS build
+#
+#
+
+include ../stm/Makefile.defs
+
+INC = \
+	ao.h \
+	ao_arch.h \
+	ao_arch_funcs.h \
+	ao_companion.h \
+	ao_data.h \
+	ao_sample.h \
+	ao_pins.h \
+	altitude-pa.h \
+	ao_kalman.h \
+	ao_product.h \
+	ao_ms5607.h \
+	ao_hmc5883.h \
+	ao_mpu6000.h \
+	ao_mma655x.h \
+	ao_cc1120_CC1120.h \
+	ao_profile.h \
+	ao_task.h \
+	ao_whiten.h \
+	ao_sample_profile.h \
+	ao_quaternion.h \
+	math.h \
+	ao_mpu.h \
+	stm32l.h \
+	math.h \
+	Makefile
+
+#
+# Common AltOS sources
+#
+#	ao_hmc5883.c
+
+#PROFILE=ao_profile.c
+#PROFILE_DEF=-DAO_PROFILE=1
+
+#SAMPLE_PROFILE=ao_sample_profile.c \
+#	ao_sample_profile_timer.c
+#SAMPLE_PROFILE_DEF=-DHAS_SAMPLE_PROFILE=1
+
+#STACK_GUARD=ao_mpu_stm.c
+#STACK_GUARD_DEF=-DHAS_STACK_GUARD=1
+
+MATH_SRC=\
+	ef_acos.c \
+	ef_sqrt.c \
+	ef_rem_pio2.c \
+	kf_cos.c \
+	kf_sin.c \
+	kf_rem_pio2.c \
+	sf_copysign.c \
+	sf_cos.c \
+	sf_fabs.c \
+	sf_floor.c \
+	sf_scalbn.c \
+	sf_sin.c
+
+ALTOS_SRC = \
+	ao_boot_chain.c \
+	ao_interrupt.c \
+	ao_product.c \
+	ao_romconfig.c \
+	ao_cmd.c \
+	ao_config.c \
+	ao_task.c \
+	ao_led.c \
+	ao_stdio.c \
+	ao_panic.c \
+	ao_timer.c \
+	ao_mutex.c \
+	ao_serial_stm.c \
+	ao_gps_ublox.c \
+	ao_gps_show.c \
+	ao_gps_report_mega.c \
+	ao_ignite.c \
+	ao_freq.c \
+	ao_dma_stm.c \
+	ao_spi_stm.c \
+	ao_cc1120.c \
+	ao_fec_tx.c \
+	ao_fec_rx.c \
+	ao_data.c \
+	ao_ms5607.c \
+	ao_mma655x.c \
+	ao_hmc5883.c \
+	ao_adc_stm.c \
+	ao_beep_stm.c \
+	ao_eeprom_stm.c \
+	ao_storage.c \
+	ao_m25.c \
+	ao_usb_stm.c \
+	ao_exti_stm.c \
+	ao_report.c \
+	ao_i2c_stm.c \
+	ao_mpu6000.c \
+	ao_convert_pa.c \
+	ao_log.c \
+	ao_log_mega.c \
+	ao_sample.c \
+	ao_kalman.c \
+	ao_flight.c \
+	ao_telemetry.c \
+	ao_packet_slave.c \
+	ao_packet.c \
+	ao_companion.c \
+	ao_pyro.c \
+	ao_aprs.c \
+	$(MATH_SRC) \
+	$(PROFILE) \
+	$(SAMPLE_PROFILE) \
+	$(STACK_GUARD)
+
+PRODUCT=TeleMega-v1.0
+PRODUCT_DEF=-DTELEMEGA
+IDPRODUCT=0x0023
+
+CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STACK_GUARD_DEF) -Os -g
+
+PROGNAME=telemega-v1.0
+PROG=$(PROGNAME)-$(VERSION).elf
+HEX=$(PROGNAME)-$(VERSION).ihx
+
+SRC=$(ALTOS_SRC) ao_telemega.c
+OBJ=$(SRC:.c=.o)
+
+all: $(PROG) $(HEX)
+
+$(PROG): Makefile $(OBJ) altos.ld
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS)
+
+../altitude-pa.h: make-altitude-pa
+	nickle $< > $@
+
+$(OBJ): $(INC)
+
+ao_product.h: ao-make-product.5c ../Version
+	$(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@
+
+distclean:	clean
+
+clean:
+	rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx
+	rm -f ao_product.h
+
+install:
+
+uninstall:
diff --git a/src/telemega-v0.3/ao_pins.h b/src/telemega-v1.0/ao_pins.h
index b1504d28..95644dae 100644
--- a/src/telemega-v0.3/ao_pins.h
+++ b/src/telemega-v1.0/ao_pins.h
@@ -64,8 +64,11 @@
 #define ao_gps_set_speed	ao_serial3_set_speed
 #define ao_gps_fifo		(ao_stm_usart3.rx_fifo)
 
+#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX	(1024 * 1024)
 #define HAS_EEPROM		1
 #define USE_INTERNAL_FLASH	0
+#define USE_EEPROM_CONFIG	1
+#define USE_STORAGE_CONFIG	0
 #define HAS_USB			1
 #define HAS_BEEP		1
 #define HAS_RADIO		1
@@ -121,28 +124,35 @@
 #define HAS_IGNITE		1
 #define HAS_IGNITE_REPORT	1
 
-#define AO_SENSE_DROGUE(p)	((p)->adc.sense[0])
-#define AO_SENSE_MAIN(p)	((p)->adc.sense[1])
+#define AO_SENSE_PYRO(p,n)	((p)->adc.sense[n])
+#define AO_SENSE_DROGUE(p)	((p)->adc.sense[4])
+#define AO_SENSE_MAIN(p)	((p)->adc.sense[5])
 #define AO_IGNITER_CLOSED	400
 #define AO_IGNITER_OPEN		60
 
-#define AO_IGNITER_DROGUE_PORT	(&stm_gpiod)
-#define AO_IGNITER_DROGUE_PIN	6
+/* Pyro A */
+#define AO_PYRO_PORT_0	(&stm_gpiod)
+#define AO_PYRO_PIN_0	6
 
-#define AO_IGNITER_MAIN_PORT	(&stm_gpiod)
-#define AO_IGNITER_MAIN_PIN	7
+/* Pyro B */
+#define AO_PYRO_PORT_1	(&stm_gpiod)
+#define AO_PYRO_PIN_1	7
 
-#define AO_PYRO_PORT_0	(&stm_gpiob)
-#define AO_PYRO_PIN_0	5
+/* Pyro C */
+#define AO_PYRO_PORT_2	(&stm_gpiob)
+#define AO_PYRO_PIN_2	5
 
-#define AO_PYRO_PORT_1	(&stm_gpioe)
-#define AO_PYRO_PIN_1	4
+/* Pyro D */
+#define AO_PYRO_PORT_3	(&stm_gpioe)
+#define AO_PYRO_PIN_3	4
 
-#define AO_PYRO_PORT_2	(&stm_gpioe)
-#define AO_PYRO_PIN_2	6
+/* Drogue */
+#define AO_IGNITER_DROGUE_PORT	(&stm_gpioe)
+#define AO_IGNITER_DROGUE_PIN	6
 
-#define AO_PYRO_PORT_3	(&stm_gpioe)
-#define AO_PYRO_PIN_3	5
+/* Main */
+#define AO_IGNITER_MAIN_PORT	(&stm_gpioe)
+#define AO_IGNITER_MAIN_PIN	5
 
 /* Number of general purpose pyro channels available */
 #define AO_PYRO_NUM	4
@@ -163,6 +173,13 @@ struct ao_adc {
 	int16_t			temp;
 };
 
+#define AO_ADC_DUMP(p) \
+	printf("tick: %5u A: %5d B: %5d C: %5d D: %5d drogue: %5d main: %5d batt: %5d pbatt: %5d temp: %5d\n", \
+	       (p)->tick, \
+	       (p)->adc.sense[0], (p)->adc.sense[1], (p)->adc.sense[2], \
+	       (p)->adc.sense[3], (p)->adc.sense[4], (p)->adc.sense[5], \
+	       (p)->adc.v_batt, (p)->adc.v_pbatt, (p)->adc.temp)
+
 #define AO_ADC_SENSE_A		0
 #define AO_ADC_SENSE_A_PORT	(&stm_gpioa)
 #define AO_ADC_SENSE_A_PIN	0
@@ -179,13 +196,13 @@ struct ao_adc {
 #define AO_ADC_SENSE_D_PORT	(&stm_gpioa)
 #define AO_ADC_SENSE_D_PIN	3
 
-#define AO_ADC_SENSE_E		4
-#define AO_ADC_SENSE_E_PORT	(&stm_gpioa)
-#define AO_ADC_SENSE_E_PIN	4
+#define AO_ADC_SENSE_DROGUE	4
+#define AO_ADC_SENSE_DROGUE_PORT	(&stm_gpioa)
+#define AO_ADC_SENSE_DROGUE_PIN	4
 
-#define AO_ADC_SENSE_F		22
-#define AO_ADC_SENSE_F_PORT	(&stm_gpioe)
-#define AO_ADC_SENSE_F_PIN	7
+#define AO_ADC_SENSE_MAIN	22
+#define AO_ADC_SENSE_MAIN_PORT	(&stm_gpioe)
+#define AO_ADC_SENSE_MAIN_PIN	7
 
 #define AO_ADC_V_BATT		8
 #define AO_ADC_V_BATT_PORT	(&stm_gpiob)
@@ -211,10 +228,10 @@ struct ao_adc {
 #define AO_ADC_PIN2_PIN		AO_ADC_SENSE_C_PIN
 #define AO_ADC_PIN3_PORT	AO_ADC_SENSE_D_PORT
 #define AO_ADC_PIN3_PIN		AO_ADC_SENSE_D_PIN
-#define AO_ADC_PIN4_PORT	AO_ADC_SENSE_E_PORT
-#define AO_ADC_PIN4_PIN		AO_ADC_SENSE_E_PIN
-#define AO_ADC_PIN5_PORT	AO_ADC_SENSE_F_PORT
-#define AO_ADC_PIN5_PIN		AO_ADC_SENSE_F_PIN
+#define AO_ADC_PIN4_PORT	AO_ADC_SENSE_DROGUE_PORT
+#define AO_ADC_PIN4_PIN		AO_ADC_SENSE_DROGUE_PIN
+#define AO_ADC_PIN5_PORT	AO_ADC_SENSE_MAIN_PORT
+#define AO_ADC_PIN5_PIN		AO_ADC_SENSE_MAIN_PIN
 #define AO_ADC_PIN6_PORT	AO_ADC_V_BATT_PORT
 #define AO_ADC_PIN6_PIN		AO_ADC_V_BATT_PIN
 #define AO_ADC_PIN7_PORT	AO_ADC_V_PBATT_PORT
@@ -226,8 +243,8 @@ struct ao_adc {
 #define AO_ADC_SQ2		AO_ADC_SENSE_B
 #define AO_ADC_SQ3		AO_ADC_SENSE_C
 #define AO_ADC_SQ4		AO_ADC_SENSE_D
-#define AO_ADC_SQ5		AO_ADC_SENSE_E
-#define AO_ADC_SQ6		AO_ADC_SENSE_F
+#define AO_ADC_SQ5		AO_ADC_SENSE_DROGUE
+#define AO_ADC_SQ6		AO_ADC_SENSE_MAIN
 #define AO_ADC_SQ7		AO_ADC_V_BATT
 #define AO_ADC_SQ8		AO_ADC_V_PBATT
 #define AO_ADC_SQ9		AO_ADC_TEMP
@@ -295,14 +312,13 @@ struct ao_adc {
  * mpu6000
  */
 
-#define HAS_MPU6000		1	
+#define HAS_MPU6000		1
 #define AO_MPU6000_INT_PORT	(&stm_gpioe)
 #define AO_MPU6000_INT_PIN	0
 #define AO_MPU6000_SPI_BUS	AO_SPI_1_PE13_PE14_PE15
 #define AO_MPU6000_SPI_CS_PORT	(&stm_gpiod)
 #define AO_MPU6000_SPI_CS_PIN	2
-
-#define HAS_HIGHG_ACCEL		1
+#define HAS_IMU			1
 
 /*
  * mma655x
diff --git a/src/telemega-v0.3/ao_telemega.c b/src/telemega-v1.0/ao_telemega.c
index fbdab64a..7b035269 100644
--- a/src/telemega-v0.3/ao_telemega.c
+++ b/src/telemega-v1.0/ao_telemega.c
@@ -24,6 +24,7 @@
 #include <ao_packet.h>
 #include <ao_companion.h>
 #include <ao_profile.h>
+#include <ao_eeprom.h>
 #if HAS_SAMPLE_PROFILE
 #include <ao_sample_profile.h>
 #endif
@@ -71,6 +72,7 @@ main(void)
 	ao_mma655x_init();
 #endif
 
+	ao_eeprom_init();
 	ao_storage_init();
 	
 	ao_flight_init();
diff --git a/src/telemega-v0.3/flash-loader/Makefile b/src/telemega-v1.0/flash-loader/Makefile
index 8fda18cd..a11f800b 100644
--- a/src/telemega-v0.3/flash-loader/Makefile
+++ b/src/telemega-v1.0/flash-loader/Makefile
@@ -4,5 +4,5 @@
 #
 
 TOPDIR=../..
-HARDWARE=telemega-v0.3
+HARDWARE=telemega-v1.0
 include $(TOPDIR)/stm/Makefile-flash.defs
diff --git a/src/telemega-v0.3/flash-loader/ao_pins.h b/src/telemega-v1.0/flash-loader/ao_pins.h
index 1af92f13..1af92f13 100644
--- a/src/telemega-v0.3/flash-loader/ao_pins.h
+++ b/src/telemega-v1.0/flash-loader/ao_pins.h
diff --git a/src/telemetrum-v0.1-sky/Makefile b/src/telemetrum-v0.1-sky/Makefile
index 69cd3461..a6634c29 100644
--- a/src/telemetrum-v0.1-sky/Makefile
+++ b/src/telemetrum-v0.1-sky/Makefile
@@ -11,6 +11,7 @@ TM_INC = \
 
 TM_SRC = \
 	ao_gps_skytraq.c \
+	ao_gps_show.c \
 	ao_25lc1024.c
 
 include ../product/Makefile.telemetrum
diff --git a/src/telemetrum-v1.0/Makefile b/src/telemetrum-v1.0/Makefile
index 4aae84c8..476a3b0a 100644
--- a/src/telemetrum-v1.0/Makefile
+++ b/src/telemetrum-v1.0/Makefile
@@ -11,6 +11,7 @@ TM_INC = \
 TM_SRC = \
 	ao_companion.c \
 	ao_gps_skytraq.c \
+	ao_gps_show.c \
 	ao_at45db161d.c
 
 include ../product/Makefile.telemetrum
diff --git a/src/telemetrum-v1.1/Makefile b/src/telemetrum-v1.1/Makefile
index 4bea03db..e44be7f9 100644
--- a/src/telemetrum-v1.1/Makefile
+++ b/src/telemetrum-v1.1/Makefile
@@ -11,6 +11,7 @@ TM_INC =
 TM_SRC = \
 	ao_companion.c \
 	ao_gps_skytraq.c \
+	ao_gps_show.c \
 	ao_m25.c
 
 include ../product/Makefile.telemetrum
diff --git a/src/telemetrum-v1.2/Makefile b/src/telemetrum-v1.2/Makefile
index 4b650adf..f2285fbe 100644
--- a/src/telemetrum-v1.2/Makefile
+++ b/src/telemetrum-v1.2/Makefile
@@ -11,6 +11,7 @@ TM_INC =
 TM_SRC = \
 	ao_companion.c \
 	ao_gps_skytraq.c \
+	ao_gps_show.c \
 	ao_m25.c
 
 include ../product/Makefile.telemetrum
diff --git a/src/telemetrum-v2.0/.gitignore b/src/telemetrum-v2.0/.gitignore
new file mode 100644
index 00000000..35ce24d4
--- /dev/null
+++ b/src/telemetrum-v2.0/.gitignore
@@ -0,0 +1,2 @@
+ao_product.h
+telemetrum-*.elf
diff --git a/src/telemega-v0.3/Makefile b/src/telemetrum-v2.0/Makefile
index 398c7dab..cebc9cab 100644
--- a/src/telemega-v0.3/Makefile
+++ b/src/telemetrum-v2.0/Makefile
@@ -17,8 +17,6 @@ INC = \
 	ao_kalman.h \
 	ao_product.h \
 	ao_ms5607.h \
-	ao_hmc5883.h \
-	ao_mpu6000.h \
 	ao_mma655x.h \
 	ao_cc1120_CC1120.h \
 	ao_profile.h \
@@ -29,11 +27,6 @@ INC = \
 	stm32l.h \
 	Makefile
 
-#
-# Common AltOS sources
-#
-#	ao_hmc5883.c
-
 #PROFILE=ao_profile.c
 #PROFILE_DEF=-DAO_PROFILE=1
 
@@ -59,7 +52,8 @@ ALTOS_SRC = \
 	ao_mutex.c \
 	ao_serial_stm.c \
 	ao_gps_ublox.c \
-	ao_gps_report_mega.c \
+	ao_gps_show.c \
+	ao_gps_report_metrum.c \
 	ao_ignite.c \
 	ao_freq.c \
 	ao_dma_stm.c \
@@ -70,19 +64,17 @@ ALTOS_SRC = \
 	ao_data.c \
 	ao_ms5607.c \
 	ao_mma655x.c \
-	ao_hmc5883.c \
 	ao_adc_stm.c \
 	ao_beep_stm.c \
 	ao_storage.c \
 	ao_m25.c \
 	ao_usb_stm.c \
 	ao_exti_stm.c \
+	ao_eeprom_stm.c \
 	ao_report.c \
-	ao_i2c_stm.c \
-	ao_mpu6000.c \
 	ao_convert_pa.c \
 	ao_log.c \
-	ao_log_mega.c \
+	ao_log_metrum.c \
 	ao_sample.c \
 	ao_kalman.c \
 	ao_flight.c \
@@ -90,28 +82,28 @@ ALTOS_SRC = \
 	ao_packet_slave.c \
 	ao_packet.c \
 	ao_companion.c \
-	ao_pyro.c \
 	ao_aprs.c \
 	$(PROFILE) \
 	$(SAMPLE_PROFILE) \
 	$(STACK_GUARD)
 
-PRODUCT=TeleMega-v0.3
-PRODUCT_DEF=-DTELEMEGA
-IDPRODUCT=0x0023
+PRODUCT=TeleMetrum-v2.0
+PRODUCT_DEF=-DTELEMETRUM_V_2_0
+IDPRODUCT=0x000b
 
 CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STACK_GUARD_DEF) -Os -g
 
-PROGNAME=telemega-v0.3
+PROGNAME=telemetrum-v2.0
 PROG=$(PROGNAME)-$(VERSION).elf
+HEX=$(PROGNAME)-$(VERSION).ihx
 
-SRC=$(ALTOS_SRC) ao_telemega.c
+SRC=$(ALTOS_SRC) ao_telemetrum.c
 OBJ=$(SRC:.c=.o)
 
-all: $(PROG)
+all: $(PROG) $(HEX)
 
 $(PROG): Makefile $(OBJ) altos.ld
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS)
 
 ../altitude-pa.h: make-altitude-pa
 	nickle $< > $@
@@ -124,7 +116,7 @@ ao_product.h: ao-make-product.5c ../Version
 distclean:	clean
 
 clean:
-	rm -f *.o $(PROGNAME)-*.elf
+	rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx
 	rm -f ao_product.h
 
 install:
diff --git a/src/telemetrum-v2.0/ao_pins.h b/src/telemetrum-v2.0/ao_pins.h
new file mode 100644
index 00000000..02f0f5e3
--- /dev/null
+++ b/src/telemetrum-v2.0/ao_pins.h
@@ -0,0 +1,293 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_PINS_H_
+#define _AO_PINS_H_
+
+#define HAS_TASK_QUEUE		1
+
+/* 8MHz High speed external crystal */
+#define AO_HSE			8000000
+
+/* PLLVCO = 96MHz (so that USB will work) */
+#define AO_PLLMUL		12
+#define AO_RCC_CFGR_PLLMUL	(STM_RCC_CFGR_PLLMUL_12)
+
+/* SYSCLK = 32MHz (no need to go faster than CPU) */
+#define AO_PLLDIV		3
+#define AO_RCC_CFGR_PLLDIV	(STM_RCC_CFGR_PLLDIV_3)
+
+/* HCLK = 32MHz (CPU clock) */
+#define AO_AHB_PRESCALER	1
+#define AO_RCC_CFGR_HPRE_DIV	STM_RCC_CFGR_HPRE_DIV_1
+
+/* Run APB1 at 16MHz (HCLK/2) */
+#define AO_APB1_PRESCALER	2
+#define AO_RCC_CFGR_PPRE1_DIV	STM_RCC_CFGR_PPRE2_DIV_2
+
+/* Run APB2 at 16MHz (HCLK/2) */
+#define AO_APB2_PRESCALER	2
+#define AO_RCC_CFGR_PPRE2_DIV	STM_RCC_CFGR_PPRE2_DIV_2
+
+#define HAS_SERIAL_1		0
+#define USE_SERIAL_1_STDIN	0
+#define SERIAL_1_PB6_PB7	0
+#define SERIAL_1_PA9_PA10	1
+
+#define HAS_SERIAL_2		0
+#define USE_SERIAL_2_STDIN	0
+#define SERIAL_2_PA2_PA3	0
+#define SERIAL_2_PD5_PD6	0
+
+#define HAS_SERIAL_3		1
+#define USE_SERIAL_3_STDIN	0
+#define SERIAL_3_PB10_PB11	1
+#define SERIAL_3_PC10_PC11	0
+#define SERIAL_3_PD8_PD9	0
+
+#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX	(512 * 1024)
+#define HAS_EEPROM		1
+#define USE_INTERNAL_FLASH	0
+#define USE_EEPROM_CONFIG	1
+#define USE_STORAGE_CONFIG	0
+#define HAS_USB			1
+#define HAS_BEEP		1
+#define BEEPER_CHANNEL		4
+#define HAS_RADIO		1
+#define HAS_TELEMETRY		1
+#define HAS_APRS		1
+
+#define HAS_SPI_1		1
+#define SPI_1_PA5_PA6_PA7	1	/* Barometer */
+#define SPI_1_PB3_PB4_PB5	1	/* Accelerometer */
+#define SPI_1_PE13_PE14_PE15	0
+#define SPI_1_OSPEEDR		STM_OSPEEDR_10MHz
+
+#define HAS_SPI_2		1
+#define SPI_2_PB13_PB14_PB15	1	/* Flash, Companion, Radio */
+#define SPI_2_PD1_PD3_PD4	0
+#define SPI_2_OSPEEDR		STM_OSPEEDR_10MHz
+
+#define SPI_2_PORT		(&stm_gpiob)
+#define SPI_2_SCK_PIN		13
+#define SPI_2_MISO_PIN		14
+#define SPI_2_MOSI_PIN		15
+
+#define HAS_I2C_1		0
+#define I2C_1_PB8_PB9		0
+
+#define HAS_I2C_2		0
+#define I2C_2_PB10_PB11		0
+
+#define PACKET_HAS_SLAVE	1
+#define PACKET_HAS_MASTER	0
+
+#define LOW_LEVEL_DEBUG		0
+
+#define LED_PORT_ENABLE		STM_RCC_AHBENR_GPIOCEN
+#define LED_PORT		(&stm_gpioc)
+#define LED_PIN_RED		14
+#define LED_PIN_GREEN		15
+#define AO_LED_RED		(1 << LED_PIN_RED)
+#define AO_LED_GREEN		(1 << LED_PIN_GREEN)
+
+#define LEDS_AVAILABLE		(AO_LED_RED | AO_LED_GREEN)
+
+#define HAS_GPS			1
+#define HAS_FLIGHT		1
+#define HAS_ADC			1
+#define HAS_ADC_TEMP		1
+#define HAS_LOG			1
+
+/*
+ * Igniter
+ */
+
+#define HAS_IGNITE		1
+#define HAS_IGNITE_REPORT	1
+
+#define AO_SENSE_DROGUE(p)	((p)->adc.sense_a)
+#define AO_SENSE_MAIN(p)	((p)->adc.sense_m)
+#define AO_IGNITER_CLOSED	400
+#define AO_IGNITER_OPEN		60
+
+/* Drogue */
+#define AO_IGNITER_DROGUE_PORT	(&stm_gpioa)
+#define AO_IGNITER_DROGUE_PIN	8
+
+/* Main */
+#define AO_IGNITER_MAIN_PORT	(&stm_gpioa)
+#define AO_IGNITER_MAIN_PIN	9
+
+#define AO_IGNITER_SET_DROGUE(v)	stm_gpio_set(AO_IGNITER_DROGUE_PORT, AO_IGNITER_DROGUE_PIN, v)
+#define AO_IGNITER_SET_MAIN(v)		stm_gpio_set(AO_IGNITER_MAIN_PORT, AO_IGNITER_MAIN_PIN, v)
+
+/*
+ * ADC
+ */
+#define AO_DATA_RING		32
+#define AO_ADC_NUM_SENSE	2
+
+struct ao_adc {
+	int16_t			sense_a;
+	int16_t			sense_m;
+	int16_t			v_batt;
+	int16_t			temp;
+};
+
+#define AO_ADC_DUMP(p) \
+	printf("tick: %5u drogue: %5d main: %5d batt: %5d\n", \
+	       (p)->tick, \
+	       (p)->adc.sense_a, (p)->adc.sense_m, \
+	       (p)->adc.v_batt);
+
+#define AO_ADC_SENSE_DROGUE	0
+#define AO_ADC_SENSE_DROGUE_PORT	(&stm_gpioa)
+#define AO_ADC_SENSE_DROGUE_PIN	0
+
+#define AO_ADC_SENSE_MAIN	1
+#define AO_ADC_SENSE_MAIN_PORT	(&stm_gpioa)
+#define AO_ADC_SENSE_MAIN_PIN	1
+
+#define AO_ADC_V_BATT		8
+#define AO_ADC_V_BATT_PORT	(&stm_gpiob)
+#define AO_ADC_V_BATT_PIN	0
+
+#define AO_ADC_TEMP		16
+
+#define AO_ADC_RCC_AHBENR	((1 << STM_RCC_AHBENR_GPIOAEN) | \
+				 (1 << STM_RCC_AHBENR_GPIOEEN) | \
+				 (1 << STM_RCC_AHBENR_GPIOBEN))
+
+#define AO_NUM_ADC_PIN		3
+
+#define AO_ADC_PIN0_PORT	AO_ADC_SENSE_DROGUE_PORT
+#define AO_ADC_PIN0_PIN		AO_ADC_SENSE_DROGUE_PIN
+#define AO_ADC_PIN1_PORT	AO_ADC_SENSE_MAIN_PORT
+#define AO_ADC_PIN1_PIN		AO_ADC_SENSE_MAIN_PIN
+#define AO_ADC_PIN2_PORT	AO_ADC_V_BATT_PORT
+#define AO_ADC_PIN2_PIN		AO_ADC_V_BATT_PIN
+
+#define AO_NUM_ADC	       	(AO_NUM_ADC_PIN + 1)
+
+#define AO_ADC_SQ1		AO_ADC_SENSE_DROGUE
+#define AO_ADC_SQ2		AO_ADC_SENSE_MAIN
+#define AO_ADC_SQ3		AO_ADC_V_BATT
+#define AO_ADC_SQ4		AO_ADC_TEMP
+
+/*
+ * GPS
+ */
+
+#define AO_SERIAL_SPEED_UBLOX	AO_SERIAL_SPEED_9600
+
+#define ao_gps_getchar		ao_serial3_getchar
+#define ao_gps_putchar		ao_serial3_putchar
+#define ao_gps_set_speed	ao_serial3_set_speed
+#define ao_gps_fifo		(ao_stm_usart3.rx_fifo)
+
+/*
+ * Pressure sensor settings
+ */
+#define HAS_MS5607		1
+#define HAS_MS5611		0
+#define AO_MS5607_PRIVATE_PINS	1
+#define AO_MS5607_CS_PORT	(&stm_gpiob)
+#define AO_MS5607_CS_PIN	12
+#define AO_MS5607_CS_MASK	(1 << AO_MS5607_CS)
+#define AO_MS5607_MISO_PORT	(&stm_gpioa)
+#define AO_MS5607_MISO_PIN	6
+#define AO_MS5607_MISO_MASK	(1 << AO_MS5607_MISO)
+#define AO_MS5607_SPI_INDEX	AO_SPI_1_PA5_PA6_PA7
+
+/*
+ * SPI Flash memory
+ */
+
+#define M25_MAX_CHIPS		1
+#define AO_M25_SPI_CS_PORT	(&stm_gpiob)
+#define AO_M25_SPI_CS_MASK	(1 << 8)
+#define AO_M25_SPI_BUS		AO_SPI_2_PB13_PB14_PB15
+
+/*
+ * Radio (cc1120)
+ */
+
+/* gets pretty close to 434.550 */
+
+#define AO_RADIO_CAL_DEFAULT 	0x6ca333
+
+#define AO_FEC_DEBUG		0
+#define AO_CC1120_SPI_CS_PORT	(&stm_gpioa)
+#define AO_CC1120_SPI_CS_PIN	2
+#define AO_CC1120_SPI_BUS	AO_SPI_2_PB13_PB14_PB15
+#define AO_CC1120_SPI		stm_spi2
+
+#define AO_CC1120_INT_PORT		(&stm_gpioa)
+#define AO_CC1120_INT_PIN		(3)
+#define AO_CC1120_MCU_WAKEUP_PORT	(&stm_gpioa)
+#define AO_CC1120_MCU_WAKEUP_PIN	(4)
+
+#define AO_CC1120_INT_GPIO	2
+#define AO_CC1120_INT_GPIO_IOCFG	CC1120_IOCFG2
+
+#define AO_CC1120_MARC_GPIO	3
+#define AO_CC1120_MARC_GPIO_IOCFG	CC1120_IOCFG3
+
+#define HAS_BOOT_RADIO		0
+
+#define HAS_HIGHG_ACCEL		1
+
+/*
+ * mma655x
+ */
+
+#define HAS_MMA655X		1
+#define AO_MMA655X_SPI_INDEX	AO_SPI_1_PB3_PB4_PB5
+#define AO_MMA655X_CS_PORT	(&stm_gpiob)
+#define AO_MMA655X_CS_PIN	9
+#define AO_MMA655X_INVERT	1
+
+#define NUM_CMDS		16
+
+/*
+ * Companion
+ */
+
+#define AO_COMPANION_CS_PORT	(&stm_gpiob)
+#define AO_COMPANION_CS_PIN	(6)
+#define AO_COMPANION_SPI_BUS	AO_SPI_2_PB13_PB14_PB15
+
+/*
+ * Monitor
+ */
+
+#define HAS_MONITOR		0
+#define LEGACY_MONITOR		0
+#define HAS_MONITOR_PUT		1
+#define AO_MONITOR_LED		0
+#define HAS_RSSI		0
+
+/*
+ * Profiling Viterbi decoding
+ */
+
+#ifndef AO_PROFILE
+#define AO_PROFILE	       	0
+#endif
+
+#endif /* _AO_PINS_H_ */
diff --git a/src/telemetrum-v2.0/ao_telemetrum.c b/src/telemetrum-v2.0/ao_telemetrum.c
new file mode 100644
index 00000000..46599cb2
--- /dev/null
+++ b/src/telemetrum-v2.0/ao_telemetrum.c
@@ -0,0 +1,93 @@
+/*
+ * Copyright © 2011 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao.h>
+#include <ao_ms5607.h>
+#include <ao_mma655x.h>
+#include <ao_log.h>
+#include <ao_exti.h>
+#include <ao_packet.h>
+#include <ao_companion.h>
+#include <ao_eeprom.h>
+#include <ao_profile.h>
+#if HAS_SAMPLE_PROFILE
+#include <ao_sample_profile.h>
+#endif
+#if HAS_STACK_GUARD
+#include <ao_mpu.h>
+#endif
+
+int
+main(void)
+{
+	ao_clock_init();
+	
+#if HAS_STACK_GUARD
+	ao_mpu_init();
+#endif
+
+	ao_task_init();
+	ao_serial_init();
+	ao_led_init(LEDS_AVAILABLE);
+	ao_led_on(AO_LED_RED);
+	ao_timer_init();
+
+	ao_spi_init();
+	ao_dma_init();
+	ao_exti_init();
+
+	ao_adc_init();
+#if HAS_BEEP
+	ao_beep_init();
+#endif
+	ao_cmd_init();
+
+#if HAS_MS5607
+	ao_ms5607_init();
+#endif
+#if HAS_MMA655X
+	ao_mma655x_init();
+#endif
+
+	ao_eeprom_init();
+
+	ao_storage_init();
+	
+	ao_flight_init();
+	ao_log_init();
+	ao_report_init();
+
+	ao_usb_init();
+	ao_gps_init();
+	ao_gps_report_metrum_init();
+	ao_telemetry_init();
+	ao_radio_init();
+	ao_packet_slave_init(FALSE);
+	ao_igniter_init();
+	ao_companion_init();
+
+	ao_config_init();
+#if AO_PROFILE
+	ao_profile_init();
+#endif
+#if HAS_SAMPLE_PROFILE
+	ao_sample_profile_init();
+#endif
+	
+	ao_start_scheduler();
+	return 0;
+}
diff --git a/src/telemetrum-v2.0/flash-loader/Makefile b/src/telemetrum-v2.0/flash-loader/Makefile
new file mode 100644
index 00000000..cb99c51e
--- /dev/null
+++ b/src/telemetrum-v2.0/flash-loader/Makefile
@@ -0,0 +1,8 @@
+#
+# AltOS flash loader build
+#
+#
+
+TOPDIR=../..
+HARDWARE=telemetrum-v2.0
+include $(TOPDIR)/stm/Makefile-flash.defs
diff --git a/src/telemetrum-v2.0/flash-loader/ao_pins.h b/src/telemetrum-v2.0/flash-loader/ao_pins.h
new file mode 100644
index 00000000..304bb7c3
--- /dev/null
+++ b/src/telemetrum-v2.0/flash-loader/ao_pins.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_PINS_H_
+#define _AO_PINS_H_
+
+/* External crystal at 8MHz */
+#define AO_HSE		8000000
+
+#include <ao_flash_stm_pins.h>
+
+/* Companion port cs_companion0 PB6 */
+
+#define AO_BOOT_PIN			1
+#define AO_BOOT_APPLICATION_GPIO	stm_gpiob
+#define AO_BOOT_APPLICATION_PIN		6
+#define AO_BOOT_APPLICATION_VALUE	1
+#define AO_BOOT_APPLICATION_MODE	AO_EXTI_MODE_PULL_UP
+
+#endif /* _AO_PINS_H_ */
diff --git a/src/telemini-v2.0/.gitignore b/src/telemini-v2.0/.gitignore
new file mode 100644
index 00000000..568925a2
--- /dev/null
+++ b/src/telemini-v2.0/.gitignore
@@ -0,0 +1,2 @@
+ao_product.h
+telemini-v2.0*
diff --git a/src/telemini-v2.0/.sdcdbrc b/src/telemini-v2.0/.sdcdbrc
new file mode 100644
index 00000000..b9f6129c
--- /dev/null
+++ b/src/telemini-v2.0/.sdcdbrc
@@ -0,0 +1,2 @@
+--directory=../cc1111:../product:../core:../drivers:.
+
diff --git a/src/telemini-v2.0/Makefile b/src/telemini-v2.0/Makefile
new file mode 100644
index 00000000..fcac2c48
--- /dev/null
+++ b/src/telemini-v2.0/Makefile
@@ -0,0 +1,115 @@
+#
+# TeleMini build file
+#
+
+TELEMINI_VER=2.0
+TELEMINI_DEF=2_0
+
+vpath %.c ..:../core:../cc1111:../drivers:../product
+vpath %.h ..:../core:../cc1111:../drivers:../product
+vpath ao-make-product.5c ../util
+
+ifndef VERSION
+include ../Version
+endif
+
+INC = \
+	ao.h \
+	ao_pins.h \
+	ao_arch.h \
+	ao_arch_funcs.h \
+	cc1111.h \
+	ao_ms5607.h \
+	ao_ms5607_convert_8051.c \
+	ao_product.h \
+	ao_int64.h \
+	ao_sample.h \
+	ao_exti.h \
+	ao_task.h
+
+CORE_SRC = \
+	ao_cmd.c \
+	ao_config.c \
+	ao_convert.c \
+	ao_flight.c \
+	ao_kalman.c \
+	ao_log.c \
+	ao_log_mini.c \
+	ao_mutex.c \
+	ao_panic.c \
+	ao_report.c \
+	ao_sample.c \
+	ao_stdio.c \
+	ao_storage.c \
+	ao_task.c \
+	ao_telemetry.c \
+	ao_freq.c \
+	ao_int64.c
+
+CC1111_SRC = \
+	ao_adc.c \
+	ao_dma.c \
+	ao_ignite.c \
+	ao_led.c \
+	ao_packet.c \
+	ao_packet_slave.c \
+	ao_radio.c \
+	ao_romconfig.c \
+	ao_string.c \
+	ao_spi.c \
+	ao_usb.c \
+	ao_convert_pa.c \
+	ao_beep.c \
+	ao_timer.c \
+	ao_exti.c \
+	_bp.c
+
+DRIVER_SRC = \
+	ao_ms5607.c \
+	ao_m25.c
+
+PRODUCT_SRC = \
+	ao_telemini.c
+
+SRC = \
+	$(CORE_SRC) \
+	$(CC1111_SRC) \
+	$(DRIVER_SRC) \
+	$(PRODUCT_SRC)
+
+PROGNAME = telemini-v$(TELEMINI_VER)
+PROG = $(PROGNAME)-$(VERSION).ihx
+PRODUCT=TeleMini-v$(TELEMINI_VER)
+PRODUCT_DEF=-DTELEMINI_V_$(TELEMINI_DEF)
+IDPRODUCT=0x0027
+
+include ../cc1111/Makefile.cc1111
+
+NICKLE=nickle
+CHECK_STACK=sh ../util/check-stack
+
+V=0
+# The user has explicitly enabled quiet compilation.
+ifeq ($(V),0)
+quiet = @printf "  $1 $2 $@\n"; $($1)
+endif
+# Otherwise, print the full command line.
+quiet ?= $($1)
+
+all: $(PROG)
+
+$(PROG): $(REL) Makefile
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL)
+	$(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@
+
+ao_product.h: ao-make-product.5c ../Version
+	$(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@
+
+distclean:	clean
+
+clean: clean-cc1111
+
+install:
+
+uninstall:
+
diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h
new file mode 100644
index 00000000..c4681ee2
--- /dev/null
+++ b/src/telemini-v2.0/ao_pins.h
@@ -0,0 +1,161 @@
+/*
+ * Copyright © 2010 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_PINS_H_
+#define _AO_PINS_H_
+
+#define HAS_RADIO	1
+
+#define HAS_FLIGHT		1
+#define HAS_USB			1
+#define USB_FORCE_FLIGHT_IDLE	1
+#define HAS_BEEP		1
+#define HAS_GPS			0
+#define HAS_SERIAL_1		0
+#define HAS_EEPROM		1
+#define HAS_LOG			1
+#define USE_INTERNAL_FLASH	0
+#define HAS_DBG			0
+#define PACKET_HAS_SLAVE	1
+
+#define AO_LED_GREEN		1
+#define AO_LED_RED		2
+#define LEDS_AVAILABLE		(AO_LED_RED|AO_LED_GREEN)
+#define HAS_EXTERNAL_TEMP	0
+#define HAS_ACCEL		0
+#define HAS_IGNITE		1
+#define HAS_IGNITE_REPORT 1
+#define HAS_MONITOR		0
+
+/*
+ * SPI
+ */
+
+#define HAS_SPI_0		1
+#define SPI_0_ALT_1		1
+#define HAS_SPI_1		1
+#define SPI_1_ALT_2		1
+#define SPI_CS_PORT		P1
+#define SPI_CS_SEL		P1SEL
+#define SPI_CS_DIR		P1DIR
+
+/*
+ * Flash
+ */
+#define AO_M25_SPI_BUS		1
+#define AO_M25_SPI_CS_PORT	SPI_CS_PORT
+#define AO_M25_SPI_CS_MASK	0x04	/* cs_flash is P1_2 */
+#define M25_MAX_CHIPS		1
+
+/*
+ * MS5607
+ */
+
+#define HAS_MS5607		1
+#define HAS_MS5611		0
+#define AO_MS5607_PRIVATE_PINS	0
+#define AO_MS5607_CS_PORT	P1
+#define AO_MS5607_CS_PIN	3
+#define AO_MS5607_CS		P1_3
+#define AO_MS5607_CS_MASK	(1 << AO_MS5607_CS_PIN)
+#define AO_MS5607_MISO_PORT	P0
+#define AO_MS5607_MISO_PIN	2
+#define AO_MS5607_MISO		P0_2
+#define AO_MS5607_MISO_MASK	(1 << AO_MS5607_MISO_PIN)
+#define AO_MS5607_SPI_INDEX	0
+#define HAS_EXTI_0		1
+
+/*
+ * Igniters
+ */
+#define AO_IGNITER_PORT		P2
+#define AO_IGNITER_DROGUE_PORT	AO_IGNITER_PORT
+#define AO_IGNITER_DROGUE	P2_3
+#define AO_IGNITER_MAIN		P2_4
+#define AO_IGNITER_DIR		P2DIR
+#define AO_IGNITER_DROGUE_BIT	(1 << 3)
+#define AO_IGNITER_MAIN_BIT	(1 << 4)
+#define AO_IGNITER_DROGUE_PIN	3
+#define AO_IGNITER_MAIN_PIN	4
+
+#define AO_IGNITER_DROGUE_PORT	AO_IGNITER_PORT
+#define AO_IGNITER_MAIN_PORT	AO_IGNITER_PORT
+
+/* test these values with real igniters */
+#define AO_IGNITER_OPEN		1000
+#define AO_IGNITER_CLOSED	7000
+#define AO_IGNITER_FIRE_TIME	AO_MS_TO_TICKS(50)
+#define AO_IGNITER_CHARGE_TIME	AO_MS_TO_TICKS(2000)
+
+#define AO_SEND_MINI
+#define AO_LOG_FORMAT		AO_LOG_FORMAT_TELEMINI
+
+/*
+ * ADC
+ */
+
+#define HAS_ADC			1
+#define AO_ADC_FIRST_PIN	0
+
+struct ao_adc {
+	int16_t		sense_a;	/* apogee continuity sense */
+	int16_t		sense_m;	/* main continuity sense */
+	int16_t		v_batt;		/* battery voltage */
+};
+
+#define ao_data_count	ao_adc_count
+
+#define AO_SENSE_DROGUE(p)	((p)->adc.sense_a)
+#define AO_SENSE_MAIN(p)	((p)->adc.sense_m)
+
+#define AO_NUM_TASKS	10
+
+#define AO_ADC_DUMP(p) \
+	printf("tick: %5u apogee: %5d main: %5d batt: %5d\n", \
+	       (p)->tick, (p)->adc.sense_a, (p)->adc.sense_m, (p)->adc.v_batt)
+
+#define AO_ADC_PINS	((1 << 0) | (1 << 1) | (1 << 4))
+
+#define FETCH_ADC() do {						\
+		a = (uint8_t __xdata *) (&ao_data_ring[ao_data_head].adc); \
+		switch (sequence) {					\
+		case 4:							\
+			a += 4;						\
+			sequence = 0;					\
+			break;						\
+		case 1:							\
+			a += 2;						\
+			sequence = 4;					\
+			break;						\
+		case 0:							\
+			sequence = 1;					\
+			break;						\
+		}							\
+		a[0] = ADCL;						\
+		a[1] = ADCH;						\
+		if (sequence) {						\
+			ADCCON3 = ADCCON3_EREF_VDD | ADCCON3_EDIV_512 | sequence; \
+			return;						\
+		}							\
+		AO_DATA_PRESENT(AO_DATA_ADC);				\
+		if (ao_data_present != AO_DATA_ALL)			\
+			return;						\
+		ao_data_ring[ao_data_head].ms5607_raw.pres = ao_ms5607_current.pres; \
+		ao_data_ring[ao_data_head].ms5607_raw.temp = ao_ms5607_current.temp; \
+	} while (0)
+
+#endif /* _AO_PINS_H_ */
diff --git a/src/telemini-v2.0/ao_telemini.c b/src/telemini-v2.0/ao_telemini.c
new file mode 100644
index 00000000..0d8dd1cb
--- /dev/null
+++ b/src/telemini-v2.0/ao_telemini.c
@@ -0,0 +1,65 @@
+/*
+ * Copyright © 2011 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "ao.h"
+#include "ao_pins.h"
+#include <ao_exti.h>
+
+__xdata uint8_t ao_force_freq;
+
+void
+main(void)
+{
+	ao_clock_init();
+
+#if HAS_STACK_GUARD
+	ao_mpu_init();
+#endif
+	ao_task_init();
+
+	/* Turn on the red LED until the system is stable */
+	ao_led_init(LEDS_AVAILABLE);
+	ao_led_on(AO_LED_RED);
+
+	ao_timer_init();
+
+	ao_spi_init();
+	ao_exti_init();
+	ao_adc_init();
+#if HAS_BEEP
+	ao_beep_init();
+#endif
+	ao_cmd_init();
+#if HAS_MS5607
+	ao_ms5607_init();
+#endif
+	ao_storage_init();
+
+	ao_flight_init();
+	ao_log_init();
+	ao_report_init();
+
+	ao_usb_init();
+	ao_telemetry_init();
+	ao_radio_init();
+	ao_packet_slave_init(TRUE);
+
+	ao_igniter_init();
+
+	ao_config_init();
+	ao_start_scheduler();
+}
diff --git a/src/telepyro-v0.1/Makefile b/src/telepyro-v0.1/Makefile
index 6743ba66..025b324a 100644
--- a/src/telepyro-v0.1/Makefile
+++ b/src/telepyro-v0.1/Makefile
@@ -5,18 +5,12 @@
 vpath % .:..:../core:../product:../drivers:../avr
 vpath ao-make-product.5c ../util
 
+include ../avr/Makefile.defs
+
 MCU=atmega32u4
 DUDECPUTYPE=m32u4
 #PROGRAMMER=stk500v2 -P usb
-PROGRAMMER=usbtiny
-LOADCMD=avrdude
 LOADARG=-p $(DUDECPUTYPE) -c $(PROGRAMMER) -e -U flash:w:
-CC=avr-gcc
-OBJCOPY=avr-objcopy
-
-ifndef VERSION
-include ../Version
-endif
 
 INC = \
 	ao.h \
diff --git a/src/telescience-pwm/.gitignore b/src/telescience-pwm/.gitignore
index dfccadf8..5d9c0970 100644
--- a/src/telescience-pwm/.gitignore
+++ b/src/telescience-pwm/.gitignore
@@ -1,2 +1,2 @@
-telescience-v0.1*
+telescience-pwm*
 ao_product.h
diff --git a/src/telescience-pwm/Makefile b/src/telescience-pwm/Makefile
index 43d77e2e..de81b8d7 100644
--- a/src/telescience-pwm/Makefile
+++ b/src/telescience-pwm/Makefile
@@ -5,18 +5,14 @@
 vpath % ..:../core:../product:../drivers:../avr
 vpath ao-make-product.5c ../util
 
+include ../avr/Makefile.defs
+
 MCU=atmega32u4
 DUDECPUTYPE=m32u4
 #PROGRAMMER=stk500v2 -P usb
-PROGRAMMER=usbtiny
-LOADCMD=avrdude
 LOADARG=-p $(DUDECPUTYPE) -c $(PROGRAMMER) -e -U flash:w:
-CC=avr-gcc
-OBJCOPY=avr-objcopy
 
-ifndef VERSION
-include ../Version
-endif
+#LDFLAGS=-L$(LDSCRIPTS) -Tavr5.x
 
 INC = \
 	ao.h \
diff --git a/src/telescience-v0.1/Makefile b/src/telescience-v0.1/Makefile
index d24128ef..6e4eb6de 100644
--- a/src/telescience-v0.1/Makefile
+++ b/src/telescience-v0.1/Makefile
@@ -5,18 +5,14 @@
 vpath % ..:../core:../product:../drivers:../avr
 vpath ao-make-product.5c ../util
 
+include ../avr/Makefile.defs
+
 MCU=atmega32u4
 DUDECPUTYPE=m32u4
 #PROGRAMMER=stk500v2 -P usb
-PROGRAMMER=usbtiny
-LOADCMD=avrdude
 LOADARG=-p $(DUDECPUTYPE) -c $(PROGRAMMER) -e -U flash:w:
-CC=avr-gcc
-OBJCOPY=avr-objcopy
 
-ifndef VERSION
-include ../Version
-endif
+#LDFLAGS=-L$(LDSCRIPTS) -Tavr5.x
 
 INC = \
 	ao.h \
diff --git a/src/telescience-v0.2/Makefile b/src/telescience-v0.2/Makefile
index f16ef268..6b7ea8c7 100644
--- a/src/telescience-v0.2/Makefile
+++ b/src/telescience-v0.2/Makefile
@@ -61,14 +61,15 @@ CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STA
 
 PROGNAME=telescience-v0.2
 PROG=$(PROGNAME)-$(VERSION).elf
+HEX=$(PROGNAME)-$(VERSION).ihx
 
 SRC=$(ALTOS_SRC) ao_telescience.c
 OBJ=$(SRC:.c=.o)
 
-all: $(PROG)
+all: $(PROG) $(HEX)
 
 $(PROG): Makefile $(OBJ) altos.ld
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS)
 
 $(OBJ): $(INC)
 
@@ -78,7 +79,7 @@ ao_product.h: ao-make-product.5c ../Version
 distclean:	clean
 
 clean:
-	rm -f *.o $(PROGNAME)-*.elf
+	rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx
 	rm -f ao_product.h
 
 install:
diff --git a/src/telescience-v0.2/flash-loader/Makefile b/src/telescience-v0.2/flash-loader/Makefile
new file mode 100644
index 00000000..7a2ceb77
--- /dev/null
+++ b/src/telescience-v0.2/flash-loader/Makefile
@@ -0,0 +1,8 @@
+#
+# AltOS flash loader build
+#
+#
+
+TOPDIR=../..
+HARDWARE=telescience-v0.2
+include $(TOPDIR)/stm/Makefile-flash.defs
diff --git a/src/telescience-v0.2/flash-loader/ao_pins.h b/src/telescience-v0.2/flash-loader/ao_pins.h
new file mode 100644
index 00000000..907ff341
--- /dev/null
+++ b/src/telescience-v0.2/flash-loader/ao_pins.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef _AO_PINS_H_
+#define _AO_PINS_H_
+
+/* External crystal at 8MHz */
+#define AO_HSE		8000000
+
+#include <ao_flash_stm_pins.h>
+
+/* Companion port SS PA4 */
+
+#define AO_BOOT_PIN		1
+#define AO_BOOT_APPLICATION_GPIO	stm_gpioa
+#define AO_BOOT_APPLICATION_PIN		4
+#define AO_BOOT_APPLICATION_VALUE	1
+#define AO_BOOT_APPLICATION_MODE	AO_EXTI_MODE_PULL_UP
+
+#endif /* _AO_PINS_H_ */
diff --git a/src/teleshield-v0.1/Makefile b/src/teleshield-v0.1/Makefile
index ab2a025f..e8b262ef 100644
--- a/src/teleshield-v0.1/Makefile
+++ b/src/teleshield-v0.1/Makefile
@@ -92,10 +92,10 @@ endif
 # Otherwise, print the full command line.
 quiet ?= $($1)
 
-all: ../$(PROG)
+all: $(PROG)
 
-../$(PROG): $(REL) Makefile
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) ..
+$(PROG): $(REL) Makefile
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL)
 	$(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@
 
 ao_product.h: ao-make-product.5c ../Version
diff --git a/src/teleterra-v0.2/Makefile b/src/teleterra-v0.2/Makefile
index 65db57ce..88637360 100644
--- a/src/teleterra-v0.2/Makefile
+++ b/src/teleterra-v0.2/Makefile
@@ -52,7 +52,8 @@ CC1111_SRC = \
 DRIVER_SRC = \
 	ao_m25.c \
 	ao_lcd.c \
-	ao_gps_skytraq.c
+	ao_gps_skytraq.c \
+	ao_gps_show.c
 
 PRODUCT_SRC = \
 	ao_teleterra_0_2.c \
@@ -85,10 +86,10 @@ endif
 # Otherwise, print the full command line.
 quiet ?= $($1)
 
-all: ../$(PROG)
+all: $(PROG)
 
-../$(PROG): $(REL) Makefile
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) ..
+$(PROG): $(REL) Makefile
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL)
 	$(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@
 
 ao_product.h: ao-make-product.5c ../Version
diff --git a/src/test/.gitignore b/src/test/.gitignore
index 90af6517..b8b2513e 100644
--- a/src/test/.gitignore
+++ b/src/test/.gitignore
@@ -5,6 +5,10 @@ ao_flight_test_baro
 ao_flight_test_accel
 ao_gps_test
 ao_gps_test_skytraq
+ao_gps_test_ublox
+ao_int64_test
+ao_ms5607_convert_test
+ao_quaternion_test
 ao_convert_test
 ao_convert_pa_test
 ao_fat_test
diff --git a/src/test/Makefile b/src/test/Makefile
index 76d50f16..7ae06a80 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -1,14 +1,15 @@
-vpath % ..:../core:../drivers:../util:../micropeak:../aes
+vpath % ..:../core:../drivers:../util:../micropeak:../aes:../product
 
 PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \
 	ao_gps_test ao_gps_test_skytraq ao_gps_test_ublox ao_convert_test ao_convert_pa_test ao_fec_test \
-	ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test
+	ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test \
+	ao_ms5607_convert_test ao_quaternion_test
 
-INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h
+INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h ao_quaternion.h
 
 KALMAN=make-kalman 
 
-CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -O0 -g -Wall
+CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -I../product -O0 -g -Wall
 
 all: $(PROGS) ao_aprs_data.wav
 
@@ -29,16 +30,16 @@ ao_flight_test_baro: ao_flight_test.c ao_host.h ao_flight.c  ao_sample.c ao_kalm
 ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c  ao_sample.c ao_kalman.c $(INCS)
 	cc $(CFLAGS) -o $@ -DFORCE_ACCEL=1 ao_flight_test.c
 
-ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS)
+ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c ao_pyro.c ao_pyro.h $(INCS)
 	cc -DTELEMEGA=1 $(CFLAGS) -o $@ $< -lm
 
 ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h
 	cc $(CFLAGS) -o $@ $<
 
-ao_gps_test_skytraq: ao_gps_test_skytraq.c ao_gps_skytraq.c ao_gps_print.c ao_host.h
+ao_gps_test_skytraq: ao_gps_test_skytraq.c ao_gps_skytraq.c ao_gps_print.c ao_gps_show.c ao_host.h
 	cc $(CFLAGS) -o $@ $<
 
-ao_gps_test_ublox: ao_gps_test_ublox.c ao_gps_ublox.c ao_gps_print.c ao_host.h ao_gps_ublox.h
+ao_gps_test_ublox: ao_gps_test_ublox.c ao_gps_ublox.c ao_gps_print.c ao_gps_show.c ao_host.h ao_gps_ublox.h
 	cc $(CFLAGS) -o $@ $<
 
 ao_convert_test: ao_convert_test.c ao_convert.c altitude.h
@@ -73,3 +74,12 @@ ao_fat_test: ao_fat_test.c ao_fat.c ao_bufio.c
 
 ao_aes_test: ao_aes_test.c ao_aes.c ao_aes_tables.c
 	cc $(CFLAGS) -o $@ ao_aes_test.c
+
+ao_int64_test: ao_int64_test.c ao_int64.c ao_int64.h
+	cc $(CFLAGS) -o $@ ao_int64_test.c
+
+ao_ms5607_convert_test: ao_ms5607_convert_test.c ao_ms5607_convert_8051.c ao_int64.c ao_int64.h
+	cc $(CFLAGS) -o $@ ao_ms5607_convert_test.c
+
+ao_quaternion_test: ao_quaternion_test.c ao_quaternion.h
+	cc $(CFLAGS) -o $@ ao_quaternion_test.c -lm
diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index 99bed7ee..0abb4090 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -20,10 +20,13 @@
 #include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <stddef.h>
 #include <string.h>
 #include <getopt.h>
 #include <math.h>
 
+#define GRAVITY 9.80665
+
 #define AO_HERTZ	100
 
 #define HAS_ADC 1
@@ -35,11 +38,17 @@
 #define AO_MS_TO_SPEED(ms)	((int16_t) ((ms) * 16))
 #define AO_MSS_TO_ACCEL(mss)	((int16_t) ((mss) * 16))
 
+#define AO_GPS_NEW_DATA		1
+#define AO_GPS_NEW_TRACKING	2
+
+int ao_gps_new;
+
 #if TELEMEGA
 #define AO_ADC_NUM_SENSE	6
 #define HAS_MS5607		1
 #define HAS_MPU6000		1
 #define HAS_MMA655X		1
+#define HAS_HMC5883 		1
 
 struct ao_adc {
 	int16_t			sense[AO_ADC_NUM_SENSE];
@@ -83,6 +92,95 @@ struct ao_adc {
 
 #include <ao_data.h>
 #include <ao_log.h>
+#include <ao_telemetry.h>
+
+#if TELEMEGA
+int ao_gps_count;
+struct ao_telemetry_location ao_gps_first;
+struct ao_telemetry_location ao_gps_prev;
+struct ao_telemetry_location ao_gps_static;
+
+struct ao_telemetry_satellite ao_gps_tracking;
+
+static inline double sqr(double a) { return a * a; }
+
+void
+cc_great_circle (double start_lat, double start_lon,
+		 double end_lat, double end_lon,
+		 double *dist, double *bearing)
+{
+	const double rad = M_PI / 180;
+	const double earth_radius = 6371.2 * 1000;	/* in meters */
+	double lat1 = rad * start_lat;
+	double lon1 = rad * -start_lon;
+	double lat2 = rad * end_lat;
+	double lon2 = rad * -end_lon;
+
+//	double d_lat = lat2 - lat1;
+	double d_lon = lon2 - lon1;
+
+	/* From http://en.wikipedia.org/wiki/Great-circle_distance */
+	double vdn = sqrt(sqr(cos(lat2) * sin(d_lon)) +
+			  sqr(cos(lat1) * sin(lat2) -
+			      sin(lat1) * cos(lat2) * cos(d_lon)));
+	double vdd = sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(d_lon);
+	double d = atan2(vdn,vdd);
+	double course;
+
+	if (cos(lat1) < 1e-20) {
+		if (lat1 > 0)
+			course = M_PI;
+		else
+			course = -M_PI;
+	} else {
+		if (d < 1e-10)
+			course = 0;
+		else
+			course = acos((sin(lat2)-sin(lat1)*cos(d)) /
+				      (sin(d)*cos(lat1)));
+		if (sin(lon2-lon1) > 0)
+			course = 2 * M_PI-course;
+	}
+	*dist = d * earth_radius;
+	*bearing = course * 180/M_PI;
+}
+
+double
+ao_distance_from_pad(void)
+{
+	double	dist, bearing;
+	if (!ao_gps_count)
+		return 0;
+	
+	cc_great_circle(ao_gps_first.latitude / 1e7,
+			ao_gps_first.longitude / 1e7,
+			ao_gps_static.latitude / 1e7,
+			ao_gps_static.longitude / 1e7,
+			&dist, &bearing);
+	return dist;
+}
+
+double
+ao_gps_angle(void)
+{
+	double	dist, bearing;
+	double	height;
+	double	angle;
+
+	if (ao_gps_count < 2)
+		return 0;
+
+	cc_great_circle(ao_gps_prev.latitude / 1e7,
+			ao_gps_prev.longitude / 1e7,
+			ao_gps_static.latitude / 1e7,
+			ao_gps_static.longitude / 1e7,
+			&dist, &bearing);
+	height = ao_gps_static.altitude - ao_gps_prev.altitude;
+
+	angle = atan2(dist, height);
+	return angle * 180/M_PI;
+}
+#endif
 
 #define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5))
 #define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
@@ -120,6 +218,7 @@ int	ao_summary = 0;
 #define ao_rdf_set(rdf)
 #define ao_packet_slave_start()
 #define ao_packet_slave_stop()
+#define flush()
 
 enum ao_igniter {
 	ao_igniter_drogue = 0,
@@ -137,6 +236,18 @@ int	tick_offset;
 
 static int32_t	ao_k_height;
 
+int16_t
+ao_time(void)
+{
+	return ao_data_static.tick;
+}
+
+void
+ao_delay(int16_t interval)
+{
+	return;
+}
+
 void
 ao_ignite(enum ao_igniter igniter)
 {
@@ -200,6 +311,8 @@ struct ao_cmds {
 #include <ao_ms5607.h>
 struct ao_ms5607_prom	ms5607_prom;
 #include "ao_ms5607_convert.c"
+#define AO_PYRO_NUM	4
+#include <ao_pyro.h>
 #else
 #include "ao_convert.c"
 #endif
@@ -210,6 +323,12 @@ struct ao_config {
 	int16_t		accel_minus_g;
 	uint8_t		pad_orientation;
 	uint16_t	apogee_lockout;
+#if TELEMEGA
+	struct ao_pyro	pyro[AO_PYRO_NUM];	/* minor version 12 */
+	int16_t		accel_zero_along;
+	int16_t		accel_zero_across;
+	int16_t		accel_zero_through;
+#endif
 };
 
 #define AO_PAD_ORIENTATION_ANTENNA_UP	0
@@ -222,7 +341,6 @@ struct ao_config ao_config;
 #define DATA_TO_XDATA(x) (x)
 
 
-#define GRAVITY 9.80665
 extern int16_t ao_ground_accel, ao_flight_accel;
 extern int16_t ao_accel_2g;
 
@@ -246,6 +364,22 @@ uint16_t	prev_tick;
 #include "ao_sqrt.c"
 #include "ao_sample.c"
 #include "ao_flight.c"
+#if TELEMEGA
+#define AO_PYRO_NUM	4
+
+#define AO_PYRO_0	0
+#define AO_PYRO_1	1
+#define AO_PYRO_2	2
+#define AO_PYRO_3	3
+
+static void
+ao_pyro_pin_set(uint8_t pin, uint8_t value)
+{
+	printf ("set pyro %d %d\n", pin, value);
+}
+
+#include "ao_pyro.c"
+#endif
 
 #define to_double(f)	((f) / 65536.0)
 
@@ -305,17 +439,20 @@ ao_test_exit(void)
 	exit(0);
 }
 
-#if HAS_MPU6000
-static double
-ao_mpu6000_accel(int16_t sensor)
-{
-	return sensor / 32767.0 * MPU6000_ACCEL_FULLSCALE * GRAVITY;
-}
+#ifdef TELEMEGA
+struct ao_azel {
+	int	az;
+	int	el;
+};
 
-static double
-ao_mpu6000_gyro(int32_t sensor)
+static void
+azel (struct ao_azel *r, struct ao_quaternion *q)
 {
-	return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE;
+	double	v;
+
+	r->az = floor (atan2(q->y, q->x) * 180/M_PI + 0.5);
+	v = sqrt (q->x*q->x + q->y*q->y);
+	r->el = floor (atan2(q->z, v) * 180/M_PI + 0.5);
 }
 #endif
 
@@ -342,6 +479,7 @@ ao_insert(void)
 		double	height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height;
 #endif
 
+		(void) accel;
 		if (!tick_offset)
 			tick_offset = -ao_data_static.tick;
 		if ((prev_tick - ao_data_static.tick) > 0x400)
@@ -372,25 +510,115 @@ ao_insert(void)
 		}
 
 		if (!ao_summary) {
+#if TELEMEGA
+			static struct ao_quaternion	ao_ground_mag;
+			static int			ao_ground_mag_set;
+
+			if (!ao_ground_mag_set) {
+				ao_quaternion_init_vector (&ao_ground_mag,
+							   ao_data_mag_across(&ao_data_static),
+							   ao_data_mag_through(&ao_data_static),
+							   ao_data_mag_along(&ao_data_static));
+				ao_quaternion_normalize(&ao_ground_mag, &ao_ground_mag);
+				ao_quaternion_rotate(&ao_ground_mag, &ao_ground_mag, &ao_rotation);
+				ao_ground_mag_set = 1;
+			}
+
+			struct ao_quaternion		ao_mag, ao_mag_rot;
+
+			ao_quaternion_init_vector(&ao_mag,
+						  ao_data_mag_across(&ao_data_static),
+						  ao_data_mag_through(&ao_data_static),
+						  ao_data_mag_along(&ao_data_static));
+
+			ao_quaternion_normalize(&ao_mag, &ao_mag);
+			ao_quaternion_rotate(&ao_mag_rot, &ao_mag, &ao_rotation);
+			
+			float				ao_dot;
+			int				ao_mag_angle;
+
+			ao_dot = ao_quaternion_dot(&ao_mag_rot, &ao_ground_mag);
+
+			struct ao_azel			ground_azel, mag_azel, rot_azel;
+
+			azel(&ground_azel, &ao_ground_mag);
+			azel(&mag_azel, &ao_mag);
+			azel(&rot_azel, &ao_mag_rot);
+
+			ao_mag_angle = floor (acos(ao_dot) * 180 / M_PI + 0.5);
+
+			(void) ao_mag_angle;
+
+			static struct ao_quaternion	ao_x = { .r = 0, .x = 1, .y = 0, .z = 0 };
+			struct ao_quaternion		ao_out;
+
+			ao_quaternion_rotate(&ao_out, &ao_x, &ao_rotation);
+
+			int	out = floor (atan2(ao_out.y, ao_out.x) * 180 / M_PI);
+
+#if 0
+			printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d mag_tilt %4d mag_rot %4d\n",
+				time,
+				ao_state_names[ao_flight_state],
+				ao_k_height / 65536.0,
+				ao_sample_orient, out,
+				mag_azel.el,
+				mag_azel.az);
+#endif
+			printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d dist %12.2f gps_tilt %4d gps_sats %2d\n",
+				time,
+				ao_state_names[ao_flight_state],
+				ao_k_height / 65536.0,
+				ao_sample_orient, out,
+				ao_distance_from_pad(),
+				(int) floor (ao_gps_angle() + 0.5),
+				(ao_gps_static.flags & 0xf) * 10);
+
+#if 0
+			printf ("\t\tstate %-8.8s ground az: %4d el %4d mag az %4d el %4d rot az %4d el %4d el_diff %4d az_diff %4d angle %4d tilt %4d ground %8.5f %8.5f %8.5f cur %8.5f %8.5f %8.5f rot %8.5f %8.5f %8.5f\n",
+				ao_state_names[ao_flight_state],
+				ground_azel.az, ground_azel.el,
+				mag_azel.az, mag_azel.el,
+				rot_azel.az, rot_azel.el,
+				ground_azel.el - rot_azel.el,
+				ground_azel.az - rot_azel.az,
+				ao_mag_angle,
+				ao_sample_orient,
+				ao_ground_mag.x,
+				ao_ground_mag.y,
+				ao_ground_mag.z,
+				ao_mag.x,
+				ao_mag.y,
+				ao_mag.z,
+				ao_mag_rot.x,
+				ao_mag_rot.y,
+				ao_mag_rot.z);
+#endif
+#endif
+
+#if 0
 			printf("%7.2f height %8.2f accel %8.3f "
 #if TELEMEGA
-			       "roll %8.3f angle %8.3f qangle %8.3f "
-			       "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f "
+			       "angle %5d "
+			       "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f mag_x %8d mag_y %8d, mag_z %8d mag_angle %4d "
 #endif
 			       "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n",
 			       time,
 			       height,
 			       accel,
 #if TELEMEGA
-			       ao_mpu6000_gyro(ao_sample_roll_angle) / 100.0,
-			       ao_mpu6000_gyro(ao_sample_angle) / 100.0,
-			       ao_sample_qangle,
+			       ao_sample_orient,
+
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_x),
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_y),
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_z),
 			       ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x),
 			       ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y),
 			       ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z),
+			       ao_data_static.hmc5883.x,
+			       ao_data_static.hmc5883.y,
+			       ao_data_static.hmc5883.z,
+			       ao_mag_angle,
 #endif
 			       ao_state_names[ao_flight_state],
 			       ao_k_height / 65536.0,
@@ -400,6 +628,7 @@ ao_insert(void)
 			       drogue_height,
 			       main_height,
 			       ao_error_h_sq_avg);
+#endif
 			
 //			if (ao_flight_state == ao_flight_landed)
 //				ao_test_exit();
@@ -407,125 +636,6 @@ ao_insert(void)
 	}
 }
 
-#define AO_MAX_CALLSIGN			8
-#define AO_MAX_VERSION			8
-#define AO_MAX_TELEMETRY		128
-
-struct ao_telemetry_generic {
-	uint16_t	serial;		/* 0 */
-	uint16_t	tick;		/* 2 */
-	uint8_t		type;		/* 4 */
-	uint8_t		payload[27];	/* 5 */
-	/* 32 */
-};
-
-#define AO_TELEMETRY_SENSOR_TELEMETRUM	0x01
-#define AO_TELEMETRY_SENSOR_TELEMINI	0x02
-#define AO_TELEMETRY_SENSOR_TELENANO	0x03
-
-struct ao_telemetry_sensor {
-	uint16_t	serial;		/*  0 */
-	uint16_t	tick;		/*  2 */
-	uint8_t		type;		/*  4 */
-
-	uint8_t         state;          /*  5 flight state */
-	int16_t		accel;		/*  6 accelerometer (TM only) */
-	int16_t		pres;		/*  8 pressure sensor */
-	int16_t		temp;		/* 10 temperature sensor */
-	int16_t		v_batt;		/* 12 battery voltage */
-	int16_t		sense_d;	/* 14 drogue continuity sense (TM/Tm) */
-	int16_t		sense_m;	/* 16 main continuity sense (TM/Tm) */
-
-	int16_t         acceleration;   /* 18 m/s² * 16 */
-	int16_t         speed;          /* 20 m/s * 16 */
-	int16_t         height;         /* 22 m */
-
-	int16_t		ground_pres;	/* 24 average pres on pad */
-	int16_t		ground_accel;	/* 26 average accel on pad */
-	int16_t		accel_plus_g;	/* 28 accel calibration at +1g */
-	int16_t		accel_minus_g;	/* 30 accel calibration at -1g */
-	/* 32 */
-};
-
-#define AO_TELEMETRY_CONFIGURATION	0x04
-
-struct ao_telemetry_configuration {
-	uint16_t	serial;				/*  0 */
-	uint16_t	tick;				/*  2 */
-	uint8_t		type;				/*  4 */
-
-	uint8_t         device;         		/*  5 device type */
-	uint16_t        flight;				/*  6 flight number */
-	uint8_t		config_major;			/*  8 Config major version */
-	uint8_t		config_minor;			/*  9 Config minor version */
-	uint16_t	apogee_delay;			/* 10 Apogee deploy delay in seconds */
-	uint16_t	main_deploy;			/* 12 Main deploy alt in meters */
-	uint16_t	flight_log_max;			/* 14 Maximum flight log size in kB */
-	char		callsign[AO_MAX_CALLSIGN];	/* 16 Radio operator identity */
-	char		version[AO_MAX_VERSION];	/* 24 Software version */
-	/* 32 */
-};
-
-#define AO_TELEMETRY_LOCATION		0x05
-
-#define AO_GPS_MODE_NOT_VALID		'N'
-#define AO_GPS_MODE_AUTONOMOUS		'A'
-#define AO_GPS_MODE_DIFFERENTIAL	'D'
-#define AO_GPS_MODE_ESTIMATED		'E'
-#define AO_GPS_MODE_MANUAL		'M'
-#define AO_GPS_MODE_SIMULATED		'S'
-
-struct ao_telemetry_location {
-	uint16_t	serial;		/*  0 */
-	uint16_t	tick;		/*  2 */
-	uint8_t		type;		/*  4 */
-
-	uint8_t         flags;		/*  5 Number of sats and other flags */
-	int16_t         altitude;	/*  6 GPS reported altitude (m) */
-	int32_t         latitude;	/*  8 latitude (degrees * 10⁷) */
-	int32_t         longitude;	/* 12 longitude (degrees * 10⁷) */
-	uint8_t         year;		/* 16 (- 2000) */
-	uint8_t         month;		/* 17 (1-12) */
-	uint8_t         day;		/* 18 (1-31) */
-	uint8_t         hour;		/* 19 (0-23) */
-	uint8_t         minute;		/* 20 (0-59) */
-	uint8_t         second;		/* 21 (0-59) */
-	uint8_t         pdop;		/* 22 (m * 5) */
-	uint8_t         hdop;		/* 23 (m * 5) */
-	uint8_t         vdop;		/* 24 (m * 5) */
-	uint8_t         mode;		/* 25 */
-	uint16_t	ground_speed;	/* 26 cm/s */
-	int16_t		climb_rate;	/* 28 cm/s */
-	uint8_t		course;		/* 30 degrees / 2 */
-	uint8_t		unused[1];	/* 31 */
-	/* 32 */
-};
-
-#define AO_TELEMETRY_SATELLITE		0x06
-
-struct ao_telemetry_satellite_info {
-	uint8_t		svid;
-	uint8_t		c_n_1;
-};
-
-struct ao_telemetry_satellite {
-	uint16_t				serial;		/*  0 */
-	uint16_t				tick;		/*  2 */
-	uint8_t					type;		/*  4 */
-	uint8_t					channels;	/*  5 number of reported sats */
-
-	struct ao_telemetry_satellite_info	sats[12];	/* 6 */
-	uint8_t					unused[2];	/* 30 */
-	/* 32 */
-};
-
-union ao_telemetry_all {
-	struct ao_telemetry_generic		generic;
-	struct ao_telemetry_sensor		sensor;
-	struct ao_telemetry_configuration	configuration;
-	struct ao_telemetry_location		location;
-	struct ao_telemetry_satellite		satellite;
-};
 
 uint16_t
 uint16(uint8_t *bytes, int off)
@@ -555,207 +665,6 @@ int32(uint8_t *bytes, int off)
 
 static int log_format;
 
-#if TELEMEGA
-
-static double
-ao_vec_norm(double x, double y, double z)
-{
-	return x*x + y*y + z*z;
-}
-
-static void
-ao_vec_normalize(double *x, double *y, double *z)
-{
-	double	scale = 1/sqrt(ao_vec_norm(*x, *y, *z));
-
-	*x *= scale;
-	*y *= scale;
-	*z *= scale;
-}
-
-struct ao_quat {
-	double	q0, q1, q2, q3;
-};
-
-static void
-ao_quat_mul(struct ao_quat *r, struct ao_quat *a, struct ao_quat *b)
-{
-	r->q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3;
-	r->q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2;
-	r->q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1;
-	r->q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0;
-}
-
-#if 0
-static void
-ao_quat_scale(struct ao_quat *r, struct ao_quat *a, double s)
-{
-	r->q0 = a->q0 * s;
-	r->q1 = a->q1 * s;
-	r->q2 = a->q2 * s;
-	r->q3 = a->q3 * s;
-}
-#endif
-
-static void
-ao_quat_conj(struct ao_quat *r, struct ao_quat *a)
-{
-	r->q0 =  a->q0;
-	r->q1 = -a->q1;
-	r->q2 = -a->q2;
-	r->q3 = -a->q3;
-}
-
-static void
-ao_quat_rot(struct ao_quat *r, struct ao_quat *a, struct ao_quat *q)
-{
-	struct ao_quat	t;
-	struct ao_quat	c;
-	ao_quat_mul(&t, q, a);
-	ao_quat_conj(&c, q);
-	ao_quat_mul(r, &t, &c);
-}
-
-static void
-ao_quat_from_angle(struct ao_quat *r,
-		   double x_rad,
-		   double y_rad,
-		   double z_rad)
-{
-	double angle = sqrt (x_rad * x_rad + y_rad * y_rad + z_rad * z_rad);
-	double s = sin(angle/2);
-	double c = cos(angle/2);
-
-	r->q0 = c;
-	r->q1 = x_rad * s / angle;
-	r->q2 = y_rad * s / angle;
-	r->q3 = z_rad * s / angle;
-}
-
-static void
-ao_quat_from_vector(struct ao_quat *r, double x, double y, double z)
-{
-	ao_vec_normalize(&x, &y, &z);
-	double	x_rad = atan2(z, y);
-	double	y_rad = atan2(x, z);
-	double	z_rad = atan2(y, x);
-
-	ao_quat_from_angle(r, x_rad, y_rad, z_rad);
-}
-
-static double
-ao_quat_norm(struct ao_quat *a)
-{
-	return (a->q0 * a->q0 +
-		a->q1 * a->q1 +
-		a->q2 * a->q2 +
-		a->q3 * a->q3);
-}
-
-static void
-ao_quat_normalize(struct ao_quat *a)
-{
-	double	norm = ao_quat_norm(a);
-
-	if (norm) {
-		double m = 1/sqrt(norm);
-
-		a->q0 *= m;
-		a->q1 *= m;
-		a->q2 *= m;
-		a->q3 *= m;
-	}
-}
-
-static struct ao_quat	ao_up, ao_current;
-static struct ao_quat	ao_orient;
-static int		ao_orient_tick;
-
-void
-set_orientation(double x, double y, double z, int tick)
-{
-	struct ao_quat	t;
-
-	printf ("set_orientation %g %g %g\n", x, y, z);
-	ao_quat_from_vector(&ao_orient, x, y, z);
-	ao_up.q1 = ao_up.q2 = 0;
-	ao_up.q0 = ao_up.q3 = sqrt(2)/2;
-	ao_orient_tick = tick;
-
-	ao_orient.q0 = 1;
-	ao_orient.q1 = 0;
-	ao_orient.q2 = 0;
-	ao_orient.q3 = 0;
-
-	printf ("orient (%g) %g %g %g up (%g) %g %g %g\n",
-		ao_orient.q0,
-		ao_orient.q1,
-		ao_orient.q2,
-		ao_orient.q3,
-		ao_up.q0,
-		ao_up.q1,
-		ao_up.q2,
-		ao_up.q3);
-
-	ao_quat_rot(&t, &ao_up, &ao_orient);
-	printf ("pad orient (%g) %g %g %g\n",
-		t.q0,
-		t.q1,
-		t.q2,
-		t.q3);
-
-}
-
-void
-update_orientation (double rate_x, double rate_y, double rate_z, int tick)
-{
-	struct ao_quat	q_dot;
-	double		lambda;
-	double		dt = (tick - ao_orient_tick) / 100.0;
-
-	ao_orient_tick = tick;
- 
-//	lambda = 1 - ao_quat_norm(&ao_orient);
-	lambda = 0;
-
-	q_dot.q0 = -0.5 * (ao_orient.q1 * rate_x + ao_orient.q2 * rate_y + ao_orient.q3 * rate_z) + lambda * ao_orient.q0;
-	q_dot.q1 =  0.5 * (ao_orient.q0 * rate_x + ao_orient.q2 * rate_z - ao_orient.q3 * rate_y) + lambda * ao_orient.q1;
-	q_dot.q2 =  0.5 * (ao_orient.q0 * rate_y + ao_orient.q3 * rate_x - ao_orient.q1 * rate_z) + lambda * ao_orient.q2;
-	q_dot.q3 =  0.5 * (ao_orient.q0 * rate_z + ao_orient.q1 * rate_y - ao_orient.q2 * rate_x) + lambda * ao_orient.q3;
-
-#if 0
-	printf ("update_orientation %g %g %g (%g s)\n", rate_x, rate_y, rate_z, dt);
-	printf ("q_dot (%g) %g %g %g\n",
-		q_dot.q0,
-		q_dot.q1,
-		q_dot.q2,
-		q_dot.q3);
-#endif
-
-	ao_orient.q0 += q_dot.q0 * dt;
-	ao_orient.q1 += q_dot.q1 * dt;
-	ao_orient.q2 += q_dot.q2 * dt;
-	ao_orient.q3 += q_dot.q3 * dt;
-
-	ao_quat_normalize(&ao_orient);
-
-	ao_quat_rot(&ao_current, &ao_up, &ao_orient);
-
-	ao_sample_qangle = 180 / M_PI * acos(ao_current.q3 * sqrt(2));
-#if 0
-	printf ("orient (%g) %g %g %g current (%g) %g %g %g\n",
-		ao_orient.q0,
-		ao_orient.q1,
-		ao_orient.q2,
-		ao_orient.q3,
-		ao_current.q0,
-		ao_current.q1,
-		ao_current.q2,
-		ao_current.q3);
-#endif
-}
-#endif
-
 void
 ao_sleep(void *wchan)
 {
@@ -771,6 +680,10 @@ ao_sleep(void *wchan)
 		char		*words[64];
 		int		nword;
 
+#if TELEMEGA
+		if (ao_flight_state >= ao_flight_boost && ao_flight_state < ao_flight_landed)
+			ao_pyro_check();
+#endif
 		for (;;) {
 			if (ao_records_read > 2 && ao_flight_state == ao_flight_startup)
 			{
@@ -826,43 +739,31 @@ ao_sleep(void *wchan)
 					ao_data_static.ms5607_raw.temp = int32(bytes, 4);
 					ao_ms5607_convert(&ao_data_static.ms5607_raw, &value);
 					ao_data_static.mpu6000.accel_x = int16(bytes, 8);
-					ao_data_static.mpu6000.accel_y = -int16(bytes, 10);
+					ao_data_static.mpu6000.accel_y = int16(bytes, 10);
 					ao_data_static.mpu6000.accel_z = int16(bytes, 12);
 					ao_data_static.mpu6000.gyro_x = int16(bytes, 14);
-					ao_data_static.mpu6000.gyro_y = -int16(bytes, 16);
+					ao_data_static.mpu6000.gyro_y = int16(bytes, 16);
 					ao_data_static.mpu6000.gyro_z = int16(bytes, 18);
+					ao_data_static.hmc5883.x = int16(bytes, 20);
+					ao_data_static.hmc5883.y = int16(bytes, 22);
+					ao_data_static.hmc5883.z = int16(bytes, 24);
 #if HAS_MMA655X
 					ao_data_static.mma655x = int16(bytes, 26);
 #endif
-					if (ao_records_read == 0)
-						ao_ground_mpu6000 = ao_data_static.mpu6000;
-					else if (ao_records_read < 10) {
-#define f(f) ao_ground_mpu6000.f = ao_ground_mpu6000.f + ((ao_data_static.mpu6000.f - ao_ground_mpu6000.f) >> 2)
-						f(accel_x);
-						f(accel_y);
-						f(accel_z);
-						f(gyro_x);
-						f(gyro_y);
-						f(gyro_z);
-
-						double		accel_x = ao_mpu6000_accel(ao_ground_mpu6000.accel_x);
-						double		accel_y = ao_mpu6000_accel(ao_ground_mpu6000.accel_y);
-						double		accel_z = ao_mpu6000_accel(ao_ground_mpu6000.accel_z);
-
-						/* X and Y are in the ground plane, arbitraryily picked as MPU X and Z axes
-						 * Z is normal to the ground, the MPU y axis
-						 */
-						set_orientation(accel_x, accel_z, accel_y, tick);
-					} else {
-						double		rate_x = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x);
-						double		rate_y = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y);
-						double		rate_z = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z);
-
-						update_orientation(rate_x * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick);
-					}
 					ao_records_read++;
 					ao_insert();
 					return;
+				case 'G':
+					ao_gps_prev = ao_gps_static;
+					ao_gps_static.tick = tick;
+					ao_gps_static.latitude = int32(bytes, 0);
+					ao_gps_static.longitude = int32(bytes, 4);
+					ao_gps_static.altitude = int32(bytes, 8);
+					ao_gps_static.flags = bytes[13];
+					if (!ao_gps_count)
+						ao_gps_first = ao_gps_static;
+					ao_gps_count++;
+					break;
 				}
 				continue;
 			} else if (nword == 3 && strcmp(words[0], "ms5607") == 0) {
@@ -883,6 +784,23 @@ ao_sleep(void *wchan)
 				else if (strcmp(words[1], "crc:") == 0)
 					ms5607_prom.crc = strtoul(words[2], NULL, 10);
 				continue;
+			} else if (nword >= 3 && strcmp(words[0], "Pyro") == 0) {
+				int	p = strtoul(words[1], NULL, 10);
+				int	i, j;
+				struct ao_pyro	*pyro = &ao_config.pyro[p];
+
+				for (i = 2; i < nword; i++) {
+					for (j = 0; j < NUM_PYRO_VALUES; j++)
+						if (!strcmp (words[2], ao_pyro_values[j].name))
+							break;
+					if (j == NUM_PYRO_VALUES)
+						continue;
+					pyro->flags |= ao_pyro_values[j].flag;
+					if (ao_pyro_values[j].offset != NO_VALUE && i + 1 < nword) {
+						int16_t	val = strtoul(words[++i], NULL, 10);
+						*((int16_t *) ((char *) pyro + ao_pyro_values[j].offset)) = val;
+					}
+				}
 			}
 #else
 			if (nword == 4 && log_format != AO_LOG_FORMAT_TELEMEGA) {
@@ -899,6 +817,13 @@ ao_sleep(void *wchan)
 			} else if (nword >= 6 && strcmp(words[0], "Accel") == 0) {
 				ao_config.accel_plus_g = atoi(words[3]);
 				ao_config.accel_minus_g = atoi(words[5]);
+#ifdef TELEMEGA
+			} else if (nword >= 8 && strcmp(words[0], "IMU") == 0) {
+				ao_config.accel_zero_along = atoi(words[3]);
+				ao_config.accel_zero_across = atoi(words[5]);
+				ao_config.accel_zero_through = atoi(words[7]);
+				printf ("%d %d %d\n", ao_config.accel_zero_along, ao_config.accel_zero_across, ao_config.accel_zero_through);
+#endif
 			} else if (nword >= 4 && strcmp(words[0], "Main") == 0) {
 				ao_config.main_deploy = atoi(words[2]);
 			} else if (nword >= 3 && strcmp(words[0], "Apogee") == 0 &&
diff --git a/src/test/ao_gps_test.c b/src/test/ao_gps_test.c
index 3844a326..e799ab0f 100644
--- a/src/test/ao_gps_test.c
+++ b/src/test/ao_gps_test.c
@@ -26,6 +26,9 @@
 #define AO_GPS_NUM_SAT_MASK	(0xf << 0)
 #define AO_GPS_NUM_SAT_SHIFT	(0)
 
+#define AO_GPS_NEW_DATA		1
+#define AO_GPS_NEW_TRACKING	2
+
 #define AO_GPS_VALID		(1 << 4)
 #define AO_GPS_RUNNING		(1 << 5)
 #define AO_GPS_DATE_VALID	(1 << 6)
@@ -427,11 +430,18 @@ void
 ao_dump_state(void *wchan)
 {
 	int	i;
-	if (wchan == &ao_gps_data)
+
+	if (wchan != &ao_gps_new)
+		return;
+	
+	if (ao_gps_new & AO_GPS_NEW_DATA) {
 		ao_gps_print(&ao_gps_data);
-	else
+		putchar('\n');
+	}
+	if (ao_gps_new & AO_GPS_NEW_TRACKING) {
 		ao_gps_tracking_print(&ao_gps_tracking_data);
-	putchar('\n');
+		putchar('\n');
+	}
 	return;
 	printf ("%02d:%02d:%02d",
 		ao_gps_data.hour, ao_gps_data.minute,
diff --git a/src/test/ao_gps_test_skytraq.c b/src/test/ao_gps_test_skytraq.c
index 81008b39..1b590d5e 100644
--- a/src/test/ao_gps_test_skytraq.c
+++ b/src/test/ao_gps_test_skytraq.c
@@ -26,6 +26,9 @@
 #define AO_GPS_NUM_SAT_MASK	(0xf << 0)
 #define AO_GPS_NUM_SAT_SHIFT	(0)
 
+#define AO_GPS_NEW_DATA		1
+#define AO_GPS_NEW_TRACKING	2
+
 #define AO_GPS_VALID		(1 << 4)
 #define AO_GPS_RUNNING		(1 << 5)
 #define AO_GPS_DATE_VALID	(1 << 6)
@@ -75,6 +78,11 @@ struct ao_gps_tracking_orig {
 #define ao_telemetry_satellite ao_gps_tracking_orig
 #define ao_telemetry_satellite_info ao_gps_sat_orig
 
+extern __xdata struct ao_telemetry_location	ao_gps_data;
+extern __xdata struct ao_telemetry_satellite	ao_gps_tracking_data;
+
+uint8_t ao_gps_mutex;
+
 void
 ao_mutex_get(uint8_t *mutex)
 {
@@ -432,17 +440,14 @@ uint8_t	ao_task_minimize_latency;
 #define ao_usb_getchar()	0
 
 #include "ao_gps_print.c"
+#include "ao_gps_show.c"
 #include "ao_gps_skytraq.c"
 
 void
 ao_dump_state(void *wchan)
 {
-	if (wchan == &ao_gps_data)
-		ao_gps_print(&ao_gps_data);
-	else
-		ao_gps_tracking_print(&ao_gps_tracking_data);
-	putchar('\n');
-	return;
+	if (wchan == &ao_gps_new)
+		ao_gps_show();
 }
 
 int
diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c
index 80671735..4eb4b837 100644
--- a/src/test/ao_gps_test_ublox.c
+++ b/src/test/ao_gps_test_ublox.c
@@ -26,6 +26,9 @@
 #define AO_GPS_NUM_SAT_MASK	(0xf << 0)
 #define AO_GPS_NUM_SAT_SHIFT	(0)
 
+#define AO_GPS_NEW_DATA		1
+#define AO_GPS_NEW_TRACKING	2
+
 #define AO_GPS_VALID		(1 << 4)
 #define AO_GPS_RUNNING		(1 << 5)
 #define AO_GPS_DATE_VALID	(1 << 6)
@@ -77,6 +80,11 @@ struct ao_telemetry_satellite {
 #define ao_gps_tracking_orig ao_telemetry_satellite
 #define ao_gps_sat_orig ao_telemetry_satellite_info
 
+extern __xdata struct ao_telemetry_location	ao_gps_data;
+extern __xdata struct ao_telemetry_satellite	ao_gps_tracking_data;
+
+uint8_t ao_gps_mutex;
+
 void
 ao_mutex_get(uint8_t *mutex)
 {
@@ -125,7 +133,7 @@ static uint16_t	recv_len;
 static void check_ublox_message(char *which, uint8_t *msg);
 
 char
-ao_serial1_getchar(void)
+ao_gps_getchar(void)
 {
 	char	c;
 	uint8_t	uc;
@@ -158,7 +166,7 @@ static int	message_len;
 static uint16_t	send_len;
 
 void
-ao_serial1_putchar(char c)
+ao_gps_putchar(char c)
 {
 	int	i;
 	uint8_t	uc = (uint8_t) c;
@@ -191,7 +199,7 @@ ao_serial1_putchar(char c)
 #define AO_SERIAL_SPEED_115200	3
 
 static void
-ao_serial1_set_speed(uint8_t speed)
+ao_gps_set_speed(uint8_t speed)
 {
 	int	fd = ao_gps_fd;
 	struct termios	termios;
@@ -224,6 +232,7 @@ uint8_t	ao_task_minimize_latency;
 #define ao_usb_getchar()	0
 
 #include "ao_gps_print.c"
+#include "ao_gps_show.c"
 #include "ao_gps_ublox.c"
 
 static void
@@ -341,11 +350,8 @@ check_ublox_message(char *which, uint8_t *msg)
 void
 ao_dump_state(void *wchan)
 {
-	if (wchan == &ao_gps_data)
-		ao_gps_print(&ao_gps_data);
-	else
-		ao_gps_tracking_print(&ao_gps_tracking_data);
-	putchar('\n');
+	if (wchan == &ao_gps_new)
+		ao_gps_show();
 	return;
 }
 
diff --git a/src/test/ao_int64_test.c b/src/test/ao_int64_test.c
new file mode 100644
index 00000000..8557a1c7
--- /dev/null
+++ b/src/test/ao_int64_test.c
@@ -0,0 +1,115 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#define __data
+#define __pdata
+#define __xdata
+#define __reentrant
+
+#include <ao_int64.h>
+#include <ao_int64.c>
+#include <stdio.h>
+#include <stdlib.h>
+
+int	errors;
+
+#define test_o(op,func,mod,a,b,ao_a,ao_b) do {				\
+		r = (a) op (b);						\
+		func(&ao_r, ao_a, ao_b);				\
+		c = ao_cast64(&ao_r);					\
+		if (c != r) {						\
+			printf ("trial %4d: %lld " #func mod " %lld = %lld (should be %lld)\n", \
+				trial, (int64_t) (a), (int64_t) b, c, r); \
+			++errors;					\
+		}							\
+	} while (0)
+
+#define test(op,func,a,b,ao_a,ao_b) test_o(op,func,"",a,b,ao_a,ao_b)
+
+#define test_a(op,func,a,b,ao_a,ao_b) do {	\
+		ao_r = *ao_a;			\
+		test_o(op,func,"_a",a,b,&ao_r,ao_b);	\
+	} while (0)
+
+#define test_b(op,func,a,b,ao_a,ao_b) do {	\
+		ao_r = *ao_b;			\
+		test_o(op,func,"_b",a,b,ao_a,&ao_r);	\
+	} while (0)
+
+#define test_x(op,func,a,b,ao_a,ao_b) do {	\
+		ao_r = *ao_a;			\
+		test_o(op,func,"_xa",a,a,&ao_r,&ao_r);	\
+		ao_r = *ao_b;			\
+		test_o(op,func,"_xb",b,b,&ao_r,&ao_r);	\
+	} while (0)
+
+void
+do_test(int trial, int64_t a, int64_t b)
+{
+	int64_t	r, c;
+	ao_int64_t	ao_a, ao_b, ao_r;
+
+	ao_int64_init64(&ao_a, a >> 32, a);
+	ao_int64_init64(&ao_b, b >> 32, b);
+
+	test(+, ao_plus64, a, b, &ao_a, &ao_b);
+	test_a(+, ao_plus64, a, b, &ao_a, &ao_b);
+	test_b(+, ao_plus64, a, b, &ao_a, &ao_b);
+	test_x(+, ao_plus64, a, b, &ao_a, &ao_b);
+	test(-, ao_minus64, a, b, &ao_a, &ao_b);
+	test_a(-, ao_minus64, a, b, &ao_a, &ao_b);
+	test_b(-, ao_minus64, a, b, &ao_a, &ao_b);
+	test_x(-, ao_minus64, a, b, &ao_a, &ao_b);
+	test(*, ao_mul64_32_32,(int64_t) (int32_t) a, (int32_t) b, (int32_t) a, (int32_t) b);
+	test(*, ao_mul64, a, b, &ao_a, &ao_b);
+	test_a(*, ao_mul64, a, b, &ao_a, &ao_b);
+	test_b(*, ao_mul64, a, b, &ao_a, &ao_b);
+	test_x(*, ao_mul64, a, b, &ao_a, &ao_b);
+	test(*, ao_mul64_64_16, a, (uint16_t) b, &ao_a, (uint16_t) b);
+	test_a(*, ao_mul64_64_16, a, (uint16_t) b, &ao_a, (uint16_t) b);
+	test(>>, ao_rshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f);
+	test_a(>>, ao_rshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f);
+	test(<<, ao_lshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f);
+	test_a(<<, ao_lshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f);
+}
+
+#define TESTS	10000000
+
+static int64_t
+random64(void)
+{
+	return (int64_t) random() + ((int64_t) random() << 31) /* + ((int64_t) random() << 33) */;
+}
+
+int
+main (int argc, char **argv)
+{
+	int	i, start;
+
+	if (argv[1])
+		start = atoi(argv[1]);
+	else
+		start = 0;
+	srandom(1000);
+	for (i = 0; i < TESTS; i++) {
+		int64_t a = random64();
+		int64_t b = random64();
+		if (i >= start)
+			do_test(i, a, b);
+	}
+	return errors;
+}
diff --git a/src/test/ao_ms5607_convert_test.c b/src/test/ao_ms5607_convert_test.c
new file mode 100644
index 00000000..ad593204
--- /dev/null
+++ b/src/test/ao_ms5607_convert_test.c
@@ -0,0 +1,96 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#define __xdata
+#define __data
+#define __pdata
+#define __reentrant
+
+#include <stdint.h>
+#include <ao_ms5607.h>
+
+struct ao_ms5607_prom ms5607_prom = {
+	0x002c,
+	0xa6e0,
+	0x988e,
+	0x6814,
+	0x5eff,
+	0x8468,
+	0x6c86,
+	0xa271,
+};
+
+int32_t D1_mm = 6179630;
+int32_t D2_mm = 8933155;
+
+#include <ao_ms5607_convert.c>
+#define ao_ms5607_convert ao_ms5607_convert_8051
+#include <ao_ms5607_convert_8051.c>
+#include <ao_int64.c>
+#include <stdio.h>
+#include <stdlib.h>
+
+struct ao_ms5607_sample ao_sample = {
+	6179630,
+	8933155
+};
+
+int errors;
+
+void test(int trial, struct ao_ms5607_sample *sample)
+{
+	struct ao_ms5607_value	value, value_8051;
+
+	ao_ms5607_convert(sample, &value);
+	ao_ms5607_convert_8051(sample, &value_8051);
+	if (value.temp != value_8051.temp || value.pres != value_8051.pres) {
+		++errors;
+		printf ("trial %d: %d, %d -> %d, %d (should be %d, %d)\n",
+			trial,
+			sample->pres, sample->temp,
+			value_8051.pres, value_8051.temp,
+			value.pres, value.temp);
+	}
+}
+
+#define TESTS	10000000
+
+#include <stdlib.h>
+
+static int32_t rand24(void) { return random() & 0xffffff; }
+
+int
+main(int argc, char **argv)
+{
+	struct ao_ms5607_sample sample;
+	int	i, start;
+
+	if (argv[1])
+		start = atoi(argv[1]);
+	else
+		start = 0;
+
+	srandom(10000);
+	test(-1, &ao_sample);
+	for (i = 0; i < TESTS; i++) {
+		sample.pres = rand24();
+		sample.temp = rand24();
+		if (i >= start)
+			test(i, &sample);
+	}
+	return errors;
+}
diff --git a/src/test/ao_quaternion_test.c b/src/test/ao_quaternion_test.c
new file mode 100644
index 00000000..b630f1d3
--- /dev/null
+++ b/src/test/ao_quaternion_test.c
@@ -0,0 +1,154 @@
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#define _GNU_SOURCE
+
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stddef.h>
+#include <string.h>
+#include <getopt.h>
+#include <math.h>
+
+#include "ao_quaternion.h"
+
+#if 0
+static void
+print_q(char *name, struct ao_quaternion *q)
+{
+	printf ("%8.8s: r%8.5f x%8.5f y%8.5f z%8.5f ", name,
+		q->r, q->x, q->y, q->z);
+}
+#endif
+
+#define STEPS	16
+
+#define DEG	(1.0f * 3.1415926535f / 180.0f)
+
+struct ao_rotation {
+	int	steps;
+	float	x, y, z;
+};
+
+static struct ao_rotation ao_path[] = {
+	{ .steps = 45, .x =  2*DEG, .y = 0, .z = 0 },
+
+	{ .steps = 45, .x = 0, .y = 2*DEG, .z = 0 },
+	
+	{ .steps = 45, .x = -2*DEG, .y = 0, .z = 0 },
+
+	{ .steps = 45, .x = 0, .y = -2*DEG, .z = 0 },
+};
+
+#define NUM_PATH	(sizeof ao_path / sizeof ao_path[0])
+
+static int close(float a, float b) {
+	return fabsf (a - b) < 1e-5;
+}
+
+static int check_quaternion(char *where, struct ao_quaternion *got, struct ao_quaternion *expect) {
+	if (!close (got->r, expect->r) ||
+	    !close (got->x, expect->x) ||
+	    !close (got->y, expect->y) ||
+	    !close (got->z, expect->z))
+	{
+		printf ("%s: got r%8.5f x%8.5f y%8.5f z%8.5f expect r%8.5f x%8.5f y%8.5f z%8.5f\n",
+			where,
+			got->r, got->x, got->y, got->z,
+			expect->r, expect->x, expect->y, expect->z);
+		return 1;
+	}
+	return 0;
+}
+
+int main(int argc, char **argv)
+{
+	struct ao_quaternion	position;
+	struct ao_quaternion 	position_expect;
+	struct ao_quaternion	rotation;
+	struct ao_quaternion	rotated;
+	struct ao_quaternion	little_rotation;
+	int			i;
+	int			p;
+	int			ret = 0;
+
+	/* vector */
+	ao_quaternion_init_vector(&position, 1, 1, 1);
+	ao_quaternion_init_vector(&position_expect, -1, -1, 1);
+
+	/* zero rotation */
+	ao_quaternion_init_zero_rotation(&rotation);
+
+#define dump() do {							\
+									\
+		ao_quaternion_rotate(&rotated, &position, &rotation);	\
+		print_q("rotated", &rotated);				\
+		print_q("rotation", &rotation);				\
+		printf ("\n");						\
+	} while (0)
+
+//	dump();
+
+	for (p = 0; p < NUM_PATH; p++) {
+		ao_quaternion_init_half_euler(&little_rotation,
+					      ao_path[p].x / 2.0f,
+					      ao_path[p].y / 2.0f,
+					      ao_path[p].z / 2.0f);
+//		printf ("\t\tx: %8.4f, y: %8.4f, z: %8.4f ", ao_path[p].x, ao_path[p].y, ao_path[p].z);
+//		print_q("step", &little_rotation);
+//		printf("\n");
+		for (i = 0; i < ao_path[p].steps; i++) {
+			ao_quaternion_multiply(&rotation, &little_rotation, &rotation);
+
+			ao_quaternion_normalize(&rotation, &rotation);
+
+//			dump();
+		}
+	}
+
+	ao_quaternion_rotate(&rotated, &position, &rotation);
+
+	ret += check_quaternion("rotation", &rotated, &position_expect);
+
+	struct ao_quaternion	vertical;
+	struct ao_quaternion	angle;
+	struct ao_quaternion	rot;
+
+	ao_quaternion_init_vector(&vertical, 0, 0, 1);
+	ao_quaternion_init_vector(&angle, 0, 0, 1);
+
+	ao_quaternion_init_half_euler(&rot,
+				      M_PI * 3.0 / 8.0 , 0, 0);
+
+	ao_quaternion_rotate(&angle, &angle, &rot);
+
+	struct ao_quaternion	rot_compute;
+
+	ao_quaternion_vectors_to_rotation(&rot_compute, &vertical, &angle);
+
+	ret += check_quaternion("vector rotation", &rot_compute, &rot);
+
+	struct ao_quaternion	rotd;
+
+	ao_quaternion_rotate(&rotd, &vertical, &rot_compute);
+
+	ret += check_quaternion("vector rotated", &rotd, &angle);
+
+	return ret;
+}
+
diff --git a/src/test/mega.flights b/src/test/mega.flights
new file mode 100644
index 00000000..71c44e65
--- /dev/null
+++ b/src/test/mega.flights
@@ -0,0 +1,12 @@
+2013-10-12-serial-0094-flight-0001.eeprom
+2013-09-02-serial-094-flight-002.eeprom
+2013-03-02-serial-069-flight-002.eeprom
+2013-03-02-serial-069-flight-001.eeprom
+2013-05-27-serial-084-flight-001.mega
+2013-05-26-serial-085-flight-002.mega
+2013-05-19-serial-084-flight-003.mega
+2013-04-21-serial-069-flight-002.mega
+2012-10-20-serial-068-flight-004.mega
+2012-07-03-serial-058-flight-007.mega
+2012-06-30-serial-058-flight-006.mega
+2012-06-30-serial-058-flight-005.mega
diff --git a/src/test/plot-rot b/src/test/plot-rot
new file mode 100755
index 00000000..b6b77c93
--- /dev/null
+++ b/src/test/plot-rot
@@ -0,0 +1,30 @@
+#!/bin/sh
+
+case $# in
+1)
+	file="$1"
+	title="$1"
+	;;
+2)
+	file="$1"
+	title="$2"
+	;;
+*)
+	echo "Usage: $0 <data-file> <title>"
+	exit 1
+esac
+
+gnuplot -persist << EOF
+set ylabel "altitude (m)"
+set y2label "angle (d)"
+set xlabel "time (s)"
+set xtics border out nomirror
+set ytics border out nomirror
+set y2tics border out nomirror
+set title "$title"
+plot "$file" using 1:5 with lines axes x1y1 title "height",\
+"$file" using 1:9 with lines axes x1y2 title "gyro rot", \
+"$file" using 1:7 with lines axes x1y2 title "gyro tilt", \
+"$file" using 1:13 with lines axes x1y2 title "mag rot", \
+"$file" using 1:11 with lines axes x1y2 title "mag tilt"
+EOF
diff --git a/src/test/plotmm b/src/test/plotmm
index 5f5bd2ca..27f8ddcd 100755
--- a/src/test/plotmm
+++ b/src/test/plotmm
@@ -1,11 +1,29 @@
+#!/bin/sh
+
+case $# in
+1)
+	file="$1"
+	title="$1"
+	;;
+2)
+	file="$1"
+	title="$2"
+	;;
+*)
+	echo "Usage: $0 <data-file> <title>"
+	exit 1
+esac
+
 gnuplot -persist << EOF
-set ylabel "altitude (m)"
+set ylabel "distance (m)"
 set y2label "angle (d)"
 set xlabel "time (s)"
 set xtics border out nomirror
 set ytics border out nomirror
 set y2tics border out nomirror
-plot "$1" using 1:3 with lines axes x1y1 title "raw height",\
-"$1" using 1:9 with lines axes x1y2 title "angle",\
-"$1" using 1:11 with lines axes x1y2 title "qangle"
+set title "$title"
+plot "$file" using 1:5 with lines axes x1y1 title "height",\
+"$file" using 1:7 with lines axes x1y2 title "angle",\
+"$file" using 1:13 with lines axes x1y2 title "gps angle",\
+"$file" using 1:15 with lines axes x1y2 title "sats"
 EOF
diff --git a/src/test/run-all-mm b/src/test/run-all-mm
new file mode 100755
index 00000000..d9c2043d
--- /dev/null
+++ b/src/test/run-all-mm
@@ -0,0 +1,2 @@
+#!/bin/sh
+./run-mm `cat mega.flights`
diff --git a/src/test/run-mm b/src/test/run-mm
index 6f3d97a2..ae5e5f42 100755
--- a/src/test/run-mm
+++ b/src/test/run-mm
@@ -3,15 +3,20 @@
 DIR=~/misc/rockets/flights
 
 for i in "$@"; do
-case "$i" in
-    */*)
-    file="$i"
-    ;;
-    *)
-    file="$DIR/$i"
-    ;;
-esac
-./ao_flight_test_mm "$file" > run-out.mm
+    case "$i" in
+	*/*)
+	file="$i"
+	;;
+	*)
+	file="$DIR/$i"
+	;;
+    esac
+    base=`basename "$i" .eeprom`
+
+    ./ao_flight_test_mm "$file" > $base.plot
+
+    sh ./plotmm $base.plot `basename "$file"`
+done
 
 #./ao_flight_test_accel "$file" > run-out.accel
 #"run-out.accel" using 1:9 with lines lt 4 axes x1y1 title "accel height",\
@@ -21,21 +26,21 @@ esac
 #"run-out.accel" using 1:17 with lines lt 4 axes x1y1 title "accel main",\
 #
 
-gnuplot << EOF
-set ylabel "altitude (m)"
-set y2label "velocity (m/s), acceleration(m/s²)"
-set xlabel "time (s)"
-set xtics border out nomirror
-set ytics border out nomirror
-set y2tics border out nomirror
-set title "$i"
-plot "run-out.mm" using 1:3 with lines lw 2 lt 1 axes x1y1 title "raw height",\
-"run-out.mm" using 1:5 with lines lw 2 lt 1 axes x1y2 title "raw accel",\
-"run-out.mm" using 1:21 with lines lt 2 axes x1y1 title "mm height",\
-"run-out.mm" using 1:23 with lines lt 2 axes x1y2 title "mm speed",\
-"run-out.mm" using 1:25 with lines lt 2 axes x1y2 title "mm accel",\
-"run-out.mm" using 1:29 with lines lt 2 axes x1y1 title "mm drogue",\
-"run-out.mm" using 1:31 with lines lt 2 axes x1y1 title "mm main"
-pause mouse close
-EOF
-done
-\ No newline at end of file
+#gnuplot << EOF
+#set ylabel "altitude (m)"
+#set y2label "velocity (m/s), acceleration(m/s²)"
+#set xlabel "time (s)"
+#set xtics border out nomirror
+#set ytics border out nomirror
+#set y2tics border out nomirror
+#set title "$i"
+#plot "run-out.mm" using 1:3 with lines lw 2 lt 1 axes x1y1 title "raw height",\
+#"run-out.mm" using 1:5 with lines lw 2 lt 1 axes x1y2 title "raw accel",\
+#"run-out.mm" using 1:21 with lines lt 2 axes x1y1 title "mm height",\
+#"run-out.mm" using 1:23 with lines lt 2 axes x1y2 title "mm speed",\
+#"run-out.mm" using 1:25 with lines lt 2 axes x1y2 title "mm accel",\
+#"run-out.mm" using 1:29 with lines lt 2 axes x1y1 title "mm drogue",\
+#"run-out.mm" using 1:31 with lines lt 2 axes x1y1 title "mm main"
+#pause mouse close
+#EOF
+#done
+\ No newline at end of file
diff --git a/src/tidongle/Makefile b/src/tidongle/Makefile
index 1514c4df..b2ba537b 100644
--- a/src/tidongle/Makefile
+++ b/src/tidongle/Makefile
@@ -72,10 +72,10 @@ endif
 # Otherwise, print the full command line.
 quiet ?= $($1)
 
-all: ../$(PROG)
+all: $(PROG)
 
-../$(PROG): $(REL) Makefile
-	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) ..
+$(PROG): $(REL) Makefile
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL)
 	$(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@
 
 ao_product.h: ao-make-product.5c ../Version
author	Bdale Garbee <bdale@gag.com>	2013-12-19 01:38:40 -0700
committer	Bdale Garbee <bdale@gag.com>	2013-12-19 01:38:40 -0700
commit	575bbaf976c5840fd0e308549c45a466fdec1352 (patch)
tree	11bfb498348bf7687bffc24699c4b1a998988ee4 /src
parent	b825116df173b77e2cab217a7b76112c742f9279 (diff)
parent	bc3610d8cecbfed40c62d4dcb93fc9a4d2a7c9e3 (diff)