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

23 files changed, 937 insertions, 391 deletions

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