altos: Rename 'core' to 'kernel'

core remains a bad name to use -- dirvish skips files (and
directories, it seems) with that name.

Signed-off-by: Keith Packard <keithp@keithp.com>

92 files changed, 14747 insertions, 0 deletions

diff --git a/src/kernel/altitude.h b/src/kernel/altitude.h
new file mode 100644
index 00000000..a278bbc6
--- /dev/null
+++ b/src/kernel/altitude.h
@@ -0,0 +1,132 @@
+/*max error 3.197865153490684 at   0.782%. Average error 0.260150920474668*/
+#define NALT 129
+#define ALT_FRAC_BITS 8
+    15835, /*  10.56 kPa   0.000% */
+    15332, /*  11.42 kPa   0.781% */
+    14868, /*  12.29 kPa   1.563% */
+    14435, /*  13.16 kPa   2.344% */
+    14030, /*  14.02 kPa   3.125% */
+    13649, /*  14.90 kPa   3.906% */
+    13290, /*  15.76 kPa   4.688% */
+    12950, /*  16.63 kPa   5.469% */
+    12627, /*  17.50 kPa   6.250% */
+    12320, /*  18.37 kPa   7.031% */
+    12027, /*  19.24 kPa   7.813% */
+    11747, /*  20.10 kPa   8.594% */
+    11479, /*  20.97 kPa   9.375% */
+    11222, /*  21.84 kPa  10.156% */
+    10975, /*  22.71 kPa  10.938% */
+    10736, /*  23.58 kPa  11.719% */
+    10504, /*  24.44 kPa  12.500% */
+    10278, /*  25.31 kPa  13.281% */
+    10059, /*  26.18 kPa  14.063% */
+     9846, /*  27.05 kPa  14.844% */
+     9638, /*  27.91 kPa  15.625% */
+     9435, /*  28.78 kPa  16.406% */
+     9237, /*  29.65 kPa  17.188% */
+     9044, /*  30.52 kPa  17.969% */
+     8855, /*  31.39 kPa  18.750% */
+     8670, /*  32.26 kPa  19.531% */
+     8490, /*  33.13 kPa  20.313% */
+     8313, /*  33.99 kPa  21.094% */
+     8140, /*  34.86 kPa  21.875% */
+     7970, /*  35.73 kPa  22.656% */
+     7803, /*  36.60 kPa  23.438% */
+     7640, /*  37.47 kPa  24.219% */
+     7480, /*  38.33 kPa  25.000% */
+     7322, /*  39.20 kPa  25.781% */
+     7168, /*  40.07 kPa  26.563% */
+     7016, /*  40.94 kPa  27.344% */
+     6867, /*  41.80 kPa  28.125% */
+     6720, /*  42.67 kPa  28.906% */
+     6575, /*  43.54 kPa  29.688% */
+     6433, /*  44.41 kPa  30.469% */
+     6294, /*  45.28 kPa  31.250% */
+     6156, /*  46.15 kPa  32.031% */
+     6020, /*  47.01 kPa  32.813% */
+     5887, /*  47.88 kPa  33.594% */
+     5755, /*  48.75 kPa  34.375% */
+     5625, /*  49.62 kPa  35.156% */
+     5497, /*  50.49 kPa  35.938% */
+     5371, /*  51.35 kPa  36.719% */
+     5247, /*  52.22 kPa  37.500% */
+     5124, /*  53.09 kPa  38.281% */
+     5003, /*  53.96 kPa  39.063% */
+     4883, /*  54.83 kPa  39.844% */
+     4765, /*  55.69 kPa  40.625% */
+     4648, /*  56.56 kPa  41.406% */
+     4533, /*  57.43 kPa  42.188% */
+     4419, /*  58.30 kPa  42.969% */
+     4307, /*  59.17 kPa  43.750% */
+     4196, /*  60.03 kPa  44.531% */
+     4086, /*  60.90 kPa  45.313% */
+     3977, /*  61.77 kPa  46.094% */
+     3870, /*  62.63 kPa  46.875% */
+     3764, /*  63.51 kPa  47.656% */
+     3659, /*  64.38 kPa  48.438% */
+     3555, /*  65.24 kPa  49.219% */
+     3453, /*  66.11 kPa  50.000% */
+     3351, /*  66.98 kPa  50.781% */
+     3250, /*  67.85 kPa  51.563% */
+     3151, /*  68.72 kPa  52.344% */
+     3052, /*  69.58 kPa  53.125% */
+     2955, /*  70.45 kPa  53.906% */
+     2858, /*  71.32 kPa  54.688% */
+     2763, /*  72.19 kPa  55.469% */
+     2668, /*  73.06 kPa  56.250% */
+     2574, /*  73.92 kPa  57.031% */
+     2482, /*  74.79 kPa  57.813% */
+     2390, /*  75.66 kPa  58.594% */
+     2298, /*  76.52 kPa  59.375% */
+     2208, /*  77.40 kPa  60.156% */
+     2119, /*  78.26 kPa  60.938% */
+     2030, /*  79.13 kPa  61.719% */
+     1942, /*  80.00 kPa  62.500% */
+     1855, /*  80.87 kPa  63.281% */
+     1769, /*  81.74 kPa  64.063% */
+     1683, /*  82.60 kPa  64.844% */
+     1598, /*  83.47 kPa  65.625% */
+     1514, /*  84.34 kPa  66.406% */
+     1430, /*  85.21 kPa  67.188% */
+     1347, /*  86.08 kPa  67.969% */
+     1265, /*  86.94 kPa  68.750% */
+     1184, /*  87.81 kPa  69.531% */
+     1103, /*  88.68 kPa  70.313% */
+     1023, /*  89.55 kPa  71.094% */
+      943, /*  90.41 kPa  71.875% */
+      864, /*  91.28 kPa  72.656% */
+      786, /*  92.15 kPa  73.438% */
+      708, /*  93.02 kPa  74.219% */
+      631, /*  93.89 kPa  75.000% */
+      554, /*  94.76 kPa  75.781% */
+      478, /*  95.63 kPa  76.563% */
+      403, /*  96.49 kPa  77.344% */
+      328, /*  97.36 kPa  78.125% */
+      254, /*  98.23 kPa  78.906% */
+      180, /*  99.10 kPa  79.688% */
+      106, /*  99.97 kPa  80.469% */
+       34, /* 100.83 kPa  81.250% */
+      -39, /* 101.70 kPa  82.031% */
+     -111, /* 102.57 kPa  82.813% */
+     -182, /* 103.44 kPa  83.594% */
+     -253, /* 104.30 kPa  84.375% */
+     -323, /* 105.17 kPa  85.156% */
+     -393, /* 106.04 kPa  85.938% */
+     -462, /* 106.91 kPa  86.719% */
+     -531, /* 107.78 kPa  87.500% */
+     -600, /* 108.65 kPa  88.281% */
+     -668, /* 109.51 kPa  89.063% */
+     -736, /* 110.38 kPa  89.844% */
+     -803, /* 111.25 kPa  90.625% */
+     -870, /* 112.12 kPa  91.406% */
+     -936, /* 112.99 kPa  92.188% */
+    -1002, /* 113.85 kPa  92.969% */
+    -1068, /* 114.72 kPa  93.750% */
+    -1133, /* 115.59 kPa  94.531% */
+    -1198, /* 116.46 kPa  95.313% */
+    -1262, /* 117.33 kPa  96.094% */
+    -1326, /* 118.19 kPa  96.875% */
+    -1389, /* 119.06 kPa  97.656% */
+    -1453, /* 119.93 kPa  98.438% */
+    -1516, /* 120.80 kPa  99.219% */
+    -1578, /* 121.67 kPa 100.000% */
diff --git a/src/kernel/ao.h b/src/kernel/ao.h
new file mode 100644
index 00000000..29ad2603
--- /dev/null
+++ b/src/kernel/ao.h
@@ -0,0 +1,1041 @@
+/*
+ * 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.
+ */
+
+#ifndef _AO_H_
+#define _AO_H_
+
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <stddef.h>
+#include <ao_pins.h>
+#include <ao_arch.h>
+
+#define TRUE 1
+#define FALSE 0
+
+/* Convert a __data pointer into an __xdata pointer */
+#ifndef DATA_TO_XDATA
+#define DATA_TO_XDATA(a)	(a)
+#endif
+#ifndef PDATA_TO_XDATA
+#define PDATA_TO_XDATA(a)	(a)
+#endif
+#ifndef CODE_TO_XDATA
+#define CODE_TO_XDATA(a)	(a)
+#endif
+
+#ifndef HAS_TASK
+#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
+#include <ao_notask.h>
+#endif
+
+/*
+ * ao_panic.c
+ */
+
+#define AO_PANIC_NO_TASK	1	/* AO_NUM_TASKS is not large enough */
+#define AO_PANIC_DMA		2	/* Attempt to start DMA while active */
+#define AO_PANIC_MUTEX		3	/* Mis-using mutex API */
+#define AO_PANIC_EE		4	/* Mis-using eeprom API */
+#define AO_PANIC_LOG		5	/* Failing to read/write log data */
+#define AO_PANIC_CMD		6	/* Too many command sets registered */
+#define AO_PANIC_STDIO		7	/* Too many stdio handlers registered */
+#define AO_PANIC_REBOOT		8	/* Reboot failed */
+#define AO_PANIC_FLASH		9	/* Invalid flash part (or wrong blocksize) */
+#define AO_PANIC_USB		10	/* Trying to send USB packet while busy */
+#define AO_PANIC_BT		11	/* Communications with bluetooth device failed */
+#define AO_PANIC_STACK		12	/* Stack overflow */
+#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 */
+#define AO_PANIC_SELF_TEST_MS5607	0x40 | 4	/* Self test failure */
+
+/* Stop the operating system, beeping and blinking the reason */
+void
+ao_panic(uint8_t reason);
+
+/*
+ * ao_timer.c
+ */
+
+#ifndef AO_TICK_TYPE
+#define AO_TICK_TYPE	uint16_t
+#define AO_TICK_SIGNED	int16_t
+#endif
+
+extern volatile __data AO_TICK_TYPE ao_tick_count;
+
+/* Our timer runs at 100Hz */
+#ifndef AO_HERTZ
+#define AO_HERTZ		100
+#endif
+#define AO_MS_TO_TICKS(ms)	((ms) / (1000 / AO_HERTZ))
+#define AO_SEC_TO_TICKS(s)	((s) * AO_HERTZ)
+
+/* Returns the current time in ticks */
+AO_TICK_TYPE
+ao_time(void);
+
+/* Suspend the current task until ticks time has passed */
+void
+ao_delay(uint16_t ticks);
+
+/* Set the ADC interval */
+void
+ao_timer_set_adc_interval(uint8_t interval);
+
+/* Timer interrupt */
+void
+ao_timer_isr(void) ao_arch_interrupt(9);
+
+/* Initialize the timer */
+void
+ao_timer_init(void);
+
+/* Initialize the hardware clock. Must be called first */
+void
+ao_clock_init(void);
+
+/*
+ * ao_mutex.c
+ */
+
+#ifndef ao_mutex_get
+void
+ao_mutex_get(__xdata uint8_t *ao_mutex) __reentrant;
+
+void
+ao_mutex_put(__xdata uint8_t *ao_mutex) __reentrant;
+#endif
+
+/*
+ * ao_cmd.c
+ */
+
+enum ao_cmd_status {
+	ao_cmd_success = 0,
+	ao_cmd_lex_error = 1,
+	ao_cmd_syntax_error = 2,
+};
+
+extern __pdata uint16_t ao_cmd_lex_i;
+extern __pdata uint32_t ao_cmd_lex_u32;
+extern __pdata char	ao_cmd_lex_c;
+extern __pdata enum ao_cmd_status ao_cmd_status;
+
+void
+ao_put_string(__code char *s);
+
+void
+ao_cmd_lex(void);
+
+void
+ao_cmd_put8(uint8_t v);
+
+void
+ao_cmd_put16(uint16_t v);
+
+uint8_t
+ao_cmd_is_white(void);
+
+void
+ao_cmd_white(void);
+
+int8_t
+ao_cmd_hexchar(char c);
+
+void
+ao_cmd_hexbyte(void);
+
+void
+ao_cmd_hex(void);
+
+void
+ao_cmd_decimal(void) __reentrant;
+
+/* Read a single hex nibble off stdin. */
+uint8_t
+ao_getnibble(void);
+
+uint8_t
+ao_match_word(__code char *word);
+
+struct ao_cmds {
+	void		(*func)(void);
+	__code char	*help;
+};
+
+void
+ao_cmd_register(const __code struct ao_cmds *cmds);
+
+void
+ao_cmd_init(void);
+
+#if HAS_CMD_FILTER
+/*
+ * Provided by an external module to filter raw command lines
+ */
+uint8_t
+ao_cmd_filter(void);
+#endif
+
+/*
+ * Various drivers
+ */
+#if HAS_ADC
+#include <ao_adc.h>
+#endif
+
+#if HAS_BEEP
+#include <ao_beep.h>
+#endif
+
+#if LEDS_AVAILABLE
+#include <ao_led.h>
+#endif
+
+#if HAS_USB
+#include <ao_usb.h>
+#endif
+
+#if HAS_EEPROM
+#include <ao_storage.h>
+#endif
+
+#if HAS_LOG
+#include <ao_log.h>
+#endif
+
+#if HAS_FLIGHT
+#include <ao_flight.h>
+#include <ao_sample.h>
+#endif
+
+/*
+ * ao_report.c
+ */
+
+#define AO_RDF_INTERVAL_TICKS	AO_SEC_TO_TICKS(5)
+#define AO_RDF_LENGTH_MS	500
+#define AO_RDF_CONTINUITY_MS	32
+#define AO_RDF_CONTINUITY_PAUSE	96
+#define AO_RDF_CONTINUITY_TOTAL	((AO_RDF_CONTINUITY_PAUSE + AO_RDF_CONTINUITY_MS) * 3 + AO_RDF_CONTINUITY_PAUSE)
+
+/* This assumes that we're generating a 1kHz tone, which
+ * modulates the carrier at 2kbps, or 250kBps
+ */
+#define AO_MS_TO_RDF_LEN(ms) ((ms) / 4)
+
+#define AO_RADIO_RDF_LEN	AO_MS_TO_RDF_LEN(AO_RDF_LENGTH_MS)
+#define AO_RADIO_CONT_TONE_LEN	AO_MS_TO_RDF_LEN(AO_RDF_CONTINUITY_MS)
+#define AO_RADIO_CONT_PAUSE_LEN	AO_MS_TO_RDF_LEN(AO_RDF_CONTINUITY_PAUSE)
+#define AO_RADIO_CONT_TOTAL_LEN	AO_MS_TO_RDF_LEN(AO_RDF_CONTINUITY_TOTAL)
+
+/* returns a value 0-3 to indicate igniter continuity */
+uint8_t
+ao_report_igniter(void);
+
+void
+ao_report_init(void);
+
+/*
+ * ao_convert.c
+ *
+ * Given raw data, convert to SI units
+ */
+
+/* pressure from the sensor to altitude in meters */
+int16_t
+ao_pres_to_altitude(int16_t pres) __reentrant;
+
+int16_t
+ao_altitude_to_pres(int16_t alt) __reentrant;
+
+int16_t
+ao_temp_to_dC(int16_t temp) __reentrant;
+
+/*
+ * ao_convert_pa.c
+ *
+ * Convert between pressure in Pa and altitude in meters
+ */
+
+#include <ao_data.h>
+
+alt_t
+ao_pa_to_altitude(int32_t pa);
+
+int32_t
+ao_altitude_to_pa(alt_t alt);
+
+#if HAS_DBG
+#include <ao_dbg.h>
+#endif
+
+#if HAS_SERIAL_0 || HAS_SERIAL_1 || HAS_SERIAL_2 || HAS_SERIAL_3
+#include <ao_serial.h>
+#endif
+
+/*
+ * ao_convert_volt.c
+ *
+ * Convert ADC readings to decivolts
+ */
+
+int16_t
+ao_battery_decivolt(int16_t adc);
+
+int16_t
+ao_ignite_decivolt(int16_t adc);
+
+/*
+ * ao_spi_slave.c
+ */
+
+uint8_t
+ao_spi_slave_recv(void *buf, uint16_t len);
+
+void
+ao_spi_slave_send(void *buf, uint16_t len);
+
+void
+ao_spi_slave_init(void);
+
+/* This must be defined by the product; it will get called when chip
+ * select goes low, at which point it should use ao_spi_read and
+ * ao_spi_write to deal with the request
+ */
+
+void
+ao_spi_slave(void);
+
+#include <ao_telemetry.h>
+/*
+ * ao_gps.c
+ */
+
+#define AO_GPS_NUM_SAT_MASK	(0xf << 0)
+#define AO_GPS_NUM_SAT_SHIFT	(0)
+
+#define AO_GPS_VALID		(1 << 4)
+#define AO_GPS_RUNNING		(1 << 5)
+#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;
+extern __xdata struct ao_telemetry_satellite ao_gps_tracking_data;
+
+struct ao_gps_orig {
+	uint8_t			year;
+	uint8_t			month;
+	uint8_t			day;
+	uint8_t			hour;
+	uint8_t			minute;
+	uint8_t			second;
+	uint8_t			flags;
+	int32_t			latitude;	/* degrees * 10⁷ */
+	int32_t			longitude;	/* degrees * 10⁷ */
+	int16_t			altitude;	/* m */
+	uint16_t		ground_speed;	/* cm/s */
+	uint8_t			course;		/* degrees / 2 */
+	uint8_t			hdop;		/* * 5 */
+	int16_t			climb_rate;	/* cm/s */
+	uint16_t		h_error;	/* m */
+	uint16_t		v_error;	/* m */
+};
+
+struct ao_gps_sat_orig {
+	uint8_t		svid;
+	uint8_t		c_n_1;
+};
+
+#define AO_MAX_GPS_TRACKING	12
+
+struct ao_gps_tracking_orig {
+	uint8_t			channels;
+	struct ao_gps_sat_orig	sats[AO_MAX_GPS_TRACKING];
+};
+
+void
+ao_gps(void);
+
+void
+ao_gps_print(__xdata struct ao_gps_orig *gps_data);
+
+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);
+
+/*
+ * ao_gps_report.c
+ */
+
+void
+ao_gps_report(void);
+
+void
+ao_gps_report_init(void);
+
+/*
+ * ao_gps_report_mega.c
+ */
+
+void
+ao_gps_report_mega(void);
+
+void
+ao_gps_report_mega_init(void);
+
+/*
+ * ao_telemetry_orig.c
+ */
+
+#if LEGACY_MONITOR
+struct ao_adc_orig {
+	uint16_t	tick;		/* tick when the sample was read */
+	int16_t		accel;		/* accelerometer */
+	int16_t		pres;		/* pressure sensor */
+	int16_t		temp;		/* temperature sensor */
+	int16_t		v_batt;		/* battery voltage */
+	int16_t		sense_d;	/* drogue continuity sense */
+	int16_t		sense_m;	/* main continuity sense */
+};
+
+struct ao_telemetry_orig {
+	uint16_t		serial;
+	uint16_t		flight;
+	uint8_t			flight_state;
+	int16_t			accel;
+	int16_t			ground_accel;
+	union {
+		struct {
+			int16_t			speed;
+			int16_t			unused;
+		} k;
+		int32_t		flight_vel;
+	} u;
+	int16_t			height;
+	int16_t			ground_pres;
+	int16_t			accel_plus_g;
+	int16_t			accel_minus_g;
+	struct ao_adc_orig	adc;
+	struct ao_gps_orig	gps;
+	char			callsign[AO_MAX_CALLSIGN];
+	struct ao_gps_tracking_orig	gps_tracking;
+};
+
+struct ao_telemetry_tiny {
+	uint16_t		serial;
+	uint16_t		flight;
+	uint8_t			flight_state;
+	int16_t			height;		/* AGL in meters */
+	int16_t			speed;		/* in m/s * 16 */
+	int16_t			accel;		/* in m/s² * 16 */
+	int16_t			ground_pres;	/* sensor units */
+	struct ao_adc		adc;		/* raw ADC readings */
+	char			callsign[AO_MAX_CALLSIGN];
+};
+
+struct ao_telemetry_orig_recv {
+	struct ao_telemetry_orig	telemetry_orig;
+	int8_t				rssi;
+	uint8_t				status;
+};
+
+struct ao_telemetry_tiny_recv {
+	struct ao_telemetry_tiny	telemetry_tiny;
+	int8_t				rssi;
+	uint8_t				status;
+};
+
+#endif /* LEGACY_MONITOR */
+
+/* Unfortunately, we've exposed the CC1111 rssi units as the 'usual' method
+ * for reporting RSSI. So, now we use these values everywhere
+ */
+#define AO_RSSI_FROM_RADIO(radio)	((int16_t) ((int8_t) (radio) >> 1) - 74)
+#define AO_RADIO_FROM_RSSI(rssi)	(((int8_t) (rssi) + 74) << 1)
+
+/*
+ * ao_radio_recv tacks on rssi and status bytes
+ */
+
+struct ao_telemetry_raw_recv {
+	uint8_t			packet[AO_MAX_TELEMETRY + 2];
+};
+
+/* Set delay between telemetry reports (0 to disable) */
+
+#ifdef AO_SEND_ALL_BARO
+#define AO_TELEMETRY_INTERVAL_PAD	AO_MS_TO_TICKS(100)
+#define AO_TELEMETRY_INTERVAL_FLIGHT	AO_MS_TO_TICKS(100)
+#define AO_TELEMETRY_INTERVAL_RECOVER	AO_MS_TO_TICKS(100)
+#else
+#define AO_TELEMETRY_INTERVAL_PAD	AO_MS_TO_TICKS(1000)
+#define AO_TELEMETRY_INTERVAL_FLIGHT	AO_MS_TO_TICKS(100)
+#define AO_TELEMETRY_INTERVAL_RECOVER	AO_MS_TO_TICKS(1000)
+#endif
+
+void
+ao_telemetry_set_interval(uint16_t interval);
+
+void
+ao_rdf_set(uint8_t rdf);
+
+void
+ao_telemetry_init(void);
+
+void
+ao_telemetry_orig_init(void);
+
+void
+ao_telemetry_tiny_init(void);
+
+/*
+ * ao_radio.c
+ */
+
+extern __xdata uint8_t	ao_radio_dma;
+
+extern __xdata int8_t	ao_radio_rssi;
+
+#ifdef PKT_APPEND_STATUS_1_CRC_OK
+#define AO_RADIO_STATUS_CRC_OK	PKT_APPEND_STATUS_1_CRC_OK
+#else
+#include <ao_fec.h>
+#define AO_RADIO_STATUS_CRC_OK	AO_FEC_DECODE_CRC_OK
+#endif
+
+#ifndef HAS_RADIO_RECV
+#define HAS_RADIO_RECV HAS_RADIO
+#endif
+#ifndef HAS_RADIO_XMIT
+#define HAS_RADIO_XMIT HAS_RADIO
+#endif
+
+void
+ao_radio_general_isr(void) ao_arch_interrupt(16);
+
+#if HAS_RADIO_XMIT
+void
+ao_radio_send(const __xdata void *d, uint8_t size) __reentrant;
+#endif
+
+#if HAS_RADIO_RECV
+uint8_t
+ao_radio_recv(__xdata void *d, uint8_t size, uint8_t timeout) __reentrant;
+
+void
+ao_radio_recv_abort(void);
+#endif
+
+void
+ao_radio_test(uint8_t on);
+
+typedef int16_t (*ao_radio_fill_func)(uint8_t *buffer, int16_t len);
+
+void
+ao_radio_send_aprs(ao_radio_fill_func fill);
+
+/*
+ * ao_radio_pa
+ */
+
+#if HAS_RADIO_AMP
+void
+ao_radio_pa_on(void);
+
+void
+ao_radio_pa_off(void);
+
+void
+ao_radio_pa_init(void);
+#else
+#define ao_radio_pa_on()
+#define ao_radio_pa_off()
+#define ao_radio_pa_init()
+#endif
+
+/*
+ * Compute the packet length as follows:
+ *
+ * 2000 bps (for a 1kHz tone)
+ * so, for 'ms' milliseconds, we need
+ * 2 * ms bits, or ms / 4 bytes
+ */
+
+void
+ao_radio_rdf(void);
+
+void
+ao_radio_continuity(uint8_t c);
+
+void
+ao_radio_rdf_abort(void);
+
+void
+ao_radio_init(void);
+
+/*
+ * ao_monitor.c
+ */
+
+#if HAS_MONITOR
+
+extern const char const * const ao_state_names[];
+
+#define AO_MONITOR_RING	8
+
+union ao_monitor {
+	struct ao_telemetry_raw_recv	raw;
+	struct ao_telemetry_all_recv	all;
+#if LEGACY_MONITOR
+	struct ao_telemetry_orig_recv	orig;
+	struct ao_telemetry_tiny_recv	tiny;
+#endif
+};
+
+extern __xdata union ao_monitor ao_monitor_ring[AO_MONITOR_RING];
+
+#define ao_monitor_ring_next(n)	(((n) + 1) & (AO_MONITOR_RING - 1))
+
+extern __data uint8_t ao_monitoring;
+extern __data uint8_t ao_monitor_head;
+
+void
+ao_monitor(void);
+
+#define AO_MONITORING_OFF	0
+#define AO_MONITORING_ORIG	1
+
+void
+ao_monitor_set(uint8_t monitoring);
+
+void
+ao_monitor_disable(void);
+
+void
+ao_monitor_enable(void);
+
+void
+ao_monitor_init(void) __reentrant;
+
+#endif
+
+/*
+ * ao_stdio.c
+ */
+
+#define AO_READ_AGAIN	(-1)
+
+struct ao_stdio {
+	int	(*_pollchar)(void);	/* Called with interrupts blocked */
+	void	(*putchar)(char c) __reentrant;
+	void	(*flush)(void);
+	uint8_t	echo;
+};
+
+extern __xdata struct ao_stdio ao_stdios[];
+extern __pdata int8_t ao_cur_stdio;
+extern __pdata int8_t ao_num_stdios;
+
+void
+flush(void);
+
+extern __xdata uint8_t ao_stdin_ready;
+
+uint8_t
+ao_echo(void);
+
+int8_t
+ao_add_stdio(int (*pollchar)(void),
+	     void (*putchar)(char) __reentrant,
+	     void (*flush)(void)) __reentrant;
+
+/*
+ * ao_ignite.c
+ */
+
+enum ao_igniter {
+	ao_igniter_drogue = 0,
+	ao_igniter_main = 1
+};
+
+void
+ao_ignite(enum ao_igniter igniter);
+
+enum ao_igniter_status {
+	ao_igniter_unknown,	/* unknown status (ambiguous voltage) */
+	ao_igniter_ready,	/* continuity detected */
+	ao_igniter_active,	/* igniter firing */
+	ao_igniter_open,	/* open circuit detected */
+};
+
+struct ao_ignition {
+	uint8_t	request;
+	uint8_t fired;
+	uint8_t firing;
+};
+
+extern __code char * __code ao_igniter_status_names[];
+
+extern __xdata struct ao_ignition ao_ignition[2];
+
+enum ao_igniter_status
+ao_igniter_status(enum ao_igniter igniter);
+
+extern __pdata uint8_t ao_igniter_present;
+
+void
+ao_ignite_set_pins(void);
+
+void
+ao_igniter_init(void);
+
+/*
+ * ao_config.c
+ */
+
+#if AO_PYRO_NUM
+#include <ao_pyro.h>
+#endif
+
+#if HAS_FORCE_FREQ
+/*
+ * Set this to force the frequency to 434.550MHz
+ */
+extern __xdata uint8_t ao_force_freq;
+#endif
+
+#define AO_CONFIG_MAJOR	1
+#define AO_CONFIG_MINOR	15
+
+#define AO_AES_LEN 16
+
+extern __xdata uint8_t ao_config_aes_seq;
+
+struct ao_config {
+	uint8_t		major;
+	uint8_t		minor;
+	uint16_t	main_deploy;
+	int16_t		accel_plus_g;		/* changed for minor version 2 */
+	uint8_t		_legacy_radio_channel;
+	char		callsign[AO_MAX_CALLSIGN + 1];
+	uint8_t		apogee_delay;		/* minor version 1 */
+	int16_t		accel_minus_g;		/* minor version 2 */
+	uint32_t	radio_cal;		/* minor version 3 */
+	uint32_t	flight_log_max;		/* minor version 4 */
+	uint8_t		ignite_mode;		/* minor version 5 */
+	uint8_t		pad_orientation;	/* minor version 6 */
+	uint32_t	radio_setting;		/* minor version 7 */
+	uint8_t		radio_enable;		/* minor version 8 */
+	uint8_t		aes_key[AO_AES_LEN];	/* minor version 9 */
+	uint32_t	frequency;		/* minor version 10 */
+	uint16_t	apogee_lockout;		/* minor version 11 */
+#if AO_PYRO_NUM
+	struct ao_pyro	pyro[AO_PYRO_NUM];	/* minor version 12 */
+#endif
+	uint16_t	aprs_interval;		/* minor version 13 */
+#if HAS_RADIO_POWER
+	uint8_t		radio_power;		/* minor version 14 */
+#endif
+#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
+#define AO_IGNITE_MODE_APOGEE		1
+#define AO_IGNITE_MODE_MAIN		2
+
+#define AO_RADIO_ENABLE_CORE		1
+#define AO_RADIO_DISABLE_TELEMETRY	2
+#define AO_RADIO_DISABLE_RDF		4
+
+#define AO_PAD_ORIENTATION_ANTENNA_UP	0
+#define AO_PAD_ORIENTATION_ANTENNA_DOWN	1
+
+extern __xdata struct ao_config ao_config;
+
+#define AO_CONFIG_MAX_SIZE	128
+
+void
+_ao_config_edit_start(void);
+
+void
+_ao_config_edit_finish(void);
+
+void
+ao_config_get(void);
+
+void
+ao_config_put(void);
+
+void
+ao_config_set_radio(void);
+
+void
+ao_config_init(void);
+
+/*
+ * ao_rssi.c
+ */
+
+void
+ao_rssi_set(int rssi_value);
+
+void
+ao_rssi_init(uint8_t rssi_led);
+
+/*
+ * ao_product.c
+ *
+ * values which need to be defined for
+ * each instance of a product
+ */
+
+extern const char ao_version[];
+extern const char ao_manufacturer[];
+extern const char ao_product[];
+
+/*
+ * Fifos
+ */
+
+#define AO_FIFO_SIZE	32
+
+struct ao_fifo {
+	uint8_t	insert;
+	uint8_t	remove;
+	char	fifo[AO_FIFO_SIZE];
+};
+
+#define ao_fifo_insert(f,c) do { \
+	(f).fifo[(f).insert] = (c); \
+	(f).insert = ((f).insert + 1) & (AO_FIFO_SIZE-1); \
+} while(0)
+
+#define ao_fifo_remove(f,c) do {\
+	c = (f).fifo[(f).remove]; \
+	(f).remove = ((f).remove + 1) & (AO_FIFO_SIZE-1); \
+} while(0)
+
+#define ao_fifo_full(f)		((((f).insert + 1) & (AO_FIFO_SIZE-1)) == (f).remove)
+#define ao_fifo_empty(f)	((f).insert == (f).remove)
+
+#if PACKET_HAS_MASTER || PACKET_HAS_SLAVE
+#include <ao_packet.h>
+#endif
+
+#if HAS_BTM
+#include <ao_btm.h>
+#endif
+
+#if HAS_COMPANION
+#include <ao_companion.h>
+#endif
+
+#if HAS_LCD
+#include <ao_lcd.h>
+#endif
+
+#if HAS_AES
+#include <ao_aes.h>
+#endif
+
+/* ao_launch.c */
+
+struct ao_launch_command {
+	uint16_t	tick;
+	uint16_t	serial;
+	uint8_t		cmd;
+	uint8_t		channel;
+	uint16_t	unused;
+};
+
+#define AO_LAUNCH_QUERY		1
+
+struct ao_launch_query {
+	uint16_t	tick;
+	uint16_t	serial;
+	uint8_t		channel;
+	uint8_t		valid;
+	uint8_t		arm_status;
+	uint8_t		igniter_status;
+};
+
+#define AO_LAUNCH_ARM		2
+#define AO_LAUNCH_FIRE		3
+
+void
+ao_launch_init(void);
+
+/*
+ * ao_log_single.c
+ */
+
+#define AO_LOG_TELESCIENCE_START	((uint8_t) 's')
+#define AO_LOG_TELESCIENCE_DATA		((uint8_t) 'd')
+
+#define AO_LOG_TELESCIENCE_NUM_ADC	12
+
+struct ao_log_telescience {
+	uint8_t		type;
+	uint8_t		csum;
+	uint16_t	tick;
+	uint16_t	tm_tick;
+	uint8_t		tm_state;
+	uint8_t		unused;
+	uint16_t	adc[AO_LOG_TELESCIENCE_NUM_ADC];
+};
+
+#define AO_LOG_SINGLE_SIZE		32
+
+union ao_log_single {
+	struct ao_log_telescience	telescience;
+	union ao_telemetry_all		telemetry;
+	uint8_t				bytes[AO_LOG_SINGLE_SIZE];
+};
+
+extern __xdata union ao_log_single	ao_log_single_write_data;
+extern __xdata union ao_log_single	ao_log_single_read_data;
+
+void
+ao_log_single_extra_query(void);
+
+void
+ao_log_single_list(void);
+
+void
+ao_log_single_main(void);
+
+uint8_t
+ao_log_single_write(void);
+
+uint8_t
+ao_log_single_read(uint32_t pos);
+
+void
+ao_log_single_start(void);
+
+void
+ao_log_single_stop(void);
+
+void
+ao_log_single_restart(void);
+
+void
+ao_log_single_set(void);
+
+void
+ao_log_single_delete(void);
+
+void
+ao_log_single_init(void);
+
+void
+ao_log_single(void);
+
+/*
+ * ao_pyro_slave.c
+ */
+
+#define AO_TELEPYRO_NUM_ADC	9
+
+#ifndef ao_xmemcpy
+#define ao_xmemcpy(d,s,c) memcpy(d,s,c)
+#define ao_xmemset(d,v,c) memset(d,v,c)
+#define ao_xmemcmp(d,s,c) memcmp(d,s,c)
+#endif
+
+/*
+ * ao_terraui.c
+ */
+
+void
+ao_terraui_init(void);
+
+/*
+ * ao_battery.c
+ */
+
+#ifdef BATTERY_PIN
+void
+ao_battery_isr(void) ao_arch_interrupt(1);
+
+uint16_t
+ao_battery_get(void);
+
+void
+ao_battery_init(void);
+#endif /* BATTERY_PIN */
+
+/*
+ * ao_sqrt.c
+ */
+
+uint32_t
+ao_sqrt(uint32_t op);
+
+/*
+ * ao_freq.c
+ */
+
+int32_t ao_freq_to_set(int32_t freq, int32_t cal) __reentrant;
+
+/*
+ * ao_ms5607.c
+ */
+
+void ao_ms5607_init(void);
+
+#include <ao_arch_funcs.h>
+
+#endif /* _AO_H_ */
diff --git a/src/kernel/ao_adc.h b/src/kernel/ao_adc.h
new file mode 100644
index 00000000..373db1c4
--- /dev/null
+++ b/src/kernel/ao_adc.h
@@ -0,0 +1,35 @@
+/*
+ * 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_ADC_H_
+#define _AO_ADC_H_
+
+#include <ao_data.h>
+
+/* Trigger a conversion sequence (called from the timer interrupt) */
+void
+ao_adc_poll(void);
+
+/* Suspend the current task until another A/D sample is converted */
+void
+ao_adc_sleep(void);
+
+/* Initialize the A/D converter */
+void
+ao_adc_init(void);
+
+#endif /* _AO_ADC_H_ */
diff --git a/src/kernel/ao_aes.h b/src/kernel/ao_aes.h
new file mode 100644
index 00000000..c47bc2db
--- /dev/null
+++ b/src/kernel/ao_aes.h
@@ -0,0 +1,54 @@
+/*
+ * 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_AES_H_
+#define _AO_AES_H_
+
+/* ao_aes.c */
+
+extern __xdata uint8_t ao_aes_mutex;
+
+/* AES keys and blocks are 128 bits */
+
+enum ao_aes_mode {
+	ao_aes_mode_cbc_mac
+};
+
+#if HAS_AES
+#ifdef SDCC
+void
+ao_aes_isr(void) __interrupt 4;
+#endif
+#endif
+
+void
+ao_aes_set_mode(enum ao_aes_mode mode);
+
+void
+ao_aes_set_key(__xdata uint8_t *in);
+
+void
+ao_aes_zero_iv(void);
+
+void
+ao_aes_run(__xdata uint8_t *in,
+	   __xdata uint8_t *out);
+
+void
+ao_aes_init(void);
+
+#endif /* _AO_AES_H_ */
diff --git a/src/kernel/ao_balloon.c b/src/kernel/ao_balloon.c
new file mode 100644
index 00000000..904a9c08
--- /dev/null
+++ b/src/kernel/ao_balloon.c
@@ -0,0 +1,136 @@
+/*
+ * 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.
+ */
+
+#ifndef AO_FLIGHT_TEST
+#include "ao.h"
+#endif
+
+#ifndef HAS_ACCEL
+#error Please define HAS_ACCEL
+#endif
+
+#ifndef HAS_GPS
+#error Please define HAS_GPS
+#endif
+
+#ifndef HAS_USB
+#error Please define HAS_USB
+#endif
+
+#if HAS_SENSOR_ERRORS
+/* Any sensor can set this to mark the flight computer as 'broken' */
+__xdata uint8_t			ao_sensor_errors;
+#endif
+
+__pdata uint16_t		ao_motor_number;	/* number of motors burned so far */
+
+/* Main flight thread. */
+
+__pdata enum ao_flight_state	ao_flight_state;	/* current flight state */
+
+__pdata uint8_t			ao_flight_force_idle;
+
+void
+ao_flight(void)
+{
+	ao_sample_init();
+	ao_flight_state = ao_flight_startup;
+	for (;;) {
+
+		/*
+		 * Process ADC samples, just looping
+		 * until the sensors are calibrated.
+		 */
+		if (!ao_sample())
+			continue;
+
+		switch (ao_flight_state) {
+		case ao_flight_startup:
+
+			/* Check to see what mode we should go to.
+			 *  - Invalid mode if accel cal appears to be out
+			 *  - pad mode if we're upright,
+			 *  - idle mode otherwise
+			 */
+			if (!ao_flight_force_idle)
+ 			{
+				/* Set pad mode - we can fly! */
+				ao_flight_state = ao_flight_pad;
+#if HAS_USB
+				/* Disable the USB controller in flight mode
+				 * to save power
+				 */
+				if (!ao_usb_running)
+					ao_usb_disable();
+#endif
+
+				/* Disable packet mode in pad state */
+				ao_packet_slave_stop();
+
+				/* Turn on telemetry system */
+				ao_rdf_set(1);
+				ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_BALLOON);
+
+				/* signal successful initialization by turning off the LED */
+				ao_led_off(AO_LED_RED);
+			} else {
+				/* Set idle mode */
+ 				ao_flight_state = ao_flight_idle;
+ 
+				/* signal successful initialization by turning off the LED */
+				ao_led_off(AO_LED_RED);
+			}
+			/* wakeup threads due to state change */
+			ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
+
+			break;
+		case ao_flight_pad:
+
+			/* pad to coast:
+			 *
+			 * barometer: > 20m vertical motion
+			 */
+			if (ao_height > AO_M_TO_HEIGHT(20))
+			{
+				ao_flight_state = ao_flight_drogue;
+
+				/* start logging data */
+				ao_log_start();
+
+#if HAS_GPS
+				/* Record current GPS position by waking up GPS log tasks */
+				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));
+			}
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+static __xdata struct ao_task	flight_task;
+
+void
+ao_flight_init(void)
+{
+	ao_flight_state = ao_flight_startup;
+	ao_add_task(&flight_task, ao_flight, "flight");
+}
diff --git a/src/kernel/ao_beep.h b/src/kernel/ao_beep.h
new file mode 100644
index 00000000..55f61171
--- /dev/null
+++ b/src/kernel/ao_beep.h
@@ -0,0 +1,74 @@
+/*
+ * 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_BEEP_H_
+#define _AO_BEEP_H_
+
+/*
+ * ao_beep.c
+ */
+
+/*
+ * Various pre-defined beep frequencies
+ *
+ * frequency = 1/2 (24e6/32) / beep
+ */
+
+#define AO_BEEP_LOW	150	/* 2500Hz */
+#define AO_BEEP_MID	94	/* 3989Hz */
+#define AO_BEEP_HIGH	75	/* 5000Hz */
+#define AO_BEEP_OFF	0	/* off */
+
+#define AO_BEEP_g	240	/* 1562.5Hz */
+#define AO_BEEP_gs	227	/* 1652Hz (1655Hz) */
+#define AO_BEEP_aa	214	/* 1752Hz (1754Hz) */
+#define AO_BEEP_bbf	202	/* 1856Hz (1858Hz) */
+#define AO_BEEP_bb	190	/* 1974Hz (1969Hz) */
+#define AO_BEEP_cc	180	/* 2083Hz (2086Hz) */
+#define AO_BEEP_ccs	170	/* 2205Hz (2210Hz) */
+#define AO_BEEP_dd	160	/* 2344Hz (2341Hz) */
+#define AO_BEEP_eef	151	/* 2483Hz (2480Hz) */
+#define AO_BEEP_ee	143	/* 2622Hz (2628Hz) */
+#define AO_BEEP_ff	135	/* 2778Hz (2784Hz) */
+#define AO_BEEP_ffs	127	/* 2953Hz (2950Hz) */
+#define AO_BEEP_gg	120	/* 3125Hz */
+#define AO_BEEP_ggs	113	/* 3319Hz (3311Hz) */
+#define AO_BEEP_aaa	107	/* 3504Hz (3508Hz) */
+#define AO_BEEP_bbbf	101	/* 3713Hz (3716Hz) */
+#define AO_BEEP_bbb	95	/* 3947Hz (3937Hz) */
+#define AO_BEEP_ccc	90	/* 4167Hz (4171Hz) */
+#define AO_BEEP_cccs	85	/* 4412Hz (4419Hz) */
+#define AO_BEEP_ddd	80	/* 4688Hz (4682Hz) */
+#define AO_BEEP_eeef	76	/* 4934Hz (4961Hz) */
+#define AO_BEEP_eee	71	/* 5282Hz (5256Hz) */
+#define AO_BEEP_fff	67	/* 5597Hz (5568Hz) */
+#define AO_BEEP_fffs	64	/* 5859Hz (5899Hz) */
+#define AO_BEEP_ggg	60	/* 6250Hz */
+
+/* Set the beeper to the specified tone */
+void
+ao_beep(uint8_t beep);
+
+/* Turn on the beeper for the specified time */
+void
+ao_beep_for(uint8_t beep, uint16_t ticks) __reentrant;
+
+/* Initialize the beeper */
+void
+ao_beep_init(void);
+
+#endif /* _AO_BEEP_H_ */
diff --git a/src/kernel/ao_btm.h b/src/kernel/ao_btm.h
new file mode 100644
index 00000000..484e5d7f
--- /dev/null
+++ b/src/kernel/ao_btm.h
@@ -0,0 +1,36 @@
+/*
+ * 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_BTM_H_
+#define _AO_BTM_H_
+
+/* ao_btm.c */
+
+/* If bt_link is on P2, this interrupt is shared by USB, so the USB
+ * code calls this function. Otherwise, it's a regular ISR.
+ */
+
+void
+ao_btm_isr(void)
+#if BT_LINK_ON_P1
+	__interrupt 15
+#endif
+	;
+void
+ao_btm_init(void);
+
+#endif /* _AO_BTM_H_ */
diff --git a/src/kernel/ao_cmd.c b/src/kernel/ao_cmd.c
new file mode 100644
index 00000000..4ebaa607
--- /dev/null
+++ b/src/kernel/ao_cmd.c
@@ -0,0 +1,420 @@
+/*
+ * 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_task.h"
+
+__pdata uint16_t ao_cmd_lex_i;
+__pdata uint32_t ao_cmd_lex_u32;
+__pdata char	ao_cmd_lex_c;
+__pdata enum ao_cmd_status ao_cmd_status;
+
+#define CMD_LEN	48
+
+static __xdata char	cmd_line[CMD_LEN];
+static __pdata uint8_t	cmd_len;
+static __pdata uint8_t	cmd_i;
+
+void
+ao_put_string(__code char *s)
+{
+	char	c;
+	while ((c = *s++))
+		putchar(c);
+}
+
+static void
+backspace(void)
+{
+	ao_put_string ("\010 \010");
+}
+
+static void
+readline(void)
+{
+	char c;
+	if (ao_echo())
+		ao_put_string("> ");
+	cmd_len = 0;
+	for (;;) {
+		flush();
+		c = getchar();
+		/* backspace/delete */
+		if (c == '\010' || c == '\177') {
+			if (cmd_len != 0) {
+				if (ao_echo())
+					backspace();
+				--cmd_len;
+			}
+			continue;
+		}
+
+		/* ^U */
+		if (c == '\025') {
+			while (cmd_len != 0) {
+				if (ao_echo())
+					backspace();
+				--cmd_len;
+			}
+			continue;
+		}
+
+		/* map CR to NL */
+		if (c == '\r')
+			c = '\n';
+
+		if (c == '\n') {
+			if (ao_echo())
+				putchar('\n');
+			break;
+		}
+
+		if (cmd_len >= CMD_LEN - 2)
+			continue;
+		cmd_line[cmd_len++] = c;
+		if (ao_echo())
+			putchar(c);
+	}
+	cmd_line[cmd_len++] = '\n';
+	cmd_line[cmd_len++] = '\0';
+	cmd_i = 0;
+}
+
+void
+ao_cmd_lex(void)
+{
+	ao_cmd_lex_c = '\n';
+	if (cmd_i < cmd_len)
+		ao_cmd_lex_c = cmd_line[cmd_i++];
+}
+
+static void
+putnibble(uint8_t v)
+{
+	if (v < 10)
+		putchar(v + '0');
+	else
+		putchar(v + ('a' - 10));
+}
+
+uint8_t
+ao_getnibble(void)
+{
+	char	c;
+
+	c = getchar();
+	if ('0' <= c && c <= '9')
+		return c - '0';
+	if ('a' <= c && c <= 'f')
+		return c - ('a' - 10);
+	if ('A' <= c && c <= 'F')
+		return c - ('A' - 10);
+	ao_cmd_status = ao_cmd_lex_error;
+	return 0;
+}
+
+void
+ao_cmd_put16(uint16_t v)
+{
+	ao_cmd_put8(v >> 8);
+	ao_cmd_put8(v);
+}
+
+void
+ao_cmd_put8(uint8_t v)
+{
+	putnibble((v >> 4) & 0xf);
+	putnibble(v & 0xf);
+}
+
+uint8_t
+ao_cmd_is_white(void)
+{
+	return ao_cmd_lex_c == ' ' || ao_cmd_lex_c == '\t';
+}
+
+void
+ao_cmd_white(void)
+{
+	while (ao_cmd_is_white())
+		ao_cmd_lex();
+}
+
+int8_t
+ao_cmd_hexchar(char c)
+{
+	if ('0' <= c && c <= '9')
+		return (c - '0');
+	if ('a' <= c && c <= 'f')
+		return (c - 'a' + 10);
+	if ('A' <= c && c <= 'F')
+		return (c - 'A' + 10);
+	return -1;
+}
+
+void
+ao_cmd_hexbyte(void)
+{
+	uint8_t i;
+	int8_t	n;
+
+	ao_cmd_lex_i = 0;
+	ao_cmd_white();
+	for (i = 0; i < 2; i++) {
+		n = ao_cmd_hexchar(ao_cmd_lex_c);
+		if (n < 0) {
+			ao_cmd_status = ao_cmd_syntax_error;
+			break;
+		}
+		ao_cmd_lex_i = (ao_cmd_lex_i << 4) | n;
+		ao_cmd_lex();
+	}
+}
+
+void
+ao_cmd_hex(void)
+{
+	__pdata uint8_t	r = ao_cmd_lex_error;
+	int8_t	n;
+
+	ao_cmd_lex_i = 0;
+	ao_cmd_white();
+	for(;;) {
+		n = ao_cmd_hexchar(ao_cmd_lex_c);
+		if (n < 0)
+			break;
+		ao_cmd_lex_i = (ao_cmd_lex_i << 4) | n;
+		r = ao_cmd_success;
+		ao_cmd_lex();
+	}
+	if (r != ao_cmd_success)
+		ao_cmd_status = r;
+}
+
+void
+ao_cmd_decimal(void) __reentrant
+{
+	uint8_t	r = ao_cmd_lex_error;
+
+	ao_cmd_lex_u32 = 0;
+	ao_cmd_white();
+	for(;;) {
+		if ('0' <= ao_cmd_lex_c && ao_cmd_lex_c <= '9')
+			ao_cmd_lex_u32 = (ao_cmd_lex_u32 * 10) + (ao_cmd_lex_c - '0');
+		else
+			break;
+		r = ao_cmd_success;
+		ao_cmd_lex();
+	}
+	if (r != ao_cmd_success)
+		ao_cmd_status = r;
+	ao_cmd_lex_i = (uint16_t) ao_cmd_lex_u32;
+}
+
+uint8_t
+ao_match_word(__code char *word)
+{
+	while (*word) {
+		if (ao_cmd_lex_c != *word) {
+			ao_cmd_status = ao_cmd_syntax_error;
+			return 0;
+		}
+		word++;
+		ao_cmd_lex();
+	}
+	return 1;
+}
+
+static void
+echo(void)
+{
+	ao_cmd_hex();
+	if (ao_cmd_status == ao_cmd_success)
+		ao_stdios[ao_cur_stdio].echo = ao_cmd_lex_i != 0;
+}
+
+static void
+ao_reboot(void)
+{
+	ao_cmd_white();
+	if (!ao_match_word("eboot"))
+		return;
+	/* Delay waiting for the packet master to be turned off
+	 * so that we don't end up back in idle mode because we
+	 * received a packet after boot.
+	 */
+	flush();
+	ao_delay(AO_SEC_TO_TICKS(1));
+	ao_arch_reboot();
+	ao_panic(AO_PANIC_REBOOT);
+}
+
+#ifndef HAS_VERSION
+#define HAS_VERSION 1
+#endif
+
+#if HAS_VERSION
+static void
+version(void)
+{
+	printf("manufacturer     %s\n"
+	       "product          %s\n"
+	       "serial-number    %u\n"
+#if HAS_FLIGHT
+	       "current-flight   %u\n"
+#endif
+#if HAS_LOG
+	       "log-format       %u\n"
+#endif
+	       , ao_manufacturer
+	       , ao_product
+	       , ao_serial_number
+#if HAS_FLIGHT
+	       , ao_flight_number
+#endif
+#if HAS_LOG
+	       , ao_log_format
+#endif
+		);
+	printf("software-version %s\n", ao_version);
+}
+#endif
+
+#ifndef NUM_CMDS
+#define NUM_CMDS	11
+#endif
+
+static __code struct ao_cmds	*__xdata (ao_cmds[NUM_CMDS]);
+static __pdata uint8_t		ao_ncmds;
+
+static void
+help(void)
+{
+	__pdata uint8_t cmds;
+	__pdata uint8_t cmd;
+	__code struct ao_cmds * __pdata cs;
+	__code const char *h;
+	uint8_t e;
+
+	for (cmds = 0; cmds < ao_ncmds; cmds++) {
+		cs = ao_cmds[cmds];
+		for (cmd = 0; cs[cmd].func; cmd++) {
+			h = cs[cmd].help;
+			ao_put_string(h);
+			e = strlen(h);
+			h += e + 1;
+			e = 45 - e;
+			while (e--)
+				putchar(' ');
+			ao_put_string(h);
+			putchar('\n');
+		}
+	}
+}
+
+static void
+report(void)
+{
+	switch(ao_cmd_status) {
+	case ao_cmd_lex_error:
+	case ao_cmd_syntax_error:
+		puts("Syntax error");
+		ao_cmd_status = 0;
+	default:
+		break;
+	}
+}
+
+void
+ao_cmd_register(__code struct ao_cmds *cmds)
+{
+	if (ao_ncmds >= NUM_CMDS)
+		ao_panic(AO_PANIC_CMD);
+	ao_cmds[ao_ncmds++] = cmds;
+}
+
+void
+ao_cmd(void)
+{
+	__pdata char	c;
+	uint8_t cmd, cmds;
+	__code struct ao_cmds * __xdata cs;
+	void (*__xdata func)(void);
+
+	for (;;) {
+		readline();
+		ao_cmd_lex();
+		ao_cmd_white();
+		c = ao_cmd_lex_c;
+		ao_cmd_lex();
+		if (c == '\r' || c == '\n')
+			continue;
+		func = (void (*)(void)) NULL;
+		for (cmds = 0; cmds < ao_ncmds; cmds++) {
+			cs = ao_cmds[cmds];
+			for (cmd = 0; cs[cmd].func; cmd++)
+				if (cs[cmd].help[0] == c) {
+					func = cs[cmd].func;
+					break;
+				}
+			if (func)
+				break;
+		}
+		if (func)
+			(*func)();
+		else
+			ao_cmd_status = ao_cmd_syntax_error;
+		report();
+	}
+}
+
+#if HAS_BOOT_LOADER
+
+#include <ao_boot.h>
+
+static void
+ao_loader(void)
+{
+	flush();
+	ao_boot_loader();
+}
+#endif
+
+__xdata struct ao_task ao_cmd_task;
+
+__code struct ao_cmds	ao_base_cmds[] = {
+	{ help,		"?\0Help" },
+#if HAS_TASK_INFO
+	{ ao_task_info,	"T\0Tasks" },
+#endif
+	{ echo,		"E <0 off, 1 on>\0Echo" },
+	{ ao_reboot,	"r eboot\0Reboot" },
+#if HAS_VERSION
+	{ version,	"v\0Version" },
+#endif
+#if HAS_BOOT_LOADER
+	{ ao_loader,	"X\0Switch to boot loader" },
+#endif
+	{ 0,	NULL },
+};
+
+void
+ao_cmd_init(void)
+{
+	ao_cmd_register(&ao_base_cmds[0]);
+	ao_add_task(&ao_cmd_task, ao_cmd, "cmd");
+}
diff --git a/src/kernel/ao_companion.h b/src/kernel/ao_companion.h
new file mode 100644
index 00000000..035325a3
--- /dev/null
+++ b/src/kernel/ao_companion.h
@@ -0,0 +1,55 @@
+/*
+ * 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_COMPANION_H_
+#define _AO_COMPANION_H_
+
+/* ao_companion.c */
+
+#define AO_COMPANION_SETUP		1
+#define AO_COMPANION_FETCH		2
+#define AO_COMPANION_NOTIFY		3
+
+struct ao_companion_command {
+	uint8_t		command;
+	uint8_t		flight_state;
+	uint16_t	tick;
+	uint16_t	serial;
+	uint16_t	flight;
+	int16_t		accel;
+	int16_t		speed;
+	int16_t		height;
+	int16_t		motor_number;
+};
+
+struct ao_companion_setup {
+	uint16_t	board_id;
+	uint16_t	board_id_inverse;
+	uint8_t		update_period;
+	uint8_t		channels;
+};
+
+extern __pdata uint8_t				ao_companion_running;
+extern __xdata uint8_t				ao_companion_mutex;
+extern __xdata struct ao_companion_command	ao_companion_command;
+extern __xdata struct ao_companion_setup	ao_companion_setup;
+extern __xdata uint16_t				ao_companion_data[AO_COMPANION_MAX_CHANNELS];
+
+void
+ao_companion_init(void);
+
+#endif /* _AO_COMPANION_H_ */
diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c
new file mode 100644
index 00000000..4482f673
--- /dev/null
+++ b/src/kernel/ao_config.c
@@ -0,0 +1,805 @@
+/*
+ * 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"
+#include <ao_config.h>
+#if HAS_FLIGHT
+#include <ao_sample.h>
+#include <ao_data.h>
+#endif
+
+__xdata struct ao_config ao_config;
+__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"
+#define AO_CONFIG_DEFAULT_ACCEL_ZERO_G	16000
+#define AO_CONFIG_DEFAULT_APOGEE_DELAY	0
+#define AO_CONFIG_DEFAULT_IGNITE_MODE	AO_IGNITE_MODE_DUAL
+#define AO_CONFIG_DEFAULT_PAD_ORIENTATION	AO_PAD_ORIENTATION_ANTENNA_UP
+#if HAS_EEPROM
+#ifndef USE_INTERNAL_FLASH
+#error Please define USE_INTERNAL_FLASH
+#endif
+#endif
+#ifndef AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX
+#if USE_INTERNAL_FLASH
+#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX	ao_storage_config
+#else
+#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_config_setup();
+	ao_config_erase();
+	ao_config_write(0, &ao_config, sizeof (ao_config));
+#if HAS_FLIGHT
+	ao_log_write_erase(0);
+#endif
+	ao_config_flush();
+}
+
+void
+ao_config_put(void)
+{
+	ao_mutex_get(&ao_config_mutex);
+	_ao_config_put();
+	ao_mutex_put(&ao_config_mutex);
+}
+#endif
+
+#if HAS_RADIO
+void
+ao_config_set_radio(void)
+{
+	ao_config.radio_setting = ao_freq_to_set(ao_config.frequency, ao_config.radio_cal);
+}
+#endif /* HAS_RADIO */
+
+static void
+_ao_config_get(void)
+{
+	uint8_t	minor;
+
+	if (ao_config_loaded)
+		return;
+#if HAS_EEPROM
+	/* Yes, I know ao_storage_read calls ao_storage_setup,
+	 * but ao_storage_setup *also* sets ao_storage_config, which we
+	 * need before calling ao_storage_read here
+	 */
+	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;
+		ao_config.minor = 0;
+
+		/* Version 0 stuff */
+		ao_config.main_deploy = AO_CONFIG_DEFAULT_MAIN_DEPLOY;
+		ao_xmemset(&ao_config.callsign, '\0', sizeof (ao_config.callsign));
+		ao_xmemcpy(&ao_config.callsign, CODE_TO_XDATA(AO_CONFIG_DEFAULT_CALLSIGN),
+		       sizeof(AO_CONFIG_DEFAULT_CALLSIGN) - 1);
+		ao_config._legacy_radio_channel = 0;
+	}
+	minor = ao_config.minor;
+	if (minor != AO_CONFIG_MINOR) {
+		/* Fixups for minor version 1 */
+		if (minor < 1)
+			ao_config.apogee_delay = AO_CONFIG_DEFAULT_APOGEE_DELAY;
+		/* Fixups for minor version 2 */
+		if (minor < 2) {
+			ao_config.accel_plus_g = 0;
+			ao_config.accel_minus_g = 0;
+		}
+		/* Fixups for minor version 3 */
+#if HAS_RADIO
+		if (minor < 3)
+			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;
+		if (minor < 6)
+			ao_config.pad_orientation = AO_CONFIG_DEFAULT_PAD_ORIENTATION;
+		if (minor < 8)
+			ao_config.radio_enable = AO_RADIO_ENABLE_CORE;
+		if (minor < 9)
+			ao_xmemset(&ao_config.aes_key, '\0', AO_AES_LEN);
+		if (minor < 10)
+			ao_config.frequency = 434550 + ao_config._legacy_radio_channel * 100;
+		if (minor < 11)
+			ao_config.apogee_lockout = 0;
+#if AO_PYRO_NUM
+		if (minor < 12)
+			memset(&ao_config.pyro, '\0', sizeof (ao_config.pyro));
+#endif
+		if (minor < 13)
+			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;
+		#endif
+#if HAS_RADIO_AMP
+		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;
+	}
+#if HAS_RADIO
+#if HAS_FORCE_FREQ
+	if (ao_force_freq) {
+		ao_config.frequency = 434550;
+		ao_config.radio_cal = ao_radio_cal;
+		ao_xmemcpy(&ao_config.callsign, CODE_TO_XDATA(AO_CONFIG_DEFAULT_CALLSIGN),
+		       sizeof(AO_CONFIG_DEFAULT_CALLSIGN) - 1);
+	}
+#endif
+	ao_config_set_radio();
+#endif
+	ao_config_loaded = 1;
+}
+
+void
+_ao_config_edit_start(void)
+{
+	ao_mutex_get(&ao_config_mutex);
+	_ao_config_get();
+}
+
+void
+_ao_config_edit_finish(void)
+{
+	ao_config_dirty = 1;
+	ao_mutex_put(&ao_config_mutex);
+}
+
+void
+ao_config_get(void)
+{
+	_ao_config_edit_start();
+	ao_mutex_put(&ao_config_mutex);
+}
+
+void
+ao_config_callsign_show(void)
+{
+	printf ("Callsign: \"%s\"\n", ao_config.callsign);
+}
+
+void
+ao_config_callsign_set(void) __reentrant
+{
+	uint8_t	c;
+	static __xdata char callsign[AO_MAX_CALLSIGN + 1];
+
+	ao_xmemset(callsign, '\0', sizeof callsign);
+	ao_cmd_white();
+	c = 0;
+	while (ao_cmd_lex_c != '\n') {
+		if (c < AO_MAX_CALLSIGN)
+			callsign[c++] = ao_cmd_lex_c;
+		else
+			ao_cmd_status = ao_cmd_lex_error;
+		ao_cmd_lex();
+	}
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	_ao_config_edit_start();
+	ao_xmemcpy(&ao_config.callsign, &callsign,
+	       AO_MAX_CALLSIGN + 1);
+	_ao_config_edit_finish();
+}
+
+#if HAS_RADIO
+
+void
+ao_config_frequency_show(void) __reentrant
+{
+	printf("Frequency: %ld\n",
+	       ao_config.frequency);
+}
+
+void
+ao_config_frequency_set(void) __reentrant
+{
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	_ao_config_edit_start();
+	ao_config.frequency = ao_cmd_lex_u32;
+	ao_config_set_radio();
+	_ao_config_edit_finish();
+#if HAS_RADIO_RECV
+	ao_radio_recv_abort();
+#endif
+}
+#endif
+
+#if HAS_FLIGHT
+
+void
+ao_config_main_deploy_show(void) __reentrant
+{
+	printf("Main deploy: %d meters\n",
+	       ao_config.main_deploy);
+}
+
+void
+ao_config_main_deploy_set(void) __reentrant
+{
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	_ao_config_edit_start();
+	ao_config.main_deploy = ao_cmd_lex_i;
+	_ao_config_edit_finish();
+}
+
+#if HAS_ACCEL
+void
+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();
+	(void) getchar();
+	puts("\r\n"); flush();
+	puts("Calibrating..."); flush();
+	i = ACCEL_CALIBRATE_SAMPLES;
+	accel_total = 0;
+	cal_data_ring = ao_sample_data;
+	while (i) {
+		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;
+}
+
+void
+ao_config_accel_calibrate_set(void) __reentrant
+{
+	int16_t	up, down;
+#if HAS_GYRO
+	int16_t	accel_along_up = 0, accel_along_down = 0;
+	int16_t	accel_across_up = 0, accel_across_down = 0;
+	int16_t	accel_through_up = 0, accel_through_down = 0;
+#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();
+		if (ao_cmd_status != ao_cmd_success)
+			return;
+		down = ao_cmd_lex_i;
+	}
+	if (up >= down) {
+		printf("Invalid accel: up (%d) down (%d)\n",
+		       up, down);
+		return;
+	}
+	_ao_config_edit_start();
+	ao_config.accel_plus_g = up;
+	ao_config.accel_minus_g = down;
+#if HAS_GYRO
+	if (ao_cmd_lex_i == 0) {
+		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 */
+
+void
+ao_config_apogee_delay_show(void) __reentrant
+{
+	printf("Apogee delay: %d seconds\n",
+	       ao_config.apogee_delay);
+}
+
+void
+ao_config_apogee_delay_set(void) __reentrant
+{
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	_ao_config_edit_start();
+	ao_config.apogee_delay = ao_cmd_lex_i;
+	_ao_config_edit_finish();
+}
+
+void
+ao_config_apogee_lockout_show(void) __reentrant
+{
+	printf ("Apogee lockout: %d seconds\n",
+		ao_config.apogee_lockout);
+}
+
+void
+ao_config_apogee_lockout_set(void) __reentrant
+{
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	_ao_config_edit_start();
+	ao_config.apogee_lockout = ao_cmd_lex_i;
+	_ao_config_edit_finish();
+}
+
+#endif /* HAS_FLIGHT */
+
+#if HAS_RADIO
+void
+ao_config_radio_cal_show(void) __reentrant
+{
+	printf("Radio cal: %ld\n", ao_config.radio_cal);
+}
+
+void
+ao_config_radio_cal_set(void) __reentrant
+{
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	_ao_config_edit_start();
+	ao_config.radio_cal = ao_cmd_lex_u32;
+	ao_config_set_radio();
+	_ao_config_edit_finish();
+}
+#endif
+
+#if HAS_LOG
+void
+ao_config_log_show(void) __reentrant
+{
+	printf("Max flight log: %d kB\n", (int16_t) (ao_config.flight_log_max >> 10));
+}
+
+void
+ao_config_log_set(void) __reentrant
+{
+	uint16_t	block = (uint16_t) (ao_storage_block >> 10);
+	uint16_t	log_max = (uint16_t) (ao_storage_log_max >> 10);
+
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	if (ao_log_present())
+		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 > 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;
+		_ao_config_edit_finish();
+	}
+}
+#endif /* HAS_LOG */
+
+#if HAS_IGNITE
+void
+ao_config_ignite_mode_show(void) __reentrant
+{
+	printf("Ignite mode: %d\n", ao_config.ignite_mode);
+}
+
+void
+ao_config_ignite_mode_set(void) __reentrant
+{
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	_ao_config_edit_start();
+	ao_config.ignite_mode = ao_cmd_lex_i;
+	_ao_config_edit_finish();
+}
+#endif
+
+#if HAS_ACCEL
+void
+ao_config_pad_orientation_show(void) __reentrant
+{
+	printf("Pad orientation: %d\n", ao_config.pad_orientation);
+}
+
+#ifndef AO_ACCEL_INVERT
+#define AO_ACCEL_INVERT	0x7fff
+#endif
+
+void
+ao_config_pad_orientation_set(void) __reentrant
+{
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	_ao_config_edit_start();
+	ao_cmd_lex_i &= 1;
+	if (ao_config.pad_orientation != ao_cmd_lex_i) {
+		int16_t t;
+		t = ao_config.accel_plus_g;
+		ao_config.accel_plus_g = AO_ACCEL_INVERT - ao_config.accel_minus_g;
+		ao_config.accel_minus_g = AO_ACCEL_INVERT - t;
+	}
+	ao_config.pad_orientation = ao_cmd_lex_i;
+	_ao_config_edit_finish();
+}
+#endif
+
+#if HAS_RADIO
+void
+ao_config_radio_enable_show(void) __reentrant
+{
+	printf("Radio enable: %d\n", ao_config.radio_enable);
+}
+
+void
+ao_config_radio_enable_set(void) __reentrant
+{
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	_ao_config_edit_start();
+	ao_config.radio_enable = ao_cmd_lex_i;
+	_ao_config_edit_finish();
+}
+#endif /* HAS_RADIO */
+	
+#if HAS_AES
+
+__xdata uint8_t	ao_config_aes_seq = 1;
+
+void
+ao_config_key_show(void) __reentrant
+{
+	uint8_t	i;
+	printf("AES key: ");
+	for (i = 0; i < AO_AES_LEN; i++)
+		printf ("%02x", ao_config.aes_key[i]);
+	printf("\n");
+}
+
+void
+ao_config_key_set(void) __reentrant
+{
+	uint8_t i;
+
+	_ao_config_edit_start();
+	for (i = 0; i < AO_AES_LEN; i++) {
+		ao_cmd_hexbyte();
+		if (ao_cmd_status != ao_cmd_success)
+			break;
+		ao_config.aes_key[i] = ao_cmd_lex_i;
+	}
+	++ao_config_aes_seq;
+	_ao_config_edit_finish();
+}
+#endif
+
+#if HAS_APRS
+
+void
+ao_config_aprs_show(void)
+{
+	printf ("APRS interval: %d\n", ao_config.aprs_interval);
+}
+
+void
+ao_config_aprs_set(void)
+{
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	_ao_config_edit_start();
+	ao_config.aprs_interval = ao_cmd_lex_i;
+	_ao_config_edit_finish();
+}
+
+#endif /* HAS_APRS */
+
+#if HAS_RADIO_AMP
+
+void
+ao_config_radio_amp_show(void)
+{
+	printf ("Radio amp setting: %d\n", ao_config.radio_amp);
+}
+
+void
+ao_config_radio_amp_set(void)
+{
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	_ao_config_edit_start();
+	ao_config.radio_amp = ao_cmd_lex_i;
+	_ao_config_edit_finish();
+}
+
+#endif
+
+#if HAS_RADIO_POWER
+
+void
+ao_config_radio_power_show(void)
+{
+	printf ("Radio power setting: %d\n", ao_config.radio_power);
+}
+
+void
+ao_config_radio_power_set(void)
+{
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	_ao_config_edit_start();
+	ao_config.radio_power = ao_cmd_lex_i;
+	_ao_config_edit_finish();
+}
+
+#endif
+
+struct ao_config_var {
+	__code char	*str;
+	void		(*set)(void) __reentrant;
+	void		(*show)(void) __reentrant;
+};
+
+static void
+ao_config_help(void) __reentrant;
+
+static void
+ao_config_show(void) __reentrant;
+
+#if HAS_EEPROM
+static void
+ao_config_save(void) __reentrant;
+#endif
+
+__code struct ao_config_var ao_config_vars[] = {
+#if HAS_FLIGHT
+	{ "m <meters>\0Main deploy (m)",
+	  ao_config_main_deploy_set,	ao_config_main_deploy_show, },
+	{ "d <delay>\0Apogee delay (s)",
+	  ao_config_apogee_delay_set,	ao_config_apogee_delay_show },
+	{ "L <seconds>\0Apogee detect lockout (s)",
+	  ao_config_apogee_lockout_set, ao_config_apogee_lockout_show, },
+#endif /* HAS_FLIGHT */
+#if HAS_RADIO
+	{ "F <freq>\0Frequency (kHz)",
+	  ao_config_frequency_set, ao_config_frequency_show },
+	{ "c <call>\0Callsign (8 char max)",
+	  ao_config_callsign_set,	ao_config_callsign_show },
+	{ "e <0 disable, 1 enable>\0Enable telemetry and RDF",
+	  ao_config_radio_enable_set, ao_config_radio_enable_show },
+	{ "f <cal>\0Radio calib (cal = rf/(xtal/2^16))",
+	  ao_config_radio_cal_set,  	ao_config_radio_cal_show },
+#if HAS_RADIO_POWER
+	{ "p <setting>\0Radio power setting (0-255)",
+	  ao_config_radio_power_set,	ao_config_radio_power_show },
+#endif
+#if HAS_RADIO_AMP
+	{ "d <setting>\0Radio amplifier setting (0-3)",
+	  ao_config_radio_amp_set,	ao_config_radio_amp_show },
+#endif
+#endif /* HAS_RADIO */
+#if HAS_ACCEL
+	{ "a <+g> <-g>\0Accel calib (0 for auto)",
+	  ao_config_accel_calibrate_set,ao_config_accel_calibrate_show },
+	{ "o <0 antenna up, 1 antenna down>\0Set pad orientation",
+	  ao_config_pad_orientation_set,ao_config_pad_orientation_show },
+#endif /* HAS_ACCEL */
+#if HAS_LOG
+	{ "l <size>\0Flight log size (kB)",
+	  ao_config_log_set,		ao_config_log_show },
+#endif
+#if HAS_IGNITE
+	{ "i <0 dual, 1 apogee, 2 main>\0Set igniter mode",
+	  ao_config_ignite_mode_set,	ao_config_ignite_mode_show },
+#endif
+#if HAS_AES
+	{ "k <32 hex digits>\0Set AES encryption key",
+	  ao_config_key_set, ao_config_key_show },
+#endif
+#if AO_PYRO_NUM
+	{ "P <n,?>\0Configure pyro channels",
+	  ao_pyro_set, ao_pyro_show },
+#endif
+#if HAS_APRS
+	{ "A <secs>\0APRS packet interval (0 disable)",
+	  ao_config_aprs_set, ao_config_aprs_show },
+#endif
+	{ "s\0Show",
+	  ao_config_show,		0 },
+#if HAS_EEPROM
+	{ "w\0Write to eeprom",
+	  ao_config_save,		0 },
+#endif
+	{ "?\0Help",
+	  ao_config_help,		0 },
+	{ 0, 0, 0 }
+};
+
+void
+ao_config_set(void)
+{
+	char	c;
+	uint8_t cmd;
+
+	ao_cmd_white();
+	c = ao_cmd_lex_c;
+	ao_cmd_lex();
+	for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++)
+		if (ao_config_vars[cmd].str[0] == c) {
+			(*ao_config_vars[cmd].set)();
+			return;
+		}
+	ao_cmd_status = ao_cmd_syntax_error;
+}
+
+static void
+ao_config_help(void) __reentrant
+{
+	uint8_t cmd;
+	for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++)
+		printf("%-20s %s\n",
+		       ao_config_vars[cmd].str,
+		       ao_config_vars[cmd].str+1+
+		       strlen(ao_config_vars[cmd].str));
+}
+
+static void
+ao_config_show(void) __reentrant
+{
+	uint8_t cmd;
+	ao_config_get();
+	printf("Config version: %d.%d\n",
+	       ao_config.major, ao_config.minor);
+	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_save(void) __reentrant
+{
+	uint8_t saved = 0;
+	ao_mutex_get(&ao_config_mutex);
+	if (ao_config_dirty) {
+		_ao_config_put();
+		ao_config_dirty = 0;
+		saved = 1;
+	}
+	ao_mutex_put(&ao_config_mutex);
+	if (saved)
+		puts("Saved");
+	else
+		puts("Nothing to save");
+}
+#endif
+
+__code struct ao_cmds ao_config_cmds[] = {
+	{ ao_config_set,	"c <var> <value>\0Set config (? for help, s to show)" },
+	{ 0, NULL },
+};
+
+void
+ao_config_init(void)
+{
+	ao_cmd_register(&ao_config_cmds[0]);
+}
diff --git a/src/kernel/ao_config.h b/src/kernel/ao_config.h
new file mode 100644
index 00000000..e101af8e
--- /dev/null
+++ b/src/kernel/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/kernel/ao_convert.c b/src/kernel/ao_convert.c
new file mode 100644
index 00000000..aa9b5f48
--- /dev/null
+++ b/src/kernel/ao_convert.c
@@ -0,0 +1,89 @@
+/*
+ * 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.
+ */
+
+#if !defined(AO_CONVERT_TEST) && !defined(AO_FLIGHT_TEST)
+#include "ao.h"
+#endif
+
+static const int16_t altitude_table[] = {
+#include "altitude.h"
+};
+
+#define ALT_FRAC_SCALE	(1 << ALT_FRAC_BITS)
+#define ALT_FRAC_MASK	(ALT_FRAC_SCALE - 1)
+
+int16_t
+ao_pres_to_altitude(int16_t pres) __reentrant
+{
+	uint8_t	o;
+	int16_t	part;
+
+	if (pres < 0)
+		pres = 0;
+	o = pres >> ALT_FRAC_BITS;
+	part = pres & ALT_FRAC_MASK;
+
+	return ((int32_t) altitude_table[o] * (ALT_FRAC_SCALE - part) +
+		(int32_t) altitude_table[o+1] * part + (ALT_FRAC_SCALE >> 1)) >> ALT_FRAC_BITS;
+}
+
+#if AO_NEED_ALTITUDE_TO_PRES
+int16_t
+ao_altitude_to_pres(int16_t alt) __reentrant
+{
+	int16_t span, sub_span;
+	uint8_t	l, h, m;
+	int32_t pres;
+
+	l = 0;
+	h = NALT - 1;
+	while ((h - l) != 1) {
+		m = (l + h) >> 1;
+		if (altitude_table[m] < alt)
+			h = m;
+		else
+			l = m;
+	}
+	span = altitude_table[l] - altitude_table[h];
+	sub_span = altitude_table[l] - alt;
+	pres = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_FRAC_BITS) + (span >> 1)) / span;
+	if (pres > 32767)
+		pres = 32767;
+	if (pres < 0)
+		pres = 0;
+	return (int16_t) pres;
+}
+#endif
+
+#if 0
+int16_t
+ao_temp_to_dC(int16_t temp) __reentrant
+{
+	int16_t	ret;
+
+	/* Output voltage at 0°C = 0.755V
+	 * Coefficient = 0.00247V/°C
+	 * Reference voltage = 1.25V
+	 *
+	 * temp = ((value / 32767) * 1.25 - 0.755) / 0.00247
+	 *      = (value - 19791.268) / 32768 * 1.25 / 0.00247
+	 *	≃ (value - 19791) * 1012 / 65536
+	 */
+	ret = ((temp - 19791) * 1012L) >> 16;
+	return ret;
+}
+#endif
diff --git a/src/kernel/ao_convert_pa.c b/src/kernel/ao_convert_pa.c
new file mode 100644
index 00000000..fe6e0ef6
--- /dev/null
+++ b/src/kernel/ao_convert_pa.c
@@ -0,0 +1,82 @@
+/*
+ * 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.
+ */
+
+#if !defined(AO_CONVERT_TEST) && !defined(AO_FLIGHT_TEST)
+#include "ao.h"
+#endif
+
+#ifndef AO_CONST_ATTRIB
+#define AO_CONST_ATTRIB
+#endif
+
+static const alt_t altitude_table[] AO_CONST_ATTRIB = {
+#include "altitude-pa.h"
+};
+
+#ifndef FETCH_ALT
+#define FETCH_ALT(o)	altitude_table[o]
+#endif
+
+#define ALT_SCALE	(1 << ALT_SHIFT)
+#define ALT_MASK	(ALT_SCALE - 1)
+
+alt_t
+ao_pa_to_altitude(int32_t pa)
+{
+	int16_t	o;
+	int16_t	part;
+	int32_t low, high;
+
+	if (pa < 0)
+		pa = 0;
+	if (pa > 120000L)
+		pa = 120000L;
+	o = pa >> ALT_SHIFT;
+	part = pa & ALT_MASK;
+
+	low = (int32_t) FETCH_ALT(o) * (ALT_SCALE - part);
+	high = (int32_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1);
+	return (low + high) >> ALT_SHIFT;
+}
+
+#ifdef AO_CONVERT_TEST
+int32_t
+ao_altitude_to_pa(int32_t alt)
+{
+	int32_t 	span, sub_span;
+	uint16_t	l, h, m;
+	int32_t 	pa;
+
+	l = 0;
+	h = NALT - 1;
+	while ((h - l) != 1) {
+		m = (l + h) >> 1;
+		if (altitude_table[m] < alt)
+			h = m;
+		else
+			l = m;
+	}
+	span = altitude_table[l] - altitude_table[h];
+	sub_span = altitude_table[l] - alt;
+	pa = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span;
+	if (pa > 120000)
+		pa = 120000;
+	if (pa < 0)
+		pa = 0;
+	return pa;
+}
+#endif
diff --git a/src/kernel/ao_convert_pa_test.c b/src/kernel/ao_convert_pa_test.c
new file mode 100644
index 00000000..7d5b1922
--- /dev/null
+++ b/src/kernel/ao_convert_pa_test.c
@@ -0,0 +1,76 @@
+/*
+ * 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 <stdint.h>
+#define AO_CONVERT_TEST
+typedef int32_t alt_t;
+#include "ao_host.h"
+#include "ao_convert_pa.c"
+
+#define STEP_P	1
+#define STEP_A	1
+
+static inline int i_abs(int i) { return i < 0 ? -i : i; }
+
+int
+main (int argc, char **argv)
+{
+	int	i;
+	int32_t p_to_a, p_to_a_to_p;
+	int32_t a_to_p, a_to_p_to_a;
+	int max_p_error = 0, max_p_error_p = -1;
+	int max_a_error = 0, max_a_error_a = -1;
+	int p_error;
+	int a_error;
+	int ret = 0;
+
+	for (i = 0; i < 120000 + STEP_P; i += STEP_P) {
+		if (i > 120000)
+			i = 120000;
+		p_to_a = ao_pa_to_altitude(i);
+		p_to_a_to_p = ao_altitude_to_pa(p_to_a);
+		p_error = i_abs(p_to_a_to_p - i);
+		if (p_error > max_p_error) {
+			max_p_error = p_error;
+			max_p_error_p = i;
+		}
+//		printf ("pa %d alt %d pa %d\n",
+//			i, p_to_a, p_to_a_to_p);
+	}
+	for (i = -1450; i < 40000 + STEP_A; i += STEP_A) {
+		a_to_p = ao_altitude_to_pa(i);
+		a_to_p_to_a = ao_pa_to_altitude(a_to_p);
+		a_error = i_abs(a_to_p_to_a - i);
+		if (a_error > max_a_error) {
+			max_a_error = a_error;
+			max_a_error_a = i;
+		}
+//		printf ("alt %d pa %d alt %d\n",
+//			i, a_to_p, a_to_p_to_a);
+	}
+	if (max_p_error > 2) {
+		printf ("max p error %d at %d\n", max_p_error,
+			max_p_error_p);
+		ret++;
+	}
+	if (max_a_error > 1) {
+		printf ("max a error %d at %d\n", max_a_error,
+			max_a_error_a);
+		ret++;
+	}
+	return ret;
+}
diff --git a/src/kernel/ao_convert_test.c b/src/kernel/ao_convert_test.c
new file mode 100644
index 00000000..87e76841
--- /dev/null
+++ b/src/kernel/ao_convert_test.c
@@ -0,0 +1,76 @@
+/*
+ * 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.
+ */
+
+#include <stdint.h>
+#define AO_CONVERT_TEST
+#define AO_NEED_ALTITUDE_TO_PRES 1
+#include "ao_host.h"
+#include "ao_convert.c"
+
+#define STEP	1
+
+static inline int i_abs(int i) { return i < 0 ? -i : i; }
+
+int main (int argc, char **argv)
+{
+	int	i;
+	int16_t p_to_a, p_to_a_to_p;
+	int16_t a_to_p, a_to_p_to_a;
+	int max_p_error = 0, max_p_error_p = -1;
+	int max_a_error = 0, max_a_error_a = -1;
+	int p_error;
+	int a_error;
+	int ret = 0;
+
+	for (i = 0; i < 32767 + STEP; i += STEP) {
+		if (i > 32767)
+			i = 32767;
+		p_to_a = ao_pres_to_altitude(i);
+		p_to_a_to_p = ao_altitude_to_pres(p_to_a);
+		p_error = i_abs(p_to_a_to_p - i);
+		if (p_error > max_p_error) {
+			max_p_error = p_error;
+			max_p_error_p = i;
+		}
+//		printf ("pres %d alt %d pres %d\n",
+//			i, p_to_a, p_to_a_to_p);
+	}
+	for (i = -1578; i < 15835 + STEP; i += STEP) {
+		if (i > 15835)
+			i = 15835;
+		a_to_p = ao_altitude_to_pres(i);
+		a_to_p_to_a = ao_pres_to_altitude(a_to_p);
+		a_error = i_abs(a_to_p_to_a - i);
+		if (a_error > max_a_error) {
+			max_a_error = a_error;
+			max_a_error_a = i;
+		}
+//		printf ("alt %d pres %d alt %d\n",
+//			i, a_to_p, a_to_p_to_a);
+	}
+	if (max_p_error > 2) {
+		printf ("max p error %d at %d\n", max_p_error,
+			max_p_error_p);
+		ret++;
+	}
+	if (max_a_error > 1) {
+		printf ("max a error %d at %d\n", max_a_error,
+			max_a_error_a);
+		ret++;
+	}
+	return ret;
+}
diff --git a/src/kernel/ao_convert_volt.c b/src/kernel/ao_convert_volt.c
new file mode 100644
index 00000000..8556d423
--- /dev/null
+++ b/src/kernel/ao_convert_volt.c
@@ -0,0 +1,33 @@
+/*
+ * Copyright © 2014 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"
+
+#define scale(v,p,m)	((int32_t) (v) * (AO_ADC_REFERENCE_DV * ((p) + (m))) / (AO_ADC_MAX * (m)))
+
+int16_t
+ao_battery_decivolt(int16_t adc)
+{
+	return scale(adc, AO_BATTERY_DIV_PLUS, AO_BATTERY_DIV_MINUS);
+}
+
+int16_t
+ao_ignite_decivolt(int16_t adc)
+{
+	return scale(adc, AO_IGNITE_DIV_PLUS, AO_IGNITE_DIV_MINUS);
+}
+
diff --git a/src/kernel/ao_data.c b/src/kernel/ao_data.c
new file mode 100644
index 00000000..6a3d02a1
--- /dev/null
+++ b/src/kernel/ao_data.c
@@ -0,0 +1,36 @@
+/*
+ * 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>
+
+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)
+{
+#if HAS_FLIGHT
+	uint8_t	i = ao_data_ring_prev(ao_sample_data);
+#else
+	uint8_t	i = ao_data_ring_prev(ao_data_head);
+#endif
+	memcpy(packet, (void *) &ao_data_ring[i], sizeof (struct ao_data));
+}
+#endif
diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h
new file mode 100644
index 00000000..c4b062fd
--- /dev/null
+++ b/src/kernel/ao_data.h
@@ -0,0 +1,338 @@
+/*
+ * 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_DATA_H_
+#define _AO_DATA_H_
+
+#define GRAVITY 9.80665
+
+#if HAS_ADC
+#define AO_DATA_ADC	(1 << 0)
+#else
+#define AO_DATA_ADC	0
+#endif
+
+#if HAS_MS5607
+#include <ao_ms5607.h>
+#define AO_DATA_MS5607	(1 << 1)
+#else
+#define AO_DATA_MS5607	0
+#endif
+
+#if HAS_MPU6000
+#include <ao_mpu6000.h>
+#define AO_DATA_MPU6000	(1 << 2)
+#else
+#define AO_DATA_MPU6000	0
+#endif
+
+#if HAS_HMC5883
+#include <ao_hmc5883.h>
+#define AO_DATA_HMC5883	(1 << 3)
+#else
+#define AO_DATA_HMC5883	0
+#endif
+
+#if HAS_MMA655X
+#include <ao_mma655x.h>
+#define AO_DATA_MMA655X (1 << 4)
+#else
+#define AO_DATA_MMA655X 0
+#endif
+
+#ifdef AO_DATA_RING
+
+#define AO_DATA_ALL	(AO_DATA_ADC|AO_DATA_MS5607|AO_DATA_MPU6000|AO_DATA_HMC5883|AO_DATA_MMA655X)
+
+struct ao_data {
+	uint16_t			tick;
+#if HAS_ADC
+	struct ao_adc			adc;
+#endif
+#if HAS_MS5607
+	struct ao_ms5607_sample		ms5607_raw;
+	struct ao_ms5607_value		ms5607_cooked;
+#endif
+#if HAS_MPU6000
+	struct ao_mpu6000_sample	mpu6000;
+#if !HAS_MMA655X
+	int16_t				z_accel;
+#endif
+#endif
+#if HAS_HMC5883
+	struct ao_hmc5883_sample	hmc5883;
+#endif
+#if HAS_MMA655X
+	uint16_t			mma655x;
+#endif
+};
+
+#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;
+extern volatile __data uint8_t		ao_data_count;
+
+/*
+ * Mark a section of data as ready, check for data complete
+ */
+#define AO_DATA_PRESENT(bit)	(ao_data_present |= (bit))
+
+/*
+ * Wait until it is time to write a sensor sample; this is
+ * signaled by the timer tick
+ */
+#define AO_DATA_WAIT() do {				\
+		ao_sleep(DATA_TO_XDATA ((void *) &ao_data_count));	\
+	} while (0)
+
+#endif /* AO_DATA_RING */
+
+#if !HAS_BARO && HAS_MS5607
+
+/* Either an MS5607 or an MS5611 hooked to a SPI port
+ */
+
+#define HAS_BARO	1
+
+typedef int32_t	pres_t;
+
+#ifndef AO_ALT_TYPE
+#define AO_ALT_TYPE	int32_t
+#endif
+
+typedef AO_ALT_TYPE	alt_t;
+
+#define ao_data_pres_cook(packet)	ao_ms5607_convert(&packet->ms5607_raw, &packet->ms5607_cooked)
+
+#define ao_data_pres(packet)	((packet)->ms5607_cooked.pres)
+#define ao_data_temp(packet)	((packet)->ms5607_cooked.temp)
+
+#define pres_to_altitude(p)	ao_pa_to_altitude(p)
+
+#endif
+
+#if !HAS_BARO && HAS_ADC
+
+#define HAS_BARO	1
+
+typedef int16_t pres_t;
+typedef int16_t alt_t;
+
+#define ao_data_pres(packet)	((packet)->adc.pres)
+#define ao_data_temp(packet)	((packet)->adc.temp)
+#define pres_to_altitude(p)	ao_pres_to_altitude(p)
+#define ao_data_pres_cook(p)
+
+#endif
+
+#if !HAS_BARO
+typedef int16_t alt_t;
+#endif
+
+/*
+ * Need a few macros to pull data from the sensors:
+ *
+ * ao_data_accel_sample	- pull raw sensor and convert to normalized values
+ * ao_data_accel	- pull normalized value (lives in the same memory)
+ * ao_data_set_accel	- store normalized value back in the sensor location
+ * ao_data_accel_invert	- flip rocket ends for positive acceleration
+ */
+
+#if HAS_ACCEL
+
+/* This section is for an analog accelerometer hooked to one of the ADC pins. As
+ * those are 5V parts, this also requires that the 5V supply be hooked to to anothe ADC
+ * pin so that the both can be measured to correct for changes between the 3.3V and 5V rails
+ */
+
+typedef int16_t accel_t;
+#define ao_data_accel(packet)			((packet)->adc.accel)
+#define ao_data_set_accel(packet, a)		((packet)->adc.accel = (a))
+#define ao_data_accel_invert(a)			(0x7fff -(a))
+
+/*
+ * Ok, the math here is a bit tricky.
+ *
+ * ao_sample_accel:  ADC output for acceleration
+ * ao_accel_ref:  ADC output for the 5V reference.
+ * ao_cook_accel: Corrected acceleration value
+ * Vcc:           3.3V supply to the CC1111
+ * Vac:           5V supply to the accelerometer
+ * accel:         input voltage to accelerometer ADC pin
+ * ref:           input voltage to 5V reference ADC pin
+ *
+ *
+ * Measured acceleration is ratiometric to Vcc:
+ *
+ *     ao_sample_accel   accel
+ *     ------------ = -----
+ *        32767        Vcc
+ *
+ * Measured 5v reference is also ratiometric to Vcc:
+ *
+ *     ao_accel_ref    ref
+ *     ------------ = -----
+ *        32767        Vcc
+ *
+ *
+ *	ao_accel_ref = 32767 * (ref / Vcc)
+ *
+ * Acceleration is measured ratiometric to the 5V supply,
+ * so what we want is:
+ *
+ *	ao_cook_accel    accel
+ *      ------------- =  -----
+ *          32767         ref
+ *
+ *
+ *	                accel    Vcc
+ *                    = ----- *  ---
+ *                       Vcc     ref
+ *
+ *                      ao_sample_accel       32767
+ *                    = ------------ *  ------------
+ *                         32767        ao_accel_ref
+ *
+ * Multiply through by 32767:
+ *
+ *                      ao_sample_accel * 32767
+ *	ao_cook_accel = --------------------
+ *                          ao_accel_ref
+ *
+ * Now, the tricky part. Getting this to compile efficiently
+ * and keeping all of the values in-range.
+ *
+ * First off, we need to use a shift of 16 instead of * 32767 as SDCC
+ * does the obvious optimizations for byte-granularity shifts:
+ *
+ *	ao_cook_accel = (ao_sample_accel << 16) / ao_accel_ref
+ *
+ * Next, lets check our input ranges:
+ *
+ * 	0 <= ao_sample_accel <= 0x7fff		(singled ended ADC conversion)
+ *	0x7000 <= ao_accel_ref <= 0x7fff	(the 5V ref value is close to 0x7fff)
+ *
+ * Plugging in our input ranges, we get an output range of 0 - 0x12490,
+ * which is 17 bits. That won't work. If we take the accel ref and shift
+ * by a bit, we'll change its range:
+ *
+ *	0xe000 <= ao_accel_ref<<1 <= 0xfffe
+ *
+ *	ao_cook_accel = (ao_sample_accel << 16) / (ao_accel_ref << 1)
+ *
+ * Now the output range is 0 - 0x9248, which nicely fits in 16 bits. It
+ * is, however, one bit too large for our signed computations. So, we
+ * take the result and shift that by a bit:
+ *
+ *	ao_cook_accel = ((ao_sample_accel << 16) / (ao_accel_ref << 1)) >> 1
+ *
+ * This finally creates an output range of 0 - 0x4924. As the ADC only
+ * provides 11 bits of data, we haven't actually lost any precision,
+ * just dropped a bit of noise off the low end.
+ */
+
+#if HAS_ACCEL_REF
+
+#define ao_data_accel_cook(packet) \
+	((uint16_t) ((((uint32_t) (packet)->adc.accel << 16) / ((packet)->adc.accel_ref << 1))) >> 1)
+
+#else
+
+#define ao_data_accel_cook(packet) ((packet)->adc.accel)
+
+#endif /* HAS_ACCEL_REF */
+
+#endif	/* HAS_ACCEL */
+
+#if !HAS_ACCEL && HAS_MMA655X
+
+#define HAS_ACCEL	1
+
+typedef int16_t accel_t;
+
+/* MMA655X is hooked up so that positive values represent negative acceleration */
+
+#define AO_ACCEL_INVERT		4095
+
+#define ao_data_accel(packet)			((packet)->mma655x)
+#if AO_MMA655X_INVERT
+#define ao_data_accel_cook(packet)		(AO_ACCEL_INVERT - (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)		(AO_ACCEL_INVERT - (accel))
+
+#endif
+
+#if !HAS_ACCEL && HAS_MPU6000
+
+#define HAS_ACCEL	1
+
+#define AO_ACCEL_INVERT		0
+
+typedef int16_t accel_t;
+
+/* MPU6000 is hooked up so that positive y is positive acceleration */
+#define ao_data_accel(packet)			((packet)->z_accel)
+#define ao_data_accel_cook(packet)		(-(packet)->mpu6000.accel_y)
+#define ao_data_set_accel(packet, accel)	((packet)->z_accel = (accel))
+#define ao_data_accel_invert(a)			(-(a))
+
+#endif
+
+#if !HAS_GYRO && HAS_MPU6000
+
+#define HAS_GYRO	1
+
+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) */
+/* Z axis is aligned perpendicular to the board (through) */
+
+#define ao_data_along(packet)	((packet)->mpu6000.accel_y)
+#define ao_data_across(packet)	((packet)->mpu6000.accel_x)
+#define ao_data_through(packet)	((packet)->mpu6000.accel_z)
+
+#define ao_data_roll(packet)	((packet)->mpu6000.gyro_y)
+#define ao_data_pitch(packet)	((packet)->mpu6000.gyro_x)
+#define ao_data_yaw(packet)	((packet)->mpu6000.gyro_z)
+
+#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/kernel/ao_dbg.h b/src/kernel/ao_dbg.h
new file mode 100644
index 00000000..181e6ec2
--- /dev/null
+++ b/src/kernel/ao_dbg.h
@@ -0,0 +1,62 @@
+/*
+ * 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_DBG_H_
+#define _AO_DBG_H_
+
+/*
+ * ao_dbg.c
+ *
+ * debug another telemetrum board
+ */
+
+/* Send a byte to the dbg target */
+void
+ao_dbg_send_byte(uint8_t byte);
+
+/* Receive a byte from the dbg target */
+uint8_t
+ao_dbg_recv_byte(void);
+
+/* Start a bulk transfer to/from dbg target memory */
+void
+ao_dbg_start_transfer(uint16_t addr);
+
+/* End a bulk transfer to/from dbg target memory */
+void
+ao_dbg_end_transfer(void);
+
+/* Write a byte to dbg target memory */
+void
+ao_dbg_write_byte(uint8_t byte);
+
+/* Read a byte from dbg target memory */
+uint8_t
+ao_dbg_read_byte(void);
+
+/* Enable dbg mode, switching use of the pins */
+void
+ao_dbg_debug_mode(void);
+
+/* Reset the dbg target */
+void
+ao_dbg_reset(void);
+
+void
+ao_dbg_init(void);
+
+#endif /* _AO_DBG_H_ */
diff --git a/src/kernel/ao_debounce.c b/src/kernel/ao_debounce.c
new file mode 100644
index 00000000..b9d67729
--- /dev/null
+++ b/src/kernel/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/kernel/ao_debounce.h b/src/kernel/ao_debounce.h
new file mode 100644
index 00000000..19c620f5
--- /dev/null
+++ b/src/kernel/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/kernel/ao_ee_fake.c b/src/kernel/ao_ee_fake.c
new file mode 100644
index 00000000..7fcfcab0
--- /dev/null
+++ b/src/kernel/ao_ee_fake.c
@@ -0,0 +1,37 @@
+/*
+ * 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"
+
+/*
+ * For hardware without eeprom, the config code still
+ * wants to call these functions
+ */
+uint8_t
+ao_ee_write_config(uint8_t *buf, uint16_t len) __reentrant
+{
+	(void) buf;
+	(void) len;
+	return 1;
+}
+
+uint8_t
+ao_ee_read_config(uint8_t *buf, uint16_t len) __reentrant
+{
+	ao_xmemset(buf, '\0', len);
+	return 1;
+}
diff --git a/src/kernel/ao_eeprom.h b/src/kernel/ao_eeprom.h
new file mode 100644
index 00000000..915522bf
--- /dev/null
+++ b/src/kernel/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/kernel/ao_fec.h b/src/kernel/ao_fec.h
new file mode 100644
index 00000000..618756c1
--- /dev/null
+++ b/src/kernel/ao_fec.h
@@ -0,0 +1,84 @@
+/*
+ * 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_FEC_H_
+#define _AO_FEC_H_
+
+#include <stdint.h>
+
+#define AO_FEC_CRC_INIT			0xffff
+#define AO_FEC_TRELLIS_TERMINATOR	0x0b
+#define AO_FEC_PREPARE_EXTRA		4
+
+extern const uint8_t ao_fec_whiten_table[];
+
+#if AO_FEC_DEBUG
+void
+ao_fec_dump_bytes(const uint8_t *bytes, uint16_t len, const char *name);
+#endif
+
+static inline uint16_t
+ao_fec_crc_byte(uint8_t byte, uint16_t crc)
+{
+	uint8_t	bit;
+
+	for (bit = 0; bit < 8; bit++) {
+		if (((crc & 0x8000) >> 8) ^ (byte & 0x80))
+			crc = (crc << 1) ^ 0x8005;
+		else
+			crc = (crc << 1);
+		byte <<= 1;
+	}
+	return crc;
+}
+
+uint16_t
+ao_fec_crc(const uint8_t *bytes, uint8_t len);
+
+/*
+ * 'len' is the length of the original data; 'bytes'
+ * must  be four bytes longer than that, and the first
+ * two after 'len' must be the received crc
+ */
+uint8_t
+ao_fec_check_crc(const uint8_t *bytes, uint8_t len);
+
+/*
+ * Compute CRC, whiten, convolve and interleave data. 'out' must be (len + 4) * 2 bytes long
+ */
+uint8_t
+ao_fec_encode(const uint8_t *in, uint8_t len, uint8_t *out);
+
+/*
+ * Decode data. 'in' is one byte per bit, soft decision
+ * 'out' must be len/8 bytes long
+ */
+
+#define AO_FEC_DECODE_BLOCK	(32)	/* callback must return multiples of this many bits */
+
+#define AO_FEC_DECODE_CRC_OK	0x80	/* stored in out[out_len-1] */
+
+uint8_t
+ao_fec_decode(const uint8_t *in, uint16_t in_len, uint8_t *out, uint8_t out_len, uint16_t (*callback)(void));
+
+/*
+ * Interleave data packed in bytes. 'out' must be 'len' bytes long.
+ */
+uint16_t
+ao_fec_interleave_bytes(uint8_t *in, uint16_t len, uint8_t *out);
+
+#endif /* _AO_FEC_H_ */
diff --git a/src/kernel/ao_fec_rx.c b/src/kernel/ao_fec_rx.c
new file mode 100644
index 00000000..c4f5559a
--- /dev/null
+++ b/src/kernel/ao_fec_rx.c
@@ -0,0 +1,318 @@
+/*
+ * 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_fec.h>
+#include <stdio.h>
+
+#ifdef TELEMEGA
+#include <ao.h>
+#endif
+
+#if AO_PROFILE
+#include <ao_profile.h>
+
+uint32_t	ao_fec_decode_start, ao_fec_decode_end;
+#endif
+
+/* 
+ * byte order repeats through 3 2 1 0
+ * 	
+ * bit-pair order repeats through
+ *
+ *  1/0 3/2 5/4 7/6
+ *
+ * So, the over all order is:
+ *
+ *	3,1/0 	2,1/0	1,1/0	0,1/0
+ *	3,3/2 	2,3/2	1,3/2	0,3/2
+ *	3,5/4	2,5/4	1,5/4	0,5/4
+ *	3,7/6	2,7/6	1,7/6	0,7/6
+ *
+ * The raw bit order is thus
+ *
+ *	1e/1f   16/17   0e/0f   06/07
+ *	1c/1d	14/15	0c/0d	04/05
+ *	1a/1b	12/13	0a/0b	02/03
+ *	18/19	10/11	08/09	00/01
+ */
+
+static const uint8_t ao_interleave_order[] = {
+	0x1e, 0x16, 0x0e, 0x06,
+	0x1c, 0x14, 0x0c, 0x04,
+	0x1a, 0x12, 0x0a, 0x02,
+	0x18, 0x10, 0x08, 0x00
+};
+
+static inline uint16_t ao_interleave_index(uint16_t i) {
+	return (i & ~0x1e) | ao_interleave_order[(i & 0x1e) >> 1];
+}
+
+#define NUM_STATE	8
+#define NUM_HIST	24
+
+typedef uint32_t	bits_t;
+
+#define V_0		0xff
+#define V_1		0x00
+
+/*
+ * These are just the 'zero' states; the 'one' states mirror them
+ */
+static const uint8_t ao_fec_decode_table[NUM_STATE*2] = {
+	V_0, V_0,	/* 000 */
+	V_0, V_1,	/* 001 */
+	V_1, V_1,	/* 010 */
+	V_1, V_0,	/* 011 */
+	V_1, V_1,	/* 100 */
+	V_1, V_0,	/* 101 */
+	V_0, V_0,	/* 110 */
+	V_0, V_1	/* 111 */
+};
+
+static inline uint8_t
+ao_next_state(uint8_t state, uint8_t bit)
+{
+	return ((state << 1) | bit) & 0x7;
+}
+
+/*
+ * 'in' is 8-bits per symbol soft decision data
+ * 'len' is input byte length. 'out' must be
+ * 'len'/16 bytes long
+ */
+
+uint8_t
+ao_fec_decode(const uint8_t *in, uint16_t len, uint8_t *out, uint8_t out_len, uint16_t (*callback)(void))
+{
+	static uint32_t	cost[2][NUM_STATE];		/* path cost */
+	static bits_t	bits[2][NUM_STATE];		/* save bits to quickly output them */
+
+	uint16_t	i;				/* input byte index */
+	uint16_t	b;				/* encoded symbol index (bytes/2) */
+	uint16_t	o;				/* output bit index */
+	uint8_t		p;				/* previous cost/bits index */
+	uint8_t		n;				/* next cost/bits index */
+	uint8_t		state;				/* state index */
+	const uint8_t	*whiten = ao_fec_whiten_table;
+	uint16_t	interleave;			/* input byte array index */
+	uint8_t		s0, s1;
+	uint16_t	avail;
+	uint16_t	crc = AO_FEC_CRC_INIT;
+#if AO_PROFILE
+	uint32_t	start_tick;
+#endif
+
+	p = 0;
+	for (state = 0; state < NUM_STATE; state++) {
+		cost[0][state] = 0x7fffffff;
+		bits[0][state] = 0;
+	}
+	cost[0][0] = 0;
+
+	if (callback)
+		avail = 0;
+	else
+		avail = len;
+
+#if AO_PROFILE
+	if (!avail) {
+		avail = callback();
+		if (!avail)
+			return 0;
+	}
+	start_tick = ao_profile_tick();
+#endif
+	o = 0;
+	for (i = 0; i < len; i += 2) {
+		b = i/2;
+		n = p ^ 1;
+
+		if (!avail) {
+			avail = callback();
+			if (!avail)
+				return 0;
+		}
+
+		/* Fetch one pair of input bytes, de-interleaving
+		 * the input.
+		 */
+		interleave = ao_interleave_index(i);
+		s0 = in[interleave];
+		s1 = in[interleave+1];
+
+		avail -= 2;
+
+		/* Compute path costs and accumulate output bit path
+		 * for each state and encoded bit value. Unrolling
+		 * this loop is worth about > 30% performance boost.
+		 * Decoding 76-byte remote access packets is reduced
+		 * from 14.700ms to 9.3ms. Redoing the loop to
+		 * directly compare the two pasts for each future state
+		 * reduces this down to 5.7ms
+		 */
+
+		/* Ok, of course this is tricky, it's optimized.
+		 *
+		 * First, it's important to realize that we have 8
+		 * states representing the combinations of the three
+		 * most recent bits from the encoder. Flipping any
+		 * of these three bits flips both output bits.
+		 *
+		 * 'state<<1' represents the target state for a new
+		 * bit value of 0. '(state<<1)+1' represents the
+		 * target state for a new bit value of 1.
+		 *
+		 * 'state' is the previous state with an oldest bit
+		 * value of 0. 'state + 4' is the previous state with
+		 * an oldest bit value of 1. These two states will
+		 * either lead to 'state<<1' or '(state<<1)+1', depending
+		 * on whether the next encoded bit was a zero or a one.
+		 *
+		 * m0 and m1 are the cost of coming to 'state<<1' from
+		 * one of the two possible previous states 'state' and
+		 * 'state + 4'.
+		 *
+		 * Because we know the expected values of each
+		 * received bit are flipped between these two previous
+		 * states:
+		 * 
+		 * 	bitcost(state+4) = 510 - bitcost(state)
+		 *
+		 * With those two total costs in hand, we then pick
+		 * the lower as the cost of the 'state<<1', and compute
+		 * the path of bits leading to that state.
+		 *
+		 * Then, do the same for '(state<<1) + 1'. This time,
+		 * instead of computing the m0 and m1 values from
+		 * scratch, because the only difference is that we're
+		 * expecting a one bit instead of a zero bit, we just
+		 * flip the bitcost values around to match the
+		 * expected transmitted bits with some tricky
+		 * arithmetic which is equivalent to:
+		 *
+		 *	m0 = cost[p][state] + (510 - bitcost);
+		 *	m1 = cost[p][state+4] + bitcost
+		 *
+		 * Then, the lowest cost and bit trace of the new state
+		 * is saved.
+		 */
+
+#define DO_STATE(state) {						\
+			uint32_t	bitcost;			\
+									\
+			uint32_t	m0;				\
+			uint32_t	m1;				\
+			uint32_t	bit;				\
+									\
+			bitcost = ((uint32_t) (s0 ^ ao_fec_decode_table[(state<<1)]) + \
+				   (uint32_t) (s1 ^ ao_fec_decode_table[(state<<1)|1])); \
+									\
+			m0 = cost[p][state] + bitcost;			\
+			m1 = cost[p][state+4] + (510 - bitcost);	\
+			bit = m0 > m1;					\
+			cost[n][state<<1] = bit ? m1 : m0;		\
+			bits[n][state<<1] = (bits[p][state + (bit<<2)] << 1) | (state&1); \
+									\
+			m0 -= (bitcost+bitcost-510);			\
+			m1 += (bitcost+bitcost-510);			\
+			bit = m0 > m1;					\
+			cost[n][(state<<1)+1] = bit ? m1 : m0;		\
+			bits[n][(state<<1)+1] = (bits[p][state + (bit<<2)] << 1) | (state&1); \
+		}
+
+		DO_STATE(0);
+		DO_STATE(1);
+		DO_STATE(2);
+		DO_STATE(3);
+
+#if 0
+		printf ("bit %3d symbol %2x %2x:", i/2, s0, s1);
+		for (state = 0; state < NUM_STATE; state++) {
+			printf (" %8u(%08x)", cost[n][state], bits[n][state]);
+		}
+		printf ("\n");
+#endif
+		p = n;
+
+		/* A loop is needed to handle the last output byte. It
+		 * won't have any bits of future data to perform full
+		 * error correction, but we might as well give the
+		 * best possible answer anyways.
+		 */
+		while ((b - o) >= (8 + NUM_HIST) || (i + 2 >= len && b > o)) {
+
+			/* Compute number of bits to the end of the
+			 * last full byte of data. This is generally
+			 * NUM_HIST, unless we've reached
+			 * the end of the input, in which case
+			 * it will be seven.
+			 */
+			int8_t		dist = b - (o + 8);	/* distance to last ready-for-writing bit */
+			uint32_t	min_cost;		/* lowest cost */
+			uint8_t		min_state;		/* lowest cost state */
+			uint8_t		byte;
+
+			/* Find the best fit at the current point
+			 * of the decode.
+			 */
+			min_cost = cost[p][0];
+			min_state = 0;
+			for (state = 1; state < NUM_STATE; state++) {
+				if (cost[p][state] < min_cost) {
+					min_cost = cost[p][state];
+					min_state = state;
+				}
+			}
+
+			/* The very last byte of data has the very last bit
+			 * of data left in the state value; just smash the
+			 * bits value in place and reset the 'dist' from
+			 * -1 to 0 so that the full byte is read out
+			 */
+			if (dist < 0) {
+				bits[p][min_state] = (bits[p][min_state] << 1) | (min_state & 1);
+				dist = 0;
+			}
+
+#if 0
+			printf ("\tbit %3d min_cost %5d old bit %3d old_state %x bits %02x whiten %0x\n",
+				i/2, min_cost, o + 8, min_state, (bits[p][min_state] >> dist) & 0xff, *whiten);
+#endif
+			byte = (bits[p][min_state] >> dist) ^ *whiten++;
+			*out++ = byte;
+			if (out_len > 2)
+				crc = ao_fec_crc_byte(byte, crc);
+
+			if (!--out_len) {
+				if ((out[-2] == (uint8_t) (crc >> 8)) &&
+				    out[-1] == (uint8_t) crc)
+					out[-1] = AO_FEC_DECODE_CRC_OK;
+				else
+					out[-1] = 0;
+				out[-2] = 0;
+				goto done;
+			}
+			o += 8;
+		}
+	}
+done:
+#if AO_PROFILE
+	ao_fec_decode_start = start_tick;
+	ao_fec_decode_end = ao_profile_tick();
+#endif
+	return 1;
+}
diff --git a/src/kernel/ao_fec_tx.c b/src/kernel/ao_fec_tx.c
new file mode 100644
index 00000000..4941d745
--- /dev/null
+++ b/src/kernel/ao_fec_tx.c
@@ -0,0 +1,134 @@
+/*
+ * 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_fec.h>
+#include <stdio.h>
+
+#if AO_FEC_DEBUG
+void
+ao_fec_dump_bytes(const uint8_t *bytes, uint16_t len, const char *name)
+{
+	uint16_t	i;
+
+	printf ("%s (%d):", name, len);
+	for (i = 0; i < len; i++) {
+		if ((i & 7) == 0)
+			printf ("\n\t%02x:", i);
+		printf(" %02x", bytes[i]);
+	}
+	printf ("\n");
+}
+#endif
+
+uint16_t
+ao_fec_crc(const uint8_t *bytes, uint8_t len)
+{
+	uint16_t	crc = AO_FEC_CRC_INIT;
+
+	while (len--)
+		crc = ao_fec_crc_byte(*bytes++, crc);
+	return crc;
+}
+
+/*
+ * len is the length of the data; the crc will be
+ * the fist two bytes after that
+ */
+
+uint8_t
+ao_fec_check_crc(const uint8_t *bytes, uint8_t len)
+{
+	uint16_t	computed_crc = ao_fec_crc(bytes, len);
+	uint16_t	received_crc = (bytes[len] << 8) | (bytes[len+1]);
+
+	return computed_crc == received_crc;
+}
+
+/*
+ * Compute CRC and trellis-terminator/interleave-pad bytes
+ */
+static uint8_t
+ao_fec_prepare(const uint8_t *in, uint8_t len, uint8_t *extra)
+{
+	uint16_t	crc = ao_fec_crc (in, len);
+	uint8_t		i = 0;
+	uint8_t		num_fec;
+
+	/* Append CRC */
+	extra[i++] = crc >> 8;
+	extra[i++] = crc;
+
+	/* Append FEC -- 1 byte if odd, two bytes if even */
+	num_fec = 2 - (i & 1);
+	while (num_fec--)
+		extra[i++] = AO_FEC_TRELLIS_TERMINATOR;
+	return i;
+}
+
+const uint8_t ao_fec_whiten_table[] = {
+#include "ao_whiten.h"
+};
+
+static const uint8_t ao_fec_encode_table[16] = {
+/* next 0  1	  state */
+	0, 3,	/* 000 */
+	1, 2,	/* 001 */
+	3, 0,	/* 010 */
+	2, 1,	/* 011 */
+	3, 0,	/* 100 */
+	2, 1,	/* 101 */
+	0, 3,	/* 110 */
+	1, 2	/* 111 */
+};
+
+uint8_t
+ao_fec_encode(const uint8_t *in, uint8_t len, uint8_t *out)
+{
+	uint8_t		extra[AO_FEC_PREPARE_EXTRA];
+	uint8_t 	extra_len;
+	uint32_t	encode, interleave;
+	uint8_t		pair, byte, bit;
+	uint16_t	fec = 0;
+	const uint8_t	*whiten = ao_fec_whiten_table;
+
+	extra_len = ao_fec_prepare(in, len, extra);
+	for (pair = 0; pair < len + extra_len; pair += 2) {
+		encode = 0;
+		for (byte = 0; byte < 2; byte++) {
+			if (pair + byte == len)
+				in = extra;
+			fec |= *in++ ^ *whiten++;
+			for (bit = 0; bit < 8; bit++) {
+				encode = encode << 2 | ao_fec_encode_table[fec >> 7];
+				fec = (fec << 1) & 0x7ff;
+			}
+		}
+
+		interleave = 0;
+		for (bit = 0; bit < 4 * 4; bit++) {
+			uint8_t	byte_shift = (bit & 0x3) << 3;
+			uint8_t	bit_shift = (bit & 0xc) >> 1;
+
+			interleave = (interleave << 2) | ((encode >> (byte_shift + bit_shift)) & 0x3);
+		}
+		*out++ = interleave >> 24;
+		*out++ = interleave >> 16;
+		*out++ = interleave >> 8;
+		*out++ = interleave >> 0;
+	}
+	return (len + extra_len) * 2;
+}
diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
new file mode 100644
index 00000000..24099347
--- /dev/null
+++ b/src/kernel/ao_flight.c
@@ -0,0 +1,472 @@
+/*
+ * 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.
+ */
+
+#ifndef AO_FLIGHT_TEST
+#include "ao.h"
+#include <ao_log.h>
+#endif
+
+#if HAS_MPU6000
+#include <ao_quaternion.h>
+#endif
+
+#ifndef HAS_ACCEL
+#error Please define HAS_ACCEL
+#endif
+
+#ifndef HAS_GPS
+#error Please define HAS_GPS
+#endif
+
+#ifndef HAS_USB
+#error Please define HAS_USB
+#endif
+
+#ifndef HAS_TELEMETRY
+#define HAS_TELEMETRY	HAS_RADIO
+#endif
+
+/* Main flight thread. */
+
+__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_SENSOR_ERRORS
+/* 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
+ */
+static __data uint16_t		ao_interval_end;
+static __data int16_t		ao_interval_min_height;
+static __data int16_t		ao_interval_max_height;
+#if HAS_ACCEL
+static __data int16_t		ao_coast_avg_accel;
+#endif
+
+__pdata uint8_t			ao_flight_force_idle;
+
+/* We also have a clock, which can be used to sanity check things in
+ * case of other failures
+ */
+
+#define BOOST_TICKS_MAX	AO_SEC_TO_TICKS(15)
+
+/* Landing is detected by getting constant readings from both pressure and accelerometer
+ * for a fairly long time (AO_INTERVAL_TICKS)
+ */
+#define AO_INTERVAL_TICKS	AO_SEC_TO_TICKS(10)
+
+#define abs(a)	((a) < 0 ? -(a) : (a))
+
+void
+ao_flight(void)
+{
+	ao_sample_init();
+	ao_flight_state = ao_flight_startup;
+	for (;;) {
+
+		/*
+		 * Process ADC samples, just looping
+		 * until the sensors are calibrated.
+		 */
+		if (!ao_sample())
+			continue;
+
+		switch (ao_flight_state) {
+		case ao_flight_startup:
+
+			/* Check to see what mode we should go to.
+			 *  - Invalid mode if accel cal appears to be out
+			 *  - pad mode if we're upright,
+			 *  - idle mode otherwise
+			 */
+#if HAS_ACCEL
+			if (ao_config.accel_plus_g == 0 ||
+			    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 ||
+			    ao_ground_height < -1000 ||
+			    ao_ground_height > 7000)
+			{
+				/* Detected an accel value outside -1.5g to 1.5g
+				 * (or uncalibrated values), so we go into invalid mode
+				 */
+				ao_flight_state = ao_flight_invalid;
+
+#if HAS_RADIO && PACKET_HAS_SLAVE
+				/* Turn on packet system in invalid mode on TeleMetrum */
+				ao_packet_slave_start();
+#endif
+			} else
+#endif
+				if (!ao_flight_force_idle
+#if HAS_ACCEL
+				    && ao_ground_accel < ao_config.accel_plus_g + ACCEL_NOSE_UP
+#endif
+					)
+ 			{
+				/* Set pad mode - we can fly! */
+				ao_flight_state = ao_flight_pad;
+#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 && PACKET_HAS_SLAVE
+				/* Disable packet mode in pad state on TeleMini */
+				ao_packet_slave_stop();
+#endif
+
+#if HAS_TELEMETRY
+				/* Turn on telemetry system */
+				ao_rdf_set(1);
+				ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD);
+#endif
+#if AO_LED_RED
+				/* 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;
+#if HAS_SENSOR_ERRORS
+				if (ao_sensor_errors)
+					ao_flight_state = ao_flight_invalid;
+#endif
+ 
+#if HAS_ACCEL && HAS_RADIO && PACKET_HAS_SLAVE
+				/* Turn on packet system in idle mode on TeleMetrum */
+				ao_packet_slave_start();
+#endif
+
+#if AO_LED_RED
+				/* 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));
+
+			break;
+		case ao_flight_pad:
+
+			/* pad to boost:
+			 *
+			 * barometer: > 20m vertical motion
+			 *             OR
+			 * accelerometer: > 2g AND velocity > 5m/s
+			 *
+			 * The accelerometer should always detect motion before
+			 * the barometer, but we use both to make sure this
+			 * transition is detected. If the device
+			 * doesn't have an accelerometer, then ignore the
+			 * speed and acceleration as they are quite noisy
+			 * on the pad.
+			 */
+			if (ao_height > AO_M_TO_HEIGHT(20)
+#if HAS_ACCEL
+			    || (ao_accel > AO_MSS_TO_ACCEL(20) &&
+				ao_speed > AO_MS_TO_SPEED(5))
+#endif
+				)
+			{
+				ao_flight_state = ao_flight_boost;
+				ao_boost_tick = ao_sample_tick;
+
+				/* start logging data */
+				ao_log_start();
+
+#if HAS_TELEMETRY
+				/* Increase telemetry rate */
+				ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_FLIGHT);
+
+				/* disable RDF beacon */
+				ao_rdf_set(0);
+#endif
+
+#if HAS_GPS
+				/* Record current GPS position by waking up GPS log tasks */
+				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));
+			}
+			break;
+		case ao_flight_boost:
+
+			/* boost to fast:
+			 *
+			 * accelerometer: start to fall at > 1/4 G
+			 *              OR
+			 * time: boost for more than 15 seconds
+			 *
+			 * Detects motor burn out by the switch from acceleration to
+			 * deceleration, or by waiting until the maximum burn duration
+			 * (15 seconds) has past.
+			 */
+			if ((ao_accel < AO_MSS_TO_ACCEL(-2.5) && ao_height > AO_M_TO_HEIGHT(100)) ||
+			    (int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX)
+			{
+#if HAS_ACCEL
+				ao_flight_state = ao_flight_fast;
+				ao_coast_avg_accel = ao_accel;
+#else
+				ao_flight_state = ao_flight_coast;
+#endif
+				++ao_motor_number;
+				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
+			}
+			break;
+#if HAS_ACCEL
+		case ao_flight_fast:
+			/*
+			 * This is essentially the same as coast,
+			 * but the barometer is being ignored as
+			 * it may be unreliable.
+			 */
+			if (ao_speed < AO_MS_TO_SPEED(AO_MAX_BARO_SPEED))
+			{
+				ao_flight_state = ao_flight_coast;
+				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
+			} else
+				goto check_re_boost;
+			break;
+#endif
+		case ao_flight_coast:
+
+			/*
+			 * By customer request - allow the user
+			 * to lock out apogee detection for a specified
+			 * number of seconds.
+			 */
+			if (ao_config.apogee_lockout) {
+				if ((ao_sample_tick - ao_boost_tick) <
+				    AO_SEC_TO_TICKS(ao_config.apogee_lockout))
+					break;
+			}
+
+			/* apogee detect: coast to drogue deploy:
+			 *
+			 * speed: < 0
+			 *
+			 * Also make sure the model altitude is tracking
+			 * the measured altitude reasonably closely; otherwise
+			 * we're probably transsonic.
+			 */
+			if (ao_speed < 0
+#if !HAS_ACCEL
+			    && (ao_sample_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < 100)
+#endif
+				)
+			{
+#if HAS_IGNITE
+				/* ignite the drogue charge */
+				ao_ignite(ao_igniter_drogue);
+#endif
+
+#if HAS_TELEMETRY
+				/* slow down the telemetry system */
+				ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_RECOVER);
+
+				/* Turn the RDF beacon back on */
+				ao_rdf_set(1);
+#endif
+
+				/* and enter drogue state */
+				ao_flight_state = ao_flight_drogue;
+				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
+			}
+#if HAS_ACCEL
+			else {
+			check_re_boost:
+				ao_coast_avg_accel = ao_coast_avg_accel - (ao_coast_avg_accel >> 6) + (ao_accel >> 6);
+				if (ao_coast_avg_accel > AO_MSS_TO_ACCEL(20)) {
+					ao_boost_tick = ao_sample_tick;
+					ao_flight_state = ao_flight_boost;
+					ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
+				}
+			}
+#endif
+
+			break;
+		case ao_flight_drogue:
+
+			/* drogue to main deploy:
+			 *
+			 * barometer: reach main deploy altitude
+			 *
+			 * Would like to use the accelerometer for this test, but
+			 * the orientation of the flight computer is unknown after
+			 * drogue deploy, so we ignore it. Could also detect
+			 * high descent rate using the pressure sensor to
+			 * recognize drogue deploy failure and eject the main
+			 * at that point. Perhaps also use the drogue sense lines
+			 * to notice continutity?
+			 */
+			if (ao_height <= ao_config.main_deploy)
+			{
+#if HAS_IGNITE
+				ao_ignite(ao_igniter_main);
+#endif
+
+				/*
+				 * Start recording min/max height
+				 * to figure out when the rocket has landed
+				 */
+
+				/* initialize interval values */
+				ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
+
+				ao_interval_min_height = ao_interval_max_height = ao_avg_height;
+
+				ao_flight_state = ao_flight_main;
+				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
+			}
+			break;
+
+			/* fall through... */
+		case ao_flight_main:
+
+			/* main to land:
+			 *
+			 * barometer: altitude stable
+			 */
+
+			if (ao_avg_height < ao_interval_min_height)
+				ao_interval_min_height = ao_avg_height;
+			if (ao_avg_height > ao_interval_max_height)
+				ao_interval_max_height = ao_avg_height;
+
+			if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) {
+				if (ao_interval_max_height - ao_interval_min_height <= AO_M_TO_HEIGHT(4))
+				{
+					ao_flight_state = ao_flight_landed;
+
+					/* turn off the ADC capture */
+					ao_timer_set_adc_interval(0);
+
+					ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
+				}
+				ao_interval_min_height = ao_interval_max_height = ao_avg_height;
+				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;
+		}
+	}
+}
+
+#if HAS_FLIGHT_DEBUG
+static inline int int_part(int16_t i)	{ return i >> 4; }
+static inline int frac_part(int16_t i)	{ return ((i & 0xf) * 100 + 8) / 16; }
+
+static void
+ao_flight_dump(void)
+{
+#if HAS_ACCEL
+	int16_t	accel;
+
+	accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16;
+#endif
+
+	printf ("sample:\n");
+	printf ("  tick        %d\n", ao_sample_tick);
+	printf ("  raw pres    %d\n", ao_sample_pres);
+#if HAS_ACCEL
+	printf ("  raw accel   %d\n", ao_sample_accel);
+#endif
+	printf ("  ground pres %d\n", ao_ground_pres);
+	printf ("  ground alt  %d\n", ao_ground_height);
+#if HAS_ACCEL
+	printf ("  raw accel   %d\n", ao_sample_accel);
+	printf ("  groundaccel %d\n", ao_ground_accel);
+	printf ("  accel_2g    %d\n", ao_accel_2g);
+#endif
+
+	printf ("  alt         %d\n", ao_sample_alt);
+	printf ("  height      %d\n", ao_sample_height);
+#if HAS_ACCEL
+	printf ("  accel       %d.%02d\n", int_part(accel), frac_part(accel));
+#endif
+
+
+	printf ("kalman:\n");
+	printf ("  height      %d\n", ao_height);
+	printf ("  speed       %d.%02d\n", int_part(ao_speed), frac_part(ao_speed));
+	printf ("  accel       %d.%02d\n", int_part(ao_accel), frac_part(ao_accel));
+	printf ("  max_height  %d\n", ao_max_height);
+	printf ("  avg_height  %d\n", ao_avg_height);
+	printf ("  error_h     %d\n", ao_error_h);
+	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;
+}
+
+uint8_t ao_orient_test;
+
+static void
+ao_orient_test_select(void)
+{
+	ao_orient_test = !ao_orient_test;
+}
+
+__code struct ao_cmds ao_flight_cmds[] = {
+	{ ao_flight_dump, 	"F\0Dump flight status" },
+	{ ao_gyro_test,		"G\0Test gyro code" },
+	{ ao_orient_test_select,"O\0Test orientation code" },
+	{ 0, NULL },
+};
+#endif
+
+static __xdata struct ao_task	flight_task;
+
+void
+ao_flight_init(void)
+{
+	ao_flight_state = ao_flight_startup;
+#if HAS_FLIGHT_DEBUG
+	ao_cmd_register(&ao_flight_cmds[0]);
+#endif
+	ao_add_task(&flight_task, ao_flight, "flight");
+}
diff --git a/src/kernel/ao_flight.h b/src/kernel/ao_flight.h
new file mode 100644
index 00000000..01d21c11
--- /dev/null
+++ b/src/kernel/ao_flight.h
@@ -0,0 +1,70 @@
+/*
+ * 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_FLIGHT_H_
+#define _AO_FLIGHT_H_
+
+
+/*
+ * ao_flight.c
+ */
+
+enum ao_flight_state {
+	ao_flight_startup = 0,
+	ao_flight_idle = 1,
+	ao_flight_pad = 2,
+	ao_flight_boost = 3,
+	ao_flight_fast = 4,
+	ao_flight_coast = 5,
+	ao_flight_drogue = 6,
+	ao_flight_main = 7,
+	ao_flight_landed = 8,
+	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 || HAS_MMA655X
+#define HAS_SENSOR_ERRORS	1
+#endif
+
+#if HAS_SENSOR_ERRORS
+extern __xdata uint8_t			ao_sensor_errors;
+#endif
+
+extern __pdata uint16_t			ao_launch_time;
+extern __pdata uint8_t			ao_flight_force_idle;
+
+/* Flight thread */
+void
+ao_flight(void);
+
+/* Initialize flight thread */
+void
+ao_flight_init(void);
+
+/*
+ * ao_flight_nano.c
+ */
+
+void
+ao_flight_nano_init(void);
+
+#endif /* _AO_FLIGHT_H_ */
diff --git a/src/kernel/ao_flight_nano.c b/src/kernel/ao_flight_nano.c
new file mode 100644
index 00000000..406d81ad
--- /dev/null
+++ b/src/kernel/ao_flight_nano.c
@@ -0,0 +1,120 @@
+/*
+ * 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"
+
+/* Main flight thread. */
+
+__pdata enum ao_flight_state	ao_flight_state;	/* current flight state */
+__pdata uint16_t		ao_launch_tick;		/* time of launch detect */
+
+/*
+ * track min/max data over a long interval to detect
+ * resting
+ */
+__pdata uint16_t		ao_interval_end;
+__pdata alt_t			ao_interval_min_height;
+__pdata alt_t			ao_interval_max_height;
+
+__pdata uint8_t			ao_flight_force_idle;
+
+/* Landing is detected by getting constant readings from both pressure and accelerometer
+ * for a fairly long time (AO_INTERVAL_TICKS)
+ */
+#define AO_INTERVAL_TICKS	AO_SEC_TO_TICKS(5)
+
+static void
+ao_flight_nano(void)
+{
+	ao_sample_init();
+	ao_flight_state = ao_flight_startup;
+
+	for (;;) {
+		/*
+		 * Process ADC samples, just looping
+		 * until the sensors are calibrated.
+		 */
+		if (!ao_sample())
+			continue;
+
+		switch (ao_flight_state) {
+		case ao_flight_startup:
+			if (ao_flight_force_idle) {
+				/* Set idle mode */
+				ao_flight_state = ao_flight_idle;
+			} else {
+				ao_flight_state = ao_flight_pad;
+				/* Disable packet mode in pad state */
+				ao_packet_slave_stop();
+
+				/* Turn on telemetry system */
+				ao_rdf_set(1);
+				ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD);
+			}
+			/* signal successful initialization by turning off the LED */
+			ao_led_off(AO_LED_RED);
+
+			/* wakeup threads due to state change */
+			ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
+			break;
+		case ao_flight_pad:
+			if (ao_height> AO_M_TO_HEIGHT(20)) {
+				ao_flight_state = ao_flight_drogue;
+				ao_launch_tick = ao_sample_tick;
+
+				/* start logging data */
+				ao_log_start();
+
+				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
+			}
+			break;
+		case ao_flight_drogue:
+			/* drogue/main to land:
+			 *
+			 * barometer: altitude stable
+			 */
+
+			if (ao_height < ao_interval_min_height)
+				ao_interval_min_height = ao_height;
+			if (ao_height > ao_interval_max_height)
+				ao_interval_max_height = ao_height;
+
+			if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) {
+				if (ao_interval_max_height - ao_interval_min_height < AO_M_TO_HEIGHT(5))
+				{
+					ao_flight_state = ao_flight_landed;
+
+					/* turn off the ADC capture */
+					ao_timer_set_adc_interval(0);
+					ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
+				}
+				ao_interval_min_height = ao_interval_max_height = ao_height;
+				ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
+			}
+			break;
+		}
+	}
+}
+
+static __xdata struct ao_task	flight_task;
+
+void
+ao_flight_nano_init(void)
+{
+	ao_flight_state = ao_flight_startup;
+	ao_add_task(&flight_task, ao_flight_nano, "flight");
+}
diff --git a/src/kernel/ao_freq.c b/src/kernel/ao_freq.c
new file mode 100644
index 00000000..12496f6f
--- /dev/null
+++ b/src/kernel/ao_freq.c
@@ -0,0 +1,55 @@
+/*
+ * 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>
+
+/*
+ * The provided 'calibration' value is
+ * that needed to tune the radio to precisely 434550kHz.
+ * Use that to 'walk' to the target frequency by following
+ * a 'bresenham' line from 434550kHz to the target
+ * frequency, and updating the radio setting along the way
+ */
+
+int32_t ao_freq_to_set(int32_t freq, int32_t cal) __reentrant
+{
+	static __pdata int32_t	set;
+	static __pdata uint8_t	neg;
+	static __pdata int32_t	error;
+
+	set = 0;
+	neg = 0;
+	error = -434550 / 2;
+
+	if ((freq -= 434550) < 0) {
+		neg = 1;
+		freq = -freq;
+	}
+	for (;;) {
+		if (error > 0) {
+			error -= 434550;
+			set++;
+		} else {
+			error += cal;
+			if (--freq < 0)
+				break;
+		}
+	}
+	if (neg)
+		set = -set;
+	return cal + set;
+}
diff --git a/src/kernel/ao_gps_print.c b/src/kernel/ao_gps_print.c
new file mode 100644
index 00000000..47c945d7
--- /dev/null
+++ b/src/kernel/ao_gps_print.c
@@ -0,0 +1,112 @@
+/*
+ * 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.
+ */
+
+#ifndef AO_GPS_TEST
+#include "ao.h"
+#endif
+#include "ao_telem.h"
+
+void
+ao_gps_print(__xdata struct ao_gps_orig *gps_data) __reentrant
+{
+	char	state;
+
+	if (gps_data->flags & AO_GPS_VALID)
+		state = AO_TELEM_GPS_STATE_LOCKED;
+	else if (gps_data->flags & AO_GPS_RUNNING)
+		state = AO_TELEM_GPS_STATE_UNLOCKED;
+	else
+		state = AO_TELEM_GPS_STATE_ERROR;
+	printf(AO_TELEM_GPS_STATE " %c "
+	       AO_TELEM_GPS_NUM_SAT " %d ",
+	       state,
+	       (gps_data->flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT);
+	if (!(gps_data->flags & AO_GPS_VALID))
+		return;
+	printf(AO_TELEM_GPS_LATITUDE " %ld "
+	       AO_TELEM_GPS_LONGITUDE " %ld "
+	       AO_TELEM_GPS_ALTITUDE " %d ",
+	       (long) gps_data->latitude,
+	       (long) gps_data->longitude,
+	       gps_data->altitude);
+
+	if (gps_data->flags & AO_GPS_DATE_VALID)
+		printf(AO_TELEM_GPS_YEAR " %d "
+		       AO_TELEM_GPS_MONTH " %d "
+		       AO_TELEM_GPS_DAY " %d ",
+		       gps_data->year,
+		       gps_data->month,
+		       gps_data->day);
+
+	printf(AO_TELEM_GPS_HOUR " %d "
+	       AO_TELEM_GPS_MINUTE " %d "
+	       AO_TELEM_GPS_SECOND " %d ",
+	       gps_data->hour,
+	       gps_data->minute,
+	       gps_data->second);
+
+	printf(AO_TELEM_GPS_HDOP " %d ",
+	       gps_data->hdop * 2);
+
+	if (gps_data->flags & AO_GPS_COURSE_VALID) {
+		printf(AO_TELEM_GPS_HERROR " %d "
+		       AO_TELEM_GPS_VERROR " %d "
+		       AO_TELEM_GPS_VERTICAL_SPEED " %d "
+		       AO_TELEM_GPS_HORIZONTAL_SPEED " %d "
+		       AO_TELEM_GPS_COURSE " %d ",
+		       gps_data->h_error,
+		       gps_data->v_error,
+		       gps_data->climb_rate,
+		       gps_data->ground_speed,
+		       (int) gps_data->course * 2);
+	}
+}
+
+void
+ao_gps_tracking_print(__xdata struct ao_gps_tracking_orig *gps_tracking_data) __reentrant
+{
+	uint8_t	c, n, v;
+	__xdata struct ao_gps_sat_orig	*sat;
+
+	n = gps_tracking_data->channels;
+	if (n == 0)
+		return;
+
+	sat = gps_tracking_data->sats;
+	v = 0;
+	for (c = 0; c < n; c++) {
+		if (sat->svid)
+			v++;
+		sat++;
+	}
+
+	printf (AO_TELEM_SAT_NUM " %d ",
+		v);
+
+	sat = gps_tracking_data->sats;
+	v = 0;
+	for (c = 0; c < n; c++) {
+		if (sat->svid) {
+			printf (AO_TELEM_SAT_SVID "%d %d "
+				AO_TELEM_SAT_C_N_0 "%d %d ",
+				v, sat->svid,
+				v, sat->c_n_1);
+			v++;
+		}
+		sat++;
+	}
+}
diff --git a/src/kernel/ao_gps_report.c b/src/kernel/ao_gps_report.c
new file mode 100644
index 00000000..07201ac2
--- /dev/null
+++ b/src/kernel/ao_gps_report.c
@@ -0,0 +1,89 @@
+/*
+ * 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"
+
+void
+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 (;;) {
+		while ((new = ao_gps_new) == 0)
+			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_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_TRACKING) {
+			uint8_t c, n;
+
+			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;
+
+void
+ao_gps_report_init(void)
+{
+	ao_add_task(&ao_gps_report_task, ao_gps_report, "gps_report");
+}
diff --git a/src/kernel/ao_gps_report_mega.c b/src/kernel/ao_gps_report_mega.c
new file mode 100644
index 00000000..5e3c71bf
--- /dev/null
+++ b/src/kernel/ao_gps_report_mega.c
@@ -0,0 +1,90 @@
+/*
+ * 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_mega(void)
+{
+	static __xdata struct ao_log_mega		gps_log;
+	static __xdata struct ao_telemetry_location	gps_data;
+	static __xdata struct ao_telemetry_satellite	gps_tracking_data;
+	uint8_t	new;
+	uint8_t	c, n, i;
+
+	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_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;
+			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++;
+					if (i >= 12)
+						break;
+				}
+			gps_log.u.gps_sat.channels = i;
+			ao_log_mega(&gps_log);
+		}
+	}
+}
+
+__xdata struct ao_task ao_gps_report_mega_task;
+
+void
+ao_gps_report_mega_init(void)
+{
+	ao_add_task(&ao_gps_report_mega_task,
+		    ao_gps_report_mega,
+		    "gps_report");
+}
diff --git a/src/kernel/ao_gps_report_metrum.c b/src/kernel/ao_gps_report_metrum.c
new file mode 100644
index 00000000..696a833b
--- /dev/null
+++ b/src/kernel/ao_gps_report_metrum.c
@@ -0,0 +1,96 @@
+/*
+ * 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;
+	uint8_t svid;
+	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/kernel/ao_gps_show.c b/src/kernel/ao_gps_show.c
new file mode 100644
index 00000000..3a05e35a
--- /dev/null
+++ b/src/kernel/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/kernel/ao_host.h b/src/kernel/ao_host.h
new file mode 100644
index 00000000..6eb752c9
--- /dev/null
+++ b/src/kernel/ao_host.h
@@ -0,0 +1,135 @@
+/*
+ * 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.
+ */
+
+#define _GNU_SOURCE
+
+#include <stddef.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#define AO_ADC_RING	64
+#define ao_adc_ring_next(n)	(((n) + 1) & (AO_ADC_RING - 1))
+#define ao_adc_ring_prev(n)	(((n) - 1) & (AO_ADC_RING - 1))
+
+/*
+ * One set of samples read from the A/D converter
+ */
+struct ao_adc {
+	uint16_t	tick;		/* tick when the sample was read */
+	int16_t		accel;		/* accelerometer */
+	int16_t		pres;		/* pressure sensor */
+	int16_t		temp;		/* temperature sensor */
+	int16_t		v_batt;		/* battery voltage */
+	int16_t		sense_d;	/* drogue continuity sense */
+	int16_t		sense_m;	/* main continuity sense */
+};
+
+#define __pdata
+#define __data
+#define __xdata
+#define __code
+#define __reentrant
+
+#define DATA_TO_XDATA(a)	(a)
+#define PDATA_TO_XDATA(a)	(a)
+#define CODE_TO_XDATA(a)	(a)
+
+enum ao_flight_state {
+	ao_flight_startup = 0,
+	ao_flight_idle = 1,
+	ao_flight_pad = 2,
+	ao_flight_boost = 3,
+	ao_flight_fast = 4,
+	ao_flight_coast = 5,
+	ao_flight_drogue = 6,
+	ao_flight_main = 7,
+	ao_flight_landed = 8,
+	ao_flight_invalid = 9
+};
+
+struct ao_adc ao_adc_ring[AO_ADC_RING];
+uint8_t ao_adc_head;
+
+#define ao_led_on(l)
+#define ao_led_off(l)
+#define ao_timer_set_adc_interval(i)
+#define ao_wakeup(wchan) ao_dump_state(wchan)
+#define ao_cmd_register(c)
+#define ao_usb_disable()
+#define ao_telemetry_set_interval(x)
+#define ao_delay(x)
+
+enum ao_igniter {
+	ao_igniter_drogue = 0,
+	ao_igniter_main = 1
+};
+
+void
+ao_ignite(enum ao_igniter igniter)
+{
+	printf ("ignite %s\n", igniter == ao_igniter_drogue ? "drogue" : "main");
+}
+
+struct ao_task {
+	int dummy;
+};
+
+#define ao_add_task(t,f,n)
+
+#define ao_log_start()
+#define ao_log_stop()
+
+#define AO_MS_TO_TICKS(ms)	((ms) / 10)
+#define AO_SEC_TO_TICKS(s)	((s) * 100)
+
+#define AO_FLIGHT_TEST
+
+struct ao_adc ao_adc_static;
+
+FILE *emulator_in;
+
+void
+ao_dump_state(void *wchan);
+
+void
+ao_sleep(void *wchan);
+
+const char const * const ao_state_names[] = {
+	"startup", "idle", "pad", "boost", "fast",
+	"coast", "drogue", "main", "landed", "invalid"
+};
+
+struct ao_cmds {
+	void		(*func)(void);
+	const char	*help;
+};
+
+
+struct ao_config {
+	uint16_t	main_deploy;
+	int16_t		accel_zero_g;
+};
+
+#define ao_config_get()
+
+struct ao_config ao_config = { 250, 16000 };
+
+#define ao_xmemcpy(d,s,c) memcpy(d,s,c)
+#define ao_xmemset(d,v,c) memset(d,v,c)
+#define ao_xmemcmp(d,s,c) memcmp(d,s,c)
diff --git a/src/kernel/ao_ignite.c b/src/kernel/ao_ignite.c
new file mode 100644
index 00000000..823d003c
--- /dev/null
+++ b/src/kernel/ao_ignite.c
@@ -0,0 +1,241 @@
+/*
+ * 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_data.h>
+#if AO_PYRO_NUM
+#include <ao_pyro.h>
+#endif
+
+#if HAS_IGNITE
+__xdata struct ao_ignition ao_ignition[2];
+
+void
+ao_ignite(enum ao_igniter igniter)
+{
+	ao_arch_block_interrupts();
+	ao_ignition[igniter].request = 1;
+	ao_wakeup(&ao_ignition);
+	ao_arch_release_interrupts();
+}
+
+#ifndef AO_SENSE_DROGUE
+#define AO_SENSE_DROGUE(p)	((p)->adc.sense_d)
+#define AO_SENSE_MAIN(p)	((p)->adc.sense_m)
+#endif
+
+enum ao_igniter_status
+ao_igniter_status(enum ao_igniter igniter)
+{
+	__xdata struct ao_data packet;
+	__pdata int16_t value;
+	__pdata uint8_t request, firing, fired;
+
+	ao_arch_critical(
+		ao_data_get(&packet);
+		request = ao_ignition[igniter].request;
+		fired = ao_ignition[igniter].fired;
+		firing = ao_ignition[igniter].firing;
+		);
+	if (firing || (request && !fired))
+		return ao_igniter_active;
+
+	value = (AO_IGNITER_CLOSED>>1);
+	switch (igniter) {
+	case ao_igniter_drogue:
+		value = AO_SENSE_DROGUE(&packet);
+		break;
+	case ao_igniter_main:
+		value = AO_SENSE_MAIN(&packet);
+		break;
+	}
+	if (value < AO_IGNITER_OPEN)
+		return ao_igniter_open;
+	else if (value > AO_IGNITER_CLOSED)
+		return ao_igniter_ready;
+	else
+		return ao_igniter_unknown;
+}
+
+#ifndef AO_IGNITER_SET_DROGUE
+#define AO_IGNITER_SET_DROGUE(v)	AO_IGNITER_DROGUE = (v)
+#define AO_IGNITER_SET_MAIN(v)		AO_IGNITER_MAIN = (v)
+#endif
+
+#ifndef AO_IGNITER_FIRE_TIME
+#define AO_IGNITER_FIRE_TIME		AO_MS_TO_TICKS(50)
+#endif
+
+#ifndef AO_IGNITER_CHARGE_TIME
+#define AO_IGNITER_CHARGE_TIME		AO_MS_TO_TICKS(2000)
+#endif
+
+void
+ao_igniter_fire(enum ao_igniter igniter)
+{
+	ao_ignition[igniter].firing = 1;
+	switch(ao_config.ignite_mode) {
+	case AO_IGNITE_MODE_DUAL:
+		switch (igniter) {
+		case ao_igniter_drogue:
+			AO_IGNITER_SET_DROGUE(1);
+			ao_delay(AO_IGNITER_FIRE_TIME);
+			AO_IGNITER_SET_DROGUE(0);
+			break;
+		case ao_igniter_main:
+			AO_IGNITER_SET_MAIN(1);
+			ao_delay(AO_IGNITER_FIRE_TIME);
+			AO_IGNITER_SET_MAIN(0);
+			break;
+		}
+		break;
+	case AO_IGNITE_MODE_APOGEE:
+		switch (igniter) {
+		case ao_igniter_drogue:
+			AO_IGNITER_SET_DROGUE(1);
+			ao_delay(AO_IGNITER_FIRE_TIME);
+			AO_IGNITER_SET_DROGUE(0);
+			ao_delay(AO_IGNITER_CHARGE_TIME);
+			AO_IGNITER_SET_MAIN(1);
+			ao_delay(AO_IGNITER_FIRE_TIME);
+			AO_IGNITER_SET_MAIN(0);
+			break;
+		default:
+			break;
+		}
+		break;
+	case AO_IGNITE_MODE_MAIN:
+		switch (igniter) {
+		case ao_igniter_main:
+			AO_IGNITER_SET_DROGUE(1);
+			ao_delay(AO_IGNITER_FIRE_TIME);
+			AO_IGNITER_SET_DROGUE(0);
+			ao_delay(AO_IGNITER_CHARGE_TIME);
+			AO_IGNITER_SET_MAIN(1);
+			ao_delay(AO_IGNITER_FIRE_TIME);
+			AO_IGNITER_SET_MAIN(0);
+			break;
+		default:
+			break;
+		}
+		break;
+	}
+	ao_ignition[igniter].firing = 0;
+}
+
+void
+ao_igniter(void)
+{
+	__xdata enum ao_igniter igniter;
+
+	ao_config_get();
+	for (;;) {
+		ao_sleep(&ao_ignition);
+		for (igniter = ao_igniter_drogue; igniter <= ao_igniter_main; igniter++) {
+			if (ao_ignition[igniter].request && !ao_ignition[igniter].fired) {
+				if (igniter == ao_igniter_drogue && ao_config.apogee_delay)
+					ao_delay(AO_SEC_TO_TICKS(ao_config.apogee_delay));
+
+				ao_igniter_fire(igniter);
+				ao_delay(AO_IGNITER_CHARGE_TIME);
+				ao_ignition[igniter].fired = 1;
+			}
+		}
+	}
+}
+
+#endif
+
+void
+ao_ignite_manual(void)
+{
+	ao_cmd_white();
+	if (!ao_match_word("DoIt"))
+		return;
+	ao_cmd_white();
+#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;
+}
+
+__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,
+	       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[] = {
+	{ ao_ignite_manual,	"i <key> {main|drogue}\0Fire igniter. <key> is doit with D&I" },
+	{ ao_ignite_test,	"t\0Test igniter" },
+	{ 0,	NULL },
+};
+
+#if HAS_IGNITE
+__xdata struct ao_task ao_igniter_task;
+
+void
+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_add_task(&ao_igniter_task, ao_igniter, "igniter");
+#endif
+	ao_cmd_register(&ao_ignite_cmds[0]);
+}
diff --git a/src/kernel/ao_int64.c b/src/kernel/ao_int64.c
new file mode 100644
index 00000000..aa23dbe0
--- /dev/null
+++ b/src/kernel/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/kernel/ao_int64.h b/src/kernel/ao_int64.h
new file mode 100644
index 00000000..b16db58c
--- /dev/null
+++ b/src/kernel/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/kernel/ao_kalman.c b/src/kernel/ao_kalman.c
new file mode 100644
index 00000000..9aea1f14
--- /dev/null
+++ b/src/kernel/ao_kalman.c
@@ -0,0 +1,295 @@
+/*
+ * 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_FLIGHT_TEST
+#include "ao.h"
+#include "ao_flight.h"
+#endif
+
+#include "ao_sample.h"
+#include "ao_kalman.h"
+
+
+static __pdata int32_t		ao_k_height;
+static __pdata int32_t		ao_k_speed;
+static __pdata int32_t		ao_k_accel;
+
+#define AO_K_STEP_100		to_fix16(0.01)
+#define AO_K_STEP_2_2_100	to_fix16(0.00005)
+
+#define AO_K_STEP_10		to_fix16(0.1)
+#define AO_K_STEP_2_2_10	to_fix16(0.005)
+
+#define AO_K_STEP_1		to_fix16(1)
+#define AO_K_STEP_2_2_1		to_fix16(0.5)
+
+__pdata int16_t			ao_height;
+__pdata int16_t			ao_speed;
+__pdata int16_t			ao_accel;
+__xdata int16_t			ao_max_height;
+static __pdata int32_t		ao_avg_height_scaled;
+__xdata int16_t			ao_avg_height;
+
+__pdata int16_t			ao_error_h;
+__pdata int16_t			ao_error_h_sq_avg;
+
+#if HAS_ACCEL
+__pdata int16_t			ao_error_a;
+#endif
+
+static void
+ao_kalman_predict(void)
+{
+#ifdef AO_FLIGHT_TEST
+	if (ao_sample_tick - ao_sample_prev_tick > 50) {
+		ao_k_height += ((int32_t) ao_speed * AO_K_STEP_1 +
+				(int32_t) ao_accel * AO_K_STEP_2_2_1) >> 4;
+		ao_k_speed += (int32_t) ao_accel * AO_K_STEP_1;
+
+		return;
+	}
+	if (ao_sample_tick - ao_sample_prev_tick > 5) {
+		ao_k_height += ((int32_t) ao_speed * AO_K_STEP_10 +
+				(int32_t) ao_accel * AO_K_STEP_2_2_10) >> 4;
+		ao_k_speed += (int32_t) ao_accel * AO_K_STEP_10;
+
+		return;
+	}
+	if (ao_flight_debug) {
+		printf ("predict speed %g + (%g * %g) = %g\n",
+			ao_k_speed / (65536.0 * 16.0), ao_accel / 16.0, AO_K_STEP_100 / 65536.0,
+			(ao_k_speed + (int32_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0));
+	}
+#endif
+	ao_k_height += ((int32_t) ao_speed * AO_K_STEP_100 +
+			(int32_t) ao_accel * AO_K_STEP_2_2_100) >> 4;
+	ao_k_speed += (int32_t) ao_accel * AO_K_STEP_100;
+}
+
+static void
+ao_kalman_err_height(void)
+{
+	int16_t	e;
+	int16_t height_distrust;
+#if HAS_ACCEL
+	int16_t	speed_distrust;
+#endif
+
+	ao_error_h = ao_sample_height - (int16_t) (ao_k_height >> 16);
+
+	e = ao_error_h;
+	if (e < 0)
+		e = -e;
+	if (e > 127)
+		e = 127;
+#if HAS_ACCEL
+	ao_error_h_sq_avg -= ao_error_h_sq_avg >> 2;
+	ao_error_h_sq_avg += (e * e) >> 2;
+#else
+	ao_error_h_sq_avg -= ao_error_h_sq_avg >> 4;
+	ao_error_h_sq_avg += (e * e) >> 4;
+#endif
+
+	if (ao_flight_state >= ao_flight_drogue)
+		return;
+	height_distrust = ao_sample_alt - AO_MAX_BARO_HEIGHT;
+#if HAS_ACCEL
+	/* speed is stored * 16, but we need to ramp between 200 and 328, so
+	 * we want to multiply by 2. The result is a shift by 3.
+	 */
+	speed_distrust = (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) >> (4 - 1);
+	if (speed_distrust <= 0)
+		speed_distrust = 0;
+	else if (speed_distrust > height_distrust)
+		height_distrust = speed_distrust;
+#endif
+	if (height_distrust > 0) {
+#ifdef AO_FLIGHT_TEST
+		int	old_ao_error_h = ao_error_h;
+#endif
+		if (height_distrust > 0x100)
+			height_distrust = 0x100;
+		ao_error_h = (int16_t) (((int32_t) ao_error_h * (0x100 - height_distrust)) >> 8);
+#ifdef AO_FLIGHT_TEST
+		if (ao_flight_debug) {
+			printf("over height %g over speed %g distrust: %g height: error %d -> %d\n",
+			       (double) (ao_sample_alt - AO_MAX_BARO_HEIGHT),
+			       (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) / 16.0,
+			       height_distrust / 256.0,
+			       old_ao_error_h, ao_error_h);
+		}
+#endif
+	}
+}
+
+static void
+ao_kalman_correct_baro(void)
+{
+	ao_kalman_err_height();
+#ifdef AO_FLIGHT_TEST
+	if (ao_sample_tick - ao_sample_prev_tick > 50) {
+		ao_k_height += (int32_t) AO_BARO_K0_1 * ao_error_h;
+		ao_k_speed  += (int32_t) AO_BARO_K1_1 * ao_error_h;
+		ao_k_accel  += (int32_t) AO_BARO_K2_1 * ao_error_h;
+		return;
+	}
+	if (ao_sample_tick - ao_sample_prev_tick > 5) {
+		ao_k_height += (int32_t) AO_BARO_K0_10 * ao_error_h;
+		ao_k_speed  += (int32_t) AO_BARO_K1_10 * ao_error_h;
+		ao_k_accel  += (int32_t) AO_BARO_K2_10 * ao_error_h;
+		return;
+	}
+#endif
+	ao_k_height += (int32_t) AO_BARO_K0_100 * ao_error_h;
+	ao_k_speed  += (int32_t) AO_BARO_K1_100 * ao_error_h;
+	ao_k_accel  += (int32_t) AO_BARO_K2_100 * ao_error_h;
+}
+
+#if HAS_ACCEL
+
+static void
+ao_kalman_err_accel(void)
+{
+	int32_t	accel;
+
+	accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale;
+
+	/* Can't use ao_accel here as it is the pre-prediction value still */
+	ao_error_a = (accel - ao_k_accel) >> 16;
+}
+
+#ifndef FORCE_ACCEL
+static void
+ao_kalman_correct_both(void)
+{
+	ao_kalman_err_height();
+	ao_kalman_err_accel();
+
+#ifdef AO_FLIGHT_TEST
+	if (ao_sample_tick - ao_sample_prev_tick > 50) {
+		if (ao_flight_debug) {
+			printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n",
+				ao_k_speed / (65536.0 * 16.0),
+				(double) ao_error_h, AO_BOTH_K10_1 / 65536.0,
+				(double) ao_error_a, AO_BOTH_K11_1 / 65536.0,
+				(ao_k_speed +
+				 (int32_t) AO_BOTH_K10_1 * ao_error_h +
+				 (int32_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0));
+		}
+		ao_k_height +=
+			(int32_t) AO_BOTH_K00_1 * ao_error_h +
+			(int32_t) AO_BOTH_K01_1 * ao_error_a;
+		ao_k_speed +=
+			(int32_t) AO_BOTH_K10_1 * ao_error_h +
+			(int32_t) AO_BOTH_K11_1 * ao_error_a;
+		ao_k_accel +=
+			(int32_t) AO_BOTH_K20_1 * ao_error_h +
+			(int32_t) AO_BOTH_K21_1 * ao_error_a;
+		return;
+	}
+	if (ao_sample_tick - ao_sample_prev_tick > 5) {
+		if (ao_flight_debug) {
+			printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n",
+				ao_k_speed / (65536.0 * 16.0),
+				(double) ao_error_h, AO_BOTH_K10_10 / 65536.0,
+				(double) ao_error_a, AO_BOTH_K11_10 / 65536.0,
+				(ao_k_speed +
+				 (int32_t) AO_BOTH_K10_10 * ao_error_h +
+				 (int32_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0));
+		}
+		ao_k_height +=
+			(int32_t) AO_BOTH_K00_10 * ao_error_h +
+			(int32_t) AO_BOTH_K01_10 * ao_error_a;
+		ao_k_speed +=
+			(int32_t) AO_BOTH_K10_10 * ao_error_h +
+			(int32_t) AO_BOTH_K11_10 * ao_error_a;
+		ao_k_accel +=
+			(int32_t) AO_BOTH_K20_10 * ao_error_h +
+			(int32_t) AO_BOTH_K21_10 * ao_error_a;
+		return;
+	}
+	if (ao_flight_debug) {
+		printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n",
+			ao_k_speed / (65536.0 * 16.0),
+			(double) ao_error_h, AO_BOTH_K10_100 / 65536.0,
+			(double) ao_error_a, AO_BOTH_K11_100 / 65536.0,
+			(ao_k_speed +
+			 (int32_t) AO_BOTH_K10_100 * ao_error_h +
+			 (int32_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0));
+	}
+#endif
+	ao_k_height +=
+		(int32_t) AO_BOTH_K00_100 * ao_error_h +
+		(int32_t) AO_BOTH_K01_100 * ao_error_a;
+	ao_k_speed +=
+		(int32_t) AO_BOTH_K10_100 * ao_error_h +
+		(int32_t) AO_BOTH_K11_100 * ao_error_a;
+	ao_k_accel +=
+		(int32_t) AO_BOTH_K20_100 * ao_error_h +
+		(int32_t) AO_BOTH_K21_100 * ao_error_a;
+}
+
+#else
+
+static void
+ao_kalman_correct_accel(void)
+{
+	ao_kalman_err_accel();
+
+	if (ao_sample_tick - ao_sample_prev_tick > 5) {
+		ao_k_height +=(int32_t) AO_ACCEL_K0_10 * ao_error_a;
+		ao_k_speed  += (int32_t) AO_ACCEL_K1_10 * ao_error_a;
+		ao_k_accel  += (int32_t) AO_ACCEL_K2_10 * ao_error_a;
+		return;
+	}
+	ao_k_height += (int32_t) AO_ACCEL_K0_100 * ao_error_a;
+	ao_k_speed  += (int32_t) AO_ACCEL_K1_100 * ao_error_a;
+	ao_k_accel  += (int32_t) AO_ACCEL_K2_100 * ao_error_a;
+}
+
+#endif /* else FORCE_ACCEL */
+#endif /* HAS_ACCEL */
+
+void
+ao_kalman(void)
+{
+	ao_kalman_predict();
+#if HAS_ACCEL
+	if (ao_flight_state <= ao_flight_coast) {
+#ifdef FORCE_ACCEL
+		ao_kalman_correct_accel();
+#else
+		ao_kalman_correct_both();
+#endif
+	} else
+#endif
+		ao_kalman_correct_baro();
+	ao_height = from_fix(ao_k_height);
+	ao_speed = from_fix(ao_k_speed);
+	ao_accel = from_fix(ao_k_accel);
+	if (ao_height > ao_max_height)
+		ao_max_height = ao_height;
+	ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_sample_height;
+#ifdef AO_FLIGHT_TEST
+	if (ao_sample_tick - ao_sample_prev_tick > 50)
+		ao_avg_height = (ao_avg_height_scaled + 1) >> 1;
+	else if (ao_sample_tick - ao_sample_prev_tick > 5)
+		ao_avg_height = (ao_avg_height_scaled + 7) >> 4;
+	else 
+#endif
+		ao_avg_height = (ao_avg_height_scaled + 63) >> 7;
+}
diff --git a/src/kernel/ao_lcd.h b/src/kernel/ao_lcd.h
new file mode 100644
index 00000000..f7e1391a
--- /dev/null
+++ b/src/kernel/ao_lcd.h
@@ -0,0 +1,60 @@
+/*
+ * 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_LCD_H_
+#define _AO_LCD_H_
+
+/* ao_lcd.c */
+  
+void
+ao_lcd_putchar(uint8_t d);
+
+void
+ao_lcd_putstring(char *string);
+
+void
+ao_lcd_contrast_set(uint8_t contrast);
+
+void
+ao_lcd_clear(void);
+
+void
+ao_lcd_cursor_on(void);
+
+void
+ao_lcd_cursor_off(void);
+
+#define AO_LCD_ADDR(row,col)	((row << 6) | (col))
+
+void
+ao_lcd_goto(uint8_t addr);
+
+void
+ao_lcd_start(void);
+
+void
+ao_lcd_init(void);
+
+/* ao_lcd_port.c */
+
+void
+ao_lcd_port_put_nibble(uint8_t rs, uint8_t d);
+
+void
+ao_lcd_port_init(void);
+
+#endif /* _AO_LCD_H_ */
diff --git a/src/kernel/ao_led.h b/src/kernel/ao_led.h
new file mode 100644
index 00000000..d9a0914a
--- /dev/null
+++ b/src/kernel/ao_led.h
@@ -0,0 +1,59 @@
+/*
+ * 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_LED_H_
+#define _AO_LED_H_
+
+/*
+ * ao_led.c
+ */
+
+#define AO_LED_NONE	0
+
+#ifndef AO_LED_TYPE
+#define AO_LED_TYPE uint8_t
+#endif
+
+/* Turn on the specified LEDs */
+void
+ao_led_on(AO_LED_TYPE colors);
+
+/* Turn off the specified LEDs */
+void
+ao_led_off(AO_LED_TYPE colors);
+
+/* Set all of the LEDs to the specified state */
+void
+ao_led_set(AO_LED_TYPE colors);
+
+/* Set all LEDs in 'mask' to the specified state */
+void
+ao_led_set_mask(uint8_t colors, uint8_t mask);
+
+/* Toggle the specified LEDs */
+void
+ao_led_toggle(AO_LED_TYPE colors);
+
+/* Turn on the specified LEDs for the indicated interval */
+void
+ao_led_for(AO_LED_TYPE colors, uint16_t ticks) __reentrant;
+
+/* Initialize the LEDs */
+void
+ao_led_init(AO_LED_TYPE enable);
+
+#endif /* _AO_LED_H_ */
diff --git a/src/kernel/ao_list.h b/src/kernel/ao_list.h
new file mode 100644
index 00000000..8a6fa4d9
--- /dev/null
+++ b/src/kernel/ao_list.h
@@ -0,0 +1,213 @@
+/*
+ * 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_LIST_H_
+#define _AO_LIST_H_
+
+#include <stddef.h>
+
+struct ao_list {
+	struct ao_list	*next, *prev;
+};
+
+static inline void
+ao_list_init(struct ao_list *list)
+{
+	list->next = list->prev = list;
+}
+
+static inline void
+__ao_list_add(struct ao_list *list, struct ao_list *prev, struct ao_list *next)
+{
+	next->prev = list;
+	list->next = next;
+	list->prev = prev;
+	prev->next = list;
+}
+
+/**
+ * Insert a new element after the given list head. The new element does not
+ * need to be initialised as empty list.
+ * The list changes from:
+ *      head → some element → ...
+ * to
+ *      head → new element → older element → ...
+ *
+ * Example:
+ * struct foo *newfoo = malloc(...);
+ * ao_list_add(&newfoo->entry, &bar->list_of_foos);
+ *
+ * @param entry The new element to prepend to the list.
+ * @param head The existing list.
+ */
+static inline void
+ao_list_insert(struct ao_list *entry, struct ao_list *head)
+{
+    __ao_list_add(entry, head, head->next);
+}
+
+/**
+ * Append a new element to the end of the list given with this list head.
+ *
+ * The list changes from:
+ *      head → some element → ... → lastelement
+ * to
+ *      head → some element → ... → lastelement → new element
+ *
+ * Example:
+ * struct foo *newfoo = malloc(...);
+ * ao_list_append(&newfoo->entry, &bar->list_of_foos);
+ *
+ * @param entry The new element to prepend to the list.
+ * @param head The existing list.
+ */
+static inline void
+ao_list_append(struct ao_list *entry, struct ao_list *head)
+{
+    __ao_list_add(entry, head->prev, head);
+}
+
+static inline void
+__ao_list_del(struct ao_list *prev, struct ao_list *next)
+{
+    next->prev = prev;
+    prev->next = next;
+}
+
+/**
+ * Remove the element from the list it is in. Using this function will reset
+ * the pointers to/from this element so it is removed from the list. It does
+ * NOT free the element itself or manipulate it otherwise.
+ *
+ * Using ao_list_del on a pure list head (like in the example at the top of
+ * this file) will NOT remove the first element from
+ * the list but rather reset the list as empty list.
+ *
+ * Example:
+ * ao_list_del(&foo->entry);
+ *
+ * @param entry The element to remove.
+ */
+static inline void
+ao_list_del(struct ao_list *entry)
+{
+    __ao_list_del(entry->prev, entry->next);
+    ao_list_init(entry);
+}
+
+/**
+ * Check if the list is empty.
+ *
+ * Example:
+ * ao_list_is_empty(&bar->list_of_foos);
+ *
+ * @return True if the list contains one or more elements or False otherwise.
+ */
+static inline uint8_t
+ao_list_is_empty(struct ao_list *head)
+{
+    return head->next == head;
+}
+
+/**
+ * Returns a pointer to the container of this list element.
+ *
+ * Example:
+ * struct foo* f;
+ * f = container_of(&foo->entry, struct foo, entry);
+ * assert(f == foo);
+ *
+ * @param ptr Pointer to the struct ao_list.
+ * @param type Data type of the list element.
+ * @param member Member name of the struct ao_list field in the list element.
+ * @return A pointer to the data struct containing the list head.
+ */
+#define ao_container_of(ptr, type, member) \
+	((type *)((char *)(ptr) - offsetof(type, member)))
+
+/**
+ * Alias of ao_container_of
+ */
+#define ao_list_entry(ptr, type, member) \
+    ao_container_of(ptr, type, member)
+
+/**
+ * Retrieve the first list entry for the given list pointer.
+ *
+ * Example:
+ * struct foo *first;
+ * first = ao_list_first_entry(&bar->list_of_foos, struct foo, list_of_foos);
+ *
+ * @param ptr The list head
+ * @param type Data type of the list element to retrieve
+ * @param member Member name of the struct ao_list field in the list element.
+ * @return A pointer to the first list element.
+ */
+#define ao_list_first_entry(ptr, type, member) \
+    ao_list_entry((ptr)->next, type, member)
+
+/**
+ * Retrieve the last list entry for the given listpointer.
+ *
+ * Example:
+ * struct foo *first;
+ * first = ao_list_last_entry(&bar->list_of_foos, struct foo, list_of_foos);
+ *
+ * @param ptr The list head
+ * @param type Data type of the list element to retrieve
+ * @param member Member name of the struct ao_list field in the list element.
+ * @return A pointer to the last list element.
+ */
+#define ao_list_last_entry(ptr, type, member) \
+    ao_list_entry((ptr)->prev, type, member)
+
+/**
+ * Loop through the list given by head and set pos to struct in the list.
+ *
+ * Example:
+ * struct foo *iterator;
+ * ao_list_for_each_entry(iterator, &bar->list_of_foos, entry) {
+ *      [modify iterator]
+ * }
+ *
+ * This macro is not safe for node deletion. Use ao_list_for_each_entry_safe
+ * instead.
+ *
+ * @param pos Iterator variable of the type of the list elements.
+ * @param head List head
+ * @param member Member name of the struct ao_list in the list elements.
+ *
+ */
+#define ao_list_for_each_entry(pos, head, type, member)			\
+    for (pos = ao_container_of((head)->next, type, member);		\
+	 &pos->member != (head);					\
+	 pos = ao_container_of(pos->member.next, type, member))
+
+/**
+ * Loop through the list, keeping a backup pointer to the element. This
+ * macro allows for the deletion of a list element while looping through the
+ * list.
+ *
+ * See ao_list_for_each_entry for more details.
+ */
+#define ao_list_for_each_entry_safe(pos, tmp, head, type, member)		\
+	for ((pos = ao_container_of((head)->next, type, member)),		\
+	     (tmp = ao_container_of(pos->member.next, type, member)); 		\
+	     &pos->member != (head);						\
+	     (pos = tmp), (tmp = ao_container_of(pos->member.next, type, member)))
+
+#endif /* _AO_LIST_H_ */
diff --git a/src/kernel/ao_log.c b/src/kernel/ao_log.c
new file mode 100644
index 00000000..20febefe
--- /dev/null
+++ b/src/kernel/ao_log.c
@@ -0,0 +1,291 @@
+/*
+ * 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>
+#include <ao_config.h>
+
+__pdata uint32_t ao_log_current_pos;
+__pdata uint32_t ao_log_end_pos;
+__pdata uint32_t ao_log_start_pos;
+__xdata uint8_t	ao_log_running;
+__pdata enum ao_flight_state ao_log_state;
+__xdata uint16_t ao_flight_number;
+
+void
+ao_log_flush(void)
+{
+	ao_storage_flush();
+}
+
+/*
+ * When erasing a flight log, make sure the config block
+ * has an up-to-date version of the current flight number
+ */
+
+struct ao_log_erase {
+	uint8_t	unused;
+	uint16_t flight;
+};
+
+static __xdata struct ao_log_erase erase;
+
+#define LOG_MAX_ERASE	16
+
+static uint32_t
+ao_log_erase_pos(uint8_t i)
+{
+	return i * sizeof (struct ao_log_erase) + AO_CONFIG_MAX_SIZE;
+}
+
+void
+ao_log_write_erase(uint8_t pos)
+{
+	erase.unused = 0x00;
+	erase.flight = ao_flight_number;
+	ao_config_write(ao_log_erase_pos(pos),  &erase, sizeof (erase));
+	ao_config_flush();
+}
+
+static void
+ao_log_read_erase(uint8_t pos)
+{
+	ao_config_read(ao_log_erase_pos(pos), &erase, sizeof (erase));
+}
+
+
+static void
+ao_log_erase_mark(void)
+{
+	uint8_t				i;
+
+	for (i = 0; i < LOG_MAX_ERASE; i++) {
+		ao_log_read_erase(i);
+		if (erase.unused == 0 && erase.flight == ao_flight_number)
+			return;
+		if (erase.unused == 0xff) {
+			ao_log_write_erase(i);
+			return;
+		}
+	}
+	ao_config_put();
+}
+
+static uint8_t
+ao_log_slots()
+{
+	return (uint8_t) (ao_storage_log_max / ao_config.flight_log_max);
+}
+
+uint32_t
+ao_log_pos(uint8_t slot)
+{
+	return ((slot) * ao_config.flight_log_max);
+}
+
+static uint16_t
+ao_log_max_flight(void)
+{
+	uint8_t		log_slot;
+	uint8_t		log_slots;
+	uint16_t	log_flight;
+	uint16_t	max_flight = 0;
+
+	/* Scan the log space looking for the biggest flight number */
+	log_slots = ao_log_slots();
+	for (log_slot = 0; log_slot < log_slots; log_slot++) {
+		log_flight = ao_log_flight(log_slot);
+		if (!log_flight)
+			continue;
+		if (max_flight == 0 || (int16_t) (log_flight - max_flight) > 0)
+			max_flight = log_flight;
+	}
+	return max_flight;
+}
+
+void
+ao_log_scan(void) __reentrant
+{
+	uint8_t		log_slot;
+	uint8_t		log_slots;
+	uint8_t		log_want;
+
+	ao_config_get();
+
+	ao_flight_number = ao_log_max_flight();
+	if (ao_flight_number)
+		if (++ao_flight_number == 0)
+			ao_flight_number = 1;
+
+	/* Now look through the log of flight numbers from erase operations and
+	 * see if the last one is bigger than what we found above
+	 */
+	for (log_slot = LOG_MAX_ERASE; log_slot-- > 0;) {
+		ao_log_read_erase(log_slot);
+		if (erase.unused == 0) {
+			if (ao_flight_number == 0 ||
+			    (int16_t) (erase.flight - ao_flight_number) > 0)
+				ao_flight_number = erase.flight;
+			break;
+		}
+	}
+	if (ao_flight_number == 0)
+		ao_flight_number = 1;
+
+	/* With a flight number in hand, find a place to write a new log,
+	 * use the target flight number to index the available log slots so
+	 * that we write logs to each spot about the same number of times.
+	 */
+
+	/* Find a log slot for the next flight, if available */
+	ao_log_current_pos = ao_log_end_pos = 0;
+	log_slots = ao_log_slots();
+	log_want = (ao_flight_number - 1) % log_slots;
+	log_slot = log_want;
+	do {
+		if (ao_log_flight(log_slot) == 0) {
+			ao_log_current_pos = ao_log_pos(log_slot);
+			ao_log_end_pos = ao_log_current_pos + ao_config.flight_log_max;
+			break;
+		}
+		if (++log_slot >= log_slots)
+			log_slot = 0;
+	} while (log_slot != log_want);
+
+	ao_wakeup(&ao_flight_number);
+}
+
+void
+ao_log_start(void)
+{
+	/* start logging */
+	ao_log_running = 1;
+	ao_wakeup(&ao_log_running);
+}
+
+void
+ao_log_stop(void)
+{
+	ao_log_running = 0;
+	ao_log_flush();
+}
+
+uint8_t
+ao_log_present(void)
+{
+	return ao_log_max_flight() != 0;
+}
+
+uint8_t
+ao_log_full(void)
+{
+	return ao_log_current_pos == ao_log_end_pos;
+}
+
+#if HAS_ADC
+static __xdata struct ao_task ao_log_task;
+#endif
+
+void
+ao_log_list(void) __reentrant
+{
+	uint8_t	slot;
+	uint8_t slots;
+	uint16_t flight;
+
+	slots = ao_log_slots();
+	for (slot = 0; slot < slots; slot++)
+	{
+		flight = ao_log_flight(slot);
+		if (flight)
+			printf ("flight %d start %x end %x\n",
+				flight,
+				(uint16_t) (ao_log_pos(slot) >> 8),
+				(uint16_t) (ao_log_pos(slot+1) >> 8));
+	}
+	printf ("done\n");
+}
+
+void
+ao_log_delete(void) __reentrant
+{
+	uint8_t slot;
+	uint8_t slots;
+
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+
+	slots = ao_log_slots();
+	/* Look for the flight log matching the requested flight */
+	if (ao_cmd_lex_i) {
+		for (slot = 0; slot < slots; slot++) {
+			if (ao_log_flight(slot) == ao_cmd_lex_i) {
+				ao_log_erase_mark();
+				ao_log_current_pos = ao_log_pos(slot);
+				ao_log_end_pos = ao_log_current_pos + ao_config.flight_log_max;
+				while (ao_log_current_pos < ao_log_end_pos) {
+					uint8_t	i;
+					static __xdata uint8_t b;
+
+					/*
+					 * Check to see if we've reached the end of
+					 * the used memory to avoid re-erasing the same
+					 * memory over and over again
+					 */
+					for (i = 0; i < 16; i++) {
+						if (ao_storage_read(ao_log_current_pos + i, &b, 1))
+							if (b != 0xff)
+								break;
+					}
+					if (i == 16)
+						break;
+					ao_storage_erase(ao_log_current_pos);
+					ao_log_current_pos += ao_storage_block;
+				}
+				puts("Erased");
+				return;
+			}
+		}
+	}
+	printf("No such flight: %d\n", ao_cmd_lex_i);
+}
+
+__code struct ao_cmds ao_log_cmds[] = {
+	{ ao_log_list,	"l\0List logs" },
+	{ ao_log_delete,	"d <flight-number>\0Delete flight" },
+	{ 0,	NULL },
+};
+
+void
+ao_log_init(void)
+{
+	ao_log_running = 0;
+
+	/* For now, just log the flight starting at the begining of eeprom */
+	ao_log_state = ao_flight_invalid;
+
+	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/kernel/ao_log.h b/src/kernel/ao_log.h
new file mode 100644
index 00000000..09f31188
--- /dev/null
+++ b/src/kernel/ao_log.h
@@ -0,0 +1,386 @@
+/*
+ * 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_LOG_H_
+#define _AO_LOG_H_
+
+#include <ao_flight.h>
+
+/*
+ * ao_log.c
+ */
+
+/* We record flight numbers in the first record of
+ * the log. Tasks may wait for this to be initialized
+ * by sleeping on this variable.
+ */
+extern __xdata uint16_t ao_flight_number;
+
+extern __pdata uint32_t ao_log_current_pos;
+extern __pdata uint32_t ao_log_end_pos;
+extern __pdata uint32_t ao_log_start_pos;
+extern __xdata uint8_t	ao_log_running;
+extern __pdata enum ao_flight_state ao_log_state;
+
+/* required functions from the underlying log system */
+
+#define AO_LOG_FORMAT_UNKNOWN		0	/* unknown; altosui will have to guess */
+#define AO_LOG_FORMAT_FULL		1	/* 8 byte typed log records */
+#define AO_LOG_FORMAT_TINY		2	/* two byte state/baro records */
+#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;
+
+/* Return the flight number from the given log slot, 0 if none */
+uint16_t
+ao_log_flight(uint8_t slot);
+
+/* Flush the log */
+void
+ao_log_flush(void);
+
+/* Logging thread main routine */
+void
+ao_log(void);
+
+/* functions provided in ao_log.c */
+
+/* Figure out the current flight number */
+void
+ao_log_scan(void) __reentrant;
+
+/* Return the position of the start of the given log slot */
+uint32_t
+ao_log_pos(uint8_t slot);
+
+/* Start logging to eeprom */
+void
+ao_log_start(void);
+
+/* Stop logging */
+void
+ao_log_stop(void);
+
+/* Initialize the logging system */
+void
+ao_log_init(void);
+
+/* Write out the current flight number to the erase log */
+void
+ao_log_write_erase(uint8_t pos);
+
+/* Returns true if there are any logs stored in eeprom */
+uint8_t
+ao_log_present(void);
+
+/* Returns true if there is no more storage space available */
+uint8_t
+ao_log_full(void);
+
+/*
+ * ao_log_big.c
+ */
+
+/*
+ * The data log is recorded in the eeprom as a sequence
+ * of data packets.
+ *
+ * Each packet starts with a 4-byte header that has the
+ * packet type, the packet checksum and the tick count. Then
+ * they all contain 2 16 bit values which hold packet-specific
+ * data.
+ *
+ * For each flight, the first packet
+ * is FLIGHT packet, indicating the serial number of the
+ * device and a unique number marking the number of flights
+ * recorded by this device.
+ *
+ * During flight, data from the accelerometer and barometer
+ * are recorded in SENSOR packets, using the raw 16-bit values
+ * read from the A/D converter.
+ *
+ * Also during flight, but at a lower rate, the deployment
+ * sensors are recorded in DEPLOY packets. The goal here is to
+ * detect failure in the deployment circuits.
+ *
+ * STATE packets hold state transitions as the flight computer
+ * transitions through different stages of the flight.
+ */
+#define AO_LOG_FLIGHT		'F'
+#define AO_LOG_SENSOR		'A'
+#define AO_LOG_TEMP_VOLT	'T'
+#define AO_LOG_DEPLOY		'D'
+#define AO_LOG_STATE		'S'
+#define AO_LOG_GPS_TIME		'G'
+#define AO_LOG_GPS_LAT		'N'
+#define AO_LOG_GPS_LON		'W'
+#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)
+
+struct ao_log_record {
+	char			type;				/* 0 */
+	uint8_t			csum;				/* 1 */
+	uint16_t		tick;				/* 2 */
+	union {
+		struct {
+			int16_t		ground_accel;		/* 4 */
+			uint16_t	flight;			/* 6 */
+		} flight;
+		struct {
+			int16_t		accel;			/* 4 */
+			int16_t		pres;			/* 6 */
+		} sensor;
+		struct {
+			int16_t		temp;
+			int16_t		v_batt;
+		} temp_volt;
+		struct {
+			int16_t		drogue;
+			int16_t		main;
+		} deploy;
+		struct {
+			uint16_t	state;
+			uint16_t	reason;
+		} state;
+		struct {
+			uint8_t		hour;
+			uint8_t		minute;
+			uint8_t		second;
+			uint8_t		flags;
+		} gps_time;
+		int32_t		gps_latitude;
+		int32_t		gps_longitude;
+		struct {
+			int16_t		altitude;
+			uint16_t	unused;
+		} gps_altitude;
+		struct {
+			uint16_t	svid;
+			uint8_t		unused;
+			uint8_t		c_n;
+		} gps_sat;
+		struct {
+			uint8_t		year;
+			uint8_t		month;
+			uint8_t		day;
+			uint8_t		extra;
+		} gps_date;
+		struct {
+			uint16_t	d0;
+			uint16_t	d1;
+		} anon;
+	} u;
+};
+
+struct ao_log_mega {
+	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 */
+			int16_t		ground_accel_along;	/* 16 */
+			int16_t		ground_accel_across;	/* 12 */
+			int16_t		ground_accel_through;	/* 14 */
+			int16_t		ground_roll;		/* 18 */
+			int16_t		ground_pitch;		/* 20 */
+			int16_t		ground_yaw;		/* 22 */
+		} flight;					/* 24 */
+		/* AO_LOG_STATE */
+		struct {
+			uint16_t	state;
+			uint16_t	reason;
+		} state;
+		/* AO_LOG_SENSOR */
+		struct {
+			uint32_t	pres;		/* 4 */
+			uint32_t	temp;		/* 8 */
+			int16_t		accel_x;	/* 12 */
+			int16_t		accel_y;	/* 14 */
+			int16_t		accel_z;	/* 16 */
+			int16_t		gyro_x;		/* 18 */
+			int16_t		gyro_y;		/* 20 */
+			int16_t		gyro_z;		/* 22 */
+			int16_t		mag_x;		/* 24 */
+			int16_t		mag_y;		/* 26 */
+			int16_t		mag_z;		/* 28 */
+			int16_t		accel;		/* 30 */
+		} sensor;	/* 32 */
+		/* AO_LOG_TEMP_VOLT */
+		struct {
+			int16_t		v_batt;		/* 4 */
+			int16_t		v_pbatt;	/* 6 */
+			int16_t		n_sense;	/* 8 */
+			int16_t		sense[10];	/* 10 */
+			uint16_t	pyro;		/* 30 */
+		} volt;					/* 32 */
+		/* AO_LOG_GPS_TIME */
+		struct {
+			int32_t		latitude;	/* 4 */
+			int32_t		longitude;	/* 8 */
+			int16_t		altitude;	/* 12 */
+			uint8_t		hour;		/* 14 */
+			uint8_t		minute;		/* 15 */
+			uint8_t		second;		/* 16 */
+			uint8_t		flags;		/* 17 */
+			uint8_t		year;		/* 18 */
+			uint8_t		month;		/* 19 */
+			uint8_t		day;		/* 20 */
+			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 */
+			struct {
+				uint8_t	svid;
+				uint8_t c_n;
+			} sats[12];			/* 6 */
+		} gps_sat;				/* 30 */
+	} 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;
+
+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/kernel/ao_log_big.c b/src/kernel/ao_log_big.c
new file mode 100644
index 00000000..db01f46c
--- /dev/null
+++ b/src/kernel/ao_log_big.c
@@ -0,0 +1,162 @@
+/*
+ * 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"
+
+static __xdata uint8_t	ao_log_mutex;
+static __xdata struct ao_log_record log;
+
+__code uint8_t ao_log_format = AO_LOG_FORMAT_FULL;
+
+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_record); i++)
+		sum += *b++;
+	return -sum;
+}
+
+uint8_t
+ao_log_data(__xdata struct ao_log_record *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_record));
+			ao_log_current_pos += sizeof (struct ao_log_record);
+		}
+	} 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_records fill the eeprom block in even units */
+typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_record))] ;
+
+#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;
+
+	ao_storage_setup();
+
+	ao_log_scan();
+
+	while (!ao_log_running)
+		ao_sleep(&ao_log_running);
+
+	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.flight = ao_flight_number;
+	ao_log_data(&log);
+
+	/* Write the whole contents of the ring to the log
+	 * when starting up.
+	 */
+	ao_log_data_pos = ao_data_ring_next(ao_sample_data);
+	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_sample_data) {
+			log.tick = ao_data_ring[ao_log_data_pos].tick;
+			if ((int16_t) (log.tick - next_sensor) >= 0) {
+				log.type = AO_LOG_SENSOR;
+				log.u.sensor.accel = ao_data_ring[ao_log_data_pos].adc.accel;
+				log.u.sensor.pres = ao_data_ring[ao_log_data_pos].adc.pres;
+				ao_log_data(&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.temp_volt.temp = ao_data_ring[ao_log_data_pos].adc.temp;
+				log.u.temp_volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt;
+				ao_log_data(&log);
+				log.type = AO_LOG_DEPLOY;
+				log.u.deploy.drogue = ao_data_ring[ao_log_data_pos].adc.sense_d;
+				log.u.deploy.main = ao_data_ring[ao_log_data_pos].adc.sense_m;
+				ao_log_data(&log);
+				next_other = log.tick + AO_OTHER_INTERVAL;
+			}
+			ao_log_data_pos = ao_data_ring_next(ao_log_data_pos);
+		}
+		/* 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_sample_tick;
+			log.u.state.state = ao_log_state;
+			log.u.state.reason = 0;
+			ao_log_data(&log);
+
+			if (ao_log_state == ao_flight_landed)
+				ao_log_stop();
+		}
+
+		/* 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_record)))
+		return 0;
+
+	if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT)
+		return log.u.flight.flight;
+	return 0;
+}
diff --git a/src/kernel/ao_log_mega.c b/src/kernel/ao_log_mega.c
new file mode 100644
index 00000000..768947d5
--- /dev/null
+++ b/src/kernel/ao_log_mega.c
@@ -0,0 +1,199 @@
+/*
+ * 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_mega log;
+
+__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMEGA;
+
+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_mega); i++)
+		sum += *b++;
+	return -sum;
+}
+
+uint8_t
+ao_log_mega(__xdata struct ao_log_mega *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_mega));
+			ao_log_current_pos += sizeof (struct ao_log_mega);
+		}
+	} 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_megas fill the eeprom block in even units */
+typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_mega))] ;
+
+#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
+#if HAS_GYRO
+	log.u.flight.ground_accel_along = ao_ground_accel_along;
+	log.u.flight.ground_accel_across = ao_ground_accel_across;
+	log.u.flight.ground_accel_through = ao_ground_accel_through;
+	log.u.flight.ground_pitch = ao_ground_pitch;
+	log.u.flight.ground_yaw = ao_ground_yaw;
+	log.u.flight.ground_roll = ao_ground_roll;
+#endif
+	log.u.flight.ground_pres = ao_ground_pres;
+	log.u.flight.flight = ao_flight_number;
+	ao_log_mega(&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
+#if HAS_MPU6000
+				log.u.sensor.accel_x = ao_data_ring[ao_log_data_pos].mpu6000.accel_x;
+				log.u.sensor.accel_y = ao_data_ring[ao_log_data_pos].mpu6000.accel_y;
+				log.u.sensor.accel_z = ao_data_ring[ao_log_data_pos].mpu6000.accel_z;
+				log.u.sensor.gyro_x = ao_data_ring[ao_log_data_pos].mpu6000.gyro_x;
+				log.u.sensor.gyro_y = ao_data_ring[ao_log_data_pos].mpu6000.gyro_y;
+				log.u.sensor.gyro_z = ao_data_ring[ao_log_data_pos].mpu6000.gyro_z;
+#endif
+#if HAS_HMC5883
+				log.u.sensor.mag_x = ao_data_ring[ao_log_data_pos].hmc5883.x;
+				log.u.sensor.mag_y = ao_data_ring[ao_log_data_pos].hmc5883.y;
+				log.u.sensor.mag_z = ao_data_ring[ao_log_data_pos].hmc5883.z;
+#endif
+				log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]);
+				ao_log_mega(&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.v_pbatt = ao_data_ring[ao_log_data_pos].adc.v_pbatt;
+				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;
+			}
+			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_mega(&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_mega)))
+		return 0;
+
+	if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT)
+		return log.u.flight.flight;
+	return 0;
+}
diff --git a/src/kernel/ao_log_metrum.c b/src/kernel/ao_log_metrum.c
new file mode 100644
index 00000000..9b17adc2
--- /dev/null
+++ b/src/kernel/ao_log_metrum.c
@@ -0,0 +1,176 @@
+/*
+ * 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;
+
+	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
+#if HAS_ACCEL
+				log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]);
+#endif
+				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/kernel/ao_log_micro.c b/src/kernel/ao_log_micro.c
new file mode 100644
index 00000000..d665efb5
--- /dev/null
+++ b/src/kernel/ao_log_micro.c
@@ -0,0 +1,121 @@
+/*
+ * 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_micropeak.h>
+#include <ao_log_micro.h>
+#include <ao_async.h>
+
+static uint16_t ao_log_offset = STARTING_LOG_OFFSET;
+
+void
+ao_log_micro_save(void)
+{
+	uint16_t	n_samples = (ao_log_offset - STARTING_LOG_OFFSET) / sizeof (uint16_t);
+	ao_eeprom_write(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground));
+	ao_eeprom_write(PA_MIN_OFFSET, &pa_min, sizeof (pa_min));
+	ao_eeprom_write(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples));
+}
+
+void
+ao_log_micro_restore(void)
+{
+	ao_eeprom_read(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground));
+	ao_eeprom_read(PA_MIN_OFFSET, &pa_min, sizeof (pa_min));
+}
+
+void
+ao_log_micro_data(void)
+{
+	uint16_t	low_bits = pa;
+
+	if (ao_log_offset < MAX_LOG_OFFSET) {
+		ao_eeprom_write(ao_log_offset, &low_bits, sizeof (low_bits));
+		ao_log_offset += sizeof (low_bits);
+	}
+}
+
+#define POLY 0x8408
+
+static uint16_t
+ao_log_micro_crc(uint16_t crc, uint8_t byte)
+{
+	uint8_t	i;
+
+	for (i = 0; i < 8; i++) {
+		if ((crc & 0x0001) ^ (byte & 0x0001))
+			crc = (crc >> 1) ^ POLY;
+		else
+			crc = crc >> 1;
+		byte >>= 1;
+	}
+	return crc;
+}
+
+static void
+ao_log_hex_nibble(uint8_t b)
+{
+	if (b < 10)
+		ao_async_byte('0' + b);
+	else
+		ao_async_byte('a' - 10 + b);
+}
+
+static void
+ao_log_hex(uint8_t b)
+{
+	ao_log_hex_nibble(b>>4);
+	ao_log_hex_nibble(b&0xf);
+}
+
+static void
+ao_log_newline(void)
+{
+	ao_async_byte('\r');
+	ao_async_byte('\n');
+}
+
+void
+ao_log_micro_dump(void)
+{
+	uint16_t	n_samples;
+	uint16_t	nbytes;
+	uint8_t		byte;
+	uint16_t	b;
+	uint16_t	crc = 0xffff;
+
+	ao_eeprom_read(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples));
+	if (n_samples == 0xffff)
+		n_samples = 0;
+	nbytes = STARTING_LOG_OFFSET + sizeof (uint16_t) * n_samples;
+	ao_async_start();
+	ao_async_byte('M');
+	ao_async_byte('P');
+	for (b = 0; b < nbytes; b++) {
+		if ((b & 0xf) == 0)
+			ao_log_newline();
+		ao_eeprom_read(b, &byte, 1);
+		ao_log_hex(byte);
+		crc = ao_log_micro_crc(crc, byte);
+	}
+	ao_log_newline();
+	crc = ~crc;
+	ao_log_hex(crc >> 8);
+	ao_log_hex(crc);
+	ao_log_newline();
+	ao_async_stop();
+}
diff --git a/src/kernel/ao_log_micro.h b/src/kernel/ao_log_micro.h
new file mode 100644
index 00000000..976852ee
--- /dev/null
+++ b/src/kernel/ao_log_micro.h
@@ -0,0 +1,39 @@
+/*
+ * 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_LOG_MICRO_H_
+#define _AO_LOG_MICRO_H_
+
+#define PA_GROUND_OFFSET	0
+#define PA_MIN_OFFSET		4
+#define N_SAMPLES_OFFSET	8
+#define STARTING_LOG_OFFSET	10
+#define MAX_LOG_OFFSET		512
+
+void
+ao_log_micro_save(void);
+
+void
+ao_log_micro_restore(void);
+
+void
+ao_log_micro_data(void);
+
+void
+ao_log_micro_dump(void);
+
+#endif /* _AO_LOG_MICRO_H_ */
diff --git a/src/kernel/ao_log_mini.c b/src/kernel/ao_log_mini.c
new file mode 100644
index 00000000..29e3bd9f
--- /dev/null
+++ b/src/kernel/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;
+
+	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_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/kernel/ao_log_single.c b/src/kernel/ao_log_single.c
new file mode 100644
index 00000000..3f6235a6
--- /dev/null
+++ b/src/kernel/ao_log_single.c
@@ -0,0 +1,198 @@
+/*
+ * 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.
+ */
+
+/*
+ * ao_log_single.c
+ *
+ * Stores a sequence of fixed-size (32 byte) chunks
+ * without splitting memory up into separate flights
+ */
+
+#include "ao.h"
+#include "ao_product.h"
+
+static __xdata struct ao_task ao_log_single_task;
+
+__xdata uint8_t 	ao_log_running;
+__xdata uint8_t		ao_log_mutex;
+__pdata uint32_t	ao_log_start_pos;
+__pdata uint32_t	ao_log_end_pos;
+__pdata uint32_t	ao_log_current_pos;
+
+__xdata union ao_log_single ao_log_single_write_data;
+__xdata union ao_log_single ao_log_single_read_data;
+
+uint8_t
+ao_log_single_write(void)
+{
+	uint8_t wrote = 0;
+
+	ao_mutex_get(&ao_log_mutex); {
+		if (ao_log_current_pos >= ao_log_end_pos && ao_log_running)
+			ao_log_single_stop();
+		if (ao_log_running) {
+			wrote = 1;
+			ao_storage_write(ao_log_current_pos,
+					 &ao_log_single_write_data,
+					 AO_LOG_SINGLE_SIZE);
+			ao_log_current_pos += AO_LOG_SINGLE_SIZE;
+		}
+	} ao_mutex_put(&ao_log_mutex);
+	return wrote;
+}
+
+static uint8_t
+ao_log_single_valid(void)
+{
+	__xdata uint8_t	*d = ao_log_single_read_data.bytes;
+	uint8_t	i;
+	for (i = 0; i < AO_LOG_SINGLE_SIZE; i++)
+		if (*d++ != 0xff)
+			return 1;
+	return 0;
+}
+
+uint8_t
+ao_log_single_read(uint32_t pos)
+{
+	if (!ao_storage_read(pos, &ao_log_single_read_data, AO_LOG_SINGLE_SIZE))
+		return 0;
+	return ao_log_single_valid();
+}
+
+void
+ao_log_single_start(void)
+{
+	if (!ao_log_running) {
+		ao_log_running = 1;
+		ao_wakeup(&ao_log_running);
+	}
+}
+
+void
+ao_log_single_stop(void)
+{
+	if (ao_log_running) {
+		ao_log_running = 0;
+	}
+}
+
+void
+ao_log_single_restart(void)
+{
+	/* Find end of data */
+	ao_log_end_pos = ao_storage_config;
+	for (ao_log_current_pos = 0;
+	     ao_log_current_pos < ao_storage_config;
+	     ao_log_current_pos += ao_storage_block)
+	{
+		if (!ao_log_single_read(ao_log_current_pos))
+			break;
+	}
+	if (ao_log_current_pos > 0) {
+		ao_log_current_pos -= ao_storage_block;
+		for (; ao_log_current_pos < ao_storage_config;
+		     ao_log_current_pos += sizeof (struct ao_log_telescience))
+		{
+			if (!ao_log_single_read(ao_log_current_pos))
+				break;
+		}
+	}
+}
+
+void
+ao_log_single_set(void)
+{
+	printf("Logging currently %s\n", ao_log_running ? "on" : "off");
+	ao_cmd_hex();
+	if (ao_cmd_status == ao_cmd_success) {
+		if (ao_cmd_lex_i) {
+			printf("Logging from %ld to %ld\n", ao_log_current_pos, ao_log_end_pos);
+			ao_log_single_start();
+		} else {
+			printf ("Log stopped at %ld\n", ao_log_current_pos);
+			ao_log_single_stop();
+		}
+	}
+	ao_cmd_status = ao_cmd_success;
+}
+
+void
+ao_log_single_delete(void)
+{
+	uint32_t	pos;
+
+	ao_cmd_hex();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	if (ao_cmd_lex_i != 1) {
+		ao_cmd_status = ao_cmd_syntax_error;
+		printf("No such flight: %d\n", ao_cmd_lex_i);
+		return;
+	}
+	ao_log_single_stop();
+	for (pos = 0; pos < ao_storage_config; pos += ao_storage_block) {
+		if (!ao_log_single_read(pos))
+			break;
+		ao_storage_erase(pos);
+	}
+	ao_log_current_pos = ao_log_start_pos = 0;
+	if (pos == 0)
+		printf("No such flight: %d\n", ao_cmd_lex_i);
+	else
+		printf ("Erased\n");
+}
+
+uint8_t
+ao_log_full(void)
+{
+	return ao_log_current_pos >= ao_log_end_pos;
+}
+
+uint8_t
+ao_log_present(void)
+{
+	return ao_log_single_read(0);
+}
+
+static void
+ao_log_single_query(void)
+{
+	printf("Logging enabled: %d\n", ao_log_running);
+	printf("Log start: %ld\n", ao_log_start_pos);
+	printf("Log cur: %ld\n", ao_log_current_pos);
+	printf("Log end: %ld\n", ao_log_end_pos);
+	ao_log_single_extra_query();
+}
+
+const struct ao_cmds ao_log_single_cmds[] = {
+	{ ao_log_single_set,	"L <0 off, 1 on>\0Set logging" },
+	{ ao_log_single_list,	"l\0List stored logs" },
+	{ ao_log_single_delete, "d 1\0Delete all stored logs" },
+	{ ao_log_single_query, "q\0Query log status" },
+	{ 0,	NULL },
+};
+
+void
+ao_log_single_init(void)
+{
+	ao_log_running = 0;
+
+	ao_cmd_register(&ao_log_single_cmds[0]);
+
+	ao_add_task(&ao_log_single_task, ao_log_single, "log");
+}
diff --git a/src/kernel/ao_log_telem.c b/src/kernel/ao_log_telem.c
new file mode 100644
index 00000000..095aca37
--- /dev/null
+++ b/src/kernel/ao_log_telem.c
@@ -0,0 +1,131 @@
+/*
+ * 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_flight.h>
+#include <ao_sample.h>
+
+__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;
+__xdata int16_t				ao_max_height;	/* max of ao_height */
+__pdata int16_t				sense_d, sense_m;
+__pdata uint8_t				ao_igniter_present;
+
+static void
+ao_log_telem_track() {
+	if (ao_monitoring == sizeof (union ao_telemetry_all)) {
+		switch (ao_log_single_write_data.telemetry.generic.type) {
+		case AO_TELEMETRY_SENSOR_TELEMETRUM:
+		case AO_TELEMETRY_SENSOR_TELEMINI:
+			/* fall through ... */
+		case AO_TELEMETRY_SENSOR_TELENANO:
+			if (ao_log_single_write_data.telemetry.generic.type == AO_TELEMETRY_SENSOR_TELENANO) {
+				ao_igniter_present = 0;
+			} else {
+				sense_d = ao_log_single_write_data.telemetry.sensor.sense_d;
+				sense_m = ao_log_single_write_data.telemetry.sensor.sense_m;
+				ao_igniter_present = 1;
+			}
+			if (ao_log_single_write_data.telemetry.sensor.height > ao_max_height) {
+				ao_max_height = ao_log_single_write_data.telemetry.sensor.height;
+			}
+			if (ao_log_single_write_data.telemetry.sensor.state != ao_flight_state) {
+				ao_flight_state = ao_log_single_write_data.telemetry.sensor.state;
+				if (ao_flight_state == ao_flight_pad)
+					ao_max_height = 0;
+				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
+			}
+		}
+	}
+}
+
+enum ao_igniter_status
+ao_igniter_status(enum ao_igniter igniter)
+{
+	int16_t	value;
+
+	switch (igniter) {
+	case ao_igniter_drogue:
+		value = sense_d;
+		break;
+	case ao_igniter_main:
+		value = sense_m;
+		break;
+	default:
+		value = 0;
+		break;
+	}
+	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_log_single(void)
+{
+	ao_storage_setup();
+
+	/* This can take a while, so let the rest
+	 * of the system finish booting before we start
+	 */
+	ao_delay(AO_SEC_TO_TICKS(2));
+
+	ao_log_running = 1;
+	ao_log_single_restart();
+	ao_flight_state = ao_flight_startup;
+	ao_monitor_set(sizeof(struct ao_telemetry_generic));
+
+	for (;;) {
+		while (!ao_log_running)
+			ao_sleep(&ao_log_running);
+
+		ao_log_monitor_pos = ao_monitor_head;
+		while (ao_log_running) {
+			/* Write samples to EEPROM */
+			while (ao_log_monitor_pos != ao_monitor_head) {
+				ao_xmemcpy(&ao_log_single_write_data.telemetry,
+					   &ao_monitor_ring[ao_log_monitor_pos],
+					   AO_LOG_SINGLE_SIZE);
+				ao_log_single_write();
+				ao_log_monitor_pos = ao_monitor_ring_next(ao_log_monitor_pos);
+				ao_log_telem_track();
+			}
+			/* Wait for more telemetry data to arrive */
+			ao_sleep(DATA_TO_XDATA(&ao_monitor_head));
+		}
+	}
+}
+
+void
+ao_log_single_list(void)
+{
+	if (ao_log_current_pos != 0)
+		printf("flight 1 start %x end %x\n",
+		       0,
+		       (uint16_t) ((ao_log_current_pos + 0xff) >> 8));
+	printf ("done\n");
+}
+
+void
+ao_log_single_extra_query(void)
+{
+}
diff --git a/src/kernel/ao_log_telescience.c b/src/kernel/ao_log_telescience.c
new file mode 100644
index 00000000..002a10bd
--- /dev/null
+++ b/src/kernel/ao_log_telescience.c
@@ -0,0 +1,119 @@
+/*
+ * 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_product.h"
+#include "ao_log.h"
+#include "ao_companion.h"
+
+static uint8_t	ao_log_data_pos;
+
+__code uint8_t ao_log_format = AO_LOG_FORMAT_TELESCIENCE;
+
+static void
+ao_log_telescience_csum(void) __reentrant
+{
+	__xdata uint8_t *b = ao_log_single_write_data.bytes;
+	uint8_t	sum = 0x5a;
+	uint8_t	i;
+
+	ao_log_single_write_data.telescience.csum = 0;
+	for (i = 0; i < sizeof (struct ao_log_telescience); i++)
+		sum += *b++;
+	ao_log_single_write_data.telescience.csum = -sum;
+}
+
+void
+ao_log_single(void)
+{
+	ao_storage_setup();
+
+	/* This can take a while, so let the rest
+	 * of the system finish booting before we start
+	 */
+	ao_delay(AO_SEC_TO_TICKS(10));
+
+	ao_log_single_restart();
+	for (;;) {
+		while (!ao_log_running)
+			ao_sleep(&ao_log_running);
+
+		ao_log_start_pos = ao_log_current_pos;
+		ao_log_single_write_data.telescience.type = AO_LOG_TELESCIENCE_START;
+		ao_log_single_write_data.telescience.tick = ao_time();
+		ao_log_single_write_data.telescience.adc[0] = ao_companion_command.serial;
+		ao_log_single_write_data.telescience.adc[1] = ao_companion_command.flight;
+		ao_log_telescience_csum();
+		ao_log_single_write();
+		/* Write the whole contents of the ring to the log
+		 * when starting up.
+		 */
+		ao_log_data_pos = ao_data_ring_next(ao_data_head);
+		ao_log_single_write_data.telescience.type = AO_LOG_TELESCIENCE_DATA;
+		while (ao_log_running) {
+			/* Write samples to EEPROM */
+			while (ao_log_data_pos != ao_data_head) {
+				ao_log_single_write_data.telescience.tick = ao_data_ring[ao_log_data_pos].tick;
+				memcpy(&ao_log_single_write_data.telescience.adc, (void *) ao_data_ring[ao_log_data_pos].adc.adc,
+				       AO_LOG_TELESCIENCE_NUM_ADC * sizeof (uint16_t));
+				ao_log_telescience_csum();
+				ao_log_single_write();
+				ao_log_data_pos = ao_data_ring_next(ao_log_data_pos);
+			}
+			/* Wait for more ADC data to arrive */
+			ao_sleep((void *) &ao_data_head);
+		}
+		memset(&ao_log_single_write_data.telescience.adc, '\0', sizeof (ao_log_single_write_data.telescience.adc));
+	}
+}
+
+void
+ao_log_single_list(void)
+{
+	uint32_t	pos;
+	uint32_t	start = 0;
+	uint8_t		flight = 0;
+
+	for (pos = 0; ; pos += sizeof (struct ao_log_telescience)) {
+		if (pos >= ao_storage_config ||
+		    !ao_log_single_read(pos) ||
+		    ao_log_single_read_data.telescience.type == AO_LOG_TELESCIENCE_START)
+		{
+			if (pos != start) {
+				printf("flight %d start %x end %x\n",
+				       flight,
+				       (uint16_t) (start >> 8),
+				       (uint16_t) ((pos + 0xff) >> 8)); flush();
+			}
+			if (ao_log_single_read_data.telescience.type != AO_LOG_TELESCIENCE_START)
+				break;
+			start = pos;
+			flight++;
+		}
+	}
+	printf ("done\n");
+}
+
+void
+ao_log_single_extra_query(void)
+{
+	printf("log data tick: %04x\n", ao_log_single_write_data.telescience.tick);
+	printf("TM data tick: %04x\n", ao_log_single_write_data.telescience.tm_tick);
+	printf("TM state: %d\n", ao_log_single_write_data.telescience.tm_state);
+	printf("TM serial: %d\n", ao_companion_command.serial);
+	printf("TM flight: %d\n", ao_companion_command.flight);
+}
diff --git a/src/kernel/ao_log_tiny.c b/src/kernel/ao_log_tiny.c
new file mode 100644
index 00000000..67767dc9
--- /dev/null
+++ b/src/kernel/ao_log_tiny.c
@@ -0,0 +1,161 @@
+/*
+ * 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"
+
+static __data uint16_t	ao_log_tiny_interval;
+
+#define AO_LOG_TINY_INTERVAL_DEFAULT	AO_MS_TO_TICKS(1000)
+#if USE_FAST_ASCENT_LOG
+#define AO_LOG_TINY_INTERVAL_ASCENT	AO_MS_TO_TICKS(100)
+#define AO_PAD_RING	8
+#else
+#define AO_LOG_TINY_INTERVAL_ASCENT	AO_LOG_TINY_INTERVAL_DEFAULT
+#define AO_PAD_RING	2
+#endif
+
+__code uint8_t ao_log_format = AO_LOG_FORMAT_TINY;
+
+void
+ao_log_tiny_set_interval(uint16_t ticks)
+{
+	ao_log_tiny_interval = ticks;
+}
+
+
+static void ao_log_tiny_data(uint16_t d)
+{
+	if (ao_log_current_pos >= ao_log_end_pos && ao_log_running)
+		ao_log_stop();
+	if (ao_log_running) {
+		ao_storage_write(ao_log_current_pos, DATA_TO_XDATA(&d), 2);
+		ao_log_current_pos += 2;
+	}
+}
+
+static __xdata uint16_t ao_log_pad_ring[AO_PAD_RING];
+static __pdata uint8_t ao_log_pad_ring_pos;
+
+#define ao_pad_ring_next(n)	(((n) + 1) & (AO_PAD_RING - 1))
+
+static void ao_log_tiny_queue(uint16_t d)
+{
+	ao_log_pad_ring[ao_log_pad_ring_pos] = d;
+	ao_log_pad_ring_pos = ao_pad_ring_next(ao_log_pad_ring_pos);
+}
+
+static void ao_log_tiny_start(void)
+{
+	uint8_t		p;
+	uint16_t	d;
+
+	ao_log_tiny_data(ao_flight_number);
+	ao_log_tiny_data(ao_ground_pres);
+	p = ao_log_pad_ring_pos;
+	do {
+		d = ao_log_pad_ring[p];
+		/*
+		 * ignore unwritten slots
+		 */
+		if (d)
+			ao_log_tiny_data(d);
+		p = ao_pad_ring_next(p);
+	} while (p != ao_log_pad_ring_pos);
+}
+
+void
+ao_log(void)
+{
+	uint16_t		last_time;
+	uint16_t		now;
+	enum ao_flight_state	ao_log_tiny_state;
+	int32_t			sum;
+	int16_t			count;
+	uint8_t			ao_log_data;
+	uint8_t			ao_log_started = 0;
+
+	ao_storage_setup();
+
+	ao_log_scan();
+
+	ao_log_tiny_state = ao_flight_invalid;
+	ao_log_tiny_interval = AO_LOG_TINY_INTERVAL_ASCENT;
+	sum = 0;
+	count = 0;
+	ao_log_data = ao_sample_data;
+	last_time = ao_time();
+	for (;;) {
+
+		/*
+		 * Add in pending sample data
+		 */
+		ao_sleep(DATA_TO_XDATA(&ao_sample_data));
+		while (ao_log_data != ao_sample_data) {
+			sum += ao_data_pres(&ao_data_ring[ao_log_data]);
+			count++;
+			ao_log_data = ao_data_ring_next(ao_log_data);
+		}
+		if (ao_log_running) {
+			if (!ao_log_started) {
+				ao_log_tiny_start();
+				ao_log_started = 1;
+			}
+			if (ao_flight_state != ao_log_tiny_state) {
+				ao_log_tiny_data(ao_flight_state | 0x8000);
+				ao_log_tiny_state = ao_flight_state;
+				ao_log_tiny_interval = AO_LOG_TINY_INTERVAL_DEFAULT;
+#if AO_LOG_TINY_INTERVAL_ASCENT != AO_LOG_TINY_INTERVAL_DEFAULT
+				if (ao_log_tiny_state <= ao_flight_coast)
+					ao_log_tiny_interval = AO_LOG_TINY_INTERVAL_ASCENT;
+#endif
+				if (ao_log_tiny_state == ao_flight_landed)
+					ao_log_stop();
+			}
+		}
+
+		/* Stop logging when told to */
+		if (!ao_log_running && ao_log_started)
+			ao_exit();
+
+		/*
+		 * Write out the sample when finished
+		 */
+		now = ao_time();
+		if ((int16_t) (now - (last_time + ao_log_tiny_interval)) >= 0 && count) {
+			count = sum / count;
+			if (ao_log_started)
+				ao_log_tiny_data(count);
+			else
+				ao_log_tiny_queue(count);
+			sum = 0;
+			count = 0;
+			last_time = now;
+		}
+	}
+}
+
+uint16_t
+ao_log_flight(uint8_t slot)
+{
+	static __xdata uint16_t flight;
+
+	(void) slot;
+	ao_storage_read(0, &flight, 2);
+	if (flight == 0xffff)
+		flight = 0;
+	return flight;
+}
diff --git a/src/kernel/ao_microflight.c b/src/kernel/ao_microflight.c
new file mode 100644
index 00000000..f680e400
--- /dev/null
+++ b/src/kernel/ao_microflight.c
@@ -0,0 +1,143 @@
+/*
+ * 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_FLIGHT_TEST
+#include <ao.h>
+#endif
+#include <ao_micropeak.h>
+#include <ao_log_micro.h>
+
+uint32_t	pa;
+uint32_t	pa_ground;
+uint32_t	pa_min;
+
+static void
+ao_microsample(void)
+{
+	ao_pa_get();
+	ao_microkalman_predict();
+	ao_microkalman_correct();
+}
+
+#define NUM_PA_HIST	(GROUND_AVG)
+
+#define SKIP_PA_HIST(i,j)	(((i) + (j)) & (NUM_PA_HIST - 1))
+
+static uint32_t	pa_hist[NUM_PA_HIST];
+
+void
+ao_microflight(void)
+{
+	int16_t		sample_count;
+	uint16_t	time;
+	uint32_t	pa_interval_min, pa_interval_max;
+	int32_t		pa_diff;
+	uint8_t		h, i;
+	uint8_t		accel_lock = 0;
+	uint32_t	pa_sum = 0;
+
+	/* Wait for motion, averaging values to get ground pressure */
+
+	time = ao_time();
+	ao_pa_get();
+	ao_microkalman_init();
+	pa_ground = pa;
+	sample_count = 0;
+	h = 0;
+	for (;;) {
+		time += SAMPLE_SLEEP;
+		if ((sample_count & 0x1f) == 0)
+			ao_led_on(AO_LED_REPORT);
+		ao_delay_until(time);
+		ao_microsample();
+		if ((sample_count & 0x1f) == 0)
+			ao_led_off(AO_LED_REPORT);
+		pa_hist[h] = pa;
+		h = SKIP_PA_HIST(h,1);
+		pa_diff = pa_ground - ao_pa;
+
+		/* Check for a significant pressure change */
+		if (pa_diff > BOOST_DETECT)
+			break;
+
+		if (sample_count < GROUND_AVG * 2) {
+			if (sample_count < GROUND_AVG)
+				pa_sum += pa;
+			++sample_count;
+		} else {
+			pa_ground = pa_sum >> GROUND_AVG_SHIFT;
+			pa_sum = 0;
+			sample_count = 0;
+		}
+	}
+
+	/* 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 != SKIP_PA_HIST(h,2); i = SKIP_PA_HIST(i,-2)) {
+		if (pa_hist[i] >= pa_min)
+			break;
+	}
+
+	/* Log the remaining samples so we get a complete history since leaving the ground */
+	for (; i != h; i = SKIP_PA_HIST(i,2)) {
+		pa = pa_hist[i];
+		ao_log_micro_data();
+	}
+
+	/* Now sit around until the pressure is stable again and record the max */
+
+	sample_count = 0;
+	pa_min = ao_pa;
+	pa_interval_min = ao_pa;
+	pa_interval_max = ao_pa;
+	for (;;) {
+		time += SAMPLE_SLEEP;
+		ao_delay_until(time);
+		if ((sample_count & 3) == 0)
+			ao_led_on(AO_LED_REPORT);
+		ao_microsample();
+		if ((sample_count & 3) == 0)
+			ao_led_off(AO_LED_REPORT);
+		if (sample_count & 1)
+			ao_log_micro_data();
+
+		/* If accelerating upwards, don't look for min pressure */
+		if (ao_pa_accel < ACCEL_LOCK_PA)
+			accel_lock = ACCEL_LOCK_TIME;
+		else if (accel_lock)
+			--accel_lock;
+		else if (ao_pa < pa_min)
+			pa_min = ao_pa;
+
+		if (sample_count == (GROUND_AVG - 1)) {
+			pa_diff = pa_interval_max - pa_interval_min;
+
+			/* Check to see if the pressure is now stable */
+			if (pa_diff < LAND_DETECT)
+				break;
+			sample_count = 0;
+			pa_interval_min = ao_pa;
+			pa_interval_max = ao_pa;
+		} else {
+			if (ao_pa < pa_interval_min)
+				pa_interval_min = ao_pa;
+			if (ao_pa > pa_interval_max)
+				pa_interval_max = ao_pa;
+			++sample_count;
+		}
+	}
+}
diff --git a/src/kernel/ao_microkalman.c b/src/kernel/ao_microkalman.c
new file mode 100644
index 00000000..0684ea2b
--- /dev/null
+++ b/src/kernel/ao_microkalman.c
@@ -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_FLIGHT_TEST
+#include <ao.h>
+#endif
+#include <ao_micropeak.h>
+
+#define FIX_BITS	16
+
+#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5))
+#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
+#define from_fix8(x)	((x) >> 8)
+#define from_fix(x)	((x) >> 16)
+#define fix8_to_fix16(x)	((x) << 8)
+#define fix16_to_fix8(x)	((x) >> 8)
+
+#include <ao_kalman.h>
+
+/* Basic time step (96ms) */
+#define AO_MK_STEP	to_fix16(0.096)
+/* step ** 2 / 2 */
+#define AO_MK_STEP_2_2	to_fix16(0.004608)
+
+uint32_t	ao_k_pa;		/* 24.8 fixed point */
+int32_t		ao_k_pa_speed;		/* 16.16 fixed point */
+int32_t		ao_k_pa_accel;		/* 16.16 fixed point */
+
+uint32_t	ao_pa;			/* integer portion */
+int16_t		ao_pa_speed;		/* integer portion */
+int16_t		ao_pa_accel;		/* integer portion */
+
+void
+ao_microkalman_init(void)
+{
+	ao_pa = pa;
+	ao_k_pa = pa << 8;
+}	
+
+void
+ao_microkalman_predict(void)
+{
+	ao_k_pa       += fix16_to_fix8((int32_t) ao_pa_speed * AO_MK_STEP + (int32_t) ao_pa_accel * AO_MK_STEP_2_2);
+	ao_k_pa_speed += (int32_t) ao_pa_accel * AO_MK_STEP;
+}
+
+void
+ao_microkalman_correct(void)
+{
+	int16_t	e;	/* Height error in Pa */
+
+	e = pa - from_fix8(ao_k_pa);
+
+	ao_k_pa       += fix16_to_fix8((int32_t) e * AO_MK_BARO_K0_10);
+	ao_k_pa_speed += (int32_t) e * AO_MK_BARO_K1_10;
+	ao_k_pa_accel += (int32_t) e * AO_MK_BARO_K2_10;
+	ao_pa = from_fix8(ao_k_pa);
+	ao_pa_speed = from_fix(ao_k_pa_speed);
+	ao_pa_accel = from_fix(ao_k_pa_accel);
+}
diff --git a/src/kernel/ao_monitor.c b/src/kernel/ao_monitor.c
new file mode 100644
index 00000000..18f170b4
--- /dev/null
+++ b/src/kernel/ao_monitor.c
@@ -0,0 +1,308 @@
+/*
+ * 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_telem.h"
+#include "ao_flight.h"
+
+#if !HAS_MONITOR
+#error Must define HAS_MONITOR to 1
+#endif
+
+#ifndef LEGACY_MONITOR
+#error Must define LEGACY_MONITOR
+#endif
+
+#ifndef HAS_MONITOR_PUT
+#define HAS_MONITOR_PUT 1
+#endif
+
+#ifndef AO_MONITOR_LED
+#error Must define AO_MONITOR_LED
+#endif
+
+__data uint8_t ao_monitoring;
+static __data uint8_t ao_monitor_disabled;
+static __data uint8_t ao_internal_monitoring;
+static __data uint8_t ao_external_monitoring;
+
+__xdata union ao_monitor ao_monitor_ring[AO_MONITOR_RING];
+
+__data uint8_t	ao_monitor_head;
+
+static void
+_ao_monitor_adjust(void)
+{
+	if (ao_monitoring)
+		ao_radio_recv_abort();
+	if (ao_monitor_disabled)
+		ao_monitoring = 0;
+	else {
+		if (ao_external_monitoring)
+			ao_monitoring = ao_external_monitoring;
+		else
+			ao_monitoring = ao_internal_monitoring;
+	}
+	ao_wakeup(DATA_TO_XDATA(&ao_monitoring));
+}
+
+void
+ao_monitor_get(void)
+{
+	uint8_t	size;
+
+	for (;;) {
+		switch (ao_monitoring) {
+		case 0:
+			ao_sleep(DATA_TO_XDATA(&ao_monitoring));
+			continue;
+#if LEGACY_MONITOR
+		case AO_MONITORING_ORIG:
+			size = sizeof (struct ao_telemetry_orig_recv);
+			break;
+#endif
+		default:
+			if (ao_monitoring > AO_MAX_TELEMETRY)
+				ao_monitoring = AO_MAX_TELEMETRY;
+			size = ao_monitoring;
+			break;
+		}
+		if (!ao_radio_recv(&ao_monitor_ring[ao_monitor_head], size + 2, 0))
+			continue;
+		ao_monitor_head = ao_monitor_ring_next(ao_monitor_head);
+		ao_wakeup(DATA_TO_XDATA(&ao_monitor_head));
+	}
+}
+
+#if AO_MONITOR_LED
+__xdata struct ao_task ao_monitor_blink_task;
+
+void
+ao_monitor_blink(void)
+{
+	for (;;) {
+		ao_sleep(DATA_TO_XDATA(&ao_monitor_head));
+		ao_led_for(AO_MONITOR_LED, AO_MS_TO_TICKS(100));
+	}
+}
+#endif
+
+#if HAS_MONITOR_PUT
+
+static const char xdigit[16] = {
+	'0', '1', '2', '3', '4', '5', '6', '7',
+	'8', '9', 'a', 'b', 'c', 'd', 'e', 'f'
+};
+
+#define hex(c) do { putchar(xdigit[(c) >> 4]); putchar(xdigit[(c)&0xf]); } while (0)
+
+void
+ao_monitor_put(void)
+{
+#if LEGACY_MONITOR
+	__xdata char callsign[AO_MAX_CALLSIGN+1];
+	int16_t rssi;
+#endif
+	uint8_t ao_monitor_tail;
+	uint8_t state;
+	uint8_t sum, byte;
+	__xdata union ao_monitor	*m;
+
+#define recv_raw	((m->raw))
+#define recv_orig	((m->orig))
+#define recv_tiny	((m->tiny))
+
+	ao_monitor_tail = ao_monitor_head;
+	for (;;) {
+		while (!ao_external_monitoring)
+			ao_sleep(DATA_TO_XDATA(&ao_external_monitoring));
+		while (ao_monitor_tail == ao_monitor_head && ao_external_monitoring)
+			ao_sleep(DATA_TO_XDATA(&ao_monitor_head));
+		if (!ao_external_monitoring)
+			continue;
+		m = &ao_monitor_ring[ao_monitor_tail];
+		ao_monitor_tail = ao_monitor_ring_next(ao_monitor_tail);
+		switch (ao_monitoring) {
+		case 0:
+			break;
+#if LEGACY_MONITOR
+		case AO_MONITORING_ORIG:
+			state = recv_orig.telemetry_orig.flight_state;
+
+			rssi = (int16_t) AO_RSSI_FROM_RADIO(recv_orig.rssi);
+			ao_xmemcpy(callsign, recv_orig.telemetry_orig.callsign, AO_MAX_CALLSIGN);
+			if (state > ao_flight_invalid)
+				state = ao_flight_invalid;
+			if (recv_orig.status & PKT_APPEND_STATUS_1_CRC_OK) {
+
+				/* General header fields */
+				printf(AO_TELEM_VERSION " %d "
+				       AO_TELEM_CALL " %s "
+				       AO_TELEM_SERIAL " %d "
+				       AO_TELEM_FLIGHT " %d "
+				       AO_TELEM_RSSI " %d "
+				       AO_TELEM_STATE " %s "
+				       AO_TELEM_TICK " %d ",
+				       AO_TELEMETRY_VERSION,
+				       callsign,
+				       recv_orig.telemetry_orig.serial,
+				       recv_orig.telemetry_orig.flight,
+				       rssi,
+				       ao_state_names[state],
+				       recv_orig.telemetry_orig.adc.tick);
+
+				/* Raw sensor values */
+				printf(AO_TELEM_RAW_ACCEL " %d "
+				       AO_TELEM_RAW_BARO " %d "
+				       AO_TELEM_RAW_THERMO " %d "
+				       AO_TELEM_RAW_BATT " %d "
+				       AO_TELEM_RAW_DROGUE " %d "
+				       AO_TELEM_RAW_MAIN " %d ",
+				       recv_orig.telemetry_orig.adc.accel,
+				       recv_orig.telemetry_orig.adc.pres,
+				       recv_orig.telemetry_orig.adc.temp,
+				       recv_orig.telemetry_orig.adc.v_batt,
+				       recv_orig.telemetry_orig.adc.sense_d,
+				       recv_orig.telemetry_orig.adc.sense_m);
+
+				/* Sensor calibration values */
+				printf(AO_TELEM_CAL_ACCEL_GROUND " %d "
+				       AO_TELEM_CAL_BARO_GROUND " %d "
+				       AO_TELEM_CAL_ACCEL_PLUS " %d "
+				       AO_TELEM_CAL_ACCEL_MINUS " %d ",
+				       recv_orig.telemetry_orig.ground_accel,
+				       recv_orig.telemetry_orig.ground_pres,
+				       recv_orig.telemetry_orig.accel_plus_g,
+				       recv_orig.telemetry_orig.accel_minus_g);
+
+				if (recv_orig.telemetry_orig.u.k.unused == 0x8000) {
+					/* Kalman state values */
+					printf(AO_TELEM_KALMAN_HEIGHT " %d "
+					       AO_TELEM_KALMAN_SPEED " %d "
+					       AO_TELEM_KALMAN_ACCEL " %d ",
+					       recv_orig.telemetry_orig.height,
+					       recv_orig.telemetry_orig.u.k.speed,
+					       recv_orig.telemetry_orig.accel);
+				} else {
+					/* Ad-hoc flight values */
+					printf(AO_TELEM_ADHOC_ACCEL " %d "
+					       AO_TELEM_ADHOC_SPEED " %ld "
+					       AO_TELEM_ADHOC_BARO " %d ",
+					       recv_orig.telemetry_orig.accel,
+					       recv_orig.telemetry_orig.u.flight_vel,
+					       recv_orig.telemetry_orig.height);
+				}
+				ao_gps_print(&recv_orig.telemetry_orig.gps);
+				ao_gps_tracking_print(&recv_orig.telemetry_orig.gps_tracking);
+				putchar('\n');
+#if HAS_RSSI
+				ao_rssi_set(rssi);
+#endif
+			} else {
+				printf("CRC INVALID RSSI %3d\n", rssi);
+			}
+			break;
+#endif /* LEGACY_MONITOR */
+		default:
+#if AO_PROFILE
+		{
+			extern uint32_t	ao_rx_start_tick, ao_rx_packet_tick, ao_rx_done_tick, ao_rx_last_done_tick;
+			extern uint32_t ao_fec_decode_start, ao_fec_decode_end;
+
+			printf ("between packet: %d\n", ao_rx_start_tick - ao_rx_last_done_tick);
+			printf ("receive start delay: %d\n", ao_rx_packet_tick - ao_rx_start_tick);
+			printf ("decode time: %d\n", ao_fec_decode_end - ao_fec_decode_start);
+			printf ("rx cleanup: %d\n", ao_rx_done_tick - ao_fec_decode_end);
+		}
+#endif
+			printf("TELEM ");
+			hex((uint8_t) (ao_monitoring + 2));
+			sum = 0x5a;
+			for (state = 0; state < ao_monitoring + 2; state++) {
+				byte = recv_raw.packet[state];
+				sum += byte;
+				hex(byte);
+			}
+			hex(sum);
+			putchar ('\n');
+#if HAS_RSSI
+			if (recv_raw.packet[ao_monitoring + 1] & PKT_APPEND_STATUS_1_CRC_OK) {
+				rssi = AO_RSSI_FROM_RADIO(recv_raw.packet[ao_monitoring]);
+				ao_rssi_set(rssi);
+			}
+#endif
+			break;
+		}
+		ao_usb_flush();
+	}
+}
+
+__xdata struct ao_task ao_monitor_put_task;
+#endif
+
+__xdata struct ao_task ao_monitor_get_task;
+
+void
+ao_monitor_set(uint8_t monitoring)
+{
+	ao_internal_monitoring = monitoring;
+	_ao_monitor_adjust();
+}
+
+void
+ao_monitor_disable(void)
+{
+	++ao_monitor_disabled;
+	_ao_monitor_adjust();
+}
+
+void
+ao_monitor_enable(void)
+{
+	--ao_monitor_disabled;
+	_ao_monitor_adjust();
+}
+
+#if HAS_MONITOR_PUT
+static void
+set_monitor(void)
+{
+	ao_cmd_hex();
+	ao_external_monitoring = ao_cmd_lex_i;
+	ao_wakeup(DATA_TO_XDATA(&ao_external_monitoring));
+	ao_wakeup(DATA_TO_XDATA(&ao_monitor_head));
+	_ao_monitor_adjust();
+}
+
+__code struct ao_cmds ao_monitor_cmds[] = {
+	{ set_monitor,	"m <0 off, 1 old, 20 std>\0Set radio monitoring" },
+	{ 0,	NULL },
+};
+#endif
+
+void
+ao_monitor_init(void) __reentrant
+{
+#if HAS_MONITOR_PUT
+	ao_cmd_register(&ao_monitor_cmds[0]);
+	ao_add_task(&ao_monitor_put_task, ao_monitor_put, "monitor_put");
+#endif
+	ao_add_task(&ao_monitor_get_task, ao_monitor_get, "monitor_get");
+#if AO_MONITOR_LED
+	ao_add_task(&ao_monitor_blink_task, ao_monitor_blink, "monitor_blink");
+#endif
+}
diff --git a/src/kernel/ao_mutex.c b/src/kernel/ao_mutex.c
new file mode 100644
index 00000000..952ff462
--- /dev/null
+++ b/src/kernel/ao_mutex.c
@@ -0,0 +1,41 @@
+/*
+ * 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"
+
+void
+ao_mutex_get(__xdata uint8_t *mutex) __reentrant
+{
+	if (*mutex == ao_cur_task->task_id)
+		ao_panic(AO_PANIC_MUTEX);
+	ao_arch_critical(
+		while (*mutex)
+			ao_sleep(mutex);
+		*mutex = ao_cur_task->task_id;
+		);
+}
+
+void
+ao_mutex_put(__xdata uint8_t *mutex) __reentrant
+{
+	if (*mutex != ao_cur_task->task_id)
+		ao_panic(AO_PANIC_MUTEX);
+	ao_arch_critical(
+		*mutex = 0;
+		ao_wakeup(mutex);
+		);
+}
diff --git a/src/kernel/ao_notask.c b/src/kernel/ao_notask.c
new file mode 100644
index 00000000..6f967e6d
--- /dev/null
+++ b/src/kernel/ao_notask.c
@@ -0,0 +1,46 @@
+/*
+ * 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>
+
+static volatile void *ao_wchan;
+
+uint8_t
+ao_sleep(__xdata void *wchan)
+{
+#if 1
+	ao_wchan = wchan;
+	ao_arch_wait_interrupt();
+#else
+	uint8_t	sreg;
+
+	ao_wchan = wchan;
+	asm("in %0,__SREG__" : "=&r" (sreg));
+	sei();
+	while (ao_wchan)
+		ao_arch_cpu_idle();
+	asm("out __SREG__,%0" : : "r" (sreg));
+#endif
+	return 0;
+}
+
+void
+ao_wakeup(__xdata void *wchan)
+{
+	(void) wchan;
+	ao_wchan = 0;
+}
diff --git a/src/kernel/ao_notask.h b/src/kernel/ao_notask.h
new file mode 100644
index 00000000..6b6b5bb8
--- /dev/null
+++ b/src/kernel/ao_notask.h
@@ -0,0 +1,27 @@
+/*
+ * 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_NOTASK_H_
+#define _AO_NOTASK_H_
+
+uint8_t
+ao_sleep(__xdata void *wchan);
+
+void
+ao_wakeup(__xdata void *wchan);
+
+#endif /* _AO_NOTASK_H_ */
diff --git a/src/kernel/ao_packet.h b/src/kernel/ao_packet.h
new file mode 100644
index 00000000..b8426cf9
--- /dev/null
+++ b/src/kernel/ao_packet.h
@@ -0,0 +1,91 @@
+/*
+ * 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_PACKET_H_
+#define _AO_PACKET_H_
+
+/*
+ * ao_packet.c
+ *
+ * Packet-based command interface
+ */
+
+#define AO_PACKET_MAX		64
+#define AO_PACKET_SYN		(uint8_t) 0xff
+
+struct ao_packet {
+	uint8_t		addr;
+	uint8_t		len;
+	uint8_t		seq;
+	uint8_t		ack;
+	uint8_t		d[AO_PACKET_MAX];
+	uint8_t		callsign[AO_MAX_CALLSIGN];
+};
+
+struct ao_packet_recv {
+	struct ao_packet	packet;
+	int8_t			rssi;
+	uint8_t			status;
+};
+
+extern __xdata struct ao_packet_recv ao_rx_packet;
+extern __xdata struct ao_packet ao_tx_packet;
+extern __xdata struct ao_task	ao_packet_task;
+extern __xdata uint8_t ao_packet_enable;
+extern __xdata uint8_t ao_packet_master_sleeping;
+extern __pdata uint8_t ao_packet_rx_len, ao_packet_rx_used, ao_packet_tx_used;
+extern __xdata uint8_t ao_packet_restart;
+
+void
+ao_packet_send(void);
+
+uint8_t
+ao_packet_recv(void);
+
+void
+ao_packet_flush(void);
+
+void
+ao_packet_putchar(char c) __reentrant;
+
+int
+_ao_packet_pollchar(void);
+
+#if PACKET_HAS_MASTER
+/* ao_packet_master.c */
+
+extern __xdata int8_t ao_packet_last_rssi;
+
+void
+ao_packet_master_init(void);
+#endif
+
+#if PACKET_HAS_SLAVE
+/* ao_packet_slave.c */
+
+void
+ao_packet_slave_start(void);
+
+void
+ao_packet_slave_stop(void);
+
+void
+ao_packet_slave_init(uint8_t enable);
+
+#endif
+
+#endif /* _AO_PACKET_H_ */
diff --git a/src/kernel/ao_panic.c b/src/kernel/ao_panic.c
new file mode 100644
index 00000000..c29cd8fe
--- /dev/null
+++ b/src/kernel/ao_panic.c
@@ -0,0 +1,90 @@
+/*
+ * 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"
+
+#ifndef HAS_BEEP
+#error Please define HAS_BEEP
+#endif
+
+#if !HAS_BEEP
+#define ao_beep(x)
+#endif
+#if !LEDS_AVAILABLE
+#define ao_led_on(x)
+#define ao_led_off(x)
+#endif
+
+#ifndef AO_LED_PANIC
+#define AO_LED_PANIC	AO_LED_RED
+#endif
+
+static void
+ao_panic_delay(uint8_t n)
+{
+	uint8_t	i = 0, j = 0;
+
+	while (n--)
+		while (--j)
+			while (--i)
+				ao_arch_nop();
+}
+
+void
+ao_panic(uint8_t reason)
+{
+	uint8_t	n;
+
+#if LOW_LEVEL_DEBUG
+	ao_cur_task = NULL;
+	printf ("panic %d\n", reason);
+#endif
+	ao_arch_block_interrupts();
+	for (;;) {
+		ao_panic_delay(20);
+		for (n = 0; n < 5; n++) {
+			ao_led_on(AO_LED_PANIC);
+			ao_beep(AO_BEEP_HIGH);
+			ao_panic_delay(1);
+			ao_led_off(AO_LED_PANIC);
+			ao_beep(AO_BEEP_LOW);
+			ao_panic_delay(1);
+		}
+		ao_beep(AO_BEEP_OFF);
+		ao_panic_delay(2);
+
+#ifdef SDCC
+#pragma disable_warning 126
+#endif
+		if (reason & 0x40) {
+			ao_led_on(AO_LED_PANIC);
+			ao_beep(AO_BEEP_HIGH);
+			ao_panic_delay(40);
+			ao_led_off(AO_LED_PANIC);
+			ao_beep(AO_BEEP_OFF);
+			ao_panic_delay(10);
+		}
+		for (n = 0; n < (reason & 0x3f); n++) {
+			ao_led_on(AO_LED_PANIC);
+			ao_beep(AO_BEEP_MID);
+			ao_panic_delay(10);
+			ao_led_off(AO_LED_PANIC);
+			ao_beep(AO_BEEP_OFF);
+			ao_panic_delay(10);
+		}
+	}
+}
diff --git a/src/kernel/ao_product.c b/src/kernel/ao_product.c
new file mode 100644
index 00000000..b9327bac
--- /dev/null
+++ b/src/kernel/ao_product.c
@@ -0,0 +1,161 @@
+/*
+ * 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_product.h"
+
+/* Defines which mark this particular AltOS product */
+
+const char ao_version[AO_MAX_VERSION] = AO_iVersion_STRING;
+const char ao_manufacturer[] = AO_iManufacturer_STRING;
+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 [] =
+{
+	/* Device descriptor */
+	0x12,
+	AO_USB_DESC_DEVICE,
+	LE_WORD(0x0110),	/*  bcdUSB */
+	0x02,			/*  bDeviceClass */
+	0x00,			/*  bDeviceSubClass */
+	0x00,			/*  bDeviceProtocol */
+	AO_USB_CONTROL_SIZE,	/*  bMaxPacketSize */
+	LE_WORD(0xFFFE),	/*  idVendor */
+	LE_WORD(AO_idProduct_NUMBER),	/*  idProduct */
+	LE_WORD(0x0100),	/*  bcdDevice */
+	0x01,			/*  iManufacturer */
+	0x02,			/*  iProduct */
+	0x03,			/*  iSerialNumber */
+	0x01,			/*  bNumConfigurations */
+
+	/* Configuration descriptor */
+	0x09,
+	AO_USB_DESC_CONFIGURATION,
+	LE_WORD(67),		/*  wTotalLength */
+	0x02,			/*  bNumInterfaces */
+	0x01,			/*  bConfigurationValue */
+	0x00,			/*  iConfiguration */
+	0xC0,			/*  bmAttributes */
+	AO_USB_MAX_POWER >> 1,	/*  bMaxPower, 2mA units */
+
+	/* Control class interface */
+	0x09,
+	AO_USB_DESC_INTERFACE,
+	0x00,			/*  bInterfaceNumber */
+	0x00,			/*  bAlternateSetting */
+	0x01,			/*  bNumEndPoints */
+	0x02,			/*  bInterfaceClass */
+	0x02,			/*  bInterfaceSubClass */
+	0x01,			/*  bInterfaceProtocol, linux requires value of 1 for the cdc_acm module */
+	0x00,			/*  iInterface */
+
+	/* Header functional descriptor */
+	0x05,
+	AO_USB_CS_INTERFACE,
+	0x00,			/*  bDescriptor SubType Header */
+	LE_WORD(0x0110),	/*  CDC version 1.1 */
+
+	/* Call management functional descriptor */
+	0x05,
+	AO_USB_CS_INTERFACE,
+	0x01,			/* bDescriptor SubType Call Management */
+	0x01,			/* bmCapabilities = device handles call management */
+	0x01,			/* bDataInterface call management interface number */
+
+	/* ACM functional descriptor */
+	0x04,
+	AO_USB_CS_INTERFACE,
+	0x02,			/* bDescriptor SubType Abstract Control Management */
+	0x02,			/* bmCapabilities = D1 (Set_line_Coding, Set_Control_Line_State, Get_Line_Coding and Serial_State) */
+
+	/* Union functional descriptor */
+	0x05,
+	AO_USB_CS_INTERFACE,
+	0x06,			/* bDescriptor SubType Union Functional descriptor */
+	0x00,			/* bMasterInterface */
+	0x01,			/* bSlaveInterface0 */
+
+	/* Notification EP */
+	0x07,
+	AO_USB_DESC_ENDPOINT,
+	AO_USB_INT_EP|0x80,	/* bEndpointAddress */
+	0x03,			/* bmAttributes = intr */
+	LE_WORD(8),		/* wMaxPacketSize */
+	0xff,			/* bInterval */
+
+	/* Data class interface descriptor */
+	0x09,
+	AO_USB_DESC_INTERFACE,
+	0x01,			/* bInterfaceNumber */
+	0x00,			/* bAlternateSetting */
+	0x02,			/* bNumEndPoints */
+	0x0A,			/* bInterfaceClass = data */
+	0x00,			/* bInterfaceSubClass */
+	0x00,			/* bInterfaceProtocol */
+	0x00,			/* iInterface */
+
+	/* Data EP OUT */
+	0x07,
+	AO_USB_DESC_ENDPOINT,
+	AO_USB_OUT_EP,		/* bEndpointAddress */
+	0x02,			/* bmAttributes = bulk */
+	LE_WORD(AO_USB_OUT_SIZE),/* wMaxPacketSize */
+	0x00,			/* bInterval */
+
+	/* Data EP in */
+	0x07,
+	AO_USB_DESC_ENDPOINT,
+	AO_USB_IN_EP|0x80,	/* bEndpointAddress */
+	0x02,			/* bmAttributes = bulk */
+	LE_WORD(AO_USB_IN_SIZE),/* wMaxPacketSize */
+	0x00,			/* bInterval */
+
+	/* String descriptors */
+	0x04,
+	AO_USB_DESC_STRING,
+	LE_WORD(0x0409),
+
+	/* iManufacturer */
+	AO_iManufacturer_LEN,
+	AO_USB_DESC_STRING,
+	AO_iManufacturer_UCS2,
+
+	/* iProduct */
+	AO_iProduct_LEN,
+	AO_USB_DESC_STRING,
+	AO_iProduct_UCS2,
+
+	/* iSerial */
+	AO_iSerial_LEN,
+	AO_USB_DESC_STRING,
+	AO_iSerial_UCS2,
+
+	/* Terminating zero */
+	0
+};
+#endif
diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c
new file mode 100644
index 00000000..e59f5bc4
--- /dev/null
+++ b/src/kernel/ao_pyro.c
@@ -0,0 +1,518 @@
+/*
+ * 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_FLIGHT_TEST
+#include <ao.h>
+#include <ao_sample.h>
+#include <ao_flight.h>
+#endif
+#include <ao_pyro.h>
+
+#if IS_COMPANION
+#include <ao_companion.h>
+#define ao_accel ao_companion_command.accel
+#define ao_speed ao_companion_command.speed
+#define ao_height ao_companion_command.height
+#define ao_flight_state ao_companion_command.flight_state
+#define ao_motor_number ao_companion_command.motor_number
+#endif
+
+#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
+ * of the requirements
+ */
+static uint8_t
+ao_pyro_ready(struct ao_pyro *pyro)
+{
+	enum ao_pyro_flag flag, flags;
+
+	flags = pyro->flags;
+	while (flags != ao_pyro_none) {
+		flag = ao_lowbit(flags);
+		flags &= ~flag;
+		switch (flag) {
+
+		case ao_pyro_accel_less:
+			if (ao_accel <= pyro->accel_less)
+				continue;
+			break;
+		case ao_pyro_accel_greater:
+			if (ao_accel >= pyro->accel_greater)
+				continue;
+			break;
+
+
+		case ao_pyro_speed_less:
+			if (ao_speed <= pyro->speed_less)
+				continue;
+			break;
+		case ao_pyro_speed_greater:
+			if (ao_speed >= pyro->speed_greater)
+				continue;
+			break;
+
+		case ao_pyro_height_less:
+			if (ao_height <= pyro->height_less)
+				continue;
+			break;
+		case ao_pyro_height_greater:
+			if (ao_height >= pyro->height_greater)
+				continue;
+			break;
+
+#if HAS_GYRO
+		case ao_pyro_orient_less:
+			if (ao_sample_orient <= pyro->orient_less)
+				continue;
+			break;
+		case ao_pyro_orient_greater:
+			if (ao_sample_orient >= pyro->orient_greater)
+				continue;
+			break;
+#endif
+
+		case ao_pyro_time_less:
+			if ((int16_t) (ao_time() - ao_boost_tick) <= pyro->time_less)
+				continue;
+			break;
+		case ao_pyro_time_greater:
+			if ((int16_t) (ao_time() - ao_boost_tick) >= pyro->time_greater)
+				continue;
+			break;
+
+		case ao_pyro_ascending:
+			if (ao_speed > 0)
+				continue;
+			break;
+		case ao_pyro_descending:
+			if (ao_speed < 0)
+				continue;
+			break;
+
+		case ao_pyro_after_motor:
+			if (ao_motor_number == pyro->motor)
+				continue;
+			break;
+
+		case ao_pyro_delay:
+			/* handled separately */
+			continue;
+
+		case ao_pyro_state_less:
+			if (ao_flight_state < pyro->state_less)
+				continue;
+			break;
+		case ao_pyro_state_greater_or_equal:
+			if (ao_flight_state >= pyro->state_greater_or_equal)
+				continue;
+			break;
+
+		default:
+			continue;
+		}
+		return FALSE;
+	}
+	return TRUE;
+}
+
+#ifndef AO_FLIGHT_TEST
+static void
+ao_pyro_pin_set(uint8_t p, uint8_t v)
+{
+	switch (p) {
+#if AO_PYRO_NUM > 0
+	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_gpio_set(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, v); break;
+#endif
+#if AO_PYRO_NUM > 2
+	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_gpio_set(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, v); break;
+#endif
+#if AO_PYRO_NUM > 4
+	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_gpio_set(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, v); break;
+#endif
+#if AO_PYRO_NUM > 6
+	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_gpio_set(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, v); break;
+#endif
+	default: break;
+	}
+}
+#endif
+
+uint8_t	ao_pyro_wakeup;
+
+static void
+ao_pyro_pins_fire(uint16_t fire)
+{
+	uint8_t p;
+
+	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));
+}
+
+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 disabled igniters
+		 */
+		if (!pyro->flags)
+			continue;
+
+		any_waiting = 1;
+		/* Check pyro state to see if it should 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;
+				if (!pyro->delay_done)
+					pyro->delay_done = 1;
+			}
+		}
+
+		/* 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;
+		}
+
+		fire |= (1 << p);
+	}
+
+	if (fire)
+		ao_pyro_pins_fire(fire);
+
+	return any_waiting;
+}
+
+#define NO_VALUE	0xff
+
+#define AO_PYRO_NAME_LEN	3
+
+#if !DISABLE_HELP
+#define ENABLE_HELP 1
+#endif
+
+#if ENABLE_HELP
+#define HELP(s)	(s)
+#else
+#define HELP(s)
+#endif
+
+const struct {
+	char			name[AO_PYRO_NAME_LEN];
+	enum ao_pyro_flag	flag;
+	uint8_t			offset;
+#if ENABLE_HELP
+	char			*help;
+#endif
+} ao_pyro_values[] = {
+	{ "a<",	ao_pyro_accel_less,	offsetof(struct ao_pyro, accel_less), HELP("accel less (m/ss * 16)") },
+	{ "a>",	ao_pyro_accel_greater,	offsetof(struct ao_pyro, accel_greater), HELP("accel greater (m/ss * 16)") },
+
+	{ "s<",	ao_pyro_speed_less,	offsetof(struct ao_pyro, speed_less), HELP("speed less (m/s * 16)") },
+	{ "s>",	ao_pyro_speed_greater,	offsetof(struct ao_pyro, speed_greater), HELP("speed greater (m/s * 16)") },
+
+	{ "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_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
+
+	{ "t<",	ao_pyro_time_less,	offsetof(struct ao_pyro, time_less), HELP("time less (s * 100)") },
+	{ "t>",	ao_pyro_time_greater,	offsetof(struct ao_pyro, time_greater), HELP("time greater (s * 100)")  },
+
+	{ "f<",	ao_pyro_state_less,	offsetof(struct ao_pyro, state_less), HELP("state less") },
+	{ "f>=",ao_pyro_state_greater_or_equal,	offsetof(struct ao_pyro, state_greater_or_equal), HELP("state greater or equal")  },
+
+	{ "A", ao_pyro_ascending,	NO_VALUE, HELP("ascending") },
+	{ "D", ao_pyro_descending,	NO_VALUE, HELP("descending") },
+
+	{ "m", ao_pyro_after_motor,	offsetof(struct ao_pyro, motor), HELP("after motor") },
+
+	{ "d", ao_pyro_delay,		offsetof(struct ao_pyro, delay), HELP("delay before firing (s * 100)") },
+	{ "", 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)
+{
+	const char *s = ao_pyro_values[v].name;
+	printf ("%s%s", s, "   " + strlen(s));
+}
+
+#if ENABLE_HELP
+static void
+ao_pyro_help(void)
+{
+	uint8_t v;
+	for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) {
+		ao_pyro_print_name(v);
+		if (ao_pyro_values[v].offset != NO_VALUE)
+			printf ("<n> ");
+		else
+			printf ("    ");
+		printf ("%s\n", ao_pyro_values[v].help);
+	}
+}
+#endif
+
+void
+ao_pyro_show(void)
+{
+	uint8_t 	p;
+	uint8_t 	v;
+	struct ao_pyro	*pyro;
+
+	printf ("Pyro-count: %d\n", AO_PYRO_NUM);
+	for (p = 0; p < AO_PYRO_NUM; p++) {
+		printf ("Pyro %2d: ", p);
+		pyro = &ao_config.pyro[p];
+		if (!pyro->flags) {
+			printf ("<disabled>\n");
+			continue;
+		}
+		for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) {
+			if (!(pyro->flags & ao_pyro_values[v].flag))
+				continue;
+			ao_pyro_print_name(v);
+			if (ao_pyro_values[v].offset != NO_VALUE) {
+				int16_t	value;
+
+				value = *((int16_t *) ((char *) pyro + ao_pyro_values[v].offset));
+				printf ("%6d ", value);
+			} else {
+				printf ("       ");
+			}
+		}
+		printf ("\n");
+	}
+}
+
+void
+ao_pyro_set(void)
+{
+	uint8_t	p;
+	struct ao_pyro pyro_tmp;
+	char	name[AO_PYRO_NAME_LEN];
+	uint8_t	c;
+	uint8_t	v;
+
+	ao_cmd_white();
+
+#if ENABLE_HELP
+	switch (ao_cmd_lex_c) {
+	case '?':
+		ao_pyro_help();
+		return;
+	}
+#endif
+
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	p = ao_cmd_lex_i;
+	if (AO_PYRO_NUM <= p) {
+		printf ("invalid pyro channel %d\n", p);
+		return;
+	}
+	pyro_tmp.flags = 0;
+	for (;;) {
+		ao_cmd_white();
+		if (ao_cmd_lex_c == '\n')
+			break;
+
+		for (c = 0; c < AO_PYRO_NAME_LEN - 1; c++) {
+			if (ao_cmd_is_white())
+				break;
+			name[c] = ao_cmd_lex_c;
+			ao_cmd_lex();
+		}
+		name[c] = '\0';
+		for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) {
+			if (!strcmp (ao_pyro_values[v].name, name))
+				break;
+		}
+		if (ao_pyro_values[v].flag == ao_pyro_none) {
+			printf ("invalid pyro field %s\n", name);
+			ao_cmd_status = ao_cmd_syntax_error;
+			return;
+		}
+		pyro_tmp.flags |= ao_pyro_values[v].flag;
+		if (ao_pyro_values[v].offset != NO_VALUE) {
+			ao_cmd_decimal();
+			if (ao_cmd_status != ao_cmd_success)
+				return;
+			*((int16_t *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = ao_cmd_lex_i;
+		}
+	}
+	_ao_config_edit_start();
+	ao_config.pyro[p] = pyro_tmp;
+	_ao_config_edit_finish();
+}
+
+void
+ao_pyro_manual(uint8_t p)
+{
+	printf ("ao_pyro_manual %d\n", p);
+	if (p >= AO_PYRO_NUM) {
+		ao_cmd_status = ao_cmd_syntax_error;
+		return;
+	}
+	ao_pyro_pins_fire(1 << p);
+}
+
+void
+ao_pyro_init(void)
+{
+#if AO_PYRO_NUM > 0
+	ao_enable_output(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0, 0);
+#endif
+#if AO_PYRO_NUM > 1
+	ao_enable_output(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, 0);
+#endif
+#if AO_PYRO_NUM > 2
+	ao_enable_output(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, 0);
+#endif
+#if AO_PYRO_NUM > 3
+	ao_enable_output(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, 0);
+#endif
+#if AO_PYRO_NUM > 4
+	ao_enable_output(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, 0);
+#endif
+#if AO_PYRO_NUM > 5
+	ao_enable_output(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, 0);
+#endif
+#if AO_PYRO_NUM > 6
+	ao_enable_output(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, 0);
+#endif
+#if AO_PYRO_NUM > 7
+	ao_enable_output(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, 0);
+#endif
+	ao_add_task(&ao_pyro_task, ao_pyro, "pyro");
+}
+#endif
diff --git a/src/kernel/ao_pyro.h b/src/kernel/ao_pyro.h
new file mode 100644
index 00000000..0c5642d6
--- /dev/null
+++ b/src/kernel/ao_pyro.h
@@ -0,0 +1,83 @@
+/*
+ * 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_PYRO_H_
+#define _AO_PYRO_H_
+
+enum ao_pyro_flag {
+	ao_pyro_none			= 0x00000000,
+
+	ao_pyro_accel_less		= 0x00000001,
+	ao_pyro_accel_greater		= 0x00000002,
+
+	ao_pyro_speed_less		= 0x00000004,
+	ao_pyro_speed_greater		= 0x00000008,
+
+	ao_pyro_height_less		= 0x00000010,
+	ao_pyro_height_greater		= 0x00000020,
+
+	ao_pyro_orient_less		= 0x00000040,
+	ao_pyro_orient_greater		= 0x00000080,
+
+	ao_pyro_time_less		= 0x00000100,
+	ao_pyro_time_greater		= 0x00000200,
+
+	ao_pyro_ascending		= 0x00000400,
+	ao_pyro_descending		= 0x00000800,
+
+	ao_pyro_after_motor		= 0x00001000,
+
+	ao_pyro_delay			= 0x00002000,
+
+	ao_pyro_state_less		= 0x00004000,
+	ao_pyro_state_greater_or_equal  = 0x00008000,
+};
+
+struct ao_pyro {
+	enum ao_pyro_flag	flags;
+	int16_t			accel_less, accel_greater;
+	int16_t			speed_less, speed_greater;
+	int16_t			height_less, height_greater;
+	int16_t			orient_less, orient_greater;
+	int16_t			time_less, time_greater;
+	int16_t			delay;
+	uint8_t			state_less, state_greater_or_equal;
+	int16_t			motor;
+	uint16_t		delay_done;
+	uint8_t			fired;
+};
+
+extern uint8_t	ao_pyro_wakeup;
+
+extern uint16_t	ao_pyro_fired;
+
+void
+ao_pyro_set(void);
+
+void
+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/kernel/ao_quaternion.h b/src/kernel/ao_quaternion.h
new file mode 100644
index 00000000..044f1607
--- /dev/null
+++ b/src/kernel/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/kernel/ao_radio_cmac.c b/src/kernel/ao_radio_cmac.c
new file mode 100644
index 00000000..bff848f6
--- /dev/null
+++ b/src/kernel/ao_radio_cmac.c
@@ -0,0 +1,164 @@
+/*
+ * 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_radio_cmac.h>
+
+static __xdata uint8_t ao_radio_cmac_mutex;
+__pdata int8_t ao_radio_cmac_rssi;
+static __xdata uint8_t cmac_data[AO_CMAC_MAX_LEN + AO_CMAC_KEY_LEN + 2 + AO_CMAC_KEY_LEN];
+
+static uint8_t
+round_len(uint8_t len)
+{
+	uint8_t	rem;
+
+	/* Make sure we transfer at least one packet, and
+	 * then make sure every packet is full. Note that
+	 * there is no length encoded, and that the receiver
+	 * must deal with any extra bytes in the packet
+	 */
+	if (len < AO_CMAC_KEY_LEN)
+		len = AO_CMAC_KEY_LEN;
+	rem = len % AO_CMAC_KEY_LEN;
+	if (rem != 0)
+		len += (AO_CMAC_KEY_LEN - rem);
+	return len;
+}
+
+/*
+ * Sign and deliver the data sitting in the cmac buffer
+ */
+static void
+radio_cmac_send(uint8_t len) __reentrant
+{
+	uint8_t	i;
+
+	len = round_len(len);
+	/* Make sure the AES key is loaded */
+	ao_config_get();
+
+#if HAS_MONITOR
+	ao_monitor_set(0);
+#endif
+
+	ao_mutex_get(&ao_aes_mutex);
+	ao_aes_set_mode(ao_aes_mode_cbc_mac);
+	ao_aes_set_key(ao_config.aes_key);
+	ao_aes_zero_iv();
+	for (i = 0; i < len; i += AO_CMAC_KEY_LEN) {
+		if (i + AO_CMAC_KEY_LEN < len)
+			ao_aes_run(&cmac_data[i], NULL);
+		else
+			ao_aes_run(&cmac_data[i], &cmac_data[len]);
+	}
+	ao_mutex_put(&ao_aes_mutex);
+
+	ao_radio_send(cmac_data, len + AO_CMAC_KEY_LEN);
+}
+
+/*
+ * Receive and validate an incoming packet
+ */
+
+static int8_t
+radio_cmac_recv(uint8_t len, uint16_t timeout) __reentrant
+{
+	uint8_t	i;
+
+	len = round_len(len);
+#if HAS_MONITOR
+	ao_monitor_set(0);
+#endif
+	i = ao_radio_recv(cmac_data, len + AO_CMAC_KEY_LEN + 2, timeout);
+
+	if (!i) {
+		ao_radio_cmac_rssi = 0;
+		return AO_RADIO_CMAC_TIMEOUT;
+	}
+
+	ao_radio_cmac_rssi = ao_radio_rssi;
+	if (!(cmac_data[len + AO_CMAC_KEY_LEN +1] & AO_RADIO_STATUS_CRC_OK))
+		return AO_RADIO_CMAC_CRC_ERROR;
+
+	ao_config_get();
+
+	/* Compute the packet signature
+	 */
+	ao_mutex_get(&ao_aes_mutex);
+	ao_aes_set_mode(ao_aes_mode_cbc_mac);
+	ao_aes_set_key(ao_config.aes_key);
+	ao_aes_zero_iv();
+	for (i = 0; i < len; i += AO_CMAC_KEY_LEN) {
+		if (i + AO_CMAC_KEY_LEN < len)
+			ao_aes_run(&cmac_data[i], NULL);
+		else
+			ao_aes_run(&cmac_data[i], &cmac_data[len + AO_CMAC_KEY_LEN + 2]);
+	}
+	ao_mutex_put(&ao_aes_mutex);
+
+	/* Check the packet signature against the signature provided
+	 * over the link
+	 */
+	 
+	if (memcmp(&cmac_data[len],
+		   &cmac_data[len + AO_CMAC_KEY_LEN + 2],
+		   AO_CMAC_KEY_LEN) != 0) {
+		return AO_RADIO_CMAC_MAC_ERROR;
+	}
+
+	return AO_RADIO_CMAC_OK;
+}
+
+int8_t
+ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant
+{
+	if (len > AO_CMAC_MAX_LEN)
+		return AO_RADIO_CMAC_LEN_ERROR;
+	ao_mutex_get(&ao_radio_cmac_mutex);
+	ao_xmemcpy(cmac_data, packet, len);
+#if AO_LED_TX
+	ao_led_on(AO_LED_TX);
+#endif
+	radio_cmac_send(len);
+#if AO_LED_TX
+	ao_led_off(AO_LED_TX);
+#endif
+	ao_mutex_put(&ao_radio_cmac_mutex);
+	return AO_RADIO_CMAC_OK;
+}
+
+int8_t
+ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant
+{
+	int8_t	i;
+	if (len > AO_CMAC_MAX_LEN)
+		return AO_RADIO_CMAC_LEN_ERROR;
+	ao_mutex_get(&ao_radio_cmac_mutex);
+#if AO_LED_RX
+	ao_led_on(AO_LED_RX);
+#endif
+	i = radio_cmac_recv(len, timeout);
+#if AO_LED_RX
+	ao_led_off(AO_LED_RX);
+#endif
+	if (i == AO_RADIO_CMAC_OK)
+		ao_xmemcpy(packet, cmac_data, len);
+	ao_mutex_put(&ao_radio_cmac_mutex);
+	return i;
+}
+
diff --git a/src/kernel/ao_radio_cmac.h b/src/kernel/ao_radio_cmac.h
new file mode 100644
index 00000000..e86f31e9
--- /dev/null
+++ b/src/kernel/ao_radio_cmac.h
@@ -0,0 +1,43 @@
+/*
+ * 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_RADIO_CMAC_H_
+#define _AO_RADIO_CMAC_H_
+
+#include <ao_aes.h>
+
+#define AO_CMAC_KEY_LEN		AO_AES_LEN
+#define AO_CMAC_MAX_LEN		(128 - AO_CMAC_KEY_LEN)
+
+extern __pdata int8_t ao_radio_cmac_rssi;
+
+int8_t
+ao_radio_cmac_send(__xdata void *packet, uint8_t len) __reentrant;
+
+#define AO_RADIO_CMAC_OK	0
+#define AO_RADIO_CMAC_LEN_ERROR	-1
+#define AO_RADIO_CMAC_CRC_ERROR	-2
+#define AO_RADIO_CMAC_MAC_ERROR	-3
+#define AO_RADIO_CMAC_TIMEOUT	-4
+
+int8_t
+ao_radio_cmac_recv(__xdata void *packet, uint8_t len, uint16_t timeout) __reentrant;
+
+void
+ao_radio_cmac_init(void);
+
+#endif /* _AO_RADIO_CMAC_H_ */
diff --git a/src/kernel/ao_radio_cmac_cmd.c b/src/kernel/ao_radio_cmac_cmd.c
new file mode 100644
index 00000000..64410921
--- /dev/null
+++ b/src/kernel/ao_radio_cmac_cmd.c
@@ -0,0 +1,104 @@
+/*
+ * 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_radio_cmac_cmd.h>
+#include <ao_radio_cmac.h>
+
+static __xdata uint8_t cmac_data[AO_CMAC_MAX_LEN];
+
+static uint8_t
+getnibble(void)
+{
+	int8_t	b;
+
+	b = ao_cmd_hexchar(getchar());
+	if (b < 0) {
+		ao_cmd_status = ao_cmd_lex_error;
+		return 0;
+	}
+	return (uint8_t) b;
+}
+
+static uint8_t
+getbyte(void)
+{
+	uint8_t	b;
+	b = getnibble() << 4;
+	b |= getnibble();
+	return b;
+}
+	
+static void
+radio_cmac_send_cmd(void) __reentrant
+{
+	uint8_t	i;
+	uint8_t	len;
+
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	len = ao_cmd_lex_i;
+	if (len > AO_CMAC_MAX_LEN) {
+		ao_cmd_status = ao_cmd_syntax_error;
+		return;
+	}
+	flush();
+	len = ao_cmd_lex_i;
+	for (i = 0; i < len; i++) {
+		cmac_data[i] = getbyte();
+		if (ao_cmd_status != ao_cmd_success)
+			return;
+	}
+	ao_radio_cmac_send(cmac_data, len);
+}
+
+static void
+radio_cmac_recv_cmd(void) __reentrant
+{
+	uint8_t		len, i;
+	uint16_t	timeout;
+
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	len = ao_cmd_lex_i;
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	timeout = AO_MS_TO_TICKS(ao_cmd_lex_i);
+	i = ao_radio_cmac_recv(cmac_data, len, timeout);
+	if (i == AO_RADIO_CMAC_OK) {
+		printf ("PACKET ");
+		for (i = 0; i < len; i++)
+			printf("%02x", cmac_data[i]);
+		printf (" %d\n", ao_radio_cmac_rssi);
+	} else
+		printf ("ERROR %d %d\n", i, ao_radio_cmac_rssi);
+}
+
+static __code struct ao_cmds ao_radio_cmac_cmds[] = {
+	{ radio_cmac_send_cmd,	"s <length>\0Send AES-CMAC packet. Bytes to send follow on next line" },
+	{ radio_cmac_recv_cmd,	"S <length> <timeout>\0Receive AES-CMAC packet. Timeout in ms" },
+	{ 0, NULL },
+};
+
+void
+ao_radio_cmac_cmd_init(void)
+{
+	ao_cmd_register(&ao_radio_cmac_cmds[0]);
+}
diff --git a/src/kernel/ao_radio_cmac_cmd.h b/src/kernel/ao_radio_cmac_cmd.h
new file mode 100644
index 00000000..6b8782de
--- /dev/null
+++ b/src/kernel/ao_radio_cmac_cmd.h
@@ -0,0 +1,24 @@
+/*
+ * 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_RADIO_CMAC_CMD_H_
+#define _AO_RADIO_CMAC_CMD_H_
+
+void
+ao_radio_cmac_cmd_init(void);
+
+#endif /* _AO_RADIO_CMAC_CMD_H_ */
diff --git a/src/kernel/ao_report.c b/src/kernel/ao_report.c
new file mode 100644
index 00000000..1104cd82
--- /dev/null
+++ b/src/kernel/ao_report.c
@@ -0,0 +1,198 @@
+/*
+ * 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_flight.h>
+#include <ao_sample.h>
+
+#define BIT(i,x)    	   ((x) ? (1 << (i)) : 0)
+#define MORSE1(a)          (1 | BIT(3,a))
+#define MORSE2(a,b)        (2 | BIT(3,a) | BIT(4,b))
+#define MORSE3(a,b,c)      (3 | BIT(3,a) | BIT(4,b) | BIT(5,c))
+#define MORSE4(a,b,c,d)    (4 | BIT(3,a) | BIT(4,b) | BIT(5,c) | BIT(6,d))
+#define MORSE5(a,b,c,d,e)  (5 | BIT(3,a) | BIT(4,b) | BIT(5,c) | BIT(6,d) | BIT(7,e))
+
+static const uint8_t flight_reports[] = {
+	MORSE3(0,0,0),		/* startup, 'S' */
+	MORSE2(0,0),		/* idle 'I' */
+	MORSE4(0,1,1,0),	/* pad 'P' */
+	MORSE4(1,0,0,0),	/* boost 'B' */
+	MORSE4(0,0,1,0),	/* fast 'F' */
+	MORSE4(1,0,1,0),	/* coast 'C' */
+	MORSE3(1,0,0),		/* drogue 'D' */
+	MORSE2(1,1),		/* main 'M' */
+	MORSE4(0,1,0,0),	/* landed 'L' */
+	MORSE4(1,0,0,1),	/* invalid 'X' */
+};
+
+#if HAS_BEEP
+#define low(time)	ao_beep_for(AO_BEEP_LOW, time)
+#define mid(time)	ao_beep_for(AO_BEEP_MID, time)
+#define high(time)	ao_beep_for(AO_BEEP_HIGH, time)
+#else
+#define low(time)	ao_led_for(AO_LED_GREEN, time)
+#define mid(time)	ao_led_for(AO_LED_RED, time)
+#define high(time)	ao_led_for(AO_LED_GREEN|AO_LED_RED, time)
+#endif
+#define pause(time)	ao_delay(time)
+
+static __pdata enum ao_flight_state ao_report_state;
+
+static void
+ao_report_beep(void) __reentrant
+{
+	uint8_t r = flight_reports[ao_flight_state];
+	uint8_t l = r & 7;
+
+	if (!r)
+		return;
+	while (l--) {
+		if (r & 8)
+			mid(AO_MS_TO_TICKS(600));
+		else
+			mid(AO_MS_TO_TICKS(200));
+		pause(AO_MS_TO_TICKS(200));
+		r >>= 1;
+	}
+	pause(AO_MS_TO_TICKS(400));
+}
+
+static void
+ao_report_digit(uint8_t digit) __reentrant
+{
+	if (!digit) {
+		mid(AO_MS_TO_TICKS(500));
+		pause(AO_MS_TO_TICKS(200));
+	} else {
+		while (digit--) {
+			mid(AO_MS_TO_TICKS(200));
+			pause(AO_MS_TO_TICKS(200));
+		}
+	}
+	pause(AO_MS_TO_TICKS(300));
+}
+
+static void
+ao_report_altitude(void)
+{
+	__pdata int16_t	agl = ao_max_height;
+	__xdata uint8_t	digits[10];
+	__pdata uint8_t ndigits, i;
+
+	if (agl < 0)
+		agl = 0;
+	ndigits = 0;
+	do {
+		digits[ndigits++] = agl % 10;
+		agl /= 10;
+	} while (agl);
+
+	i = ndigits;
+	do
+		ao_report_digit(digits[--i]);
+	while (i != 0);
+}
+
+#if HAS_IGNITE_REPORT
+static uint8_t
+ao_report_igniter_ready(enum ao_igniter igniter)
+{
+	return ao_igniter_status(igniter) == ao_igniter_ready ? 1 : 0;
+}
+
+uint8_t
+ao_report_igniter(void)
+{
+	return (ao_report_igniter_ready(ao_igniter_drogue) |
+		     (ao_report_igniter_ready(ao_igniter_main) << 1));
+}
+
+static void
+ao_report_continuity(void) __reentrant
+{
+	uint8_t	c;
+
+#if !HAS_IGNITE
+	if (!ao_igniter_present)
+		return;
+#endif
+	c = ao_report_igniter();
+	if (c) {
+		while (c--) {
+			high(AO_MS_TO_TICKS(25));
+			pause(AO_MS_TO_TICKS(100));
+		}
+	} else {
+		c = 10;
+		while (c--) {
+			high(AO_MS_TO_TICKS(20));
+			low(AO_MS_TO_TICKS(20));
+		}
+	}
+#if HAS_LOG
+	if (ao_log_full()) {
+		pause(AO_MS_TO_TICKS(100));
+		c = 2;
+		while (c--) {
+			low(AO_MS_TO_TICKS(100));
+			mid(AO_MS_TO_TICKS(100));
+			high(AO_MS_TO_TICKS(100));
+			mid(AO_MS_TO_TICKS(100));
+		}
+	}
+#endif
+}
+#endif
+
+void
+ao_report(void)
+{
+	ao_report_state = ao_flight_state;
+	for(;;) {
+		ao_report_beep();
+		if (ao_flight_state == ao_flight_landed) {
+			ao_report_altitude();
+#if HAS_FLIGHT
+			ao_delay(AO_SEC_TO_TICKS(5));
+			continue;
+#endif
+		}
+#if HAS_IGNITE_REPORT
+		if (ao_flight_state == ao_flight_idle)
+			ao_report_continuity();
+		while (ao_flight_state == ao_flight_pad) {
+			uint8_t	c;
+			ao_report_continuity();
+			c = 50;
+			while (c-- && ao_flight_state == ao_flight_pad)
+				pause(AO_MS_TO_TICKS(100));
+		}
+#endif
+
+		while (ao_report_state == ao_flight_state)
+			ao_sleep(DATA_TO_XDATA(&ao_flight_state));
+		ao_report_state = ao_flight_state;
+	}
+}
+
+static __xdata struct ao_task ao_report_task;
+
+void
+ao_report_init(void)
+{
+	ao_add_task(&ao_report_task, ao_report, "report");
+}
diff --git a/src/kernel/ao_report_micro.c b/src/kernel/ao_report_micro.c
new file mode 100644
index 00000000..0e8e287f
--- /dev/null
+++ b/src/kernel/ao_report_micro.c
@@ -0,0 +1,57 @@
+/*
+ * 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>
+
+#define mid(time)	ao_led_for(AO_LED_REPORT, time)
+#define pause(time)	ao_delay(time)
+
+static void
+ao_report_digit(uint8_t digit) __reentrant
+{
+	if (!digit) {
+		mid(AO_MS_TO_TICKS(1000));
+		pause(AO_MS_TO_TICKS(300));
+	} else {
+		while (digit--) {
+			mid(AO_MS_TO_TICKS(300));
+			pause(AO_MS_TO_TICKS(300));
+		}
+	}
+	pause(AO_MS_TO_TICKS(1000));
+}
+
+void
+ao_report_altitude(void)
+{
+	__pdata alt_t	agl = ao_max_height;
+	static __xdata uint8_t	digits[11];
+	__pdata uint8_t ndigits, i;
+
+	if (agl < 0)
+		agl = 0;
+	ndigits = 0;
+	do {
+		digits[ndigits++] = agl % 10;
+		agl /= 10;
+	} while (agl);
+
+	i = ndigits;
+	do
+		ao_report_digit(digits[--i]);
+	while (i != 0);
+}
diff --git a/src/kernel/ao_rssi.c b/src/kernel/ao_rssi.c
new file mode 100644
index 00000000..244a84fe
--- /dev/null
+++ b/src/kernel/ao_rssi.c
@@ -0,0 +1,53 @@
+/*
+ * 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"
+
+static __xdata uint16_t	ao_rssi_time;
+static __pdata uint16_t	ao_rssi_delay;
+static __pdata uint8_t	ao_rssi_led;
+
+void
+ao_rssi(void)
+{
+	for (;;) {
+		while ((int16_t) (ao_time() - ao_rssi_time) > AO_SEC_TO_TICKS(3))
+			ao_sleep(&ao_rssi_time);
+		ao_led_for(ao_rssi_led, AO_MS_TO_TICKS(100));
+		ao_delay(ao_rssi_delay);
+	}
+}
+
+void
+ao_rssi_set(int rssi_value)
+{
+	if (rssi_value > 0)
+		rssi_value = 0;
+	ao_rssi_delay = AO_MS_TO_TICKS((-rssi_value) * 5);
+	ao_rssi_time = ao_time();
+	ao_wakeup(&ao_rssi_time);
+}
+
+__xdata struct ao_task ao_rssi_task;
+
+void
+ao_rssi_init(uint8_t rssi_led)
+{
+	ao_rssi_led = rssi_led;
+	ao_rssi_delay = 0;
+	ao_add_task(&ao_rssi_task, ao_rssi, "rssi");
+}
diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c
new file mode 100644
index 00000000..34658951
--- /dev/null
+++ b/src/kernel/ao_sample.c
@@ -0,0 +1,372 @@
+/*
+ * 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_FLIGHT_TEST
+#include "ao.h"
+#include <ao_data.h>
+#endif
+
+#if HAS_GYRO
+#include <ao_quaternion.h>
+#endif
+
+/*
+ * Current sensor values
+ */
+
+#ifndef PRES_TYPE
+#define PRES_TYPE int32_t
+#define ALT_TYPE int32_t
+#define ACCEL_TYPE int16_t
+#endif
+
+__pdata uint16_t	ao_sample_tick;		/* time of last data */
+__pdata pres_t		ao_sample_pres;
+__pdata alt_t		ao_sample_alt;
+__pdata alt_t		ao_sample_height;
+#if HAS_ACCEL
+__pdata accel_t		ao_sample_accel;
+#endif
+#if HAS_GYRO
+__pdata accel_t		ao_sample_accel_along;
+__pdata accel_t		ao_sample_accel_across;
+__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_orient;
+#endif
+
+__data uint8_t		ao_sample_data;
+
+/*
+ * Sensor calibration values
+ */
+
+__pdata pres_t		ao_ground_pres;		/* startup pressure */
+__pdata alt_t		ao_ground_height;	/* MSL of ao_ground_pres */
+
+#if HAS_ACCEL
+__pdata accel_t		ao_ground_accel;	/* startup acceleration */
+__pdata accel_t		ao_accel_2g;		/* factory accel calibration */
+__pdata int32_t		ao_accel_scale;		/* sensor to m/s² conversion */
+#endif
+
+#if HAS_GYRO
+__pdata accel_t		ao_ground_accel_along;
+__pdata accel_t		ao_ground_accel_across;
+__pdata accel_t		ao_ground_accel_through;
+__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 */
+
+static __pdata uint16_t	nsamples;
+__pdata int32_t ao_sample_pres_sum;
+#if HAS_ACCEL
+__pdata int32_t ao_sample_accel_sum;
+#endif
+#if HAS_GYRO
+__pdata int32_t ao_sample_accel_along_sum;
+__pdata int32_t ao_sample_accel_across_sum;
+__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
+
+#if HAS_FLIGHT_DEBUG
+extern uint8_t ao_orient_test;
+#endif
+
+static void
+ao_sample_preflight_add(void)
+{
+#if HAS_ACCEL
+	ao_sample_accel_sum += ao_sample_accel;
+#endif
+	ao_sample_pres_sum += ao_sample_pres;
+#if HAS_GYRO
+	ao_sample_accel_along_sum += ao_sample_accel_along;
+	ao_sample_accel_across_sum += ao_sample_accel_across;
+	ao_sample_accel_through_sum += ao_sample_accel_through;
+	ao_sample_pitch_sum += ao_sample_pitch;
+	ao_sample_yaw_sum += ao_sample_yaw;
+	ao_sample_roll_sum += ao_sample_roll;
+#endif
+	++nsamples;
+}
+
+static void
+ao_sample_preflight_set(void)
+{
+#if HAS_ACCEL
+	ao_ground_accel = ao_sample_accel_sum >> 9;
+	ao_sample_accel_sum = 0;
+#endif
+	ao_ground_pres = ao_sample_pres_sum >> 9;
+	ao_ground_height = pres_to_altitude(ao_ground_pres);
+	ao_sample_pres_sum = 0;
+#if HAS_GYRO
+	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;
+	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_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);
+#if HAS_FLIGHT_DEBUG
+	if (ao_orient_test)
+		printf("\n\treset\n");
+#endif	
+#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);
+
+#if HAS_FLIGHT_DEBUG
+	if (ao_orient_test) {
+		printf ("rot %d %d %d orient %d     \r",
+			(int) (x * 1000),
+			(int) (y * 1000),
+			(int) (z * 1000),
+			ao_sample_orient);
+	}
+#endif
+
+}
+#endif
+
+static void
+ao_sample_preflight(void)
+{
+	/* startup state:
+	 *
+	 * Collect 512 samples of acceleration and pressure
+	 * data and average them to find the resting values
+	 */
+	if (nsamples < 512) {
+		ao_sample_preflight_add();
+	} else {
+#if HAS_ACCEL
+		ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g;
+		ao_accel_scale = to_fix32(GRAVITY * 2 * 16) / ao_accel_2g;
+#endif
+		ao_sample_preflight_set();
+		ao_preflight = FALSE;
+	}
+}
+
+/*
+ * While in pad mode, constantly update the ground state by
+ * re-averaging the data.  This tracks changes in orientation, which
+ * might be caused by adjustments to the rocket on the pad and
+ * pressure, which might be caused by changes in the weather.
+ */
+
+static void
+ao_sample_preflight_update(void)
+{
+	if (nsamples < 512)
+		ao_sample_preflight_add();
+	else if (nsamples < 1024)
+		++nsamples;
+	else
+		ao_sample_preflight_set();
+}
+
+#if 0
+#if HAS_GYRO
+static int32_t	p_filt;
+static int32_t	y_filt;
+
+static gyro_t inline ao_gyro(void) {
+	gyro_t	p = ao_sample_pitch - ao_ground_pitch;
+	gyro_t	y = ao_sample_yaw - ao_ground_yaw;
+
+	p_filt = p_filt - (p_filt >> 6) + p;
+	y_filt = y_filt - (y_filt >> 6) + y;
+
+	p = p_filt >> 6;
+	y = y_filt >> 6;
+	return ao_sqrt(p*p + y*y);
+}
+#endif
+#endif
+
+uint8_t
+ao_sample(void)
+{
+	ao_wakeup(DATA_TO_XDATA(&ao_sample_data));
+	ao_sleep((void *) DATA_TO_XDATA(&ao_data_head));
+	while (ao_sample_data != ao_data_head) {
+		__xdata struct ao_data *ao_data;
+
+		/* Capture a sample */
+		ao_data = (struct ao_data *) &ao_data_ring[ao_sample_data];
+		ao_sample_tick = ao_data->tick;
+
+#if HAS_BARO
+		ao_data_pres_cook(ao_data);
+		ao_sample_pres = ao_data_pres(ao_data);
+		ao_sample_alt = pres_to_altitude(ao_sample_pres);
+		ao_sample_height = ao_sample_alt - ao_ground_height;
+#endif
+
+#if HAS_ACCEL
+		ao_sample_accel = ao_data_accel_cook(ao_data);
+		if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP)
+			ao_sample_accel = ao_data_accel_invert(ao_sample_accel);
+		ao_data_set_accel(ao_data, ao_sample_accel);
+#endif
+#if HAS_GYRO
+		ao_sample_accel_along = ao_data_along(ao_data);
+		ao_sample_accel_across = ao_data_across(ao_data);
+		ao_sample_accel_through = ao_data_through(ao_data);
+		ao_sample_pitch = ao_data_pitch(ao_data);
+		ao_sample_yaw = ao_data_yaw(ao_data);
+		ao_sample_roll = ao_data_roll(ao_data);
+#endif
+
+		if (ao_preflight)
+			ao_sample_preflight();
+		else {
+			if (ao_flight_state < ao_flight_boost)
+				ao_sample_preflight_update();
+			ao_kalman();
+#if HAS_GYRO
+			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;
+}
+
+void
+ao_sample_init(void)
+{
+	ao_config_get();
+	nsamples = 0;
+	ao_sample_pres_sum = 0;
+	ao_sample_pres = 0;
+#if HAS_ACCEL
+	ao_sample_accel_sum = 0;
+	ao_sample_accel = 0;
+#endif
+#if HAS_GYRO
+	ao_sample_accel_along_sum = 0;
+	ao_sample_accel_across_sum = 0;
+	ao_sample_accel_through_sum = 0;
+	ao_sample_accel_along = 0;
+	ao_sample_accel_across = 0;
+	ao_sample_accel_through = 0;
+	ao_sample_pitch_sum = 0;
+	ao_sample_yaw_sum = 0;
+	ao_sample_roll_sum = 0;
+	ao_sample_pitch = 0;
+	ao_sample_yaw = 0;
+	ao_sample_roll = 0;
+	ao_sample_orient = 0;
+#endif
+	ao_sample_data = ao_data_head;
+	ao_preflight = TRUE;
+}
diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h
new file mode 100644
index 00000000..16d4c507
--- /dev/null
+++ b/src/kernel/ao_sample.h
@@ -0,0 +1,156 @@
+/*
+ * 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_SAMPLE_H_
+#define _AO_SAMPLE_H_
+
+#include <ao_data.h>
+
+/*
+ * ao_sample.c
+ */
+
+/*
+ * Barometer calibration
+ *
+ * We directly sample the barometer. The specs say:
+ *
+ * Pressure range: 15-115 kPa
+ * Voltage at 115kPa: 2.82
+ * Output scale: 27mV/kPa
+ *
+ * If we want to detect launch with the barometer, we need
+ * a large enough bump to not be fooled by noise. At typical
+ * launch elevations (0-2000m), a 200Pa pressure change cooresponds
+ * to about a 20m elevation change. This is 5.4mV, or about 3LSB.
+ * As all of our calculations are done in 16 bits, we'll actually see a change
+ * of 16 times this though
+ *
+ * 27 mV/kPa * 32767 / 3300 counts/mV = 268.1 counts/kPa
+ */
+
+/* Accelerometer calibration
+ *
+ * We're sampling the accelerometer through a resistor divider which
+ * consists of 5k and 10k resistors. This multiplies the values by 2/3.
+ * That goes into the cc1111 A/D converter, which is running at 11 bits
+ * of precision with the bits in the MSB of the 16 bit value. Only positive
+ * values are used, so values should range from 0-32752 for 0-3.3V. The
+ * specs say we should see 40mV/g (uncalibrated), multiply by 2/3 for what
+ * the A/D converter sees (26.67 mV/g). We should see 32752/3300 counts/mV,
+ * for a final computation of:
+ *
+ * 26.67 mV/g * 32767/3300 counts/mV = 264.8 counts/g
+ *
+ * Zero g was measured at 16000 (we would expect 16384).
+ * Note that this value is only require to tell if the
+ * rocket is standing upright. Once that is determined,
+ * the value of the accelerometer is averaged for 100 samples
+ * to find the resting accelerometer value, which is used
+ * for all further flight computations
+ */
+
+/*
+ * Above this height, the baro sensor doesn't work
+ */
+#if HAS_MS5607
+#define AO_MAX_BARO_HEIGHT	30000
+#else
+#define AO_MAX_BARO_HEIGHT	12000
+#endif
+
+/*
+ * Above this speed, baro measurements are unreliable
+ */
+#define AO_MAX_BARO_SPEED	200
+
+#define ACCEL_NOSE_UP	(ao_accel_2g >> 2)
+
+/*
+ * Speed and acceleration are scaled by 16 to provide a bit more
+ * resolution while still having reasonable range. Note that this
+ * limits speed to 2047m/s (around mach 6) and acceleration to
+ * 2047m/s² (over 200g)
+ */
+
+#define AO_M_TO_HEIGHT(m)	((int16_t) (m))
+#define AO_MS_TO_SPEED(ms)	((int16_t) ((ms) * 16))
+#define AO_MSS_TO_ACCEL(mss)	((int16_t) ((mss) * 16))
+
+extern __pdata uint16_t	ao_sample_tick;		/* time of last data */
+extern __data uint8_t	ao_sample_adc;		/* Ring position of last processed sample */
+extern __data uint8_t	ao_sample_data;		/* Ring position of last processed sample */
+
+#if HAS_BARO
+extern __pdata pres_t	ao_sample_pres;		/* most recent pressure sensor reading */
+extern __pdata alt_t	ao_sample_alt;		/* MSL of ao_sample_pres */
+extern __pdata alt_t	ao_sample_height;	/* AGL of ao_sample_pres */
+extern __pdata pres_t	ao_ground_pres;		/* startup pressure */
+extern __pdata alt_t	ao_ground_height;	/* MSL of ao_ground_pres */
+#endif
+
+#if HAS_ACCEL
+extern __pdata accel_t	ao_sample_accel;	/* most recent accel sensor reading */
+extern __pdata accel_t	ao_ground_accel;	/* startup acceleration */
+extern __pdata accel_t 	ao_accel_2g;		/* factory accel calibration */
+extern __pdata int32_t	ao_accel_scale;		/* sensor to m/s² conversion */
+#endif
+#if HAS_GYRO
+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 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);
+
+/* returns FALSE in preflight mode, TRUE in flight mode */
+uint8_t ao_sample(void);
+
+/*
+ * ao_kalman.c
+ */
+
+#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5))
+#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
+#define from_fix(x)	((x) >> 16)
+
+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 __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;
+
+#if HAS_ACCEL
+extern __pdata int16_t			ao_error_a;
+#endif
+
+void ao_kalman(void);
+
+#endif /* _AO_SAMPLE_H_ */
diff --git a/src/kernel/ao_sample_profile.c b/src/kernel/ao_sample_profile.c
new file mode 100644
index 00000000..d3743d12
--- /dev/null
+++ b/src/kernel/ao_sample_profile.c
@@ -0,0 +1,173 @@
+/*
+ * 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_sample_profile.h>
+#include <ao_task.h>
+
+#ifndef AO_SAMPLE_PROFILE_LOW_PC
+#define AO_SAMPLE_PROFILE_LOW_PC	0x08002000
+#endif
+
+#ifndef AO_SAMPLE_PROFILE_HIGH_PC
+#define AO_SAMPLE_PROFILE_HIGH_PC	0x0800f000
+#endif
+
+#ifndef AO_SAMPLE_PROFILE_SHIFT
+#define AO_SAMPLE_PROFILE_SHIFT		6
+#endif
+
+#define AO_SAMPLE_PROFILE_RANGE		(AO_SAMPLE_PROFILE_HIGH_PC - AO_SAMPLE_PROFILE_LOW_PC)
+#define AO_SAMPLE_PROFILE_NUM		(AO_SAMPLE_PROFILE_RANGE >> AO_SAMPLE_PROFILE_SHIFT)
+
+static uint16_t	prev_tick;
+static uint16_t	samples[AO_SAMPLE_PROFILE_NUM];
+static uint8_t missed[AO_SAMPLE_PROFILE_NUM/8];
+static uint16_t max_miss;
+static uint32_t task, isr, os, idle;
+
+extern uint8_t ao_idle_loc;
+
+void
+ao_sample_profile_point(uint32_t pc, uint16_t tick, uint8_t in_isr)
+{
+	uint16_t	delta = tick - prev_tick;
+
+	if (pc < AO_SAMPLE_PROFILE_LOW_PC)
+		return;
+	if (pc >= AO_SAMPLE_PROFILE_HIGH_PC)
+		return;
+	if (ao_cur_task) {
+		uint8_t		*sp;
+		int32_t		sp_delta;
+		
+		asm("mov %0,sp" : "=&r" (sp));
+		sp_delta = sp - (uint8_t *) ao_cur_task->stack;
+		if (-96 < sp_delta && sp_delta < 16)
+			ao_panic(AO_PANIC_STACK);
+	}
+
+	if (in_isr)
+		isr += delta;
+	else if (ao_cur_task) {
+		ao_cur_task->ticks += delta;
+		task += delta;
+	} else if (pc == (uint32_t) &ao_idle_loc)
+		idle += delta;
+	else
+		os += delta;
+
+	pc -= AO_SAMPLE_PROFILE_LOW_PC;
+	pc >>= AO_SAMPLE_PROFILE_SHIFT;
+	samples[pc] += delta;
+
+	if (delta > 1)
+		missed[pc >> 3] |= (1 << (pc & 7));
+	if (delta > max_miss)
+		max_miss = delta;
+	prev_tick = tick;
+}
+
+static void
+ao_sample_profile_start(void)
+{
+	prev_tick = ao_sample_profile_timer_start();
+}
+
+static void
+ao_sample_profile_stop(void)
+{
+	ao_sample_profile_timer_stop();
+}
+
+static void
+ao_sample_profile_dump(void)
+{
+	uint16_t	a;
+	uint8_t		t;
+
+	printf ("task %6d\n", task);
+	printf ("isr  %6d\n", isr);
+	printf ("os   %6d\n", os);
+	printf ("idle %6d\n", idle);
+	printf ("irq blocked %d\n", max_miss);
+	for (t = 0; t < ao_num_tasks; t++)
+		printf ("task %6d %6d %6d %s\n",
+			ao_tasks[t]->ticks,
+			ao_tasks[t]->yields,
+			ao_tasks[t]->max_run,
+			ao_tasks[t]->name);
+	for (a = 0; a < AO_SAMPLE_PROFILE_NUM; a++) {
+		if (samples[a])
+			printf ("%04x %c %u\n",
+				(a << AO_SAMPLE_PROFILE_SHIFT) + AO_SAMPLE_PROFILE_LOW_PC,
+				missed[a >> 3] & (1 << (a & 7)) ? '*' : ' ',
+				samples[a]);
+	}
+}
+
+static void
+ao_sample_profile_clear(void)
+{
+	int t;
+
+	task = isr = os = idle = 0;
+	max_miss = 0;
+	memset(samples, '\0', sizeof (samples));
+	memset(missed, '\0', sizeof (missed));
+	for (t = 0; t < ao_num_tasks; t++) {
+		ao_tasks[t]->ticks = 0;
+		ao_tasks[t]->yields = 0;
+		ao_tasks[t]->max_run = 0;
+	}
+}
+
+static void
+ao_sample_profile_cmd(void)
+{
+	ao_cmd_white();
+	switch (ao_cmd_lex_c) {
+	case '1':
+		ao_sample_profile_start();
+		break;
+	case '0':
+		ao_sample_profile_stop();
+		break;
+	case 'd':
+		ao_sample_profile_dump();
+		break;
+	case 'c':
+		ao_sample_profile_clear();
+		break;
+	default:
+		ao_cmd_status = ao_cmd_syntax_error;
+		break;
+	}
+}
+
+static __code struct ao_cmds ao_sample_profile_cmds[] = {
+	{ ao_sample_profile_cmd,	"S <1 start,0 stop, d dump,c clear>\0Sample profile" },
+	{ 0, NULL }
+};
+
+void
+ao_sample_profile_init(void)
+{
+	ao_sample_profile_timer_init();
+	ao_cmd_register(&ao_sample_profile_cmds[0]);
+	ao_sample_profile_clear();
+}
diff --git a/src/kernel/ao_sample_profile.h b/src/kernel/ao_sample_profile.h
new file mode 100644
index 00000000..dbc29d3d
--- /dev/null
+++ b/src/kernel/ao_sample_profile.h
@@ -0,0 +1,29 @@
+/*
+ * 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_SAMPLE_PROFILE_H_
+#define _AO_SAMPLE_PROFILE_H_
+
+#include <ao_sample_profile_timer.h>
+
+void
+ao_sample_profile_point(uint32_t pc, uint16_t tick, uint8_t in_isr);
+
+void
+ao_sample_profile_init(void);
+
+#endif /* _AO_SAMPLE_PROFILE_H_ */
diff --git a/src/kernel/ao_send_packet.c b/src/kernel/ao_send_packet.c
new file mode 100644
index 00000000..66315d22
--- /dev/null
+++ b/src/kernel/ao_send_packet.c
@@ -0,0 +1,58 @@
+/*
+ * 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"
+
+#define AO_MAX_SEND	128
+
+static __xdata uint8_t ao_send[AO_MAX_SEND];
+
+static void
+ao_send_packet(void)
+{
+	__pdata uint16_t count;
+	uint8_t b;
+	__pdata uint8_t	i;
+
+	ao_cmd_hex();
+	count = ao_cmd_lex_i;
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	if (count > AO_MAX_SEND - 2) {
+		ao_cmd_status = ao_cmd_syntax_error;
+		return;
+	}
+	for (i = 0; i < count; i++) {
+		b = ao_getnibble() << 4;
+		b |= ao_getnibble();
+		if (ao_cmd_status != ao_cmd_success)
+			return;
+		ao_send[i] = b;
+	}
+	ao_radio_send(ao_send, count);
+}
+
+static __code struct ao_cmds ao_send_packet_cmds[] = {
+	{ ao_send_packet, "S <len>\0Send packet. Data on next line" },
+	{ 0, NULL }
+};
+
+void
+ao_send_packet_init(void)
+{
+	ao_cmd_register(&ao_send_packet_cmds[0]);
+}
diff --git a/src/kernel/ao_send_packet.h b/src/kernel/ao_send_packet.h
new file mode 100644
index 00000000..526f7b55
--- /dev/null
+++ b/src/kernel/ao_send_packet.h
@@ -0,0 +1,24 @@
+/*
+ * 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_SEND_PACKET_H_
+#define _AO_SEND_PACKET_H_
+
+void
+ao_send_packet_init(void);
+
+#endif /* _AO_SEND_PACKET_H_ */
diff --git a/src/kernel/ao_serial.h b/src/kernel/ao_serial.h
new file mode 100644
index 00000000..baf213c0
--- /dev/null
+++ b/src/kernel/ao_serial.h
@@ -0,0 +1,110 @@
+/*
+ * 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_SERIAL_H_
+#define _AO_SERIAL_H_
+
+#define AO_SERIAL_SPEED_4800	0
+#define AO_SERIAL_SPEED_9600	1
+#define AO_SERIAL_SPEED_19200	2
+#define AO_SERIAL_SPEED_57600	3
+#define AO_SERIAL_SPEED_115200	4
+
+#if HAS_SERIAL_0
+extern volatile __xdata struct ao_fifo	ao_serial0_rx_fifo;
+extern volatile __xdata struct ao_fifo	ao_serial0_tx_fifo;
+
+char
+ao_serial0_getchar(void);
+
+int
+_ao_serial0_pollchar(void);
+
+void
+ao_serial0_putchar(char c);
+
+void
+ao_serial0_drain(void);
+
+void
+ao_serial0_set_speed(uint8_t speed);
+#endif
+
+#if HAS_SERIAL_1
+extern volatile __xdata struct ao_fifo	ao_serial1_rx_fifo;
+extern volatile __xdata struct ao_fifo	ao_serial1_tx_fifo;
+
+char
+ao_serial1_getchar(void);
+
+int
+_ao_serial1_pollchar(void);
+
+void
+ao_serial1_putchar(char c);
+
+void
+ao_serial1_drain(void);
+
+void
+ao_serial1_set_speed(uint8_t speed);
+#endif
+
+#if HAS_SERIAL_2
+extern volatile __xdata struct ao_fifo	ao_serial2_rx_fifo;
+extern volatile __xdata struct ao_fifo	ao_serial2_tx_fifo;
+
+char
+ao_serial2_getchar(void);
+
+int
+_ao_serial2_pollchar(void);
+
+void
+ao_serial2_putchar(char c);
+
+void
+ao_serial2_drain(void);
+
+void
+ao_serial2_set_speed(uint8_t speed);
+#endif
+
+#if HAS_SERIAL_3
+extern volatile __xdata struct ao_fifo	ao_serial3_rx_fifo;
+extern volatile __xdata struct ao_fifo	ao_serial3_tx_fifo;
+
+char
+ao_serial3_getchar(void);
+
+int
+_ao_serial3_pollchar(void);
+
+void
+ao_serial3_putchar(char c);
+
+void
+ao_serial3_drain(void);
+
+void
+ao_serial3_set_speed(uint8_t speed);
+#endif
+
+void
+ao_serial_init(void);
+
+#endif /* _AO_SERIAL_H_ */
diff --git a/src/kernel/ao_sqrt.c b/src/kernel/ao_sqrt.c
new file mode 100644
index 00000000..3a550eaa
--- /dev/null
+++ b/src/kernel/ao_sqrt.c
@@ -0,0 +1,48 @@
+/*
+ * 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_FLIGHT_TEST
+#include "ao.h"
+#endif
+
+/* Adapted from int_sqrt.c in the linux kernel, which is licensed GPLv2 */
+/**
+ * int_sqrt - rough approximation to sqrt
+ * @x: integer of which to calculate the sqrt
+ *
+ * A very rough approximation to the sqrt() function.
+ */
+
+uint32_t
+ao_sqrt(uint32_t op)
+{
+	uint32_t	res = 0;
+	uint32_t	one = 1UL << (sizeof (one) * 8 - 2);
+
+	while (one > op)
+		one >>= 2;
+
+	while (one != 0) {
+		if (op >= res + one) {
+			op = op - (res + one);
+			res = res +  2 * one;
+		}
+		res /= 2;
+		one /= 4;
+	}
+	return res;
+}
diff --git a/src/kernel/ao_state.c b/src/kernel/ao_state.c
new file mode 100644
index 00000000..ed197aa5
--- /dev/null
+++ b/src/kernel/ao_state.c
@@ -0,0 +1,23 @@
+/*
+ * 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"
+
+const char const * const ao_state_names[] = {
+	"startup", "idle", "pad", "boost", "fast",
+	"coast", "drogue", "main", "landed", "invalid"
+};
diff --git a/src/kernel/ao_stdio.c b/src/kernel/ao_stdio.c
new file mode 100644
index 00000000..99118137
--- /dev/null
+++ b/src/kernel/ao_stdio.c
@@ -0,0 +1,158 @@
+/*
+ * 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"
+
+/*
+ * Basic I/O functions to support SDCC stdio package
+ */
+
+#ifndef USE_SERIAL_0_STDIN
+#define USE_SERIAL_0_STDIN	0
+#endif
+#ifndef USE_SERIAL_1_STDIN
+#define USE_SERIAL_1_STDIN	0
+#endif
+#ifndef USE_SERIAL_2_STDIN
+#define USE_SERIAL_2_STDIN	0
+#endif
+#ifndef USE_SERIAL_3_STDIN
+#define USE_SERIAL_3_STDIN	0
+#endif
+#ifndef USE_SERIAL_4_STDIN
+#define USE_SERIAL_4_STDIN	0
+#endif
+#ifndef USE_SERIAL_5_STDIN
+#define USE_SERIAL_5_STDIN	0
+#endif
+#ifndef USE_SERIAL_6_STDIN
+#define USE_SERIAL_6_STDIN	0
+#endif
+#ifndef USE_SERIAL_7_STDIN
+#define USE_SERIAL_7_STDIN	0
+#endif
+#ifndef USE_SERIAL_8_STDIN
+#define USE_SERIAL_8_STDIN	0
+#endif
+#ifndef USE_SERIAL_9_STDIN
+#define USE_SERIAL_9_STDIN	0
+#endif
+#ifndef PACKET_HAS_SLAVE
+#define PACKET_HAS_SLAVE	0
+#endif
+
+#define USE_SERIAL_STDIN (USE_SERIAL_0_STDIN +	\
+			  USE_SERIAL_1_STDIN +	\
+			  USE_SERIAL_2_STDIN +	\
+			  USE_SERIAL_3_STDIN +	\
+			  USE_SERIAL_4_STDIN +	\
+			  USE_SERIAL_5_STDIN +	\
+			  USE_SERIAL_6_STDIN +	\
+			  USE_SERIAL_7_STDIN +	\
+			  USE_SERIAL_8_STDIN +	\
+			  USE_SERIAL_9_STDIN)
+
+#define AO_NUM_STDIOS	(HAS_USB + PACKET_HAS_SLAVE + USE_SERIAL_STDIN)
+
+__xdata struct ao_stdio ao_stdios[AO_NUM_STDIOS];
+
+#if AO_NUM_STDIOS > 1
+__pdata int8_t ao_cur_stdio;
+__pdata int8_t ao_num_stdios;
+#else
+__pdata int8_t ao_cur_stdio;
+#define ao_cur_stdio	0
+#define ao_num_stdios	0
+#endif
+
+void
+putchar(char c)
+{
+#if LOW_LEVEL_DEBUG
+	if (!ao_cur_task) {
+		extern void ao_debug_out(char c);
+		if (c == '\n')
+			ao_debug_out('\r');
+		ao_debug_out(c);
+		return;
+	}
+#endif
+	if (c == '\n')
+		(*ao_stdios[ao_cur_stdio].putchar)('\r');
+	(*ao_stdios[ao_cur_stdio].putchar)(c);
+}
+
+void
+flush(void)
+{
+	if (ao_stdios[ao_cur_stdio].flush)
+		ao_stdios[ao_cur_stdio].flush();
+}
+
+__xdata uint8_t ao_stdin_ready;
+
+char
+getchar(void) __reentrant
+{
+	int c;
+	int8_t stdio;
+
+	ao_arch_block_interrupts();
+	stdio = ao_cur_stdio;
+	for (;;) {
+		c = ao_stdios[stdio]._pollchar();
+		if (c != AO_READ_AGAIN)
+			break;
+#if AO_NUM_STDIOS > 1
+		if (++stdio == ao_num_stdios)
+			stdio = 0;
+		if (stdio == ao_cur_stdio)
+#endif
+			ao_sleep(&ao_stdin_ready);
+	}
+#if AO_NUM_STDIOS > 1
+	ao_cur_stdio = stdio;
+#endif
+	ao_arch_release_interrupts();
+	return c;
+}
+
+uint8_t
+ao_echo(void)
+{
+	return ao_stdios[ao_cur_stdio].echo;
+}
+
+int8_t
+ao_add_stdio(int (*_pollchar)(void),
+	     void (*putchar)(char),
+	     void (*flush)(void)) __reentrant
+{
+#if AO_NUM_STDIOS > 1
+	if (ao_num_stdios == AO_NUM_STDIOS)
+		ao_panic(AO_PANIC_STDIO);
+#endif
+	ao_stdios[ao_num_stdios]._pollchar = _pollchar;
+	ao_stdios[ao_num_stdios].putchar = putchar;
+	ao_stdios[ao_num_stdios].flush = flush;
+	ao_stdios[ao_num_stdios].echo = 1;
+#if AO_NUM_STDIOS > 1
+	return ao_num_stdios++;
+#else
+	return 0;
+#endif
+}
diff --git a/src/kernel/ao_storage.c b/src/kernel/ao_storage.c
new file mode 100644
index 00000000..6eddae7f
--- /dev/null
+++ b/src/kernel/ao_storage.c
@@ -0,0 +1,186 @@
+/*
+ * 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_storage.h>
+
+uint8_t
+ao_storage_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant
+{
+	uint16_t this_len;
+	uint16_t this_off;
+
+	ao_storage_setup();
+	if (pos >= ao_storage_total || pos + len > ao_storage_total)
+		return 0;
+	while (len) {
+
+		/* Compute portion of transfer within
+		 * a single block
+		 */
+		this_off = (uint16_t) pos & (ao_storage_unit - 1);
+		this_len = ao_storage_unit - this_off;
+		if (this_len > len)
+			this_len = len;
+
+		if (!ao_storage_device_read(pos, buf, this_len))
+			return 0;
+
+		/* See how much is left */
+		buf += this_len;
+		len -= this_len;
+		pos += this_len;
+	}
+	return 1;
+}
+
+uint8_t
+ao_storage_write(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant
+{
+	uint16_t this_len;
+	uint16_t this_off;
+
+	ao_storage_setup();
+	if (pos >= ao_storage_total || pos + len > ao_storage_total)
+		return 0;
+	while (len) {
+
+		/* Compute portion of transfer within
+		 * a single block
+		 */
+		this_off = (uint16_t) pos & (ao_storage_unit - 1);
+		this_len = ao_storage_unit - this_off;
+		if (this_len > len)
+			this_len = len;
+
+		if (!ao_storage_device_write(pos, buf, this_len))
+			return 0;
+
+		/* See how much is left */
+		buf += this_len;
+		len -= this_len;
+		pos += this_len;
+	}
+	return 1;
+}
+
+static __xdata uint8_t storage_data[8];
+
+static void
+ao_storage_dump(void) __reentrant
+{
+	uint8_t i, j;
+
+	ao_cmd_hex();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	for (i = 0; ; i += 8) {
+		if (ao_storage_read(((uint32_t) (ao_cmd_lex_i) << 8) + i,
+				  storage_data,
+				  8)) {
+			ao_cmd_put16((uint16_t) i);
+			for (j = 0; j < 8; j++) {
+				putchar(' ');
+				ao_cmd_put8(storage_data[j]);
+			}
+			putchar ('\n');
+		}
+		if (i == 248)
+			break;
+	}
+}
+
+#if HAS_STORAGE_DEBUG
+
+/* not enough space for this today
+ */
+static void
+ao_storage_store(void) __reentrant
+{
+	uint16_t block;
+	uint8_t i;
+	uint16_t len;
+	static __xdata uint8_t b;
+	uint32_t addr;
+
+	ao_cmd_hex();
+	block = ao_cmd_lex_i;
+	ao_cmd_hex();
+	i = ao_cmd_lex_i;
+	addr = ((uint32_t) block << 8) | i;
+	ao_cmd_hex();
+	len = ao_cmd_lex_i;
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	while (len--) {
+		ao_cmd_hex();
+		if (ao_cmd_status != ao_cmd_success)
+			return;
+		b = ao_cmd_lex_i;
+		ao_storage_write(addr, &b, 1);
+		addr++;
+	}
+}
+#endif
+
+void
+ao_storage_zap(void) __reentrant
+{
+	ao_cmd_hex();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	ao_storage_erase((uint32_t) ao_cmd_lex_i << 8);
+}
+
+void
+ao_storage_zapall(void) __reentrant
+{
+	uint32_t	pos;
+
+	ao_cmd_white();
+	if (!ao_match_word("DoIt"))
+		return;
+	for (pos = 0; pos < ao_storage_log_max; pos += ao_storage_block)
+		ao_storage_erase(pos);
+}
+
+void
+ao_storage_info(void) __reentrant
+{
+	ao_storage_setup();
+	printf("Storage size: %ld\n", (long) ao_storage_total);
+	printf("Storage erase unit: %ld\n", (long) ao_storage_block);
+	ao_storage_device_info();
+}
+
+__code struct ao_cmds ao_storage_cmds[] = {
+	{ ao_storage_info, "f\0Show storage" },
+	{ ao_storage_dump, "e <block>\0Dump flash" },
+#if HAS_STORAGE_DEBUG
+	{ ao_storage_store, "w <block> <start> <len> <data> ...\0Write data to flash" },
+#endif
+	{ ao_storage_zap, "z <block>\0Erase <block>" },
+	{ ao_storage_zapall,"Z <key>\0Erase all. <key> is doit with D&I" },
+	{ 0, NULL },
+};
+
+void
+ao_storage_init(void)
+{
+	ao_storage_device_init();
+	ao_cmd_register(&ao_storage_cmds[0]);
+}
diff --git a/src/kernel/ao_storage.h b/src/kernel/ao_storage.h
new file mode 100644
index 00000000..6cc6fcb7
--- /dev/null
+++ b/src/kernel/ao_storage.h
@@ -0,0 +1,98 @@
+/*
+ * 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_STORAGE_H_
+#define _AO_STORAGE_H_
+
+/*
+ * Storage interface, provided by one of the eeprom or flash
+ * drivers
+ */
+
+#ifndef ao_storage_pos_t
+#define ao_storage_pos_t uint32_t
+#endif
+
+typedef ao_storage_pos_t ao_pos_t;
+
+/* Total bytes of available storage */
+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;
+
+/* Initialize above values. Can only be called once the OS is running */
+void
+ao_storage_setup(void) __reentrant;
+
+/* Write data. Returns 0 on failure, 1 on success */
+uint8_t
+ao_storage_write(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant;
+
+/* Read data. Returns 0 on failure, 1 on success */
+uint8_t
+ao_storage_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant;
+
+/* Erase a block of storage. This always clears ao_storage_block bytes */
+uint8_t
+ao_storage_erase(ao_pos_t pos) __reentrant;
+
+/* Flush any pending writes to stable storage */
+void
+ao_storage_flush(void) __reentrant;
+
+/* Initialize the storage code */
+void
+ao_storage_init(void);
+
+/*
+ * Low-level functions wrapped by ao_storage.c
+ */
+
+/* Read data within a storage unit */
+uint8_t
+ao_storage_device_read(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant;
+
+/* Write data within a storage unit */
+uint8_t
+ao_storage_device_write(ao_pos_t pos, __xdata void *buf, uint16_t len) __reentrant;
+
+/* Initialize low-level device bits */
+void
+ao_storage_device_init(void);
+
+/* Print out information about flash chips */
+void
+ao_storage_device_info(void) __reentrant;
+
+#endif /* _AO_STORAGE_H_ */
diff --git a/src/kernel/ao_task.c b/src/kernel/ao_task.c
new file mode 100644
index 00000000..bafb4943
--- /dev/null
+++ b/src/kernel/ao_task.c
@@ -0,0 +1,548 @@
+/*
+ * 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_task.h>
+#if HAS_SAMPLE_PROFILE
+#include <ao_sample_profile.h>
+#endif
+#if HAS_STACK_GUARD
+#include <ao_mpu.h>
+#endif
+
+#define DEBUG	0
+
+#define AO_NO_TASK_INDEX	0xff
+
+__xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS];
+__data uint8_t ao_num_tasks;
+__xdata struct ao_task *__data ao_cur_task;
+
+#if !HAS_TASK_QUEUE
+static __data uint8_t ao_cur_task_index;
+#endif
+
+#ifdef ao_arch_task_globals
+ao_arch_task_globals
+#endif
+
+#define AO_CHECK_STACK	0
+
+#if AO_CHECK_STACK
+static uint8_t	in_yield;
+
+static inline void ao_check_stack(void) {
+	uint8_t	q;
+	if (!in_yield && ao_cur_task && &q < &ao_cur_task->stack[0])
+		ao_panic(AO_PANIC_STACK);
+}
+#else
+#define ao_check_stack()
+#endif
+
+#if HAS_TASK_QUEUE
+
+#define SLEEP_HASH_SIZE	17
+
+static struct ao_list	run_queue;
+static struct ao_list	alarm_queue;
+static struct ao_list	sleep_queue[SLEEP_HASH_SIZE];
+
+static void
+ao_task_to_run_queue(struct ao_task *task)
+{
+	ao_list_del(&task->queue);
+	ao_list_append(&task->queue, &run_queue);
+}
+
+static struct ao_list *
+ao_task_sleep_queue(void *wchan)
+{
+	return &sleep_queue[(uintptr_t) wchan % SLEEP_HASH_SIZE];
+}
+
+static void
+ao_task_to_sleep_queue(struct ao_task *task, void *wchan)
+{
+	ao_list_del(&task->queue);
+	ao_list_append(&task->queue, ao_task_sleep_queue(wchan));
+}
+
+#if DEBUG
+static void
+ao_task_validate_alarm_queue(void)
+{
+	struct ao_task	*alarm, *prev = NULL;
+	int		i;
+
+	if (ao_list_is_empty(&alarm_queue))
+		return;
+	ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) {
+		if (prev) {
+			if ((int16_t) (alarm->alarm - prev->alarm) < 0) {
+				ao_panic(1);
+			}
+		}
+		prev = alarm;
+	}
+	for (i = 0; i < ao_num_tasks; i++) {
+		alarm = ao_tasks[i];
+		if (alarm->alarm) {
+			if (ao_list_is_empty(&alarm->alarm_queue))
+				ao_panic(2);
+		} else {
+			if (!ao_list_is_empty(&alarm->alarm_queue))
+				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()
+#endif
+
+uint16_t	ao_task_alarm_tick;
+
+static void
+ao_task_to_alarm_queue(struct ao_task *task)
+{
+	struct ao_task	*alarm;
+	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;
+		}
+	}
+	ao_list_append(&task->alarm_queue, &alarm_queue);
+	ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm;
+	ao_task_validate_alarm_queue();
+}
+
+static void
+ao_task_from_alarm_queue(struct ao_task *task)
+{
+	ao_list_del(&task->alarm_queue);
+	if (ao_list_is_empty(&alarm_queue))
+		ao_task_alarm_tick = 0;
+	else
+		ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm;
+	ao_task_validate_alarm_queue();
+}
+
+static void
+ao_task_init_queue(struct ao_task *task)
+{
+	ao_list_init(&task->queue);
+	ao_list_init(&task->alarm_queue);
+}
+
+static void
+ao_task_exit_queue(struct ao_task *task)
+{
+	ao_list_del(&task->queue);
+	ao_list_del(&task->alarm_queue);
+}
+
+void
+ao_task_check_alarm(uint16_t tick)
+{
+	struct ao_task	*alarm, *next;
+
+	ao_list_for_each_entry_safe(alarm, next, &alarm_queue, struct ao_task, alarm_queue) {
+		if ((int16_t) (tick - alarm->alarm) < 0)
+			break;
+		alarm->alarm = 0;
+		ao_task_from_alarm_queue(alarm);
+		ao_task_to_run_queue(alarm);
+	}
+}
+
+void
+ao_task_init(void)
+{
+	uint8_t	i;
+	ao_list_init(&run_queue);
+	ao_list_init(&alarm_queue);
+	ao_task_alarm_tick = 0;
+	for (i = 0; i < SLEEP_HASH_SIZE; i++)
+		ao_list_init(&sleep_queue[i]);
+}
+
+#if DEBUG
+static uint8_t
+ao_task_validate_queue(struct ao_task *task)
+{
+	uint32_t flags;
+	struct ao_task *m;
+	uint8_t ret = 0;
+	struct ao_list *queue;
+
+	flags = ao_arch_irqsave();
+	if (task->wchan) {
+		queue = ao_task_sleep_queue(task->wchan);
+		ret |= 2;
+	} else {
+		queue = &run_queue;
+		ret |= 4;
+	}
+	ao_list_for_each_entry(m, queue, struct ao_task, queue) {
+		if (m == task) {
+			ret |= 1;
+			break;
+		}
+	}
+	ao_arch_irqrestore(flags);
+	return ret;
+}
+
+static uint8_t
+ao_task_validate_alarm(struct ao_task *task)
+{
+	uint32_t	flags;
+	struct ao_task	*m;
+	uint8_t		ret = 0;
+
+	flags = ao_arch_irqsave();
+	if (task->alarm == 0)
+		return 0xff;
+	ao_list_for_each_entry(m, &alarm_queue, struct ao_task, alarm_queue) {
+		if (m == task)
+			ret |= 1;
+		else {
+			if (!(ret&1)) {
+				if ((int16_t) (m->alarm - task->alarm) > 0)
+					ret |= 2;
+			} else {
+				if ((int16_t) (task->alarm - m->alarm) > 0)
+					ret |= 4;
+			}
+		}
+	}
+	ao_arch_irqrestore(flags);
+	return ret;
+}
+
+
+static void
+ao_task_validate(void)
+{
+	uint8_t		i;
+	struct ao_task	*task;
+	uint8_t		ret;
+
+	for (i = 0; i < ao_num_tasks; i++) {
+		task = ao_tasks[i];
+		ret = ao_task_validate_queue(task);
+		if (!(ret & 1)) {
+			if (ret & 2)
+				printf ("sleeping task not on sleep queue %s %08x\n",
+					task->name, task->wchan);
+			else
+				printf ("running task not on run queue %s\n",
+					task->name);
+		}
+		ret = ao_task_validate_alarm(task);
+		if (ret != 0xff) {
+			if (!(ret & 1))
+				printf ("alarm task not on alarm queue %s %d\n",
+					task->name, task->alarm);
+			if (ret & 2)
+				printf ("alarm queue has sooner entries after %s %d\n",
+					task->name, task->alarm);
+			if (ret & 4)
+				printf ("alarm queue has later entries before %s %d\n",
+					task->name, task->alarm);
+		}
+	}
+}
+#endif /* DEBUG */
+
+#endif /* HAS_TASK_QUEUE */
+
+void
+ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *name) __reentrant
+{
+	uint8_t task_id;
+	uint8_t t;
+	if (ao_num_tasks == AO_NUM_TASKS)
+		ao_panic(AO_PANIC_NO_TASK);
+	for (task_id = 1; task_id != 0; task_id++) {
+		for (t = 0; t < ao_num_tasks; t++)
+			if (ao_tasks[t]->task_id == task_id)
+				break;
+		if (t == ao_num_tasks)
+			break;
+	}
+	task->task_id = task_id;
+	task->name = name;
+	task->wchan = NULL;
+	/*
+	 * Construct a stack frame so that it will 'return'
+	 * to the start of the task
+	 */
+	ao_arch_init_stack(task, start);
+	ao_arch_critical(
+#if HAS_TASK_QUEUE
+		ao_task_init_queue(task);
+		ao_task_to_run_queue(task);
+#endif
+		ao_tasks[ao_num_tasks] = task;
+		ao_num_tasks++;
+		);
+}
+
+__data uint8_t	ao_task_minimize_latency;
+
+/* Task switching function. This must not use any stack variables */
+void
+ao_yield(void) ao_arch_naked_define
+{
+	ao_arch_save_regs();
+
+#if HAS_TASK_QUEUE
+	if (ao_cur_task == NULL)
+		ao_cur_task = ao_tasks[ao_num_tasks-1];
+#else
+	if (ao_cur_task_index == AO_NO_TASK_INDEX)
+		ao_cur_task_index = ao_num_tasks-1;
+#endif
+	else
+	{
+#if HAS_SAMPLE_PROFILE
+		uint16_t	tick = ao_sample_profile_timer_value();
+		uint16_t	run = tick - ao_cur_task->start;
+		if (run > ao_cur_task->max_run)
+			ao_cur_task->max_run = run;
+		++ao_cur_task->yields;
+#endif
+		ao_arch_save_stack();
+	}
+
+	ao_arch_isr_stack();
+#if !HAS_TASK_QUEUE
+	if (ao_task_minimize_latency)
+		ao_arch_release_interrupts();
+	else
+#endif
+		ao_arch_block_interrupts();
+
+#if AO_CHECK_STACK
+	in_yield = 1;
+#endif
+	/* Find a task to run. If there isn't any runnable task,
+	 * this loop will run forever, which is just fine
+	 */
+#if HAS_TASK_QUEUE
+	/* If the current task is running, move it to the
+	 * end of the queue to allow other tasks a chance
+	 */
+	if (ao_cur_task->wchan == NULL)
+		ao_task_to_run_queue(ao_cur_task);
+	ao_cur_task = NULL;
+	for (;;) {
+		ao_arch_memory_barrier();
+		if (!ao_list_is_empty(&run_queue))
+			break;
+		/* Wait for interrupts when there's nothing ready */
+		ao_arch_wait_interrupt();
+	}
+	ao_cur_task = ao_list_first_entry(&run_queue, struct ao_task, queue);
+#else
+	{
+		__pdata uint8_t	ao_last_task_index = ao_cur_task_index;
+		for (;;) {
+			++ao_cur_task_index;
+			if (ao_cur_task_index == ao_num_tasks)
+				ao_cur_task_index = 0;
+
+			ao_cur_task = ao_tasks[ao_cur_task_index];
+
+			/* Check for ready task */
+			if (ao_cur_task->wchan == NULL)
+				break;
+
+			/* Check if the alarm is set for a time which has passed */
+			if (ao_cur_task->alarm &&
+			    (int16_t) (ao_time() - ao_cur_task->alarm) >= 0)
+				break;
+
+			/* Wait for interrupts when there's nothing ready */
+			if (ao_cur_task_index == ao_last_task_index && !ao_task_minimize_latency)
+				ao_arch_wait_interrupt();
+		}
+	}
+#endif
+#if HAS_SAMPLE_PROFILE
+	ao_cur_task->start = ao_sample_profile_timer_value();
+#endif
+#if HAS_STACK_GUARD
+	ao_mpu_stack_guard(ao_cur_task->stack);
+#endif
+#if AO_CHECK_STACK
+	in_yield = 0;
+#endif
+	ao_arch_restore_stack();
+}
+
+uint8_t
+ao_sleep(__xdata void *wchan)
+{
+#if HAS_TASK_QUEUE
+	uint32_t flags;
+	flags = ao_arch_irqsave();
+#endif
+	ao_cur_task->wchan = wchan;
+#if HAS_TASK_QUEUE
+	ao_task_to_sleep_queue(ao_cur_task, wchan);
+	ao_arch_irqrestore(flags);
+#endif
+	ao_yield();
+	if (ao_cur_task->wchan) {
+		ao_cur_task->wchan = NULL;
+		ao_cur_task->alarm = 0;
+		return 1;
+	}
+	return 0;
+}
+
+void
+ao_wakeup(__xdata void *wchan) __reentrant
+{
+#if HAS_TASK_QUEUE
+	struct ao_task	*sleep, *next;
+	struct ao_list	*sleep_queue;
+	uint32_t	flags;
+
+	if (ao_num_tasks == 0)
+		return;
+	sleep_queue = ao_task_sleep_queue(wchan);
+	flags = ao_arch_irqsave();
+	ao_list_for_each_entry_safe(sleep, next, sleep_queue, struct ao_task, queue) {
+		if (sleep->wchan == wchan) {
+			sleep->wchan = NULL;
+			ao_task_to_run_queue(sleep);
+		}
+	}
+	ao_arch_irqrestore(flags);
+#else
+	uint8_t	i;
+	for (i = 0; i < ao_num_tasks; i++)
+		if (ao_tasks[i]->wchan == wchan)
+			ao_tasks[i]->wchan = NULL;
+#endif
+	ao_check_stack();
+}
+
+void
+ao_alarm(uint16_t delay)
+{
+#if HAS_TASK_QUEUE
+	uint32_t flags;
+	/* Make sure we sleep *at least* delay ticks, which means adding
+	 * one to account for the fact that we may be close to the next tick
+	 */
+	flags = ao_arch_irqsave();
+#endif
+	if (!(ao_cur_task->alarm = ao_time() + delay + 1))
+		ao_cur_task->alarm = 1;
+#if HAS_TASK_QUEUE
+	ao_task_to_alarm_queue(ao_cur_task);
+	ao_arch_irqrestore(flags);
+#endif
+}
+
+void
+ao_clear_alarm(void)
+{
+#if HAS_TASK_QUEUE
+	uint32_t flags;
+
+	flags = ao_arch_irqsave();
+#endif
+	ao_cur_task->alarm = 0;
+#if HAS_TASK_QUEUE
+	ao_task_from_alarm_queue(ao_cur_task);
+	ao_arch_irqrestore(flags);
+#endif
+}
+
+static __xdata uint8_t ao_forever;
+
+void
+ao_delay(uint16_t ticks)
+{
+	ao_alarm(ticks);
+	ao_sleep(&ao_forever);
+	ao_clear_alarm();
+}
+
+void
+ao_exit(void)
+{
+	uint8_t	i;
+	ao_arch_block_interrupts();
+	ao_num_tasks--;
+#if HAS_TASK_QUEUE
+	for (i = 0; i < ao_num_tasks; i++)
+		if (ao_tasks[i] == ao_cur_task)
+			break;
+	ao_task_exit_queue(ao_cur_task);
+#else
+	i = ao_cur_task_index;
+	ao_cur_task_index = AO_NO_TASK_INDEX;
+#endif
+	for (; i < ao_num_tasks; i++)
+		ao_tasks[i] = ao_tasks[i+1];
+	ao_cur_task = NULL;
+	ao_yield();
+	/* we'll never get back here */
+}
+
+#if HAS_TASK_INFO
+void
+ao_task_info(void)
+{
+	uint8_t		i;
+	__xdata struct ao_task *task;
+
+	for (i = 0; i < ao_num_tasks; i++) {
+		task = ao_tasks[i];
+		printf("%12s: wchan %04x\n",
+		       task->name,
+		       (int) task->wchan);
+	}
+#if HAS_TASK_QUEUE && DEBUG
+	ao_task_validate();
+#endif
+}
+#endif
+
+void
+ao_start_scheduler(void)
+{
+#if !HAS_TASK_QUEUE
+	ao_cur_task_index = AO_NO_TASK_INDEX;
+#endif
+	ao_cur_task = NULL;
+#if HAS_ARCH_START_SCHEDULER
+	ao_arch_start_scheduler();
+#endif
+	ao_yield();
+}
diff --git a/src/kernel/ao_task.h b/src/kernel/ao_task.h
new file mode 100644
index 00000000..9c56b480
--- /dev/null
+++ b/src/kernel/ao_task.h
@@ -0,0 +1,117 @@
+/*
+ * 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_TASK_H_
+#define _AO_TASK_H_
+#if HAS_TASK_QUEUE
+#include <ao_list.h>
+#endif
+
+#ifndef HAS_TASK_INFO
+#define HAS_TASK_INFO 1
+#endif
+
+/* An AltOS task */
+struct ao_task {
+	__xdata void *wchan;		/* current wait channel (NULL if running) */
+	uint16_t alarm;			/* abort ao_sleep time */
+	ao_arch_task_members		/* any architecture-specific fields */
+	uint8_t task_id;		/* unique id */
+	__code char *name;		/* task name */
+#if HAS_TASK_QUEUE
+	struct ao_list	queue;
+	struct ao_list	alarm_queue;
+#endif
+	uint8_t	stack[AO_STACK_SIZE];	/* saved stack */
+#if HAS_SAMPLE_PROFILE
+	uint32_t ticks;
+	uint32_t yields;
+	uint16_t start;
+	uint16_t max_run;
+#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];
+extern __data uint8_t ao_num_tasks;
+extern __xdata struct ao_task *__data ao_cur_task;
+extern __data uint8_t ao_task_minimize_latency;	/* Reduce IRQ latency */
+
+/*
+ ao_task.c
+ */
+
+/* Suspend the current task until wchan is awoken.
+ * returns:
+ *  0 on normal wake
+ *  1 on alarm
+ */
+uint8_t
+ao_sleep(__xdata void *wchan);
+
+/* Wake all tasks sleeping on wchan */
+void
+ao_wakeup(__xdata void *wchan) __reentrant;
+
+/* set an alarm to go off in 'delay' ticks */
+void
+ao_alarm(uint16_t delay);
+
+/* Clear any pending alarm */
+void
+ao_clear_alarm(void);
+
+/* Yield the processor to another task */
+void
+ao_yield(void) ao_arch_naked_declare;
+
+/* Add a task to the run queue */
+void
+ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *name) __reentrant;
+
+#if HAS_TASK_QUEUE
+/* Called on timer interrupt to check alarms */
+extern uint16_t	ao_task_alarm_tick;
+void
+ao_task_check_alarm(uint16_t tick);
+#endif
+
+/* Terminate the current task */
+void
+ao_exit(void);
+
+/* Dump task info to console */
+void
+ao_task_info(void);
+
+/* Start the scheduler. This will not return */
+void
+ao_start_scheduler(void);
+
+#if HAS_TASK_QUEUE
+void
+ao_task_init(void);
+#else
+#define ao_task_init()
+#endif
+
+#endif
diff --git a/src/kernel/ao_telem.h b/src/kernel/ao_telem.h
new file mode 100644
index 00000000..1a8da291
--- /dev/null
+++ b/src/kernel/ao_telem.h
@@ -0,0 +1,172 @@
+/*
+ * 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_TELEM_H_
+#define _AO_TELEM_H_
+
+#define AO_TELEMETRY_VERSION		4
+
+/*
+ * Telemetry version 4 and higher format:
+ *
+ * General header fields
+ *
+ *	Name		Value
+ *
+ *	VERSION		Telemetry version number (4 or more). Must be first.
+ * 	c		Callsign (string, no spaces allowed)
+ *	n		Flight unit serial number (integer)
+ * 	f		Flight number (integer)
+ *	r		Packet RSSI value (integer)
+ * 	s		Flight computer state (string, no spaces allowed)
+ *	t		Flight computer clock (integer in centiseconds)
+ */
+
+#define AO_TELEM_VERSION	"VERSION"
+#define AO_TELEM_CALL		"c"
+#define AO_TELEM_SERIAL		"n"
+#define AO_TELEM_FLIGHT		"f"
+#define AO_TELEM_RSSI		"r"
+#define AO_TELEM_STATE		"s"
+#define AO_TELEM_TICK		"t"
+
+/*
+ * Raw sensor values
+ *
+ *	Name		Value
+ *	r_a		Accelerometer reading (integer)
+ *	r_b		Barometer reading (integer)
+ *	r_t		Thermometer reading (integer)
+ *	r_v		Battery reading (integer)
+ *	r_d		Drogue continuity (integer)
+ *	r_m		Main continuity (integer)
+ */
+
+#define AO_TELEM_RAW_ACCEL	"r_a"
+#define AO_TELEM_RAW_BARO	"r_b"
+#define AO_TELEM_RAW_THERMO	"r_t"
+#define AO_TELEM_RAW_BATT	"r_v"
+#define AO_TELEM_RAW_DROGUE	"r_d"
+#define AO_TELEM_RAW_MAIN	"r_m"
+
+/*
+ * Sensor calibration values
+ *
+ *	Name		Value
+ *	c_a		Ground accelerometer reading (integer)
+ *	c_b		Ground barometer reading (integer)
+ *	c_p		Accelerometer reading for +1g
+ *	c_m		Accelerometer reading for -1g
+ */
+
+#define AO_TELEM_CAL_ACCEL_GROUND	"c_a"
+#define AO_TELEM_CAL_BARO_GROUND	"c_b"
+#define AO_TELEM_CAL_ACCEL_PLUS		"c_p"
+#define AO_TELEM_CAL_ACCEL_MINUS	"c_m"
+
+/*
+ * Kalman state values
+ *
+ *	Name		Value
+ *	k_h		Height above pad (integer, meters)
+ *	k_s		Vertical speeed (integer, m/s * 16)
+ *	k_a		Vertical acceleration (integer, m/s² * 16)
+ */
+
+#define AO_TELEM_KALMAN_HEIGHT		"k_h"
+#define AO_TELEM_KALMAN_SPEED		"k_s"
+#define AO_TELEM_KALMAN_ACCEL		"k_a"
+
+/*
+ * Ad-hoc flight values
+ *
+ *	Name		Value
+ *	a_a		Acceleration (integer, sensor units)
+ *	a_s		Speed (integer, integrated acceleration value)
+ *	a_b		Barometer reading (integer, sensor units)
+ */
+
+#define AO_TELEM_ADHOC_ACCEL		"a_a"
+#define AO_TELEM_ADHOC_SPEED		"a_s"
+#define AO_TELEM_ADHOC_BARO		"a_b"
+
+/*
+ * GPS values
+ *
+ *	Name		Value
+ *	g		GPS state (string):
+ *				l	locked
+ *				u	unlocked
+ *				e	error (missing or broken)
+ *	g_n		Number of sats used in solution
+ *	g_ns		Latitude (degrees * 10e7)
+ *	g_ew		Longitude (degrees * 10e7)
+ *	g_a		Altitude (integer meters)
+ *	g_Y		GPS year (integer)
+ *	g_M		GPS month (integer - 1-12)
+ *	g_D		GPS day (integer - 1-31)
+ *	g_h		GPS hour (integer - 0-23)
+ *	g_m		GPS minute (integer - 0-59)
+ *	g_s		GPS second (integer - 0-59)
+ *	g_v		GPS vertical speed (integer, cm/sec)
+ *	g_g		GPS horizontal speed (integer, cm/sec)
+ *	g_c		GPS course (integer, 0-359)
+ *	g_hd		GPS hdop (integer * 10)
+ *	g_vd		GPS vdop (integer * 10)
+ *	g_he		GPS h error (integer)
+ *	g_ve		GPS v error (integer)
+ */
+
+#define AO_TELEM_GPS_STATE		"g"
+#define AO_TELEM_GPS_STATE_LOCKED	'l'
+#define AO_TELEM_GPS_STATE_UNLOCKED	'u'
+#define AO_TELEM_GPS_STATE_ERROR	'e'
+#define AO_TELEM_GPS_NUM_SAT		"g_n"
+#define AO_TELEM_GPS_LATITUDE		"g_ns"
+#define AO_TELEM_GPS_LONGITUDE		"g_ew"
+#define AO_TELEM_GPS_ALTITUDE		"g_a"
+#define AO_TELEM_GPS_YEAR		"g_Y"
+#define AO_TELEM_GPS_MONTH		"g_M"
+#define AO_TELEM_GPS_DAY		"g_D"
+#define AO_TELEM_GPS_HOUR		"g_h"
+#define AO_TELEM_GPS_MINUTE		"g_m"
+#define AO_TELEM_GPS_SECOND		"g_s"
+#define AO_TELEM_GPS_VERTICAL_SPEED	"g_v"
+#define AO_TELEM_GPS_HORIZONTAL_SPEED	"g_g"
+#define AO_TELEM_GPS_COURSE		"g_c"
+#define AO_TELEM_GPS_HDOP		"g_hd"
+#define AO_TELEM_GPS_VDOP		"g_vd"
+#define AO_TELEM_GPS_HERROR		"g_he"
+#define AO_TELEM_GPS_VERROR		"g_ve"
+
+/*
+ * GPS satellite values
+ *
+ *	Name		Value
+ *	s_n		Number of satellites reported (integer)
+ *	s_v0		Space vehicle ID (integer) for report 0
+ *	s_c0		C/N0 number (integer) for report 0
+ *	s_v1		Space vehicle ID (integer) for report 1
+ *	s_c1		C/N0 number (integer) for report 1
+ *	...
+ */
+
+#define AO_TELEM_SAT_NUM		"s_n"
+#define AO_TELEM_SAT_SVID		"s_v"
+#define AO_TELEM_SAT_C_N_0		"s_c"
+
+#endif /* _AO_TELEM_H_ */
diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c
new file mode 100644
index 00000000..a1c19185
--- /dev/null
+++ b/src/kernel/ao_telemetry.c
@@ -0,0 +1,555 @@
+/*
+ * 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_log.h"
+#include "ao_product.h"
+
+#ifndef HAS_RDF
+#define HAS_RDF 1
+#endif
+
+static __pdata uint16_t ao_telemetry_interval;
+
+#if HAS_RDF
+static __pdata uint8_t ao_rdf = 0;
+static __pdata uint16_t ao_rdf_time;
+#endif
+
+#if HAS_APRS
+static __pdata uint16_t ao_aprs_time;
+
+#include <ao_aprs.h>
+#endif
+
+#if defined(TELEMEGA)
+#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
+
+#if defined(TELEMINI_V_1_0)
+#define AO_TELEMETRY_SENSOR	AO_TELEMETRY_SENSOR_TELEMINI
+#endif
+
+#if defined(TELENANO_V_0_1)
+#define AO_TELEMETRY_SENSOR	AO_TELEMETRY_SENSOR_TELENANO
+#endif
+
+static __xdata union ao_telemetry_all	telemetry;
+
+#if defined AO_TELEMETRY_SENSOR
+/* Send sensor packet */
+static void
+ao_send_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_SENSOR;
+
+	telemetry.sensor.state = ao_flight_state;
+#if HAS_ACCEL
+	telemetry.sensor.accel = packet->adc.accel;
+#else
+	telemetry.sensor.accel = 0;
+#endif
+	telemetry.sensor.pres = ao_data_pres(packet);
+	telemetry.sensor.temp = packet->adc.temp;
+	telemetry.sensor.v_batt = packet->adc.v_batt;
+#if HAS_IGNITE
+	telemetry.sensor.sense_d = packet->adc.sense_d;
+	telemetry.sensor.sense_m = packet->adc.sense_m;
+#else
+	telemetry.sensor.sense_d = 0;
+	telemetry.sensor.sense_m = 0;
+#endif
+
+	telemetry.sensor.acceleration = ao_accel;
+	telemetry.sensor.speed = ao_speed;
+	telemetry.sensor.height = ao_height;
+
+	telemetry.sensor.ground_pres = ao_ground_pres;
+#if HAS_ACCEL
+	telemetry.sensor.ground_accel = ao_ground_accel;
+	telemetry.sensor.accel_plus_g = ao_config.accel_plus_g;
+	telemetry.sensor.accel_minus_g = ao_config.accel_minus_g;
+#else
+	telemetry.sensor.ground_accel = 0;
+	telemetry.sensor.accel_plus_g = 0;
+	telemetry.sensor.accel_minus_g = 0;
+#endif
+
+	ao_radio_send(&telemetry, sizeof (telemetry));
+}
+#endif
+
+
+#ifdef AO_SEND_MEGA
+/* Send mega sensor packet */
+static void
+ao_send_mega_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_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);
+
+#if HAS_MPU6000
+	telemetry.mega_sensor.accel_x = packet->mpu6000.accel_x;
+	telemetry.mega_sensor.accel_y = packet->mpu6000.accel_y;
+	telemetry.mega_sensor.accel_z = packet->mpu6000.accel_z;
+
+	telemetry.mega_sensor.gyro_x = packet->mpu6000.gyro_x;
+	telemetry.mega_sensor.gyro_y = packet->mpu6000.gyro_y;
+	telemetry.mega_sensor.gyro_z = packet->mpu6000.gyro_z;
+#endif
+
+#if HAS_HMC5883
+	telemetry.mega_sensor.mag_x = packet->hmc5883.x;
+	telemetry.mega_sensor.mag_y = packet->hmc5883.y;
+	telemetry.mega_sensor.mag_z = packet->hmc5883.z;
+#endif
+
+	ao_radio_send(&telemetry, sizeof (telemetry));
+}
+
+static __pdata int8_t ao_telemetry_mega_data_max;
+static __pdata int8_t ao_telemetry_mega_data_cur;
+
+/* Send mega data packet */
+static void
+ao_send_mega_data(void)
+{
+	if (--ao_telemetry_mega_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_MEGA_DATA;
+
+		telemetry.mega_data.state = ao_flight_state;
+		telemetry.mega_data.v_batt = packet->adc.v_batt;
+		telemetry.mega_data.v_pyro = packet->adc.v_pbatt;
+
+		/* ADC range is 0-4095, so shift by four to save the high 8 bits */
+		for (i = 0; i < AO_ADC_NUM_SENSE; i++)
+			telemetry.mega_data.sense[i] = packet->adc.sense[i] >> 4;
+
+		telemetry.mega_data.ground_pres = ao_ground_pres;
+		telemetry.mega_data.ground_accel = ao_ground_accel;
+		telemetry.mega_data.accel_plus_g = ao_config.accel_plus_g;
+		telemetry.mega_data.accel_minus_g = ao_config.accel_minus_g;
+
+		telemetry.mega_data.acceleration = ao_accel;
+		telemetry.mega_data.speed = ao_speed;
+		telemetry.mega_data.height = ao_height;
+
+		ao_radio_send(&telemetry, sizeof (telemetry));
+		ao_telemetry_mega_data_cur = ao_telemetry_mega_data_max;
+	}
+}
+#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;
+#if HAS_ACCEL
+	telemetry.metrum_sensor.accel = ao_data_accel(packet);
+#endif
+	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)];
+
+		telemetry.generic.tick = packet->tick;
+		telemetry.generic.type = AO_TELEMETRY_METRUM_DATA;
+
+		telemetry.metrum_data.ground_pres = ao_ground_pres;
+#if HAS_ACCEL
+		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;
+#else
+		telemetry.metrum_data.ground_accel = 1;
+		telemetry.metrum_data.accel_plus_g = 0;
+		telemetry.metrum_data.accel_minus_g = 2;
+#endif
+
+		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;
+
+static void
+ao_send_baro(void)
+{
+	uint8_t		sample = ao_sample_data;
+	uint8_t		samples = (sample - ao_baro_sample) & (AO_DATA_RING - 1);
+
+	if (samples > 12) {
+		ao_baro_sample = (ao_baro_sample + (samples - 12)) & (AO_DATA_RING - 1);
+		samples = 12;
+	}
+	telemetry.generic.tick = ao_data_ring[sample].tick;
+	telemetry.generic.type = AO_TELEMETRY_BARO;
+	telemetry.baro.samples = samples;
+	for (sample = 0; sample < samples; sample++) {
+		telemetry.baro.baro[sample] = ao_data_ring[ao_baro_sample].adc.pres;
+		ao_baro_sample = ao_data_ring_next(ao_baro_sample);
+	}
+	ao_radio_send(&telemetry, sizeof (telemetry));
+}
+#endif
+
+static __pdata int8_t ao_telemetry_config_max;
+static __pdata int8_t ao_telemetry_config_cur;
+
+static void
+ao_send_configuration(void)
+{
+	if (--ao_telemetry_config_cur <= 0)
+	{
+		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;
+		telemetry.configuration.main_deploy = ao_config.main_deploy;
+		telemetry.configuration.flight_log_max = ao_config.flight_log_max >> 10;
+		ao_xmemcpy (telemetry.configuration.callsign,
+			    ao_config.callsign,
+			    AO_MAX_CALLSIGN);
+		ao_xmemcpy (telemetry.configuration.version,
+			    CODE_TO_XDATA(ao_version),
+			    AO_MAX_VERSION);
+		ao_radio_send(&telemetry, sizeof (telemetry));
+		ao_telemetry_config_cur = ao_telemetry_config_max;
+	}
+}
+
+#if HAS_GPS
+
+static __pdata int8_t ao_telemetry_loc_cur;
+static __pdata int8_t ao_telemetry_sat_cur;
+
+static void
+ao_send_location(void)
+{
+	if (--ao_telemetry_loc_cur <= 0)
+	{
+		telemetry.generic.type = AO_TELEMETRY_LOCATION;
+		ao_mutex_get(&ao_gps_mutex);
+		ao_xmemcpy(&telemetry.location.flags,
+		       &ao_gps_data.flags,
+		       26);
+		telemetry.location.tick = ao_gps_tick;
+		ao_mutex_put(&ao_gps_mutex);
+		ao_radio_send(&telemetry, sizeof (telemetry));
+		ao_telemetry_loc_cur = ao_telemetry_config_max;
+	}
+}
+
+static void
+ao_send_satellite(void)
+{
+	if (--ao_telemetry_sat_cur <= 0)
+	{
+		telemetry.generic.type = AO_TELEMETRY_SATELLITE;
+		ao_mutex_get(&ao_gps_mutex);
+		telemetry.satellite.channels = ao_gps_tracking_data.channels;
+		ao_xmemcpy(&telemetry.satellite.sats,
+		       &ao_gps_tracking_data.sats,
+		       AO_MAX_GPS_TRACKING * sizeof (struct ao_telemetry_satellite_info));
+		ao_mutex_put(&ao_gps_mutex);
+		ao_radio_send(&telemetry, sizeof (telemetry));
+		ao_telemetry_sat_cur = ao_telemetry_config_max;
+	}
+}
+#endif
+
+#if HAS_COMPANION
+
+static __pdata int8_t ao_telemetry_companion_max;
+static __pdata int8_t ao_telemetry_companion_cur;
+
+static void
+ao_send_companion(void)
+{
+	if (--ao_telemetry_companion_cur <= 0) {
+		telemetry.generic.type = AO_TELEMETRY_COMPANION;
+		telemetry.companion.board_id = ao_companion_setup.board_id;
+		telemetry.companion.update_period = ao_companion_setup.update_period;
+		telemetry.companion.channels = ao_companion_setup.channels;
+		ao_mutex_get(&ao_companion_mutex);
+		ao_xmemcpy(&telemetry.companion.companion_data,
+		       ao_companion_data,
+		       ao_companion_setup.channels * 2);
+		ao_mutex_put(&ao_companion_mutex);
+		ao_radio_send(&telemetry, sizeof (telemetry));
+		ao_telemetry_companion_cur = ao_telemetry_companion_max;
+	}
+}
+#endif
+
+void
+ao_telemetry(void)
+{
+	uint16_t	time;
+	int16_t		delay;
+
+	ao_config_get();
+	if (!ao_config.radio_enable)
+		ao_exit();
+	while (!ao_flight_number)
+		ao_sleep(&ao_flight_number);
+
+	telemetry.generic.serial = ao_serial_number;
+	for (;;) {
+		while (ao_telemetry_interval == 0)
+			ao_sleep(&telemetry);
+		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
+			{
+#ifdef AO_SEND_ALL_BARO
+				ao_send_baro();
+#endif
+
+#if HAS_FLIGHT
+# ifdef AO_SEND_MEGA
+				ao_send_mega_sensor();
+				ao_send_mega_data();
+# 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 /* HAS_FLIGHT */
+
+#if HAS_COMPANION
+				if (ao_companion_running)
+					ao_send_companion();
+#endif
+				ao_send_configuration();
+#if HAS_GPS
+				ao_send_location();
+				ao_send_satellite();
+#endif
+			}
+#ifndef AO_SEND_ALL_BARO
+#if HAS_RDF
+			if (ao_rdf &&
+#if HAS_APRS
+			    !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) &&
+#endif /* HAS_APRS */
+			    (int16_t) (ao_time() - ao_rdf_time) >= 0)
+			{
+#if HAS_IGNITE_REPORT
+				uint8_t	c;
+#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 /* 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)
+			{
+				ao_aprs_time = ao_time() + AO_SEC_TO_TICKS(ao_config.aprs_interval);
+				ao_aprs_send();
+			}
+#endif /* HAS_APRS */
+#endif /* !AO_SEND_ALL_BARO */
+			time += ao_telemetry_interval;
+			delay = time - ao_time();
+			if (delay > 0) {
+				ao_alarm(delay);
+				ao_sleep(&telemetry);
+				ao_clear_alarm();
+			}
+			else
+				time = ao_time();
+		}
+	}
+}
+
+void
+ao_telemetry_set_interval(uint16_t interval)
+{
+	int8_t	cur = 0;
+	ao_telemetry_interval = interval;
+	
+#if AO_SEND_MEGA
+	if (interval > 1)
+		ao_telemetry_mega_data_max = 1;
+	else
+		ao_telemetry_mega_data_max = 2;
+	if (ao_telemetry_mega_data_max > cur)
+		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)
+		ao_companion_setup.update_period = AO_SEC_TO_TICKS(1);
+	ao_telemetry_companion_max = ao_companion_setup.update_period / interval;
+	if (ao_telemetry_companion_max > cur)
+		cur++;
+	ao_telemetry_companion_cur = cur;
+#endif
+
+	ao_telemetry_config_max = AO_SEC_TO_TICKS(1) / interval;
+#if HAS_COMPANION
+	if (ao_telemetry_config_max > cur)
+		cur++;
+	ao_telemetry_config_cur = cur;
+#endif
+
+#if HAS_GPS
+	if (ao_telemetry_config_max > cur)
+		cur++;
+	ao_telemetry_loc_cur = cur;
+	if (ao_telemetry_config_max > cur)
+		cur++;
+	ao_telemetry_sat_cur = cur;
+#endif
+	ao_wakeup(&telemetry);
+}
+
+#if HAS_RDF
+void
+ao_rdf_set(uint8_t rdf)
+{
+	ao_rdf = rdf;
+	if (rdf == 0)
+		ao_radio_rdf_abort();
+	else {
+		ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS;
+	}
+}
+#endif
+
+__xdata struct ao_task	ao_telemetry_task;
+
+void
+ao_telemetry_init()
+{
+	ao_add_task(&ao_telemetry_task, ao_telemetry, "telemetry");
+}
diff --git a/src/kernel/ao_telemetry.h b/src/kernel/ao_telemetry.h
new file mode 100644
index 00000000..237a35ab
--- /dev/null
+++ b/src/kernel/ao_telemetry.h
@@ -0,0 +1,321 @@
+/*
+ * 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_TELEMETRY_H_
+#define _AO_TELEMETRY_H_
+
+/*
+ * ao_telemetry.c
+ */
+#define AO_MAX_CALLSIGN			8
+#define AO_MAX_VERSION			8
+#if LEGACY_MONITOR
+#define AO_MAX_TELEMETRY		128
+#else
+#define AO_MAX_TELEMETRY		32
+#endif
+
+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;
+};
+
+#define AO_TELEMETRY_SATELLITE_MAX_SAT	12
+
+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[AO_TELEMETRY_SATELLITE_MAX_SAT];	/* 6 */
+	uint8_t					unused[2];	/* 30 */
+	/* 32 */
+};
+
+#define AO_TELEMETRY_COMPANION		0x07
+
+#define AO_COMPANION_MAX_CHANNELS	12
+
+struct ao_telemetry_companion {
+	uint16_t				serial;		/*  0 */
+	uint16_t				tick;		/*  2 */
+	uint8_t					type;		/*  4 */
+	uint8_t					board_id;	/*  5 */
+
+	uint8_t					update_period;	/*  6 */
+	uint8_t					channels;	/*  7 */
+	uint16_t				companion_data[AO_COMPANION_MAX_CHANNELS];	/*  8 */
+	/* 32 */
+};
+	
+#define AO_TELEMETRY_MEGA_SENSOR	0x08
+
+struct ao_telemetry_mega_sensor {
+	uint16_t	serial;		/*  0 */
+	uint16_t	tick;		/*  2 */
+	uint8_t		type;		/*  4 */
+
+	uint8_t		orient;		/*  5 angle from vertical */
+	int16_t		accel;		/*  6 Z axis */
+
+	int32_t		pres;		/*  8 Pa * 10 */
+	int16_t		temp;		/* 12 °C * 100 */
+
+	int16_t		accel_x;	/* 14 */
+	int16_t		accel_y;	/* 16 */
+	int16_t		accel_z;	/* 18 */
+
+	int16_t		gyro_x;		/* 20 */
+	int16_t		gyro_y;		/* 22 */
+	int16_t		gyro_z;		/* 24 */
+
+	int16_t		mag_x;		/* 26 */
+	int16_t		mag_y;		/* 28 */
+	int16_t		mag_z;		/* 30 */
+	/* 32 */
+};
+	
+#define AO_TELEMETRY_MEGA_DATA		0x09
+
+struct ao_telemetry_mega_data {
+	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		v_pyro;		/*  8 pyro battery voltage */
+	int8_t		sense[6];	/* 10 continuity sense */
+
+	int32_t		ground_pres;	/* 16 average pres on pad */
+	int16_t		ground_accel;	/* 20 average accel on pad */
+	int16_t		accel_plus_g;	/* 22 accel calibration at +1g */
+	int16_t		accel_minus_g;	/* 24 accel calibration at -1g */
+
+	int16_t         acceleration;   /* 26 m/s² * 16 */
+	int16_t         speed;          /* 28 m/s * 16 */
+	int16_t         height;         /* 30 m */
+	/* 32 */
+};
+
+
+#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
+
+/*
+ * This packet allows the full sampling rate baro
+ * data to be captured over the RF link so that the
+ * flight software can be tested using 'real' data.
+ *
+ * Along with this telemetry packet, the flight
+ * code is modified to send full-rate telemetry all the time
+ * and never send an RDF tone; this ensure that the full radio
+ * link is available.
+ */
+struct ao_telemetry_baro {
+	uint16_t				serial;		/*  0 */
+	uint16_t				tick;		/*  2 */
+	uint8_t					type;		/*  4 */
+	uint8_t					samples;	/*  5 number samples */
+
+	int16_t					baro[12];	/* 6 samples */
+	/* 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;
+	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;
+};
+
+struct ao_telemetry_all_recv {
+	union ao_telemetry_all		telemetry;
+	int8_t				rssi;
+	uint8_t				status;
+};
+
+#endif /* _AO_TELEMETRY_H_ */
diff --git a/src/kernel/ao_usb.h b/src/kernel/ao_usb.h
new file mode 100644
index 00000000..1ce4f82f
--- /dev/null
+++ b/src/kernel/ao_usb.h
@@ -0,0 +1,142 @@
+/*
+ * 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_USB_H_
+#define _AO_USB_H_
+
+/*
+ * ao_usb.c
+ */
+
+/* Put one character to the USB output queue */
+void
+ao_usb_putchar(char c);
+
+/* Get one character from the USB input queue */
+char
+ao_usb_getchar(void);
+
+/* Poll for a charcter on the USB input queue.
+ * returns AO_READ_AGAIN if none are available
+ */
+int
+ao_usb_pollchar(void);
+
+/* Flush the USB output queue */
+void
+ao_usb_flush(void);
+
+/* Enable the USB controller */
+void
+ao_usb_enable(void);
+
+/* Disable the USB controller */
+void
+ao_usb_disable(void);
+
+/* Initialize the USB system */
+void
+ao_usb_init(void);
+
+extern __code __at (0x00aa) uint8_t ao_usb_descriptors [];
+
+#define AO_USB_SETUP_DIR_MASK	(0x01 << 7)
+#define AO_USB_SETUP_TYPE_MASK	(0x03 << 5)
+#define AO_USB_SETUP_RECIP_MASK	(0x1f)
+
+#define AO_USB_DIR_OUT			0
+#define AO_USB_DIR_IN			(1 << 7)
+
+#define AO_USB_TYPE_STANDARD		0
+#define AO_USB_TYPE_CLASS		(1 << 5)
+#define AO_USB_TYPE_VENDOR		(2 << 5)
+#define AO_USB_TYPE_RESERVED		(3 << 5)
+
+#define AO_USB_RECIP_DEVICE		0
+#define AO_USB_RECIP_INTERFACE		1
+#define AO_USB_RECIP_ENDPOINT		2
+#define AO_USB_RECIP_OTHER		3
+
+/* standard requests */
+#define	AO_USB_REQ_GET_STATUS		0x00
+#define AO_USB_REQ_CLEAR_FEATURE	0x01
+#define AO_USB_REQ_SET_FEATURE		0x03
+#define AO_USB_REQ_SET_ADDRESS		0x05
+#define AO_USB_REQ_GET_DESCRIPTOR	0x06
+#define AO_USB_REQ_SET_DESCRIPTOR	0x07
+#define AO_USB_REQ_GET_CONFIGURATION	0x08
+#define AO_USB_REQ_SET_CONFIGURATION	0x09
+#define AO_USB_REQ_GET_INTERFACE	0x0A
+#define AO_USB_REQ_SET_INTERFACE	0x0B
+#define AO_USB_REQ_SYNCH_FRAME		0x0C
+
+#define AO_USB_DESC_DEVICE		1
+#define AO_USB_DESC_CONFIGURATION	2
+#define AO_USB_DESC_STRING		3
+#define AO_USB_DESC_INTERFACE		4
+#define AO_USB_DESC_ENDPOINT		5
+#define AO_USB_DESC_DEVICE_QUALIFIER	6
+#define AO_USB_DESC_OTHER_SPEED		7
+#define AO_USB_DESC_INTERFACE_POWER	8
+
+#define AO_USB_GET_DESC_TYPE(x)		(((x)>>8)&0xFF)
+#define AO_USB_GET_DESC_INDEX(x)	((x)&0xFF)
+
+#define AO_USB_CONTROL_EP	0
+#define AO_USB_CONTROL_SIZE	32
+
+#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
+ */
+#define AO_USB_IN_SIZE		64
+#define AO_USB_OUT_SIZE		64
+
+#define AO_USB_EP0_IDLE		0
+#define AO_USB_EP0_DATA_IN	1
+#define AO_USB_EP0_DATA_OUT	2
+#define AO_USB_EP0_STALL	3
+
+#define LE_WORD(x)    ((x)&0xFF),((uint8_t) (((uint16_t) (x))>>8))
+
+/* CDC definitions */
+#define AO_USB_CS_INTERFACE      	0x24
+#define AO_USB_CS_ENDPOINT       	0x25
+
+#define AO_USB_SET_LINE_CODING		0x20
+#define AO_USB_GET_LINE_CODING		0x21
+#define AO_USB_SET_CONTROL_LINE_STATE	0x22
+
+/* Data structure for GET_LINE_CODING / SET_LINE_CODING class requests */
+struct ao_usb_line_coding {
+	uint32_t	rate;
+	uint8_t		char_format;
+	uint8_t		parity;
+	uint8_t		data_bits;
+} ;
+
+extern __pdata uint8_t ao_usb_running;
+
+#endif /* _AO_USB_H_ */