Merge branch 'new-debian' into debian

Conflicts:
	ChangeLog
	debian/altos.install
	debian/changelog
	debian/control
	debian/copyright
	debian/dirs
	debian/docs
	debian/menu
	debian/rules
	src/Makefile

67 files changed, 10755 insertions, 0 deletions

diff --git a/src/core/altitude.h b/src/core/altitude.h
new file mode 100644
index 00000000..a278bbc6
--- /dev/null
+++ b/src/core/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/core/ao.h b/src/core/ao.h
new file mode 100644
index 00000000..31ec4686
--- /dev/null
+++ b/src/core/ao.h
@@ -0,0 +1,992 @@
+/*
+ * 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
+
+/* 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 */
+	uint8_t	stack[AO_STACK_SIZE];	/* saved stack */
+};
+
+extern __xdata struct ao_task *__data ao_cur_task;
+
+#define AO_NUM_TASKS		16	/* maximum number of tasks */
+#define AO_NO_TASK		0	/* no task id */
+
+/*
+ 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);
+
+/* 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;
+
+/* 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);
+
+/*
+ * 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_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 */
+#define AO_HERTZ		100
+#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 */
+uint16_t
+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) __critical;
+
+/* 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
+ */
+
+void
+ao_mutex_get(__xdata uint8_t *ao_mutex) __reentrant;
+
+void
+ao_mutex_put(__xdata uint8_t *ao_mutex) __reentrant;
+
+/*
+ * 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_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);
+
+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
+ */
+
+int32_t
+ao_pa_to_altitude(int32_t pa);
+
+int32_t
+ao_altitude_to_pa(int32_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_spi_slave.c
+ */
+
+uint8_t
+ao_spi_slave_recv(uint8_t *buf, uint8_t len);
+
+void
+ao_spi_slave_send(uint8_t *buf, uint8_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)
+
+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_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;
+
+#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
+
+void
+ao_radio_general_isr(void) ao_arch_interrupt(16);
+
+void
+ao_radio_send(const __xdata void *d, uint8_t size) __reentrant;
+
+uint8_t
+ao_radio_recv(__xdata void *d, uint8_t size) __reentrant;
+
+void
+ao_radio_recv_abort(void);
+
+void
+ao_radio_test(uint8_t on);
+
+/*
+ * 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;
+	struct ao_telemetry_orig_recv	orig;
+	struct ao_telemetry_tiny_recv	tiny;
+};
+
+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	((char) -1)
+
+struct ao_stdio {
+	char	(*pollchar)(void);
+	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(char (*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 __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	12
+
+#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
+};
+
+#define AO_IGNITE_MODE_DUAL		0
+#define AO_IGNITE_MODE_APOGEE		1
+#define AO_IGNITE_MODE_MAIN		2
+
+#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/core/ao_adc.h b/src/core/ao_adc.h
new file mode 100644
index 00000000..0dd87080
--- /dev/null
+++ b/src/core/ao_adc.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_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);
+
+/* Get a copy of the last complete sample set */
+void
+ao_data_get(__xdata struct ao_data *packet);
+
+/* Initialize the A/D converter */
+void
+ao_adc_init(void);
+
+#endif /* _AO_ADC_H_ */
diff --git a/src/core/ao_aes.h b/src/core/ao_aes.h
new file mode 100644
index 00000000..c47bc2db
--- /dev/null
+++ b/src/core/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/core/ao_beep.h b/src/core/ao_beep.h
new file mode 100644
index 00000000..55f61171
--- /dev/null
+++ b/src/core/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/core/ao_btm.h b/src/core/ao_btm.h
new file mode 100644
index 00000000..484e5d7f
--- /dev/null
+++ b/src/core/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/core/ao_cmd.c b/src/core/ao_cmd.c
new file mode 100644
index 00000000..1814cecf
--- /dev/null
+++ b/src/core/ao_cmd.c
@@ -0,0 +1,360 @@
+/*
+ * 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"
+
+__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;
+
+static void
+put_string(__code char *s)
+{
+	char	c;
+	while ((c = *s++))
+		putchar(c);
+}
+
+static void
+backspace(void)
+{
+	put_string ("\010 \010");
+}
+
+static void
+readline(void)
+{
+	char c;
+	if (ao_echo())
+		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));
+}
+
+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)
+{
+	__pdata 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);
+}
+
+static void
+version(void)
+{
+	printf("manufacturer     %s\n", ao_manufacturer);
+	printf("product          %s\n", ao_product);
+	printf("serial-number    %u\n", ao_serial_number);
+#if HAS_LOG
+	printf("log-format       %u\n", ao_log_format);
+#endif
+#if HAS_MS5607
+	ao_ms5607_info();
+#endif
+	printf("software-version %s\n", ao_version);
+}
+
+#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;
+	const char *h;
+
+	for (cmds = 0; cmds < ao_ncmds; cmds++) {
+		cs = ao_cmds[cmds];
+		for (cmd = 0; cs[cmd].func; cmd++) {
+			h = cs[cmd].help;
+			printf("%-45s %s\n", h, h + 1 + strlen(h));
+		}
+	}
+}
+
+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();
+	}
+}
+
+__xdata struct ao_task ao_cmd_task;
+
+__code struct ao_cmds	ao_base_cmds[] = {
+	{ help,		"?\0Help" },
+	{ ao_task_info,	"T\0Tasks" },
+	{ echo,		"E <0 off, 1 on>\0Echo" },
+	{ ao_reboot,	"r eboot\0Reboot" },
+	{ version,	"v\0Version" },
+	{ 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/core/ao_companion.h b/src/core/ao_companion.h
new file mode 100644
index 00000000..035325a3
--- /dev/null
+++ b/src/core/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/core/ao_config.c b/src/core/ao_config.c
new file mode 100644
index 00000000..ce855ad1
--- /dev/null
+++ b/src/core/ao_config.c
@@ -0,0 +1,635 @@
+/*
+ * 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_storage.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;
+
+#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
+
+#if HAS_EEPROM
+static void
+_ao_config_put(void)
+{
+	ao_storage_setup();
+	ao_storage_erase(ao_storage_config);
+	ao_storage_write(ao_storage_config, &ao_config, sizeof (ao_config));
+#if HAS_FLIGHT
+	ao_log_write_erase(0);
+#endif
+	ao_storage_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_storage_setup();
+	ao_storage_read(ao_storage_config, &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);
+	}
+	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 (minor < 4)
+			ao_config.flight_log_max = AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX;
+		/* 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 = TRUE;
+		if (minor < 9)
+			ao_xmemset(&ao_config.aes_key, '\0', AO_AES_LEN);
+		if (minor < 10)
+			ao_config.frequency = 434550;
+		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
+		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;
+#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();
+	ao_radio_recv_abort();
+}
+#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);
+}
+
+#define ACCEL_CALIBRATE_SAMPLES	1024
+#define ACCEL_CALIBRATE_SHIFT	10
+
+static int16_t
+ao_config_accel_calibrate_auto(char *orientation) __reentrant
+{
+	uint16_t	i;
+	int32_t		accel_total;
+	uint8_t		cal_data_ring;
+
+	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]);
+			cal_data_ring = ao_data_ring_next(cal_data_ring);
+			i--;
+		}
+	}
+	return accel_total >> ACCEL_CALIBRATE_SHIFT;
+}
+
+void
+ao_config_accel_calibrate_set(void) __reentrant
+{
+	int16_t	up, down;
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	if (ao_cmd_lex_i == 0) {
+		up = ao_config_accel_calibrate_auto("up");
+		down = ao_config_accel_calibrate_auto("down");
+	} 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;
+	_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	config = (uint16_t) (ao_storage_config >> 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 > config)
+		printf("Flight log max %d kB\n", config);
+	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);
+}
+
+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) {
+		uint16_t t;
+		t = ao_config.accel_plus_g;
+		ao_config.accel_plus_g = 0x7fff - ao_config.accel_minus_g;
+		ao_config.accel_minus_g = 0x7fff - 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
+
+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;
+
+static void
+ao_config_write(void) __reentrant;
+
+__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 },
+#endif /* HAS_RADIO */
+#if HAS_ACCEL
+	{ "a <+g> <-g>\0Accel calib (0 for auto)",
+	  ao_config_accel_calibrate_set,ao_config_accel_calibrate_show },
+#endif /* HAS_ACCEL */
+#if HAS_RADIO
+	{ "f <cal>\0Radio calib (cal = rf/(xtal/2^16))",
+	  ao_config_radio_cal_set,  	ao_config_radio_cal_show },
+#endif /* HAS_RADIO */
+#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_ACCEL
+	{ "o <0 antenna up, 1 antenna down>\0Set pad orientation",
+	  ao_config_pad_orientation_set,ao_config_pad_orientation_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
+	{ "s\0Show",
+	  ao_config_show,		0 },
+#if HAS_EEPROM
+	{ "w\0Write to eeprom",
+	  ao_config_write,		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_EEPROM
+static void
+ao_config_write(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/core/ao_convert.c b/src/core/ao_convert.c
new file mode 100644
index 00000000..aa9b5f48
--- /dev/null
+++ b/src/core/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/core/ao_convert_pa.c b/src/core/ao_convert_pa.c
new file mode 100644
index 00000000..0c93caea
--- /dev/null
+++ b/src/core/ao_convert_pa.c
@@ -0,0 +1,72 @@
+/*
+ * 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
+
+static const int32_t altitude_table[] = {
+#include "altitude-pa.h"
+};
+
+#define ALT_SCALE	(1 << ALT_SHIFT)
+#define ALT_MASK	(ALT_SCALE - 1)
+
+int32_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 > 120000)
+		pa = 120000;
+	o = pa >> ALT_SHIFT;
+	part = pa & ALT_MASK;
+
+	low = (int32_t) altitude_table[o] * (ALT_SCALE - part);
+	high = (int32_t) altitude_table[o+1] * part + (ALT_SCALE >> 1);
+	return (low + high) >> ALT_SHIFT;
+}
+
+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;
+}
diff --git a/src/core/ao_convert_pa_test.c b/src/core/ao_convert_pa_test.c
new file mode 100644
index 00000000..972a4d4c
--- /dev/null
+++ b/src/core/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
+#include "ao_host.h"
+#include "ao_convert_pa.c"
+
+#define STEP_P	1
+#define STEP_A	1
+
+static inline i_abs(int i) { return i < 0 ? -i : i; }
+
+main ()
+{
+	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 < 74250 + STEP_A; i += STEP_A) {
+		if (i > 74250)
+			i = 74250;
+		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/core/ao_convert_test.c b/src/core/ao_convert_test.c
new file mode 100644
index 00000000..87e76841
--- /dev/null
+++ b/src/core/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/core/ao_data.h b/src/core/ao_data.h
new file mode 100644
index 00000000..2b9ef5ac
--- /dev/null
+++ b/src/core/ao_data.h
@@ -0,0 +1,282 @@
+/*
+ * 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_
+
+#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
+
+#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;
+#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))
+
+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)	do {					\
+		if ((ao_data_present |= (bit)) == AO_DATA_ALL) {	\
+			ao_data_ring[ao_data_head].tick = ao_tick_count; \
+			ao_data_head = ao_data_ring_next(ao_data_head); \
+			ao_data_present = 0;				\
+			ao_wakeup((void *) &ao_data_head);		\
+		}							\
+	} while (0);
+
+/*
+ * Wait until it is time to write a sensor sample; this is
+ * signaled by the timer tick
+ */
+#define AO_DATA_WAIT() do {				\
+		ao_sleep((void *) &ao_data_count);	\
+	} while (0)
+
+#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;
+typedef int32_t 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
+
+/*
+ * 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_data_accel(packet)			((packet)->mma655x)
+#define ao_data_accel_cook(packet)		((packet)->mma655x)
+#define ao_data_set_accel(packet, accel)	((packet)->mma655x = (accel))
+#define ao_data_accel_invert(accel)		(4095 - (accel))
+
+#endif
+
+#if !HAS_ACCEL && HAS_MPU6000
+
+#define HAS_ACCEL	1
+
+typedef int16_t accel_t;
+
+/* MPU6000 is hooked up so that positive y is positive acceleration */
+#define ao_data_accel(packet)			((packet)->mpu6000.accel_y)
+#define ao_data_accel_cook(packet)		(-(packet)->mpu6000.accel_y)
+#define ao_data_set_accel(packet, accel)	((packet)->mpu6000.accel_y = (accel))
+#define ao_data_accel_invert(a)			(-(a))
+
+#endif
+
+#endif /* _AO_DATA_H_ */
diff --git a/src/core/ao_dbg.h b/src/core/ao_dbg.h
new file mode 100644
index 00000000..181e6ec2
--- /dev/null
+++ b/src/core/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/core/ao_ee_fake.c b/src/core/ao_ee_fake.c
new file mode 100644
index 00000000..7fcfcab0
--- /dev/null
+++ b/src/core/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/core/ao_fec.h b/src/core/ao_fec.h
new file mode 100644
index 00000000..eedea8f4
--- /dev/null
+++ b/src/core/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 uint16_t inline
+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/core/ao_fec_rx.c b/src/core/ao_fec_rx.c
new file mode 100644
index 00000000..072a9e90
--- /dev/null
+++ b/src/core/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 MEGAMETRUM
+#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/core/ao_fec_tx.c b/src/core/ao_fec_tx.c
new file mode 100644
index 00000000..4941d745
--- /dev/null
+++ b/src/core/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/core/ao_flight.c b/src/core/ao_flight.c
new file mode 100644
index 00000000..aa4f6961
--- /dev/null
+++ b/src/core/ao_flight.c
@@ -0,0 +1,423 @@
+/*
+ * 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
+
+#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 */
+
+/*
+ * 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)
+			{
+				/* 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_RADIO && !HAS_FLIGHT_DEBUG
+				/* Disable the USB controller in flight mode
+				 * to save power
+				 */
+				ao_usb_disable();
+#endif
+
+#if !HAS_ACCEL
+				/* 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
+				/* signal successful initialization by turning off the LED */
+				ao_led_off(AO_LED_RED);
+			} else {
+				/* Set idle mode */
+ 				ao_flight_state = ao_flight_idle;
+ 
+#if HAS_ACCEL && HAS_RADIO && PACKET_HAS_SLAVE
+				/* Turn on packet system in idle mode on TeleMetrum */
+				ao_packet_slave_start();
+#endif
+
+				/* 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 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_wakeup(&ao_gps_data);
+				ao_wakeup(&ao_gps_tracking_data);
+#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;
+		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_ground_accel - 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);
+}
+
+__code struct ao_cmds ao_flight_cmds[] = {
+	{ ao_flight_dump, 	"F\0Dump flight status" },
+	{ 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/core/ao_flight.h b/src/core/ao_flight.h
new file mode 100644
index 00000000..b80202f0
--- /dev/null
+++ b/src/core/ao_flight.h
@@ -0,0 +1,61 @@
+/*
+ * 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
+};
+
+extern __pdata enum ao_flight_state	ao_flight_state;
+extern __pdata uint16_t			ao_boost_tick;
+extern __pdata uint16_t			ao_motor_number;
+
+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/core/ao_flight_nano.c b/src/core/ao_flight_nano.c
new file mode 100644
index 00000000..406d81ad
--- /dev/null
+++ b/src/core/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/core/ao_freq.c b/src/core/ao_freq.c
new file mode 100644
index 00000000..12496f6f
--- /dev/null
+++ b/src/core/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/core/ao_gps_print.c b/src/core/ao_gps_print.c
new file mode 100644
index 00000000..47c945d7
--- /dev/null
+++ b/src/core/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/core/ao_gps_report.c b/src/core/ao_gps_report.c
new file mode 100644
index 00000000..c52ef621
--- /dev/null
+++ b/src/core/ao_gps_report.c
@@ -0,0 +1,99 @@
+/*
+ * 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;
+	uint8_t	date_reported = 0;
+
+	for (;;) {
+		ao_sleep(&ao_gps_data);
+		ao_mutex_get(&ao_gps_mutex);
+		ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
+		ao_mutex_put(&ao_gps_mutex);
+
+		if (!(gps_data.flags & AO_GPS_VALID))
+			continue;
+
+		gps_log.tick = ao_gps_tick;
+		gps_log.type = AO_LOG_GPS_TIME;
+		gps_log.u.gps_time.hour = gps_data.hour;
+		gps_log.u.gps_time.minute = gps_data.minute;
+		gps_log.u.gps_time.second = gps_data.second;
+		gps_log.u.gps_time.flags = gps_data.flags;
+		ao_log_data(&gps_log);
+		gps_log.type = AO_LOG_GPS_LAT;
+		gps_log.u.gps_latitude = gps_data.latitude;
+		ao_log_data(&gps_log);
+		gps_log.type = AO_LOG_GPS_LON;
+		gps_log.u.gps_longitude = gps_data.longitude;
+		ao_log_data(&gps_log);
+		gps_log.type = AO_LOG_GPS_ALT;
+		gps_log.u.gps_altitude.altitude = gps_data.altitude;
+		gps_log.u.gps_altitude.unused = 0xffff;
+		ao_log_data(&gps_log);
+		if (!date_reported && (gps_data.flags & AO_GPS_DATE_VALID)) {
+			gps_log.type = AO_LOG_GPS_DATE;
+			gps_log.u.gps_date.year = gps_data.year;
+			gps_log.u.gps_date.month = gps_data.month;
+			gps_log.u.gps_date.day = gps_data.day;
+			gps_log.u.gps_date.extra = 0;
+			date_reported = ao_log_data(&gps_log);
+		}
+	}
+}
+
+void
+ao_gps_tracking_report(void)
+{
+	static __xdata struct ao_log_record		gps_log;
+	static __xdata struct ao_telemetry_satellite	gps_tracking_data;
+	uint8_t	c, n;
+
+	for (;;) {
+		ao_sleep(&ao_gps_tracking_data);
+		ao_mutex_get(&ao_gps_mutex);
+		gps_log.tick = ao_gps_tick;
+		ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
+		ao_mutex_put(&ao_gps_mutex);
+
+		if (!(n = gps_tracking_data.channels))
+			continue;
+
+		gps_log.type = AO_LOG_GPS_SAT;
+		for (c = 0; c < n; c++)
+			if ((gps_log.u.gps_sat.svid = gps_tracking_data.sats[c].svid))
+			{
+				gps_log.u.gps_sat.c_n = gps_tracking_data.sats[c].c_n_1;
+				ao_log_data(&gps_log);
+			}
+	}
+}
+
+__xdata struct ao_task ao_gps_report_task;
+__xdata struct ao_task ao_gps_tracking_report_task;
+
+void
+ao_gps_report_init(void)
+{
+	ao_add_task(&ao_gps_report_task, ao_gps_report, "gps_report");
+	ao_add_task(&ao_gps_tracking_report_task, ao_gps_tracking_report, "gps_tracking_report");
+}
diff --git a/src/core/ao_gps_report_mega.c b/src/core/ao_gps_report_mega.c
new file mode 100644
index 00000000..47891cab
--- /dev/null
+++ b/src/core/ao_gps_report_mega.c
@@ -0,0 +1,95 @@
+/*
+ * 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_mega(void)
+{
+	static __xdata struct ao_log_mega		gps_log;
+	static __xdata struct ao_telemetry_location	gps_data;
+	uint8_t	date_reported = 0;
+
+	for (;;) {
+		ao_sleep(&ao_gps_data);
+		ao_mutex_get(&ao_gps_mutex);
+		ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
+		ao_mutex_put(&ao_gps_mutex);
+
+		if (!(gps_data.flags & AO_GPS_VALID))
+			continue;
+
+		gps_log.tick = ao_gps_tick;
+		gps_log.type = AO_LOG_GPS_TIME;
+		gps_log.u.gps.latitude = gps_data.latitude;
+		gps_log.u.gps.longitude = gps_data.longitude;
+		gps_log.u.gps.altitude = gps_data.altitude;
+
+		gps_log.u.gps.hour = gps_data.hour;
+		gps_log.u.gps.minute = gps_data.minute;
+		gps_log.u.gps.second = gps_data.second;
+		gps_log.u.gps.flags = gps_data.flags;
+		gps_log.u.gps.year = gps_data.year;
+		gps_log.u.gps.month = gps_data.month;
+		gps_log.u.gps.day = gps_data.day;
+		ao_log_mega(&gps_log);
+	}
+}
+
+void
+ao_gps_tracking_report_mega(void)
+{
+	static __xdata struct ao_log_mega		gps_log;
+	static __xdata struct ao_telemetry_satellite	gps_tracking_data;
+	uint8_t	c, n, i;
+
+	for (;;) {
+		ao_sleep(&ao_gps_tracking_data);
+		ao_mutex_get(&ao_gps_mutex);
+		ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
+		ao_mutex_put(&ao_gps_mutex);
+
+		if (!(n = gps_tracking_data.channels))
+			continue;
+
+		gps_log.type = AO_LOG_GPS_SAT;
+		gps_log.tick = ao_gps_tick;
+		i = 0;
+		for (c = 0; c < n; c++)
+			if ((gps_log.u.gps_sat.sats[i].svid = gps_tracking_data.sats[c].svid))
+			{
+				gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1;
+				i++;
+			}
+		gps_log.u.gps_sat.channels = i;
+		ao_log_mega(&gps_log);
+	}
+}
+
+__xdata struct ao_task ao_gps_report_mega_task;
+__xdata struct ao_task ao_gps_tracking_report_mega_task;
+
+void
+ao_gps_report_mega_init(void)
+{
+	ao_add_task(&ao_gps_report_mega_task,
+		    ao_gps_report_mega,
+		    "gps_report");
+	ao_add_task(&ao_gps_tracking_report_mega_task,
+		    ao_gps_tracking_report_mega,
+		    "gps_tracking_report");
+}
diff --git a/src/core/ao_host.h b/src/core/ao_host.h
new file mode 100644
index 00000000..6eb752c9
--- /dev/null
+++ b/src/core/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/core/ao_ignite.c b/src/core/ao_ignite.c
new file mode 100644
index 00000000..c7829fc3
--- /dev/null
+++ b/src/core/ao_ignite.c
@@ -0,0 +1,208 @@
+/*
+ * 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>
+
+__xdata struct ao_ignition ao_ignition[2];
+
+void
+ao_ignite(enum ao_igniter igniter) __critical
+{
+	ao_ignition[igniter].request = 1;
+	ao_wakeup(&ao_ignition);
+}
+
+#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;
+
+	__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) __critical
+{
+	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;
+		}
+		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;
+		}
+		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;
+			}
+		}
+	}
+}
+
+void
+ao_ignite_manual(void)
+{
+	ao_cmd_white();
+	if (!ao_match_word("DoIt"))
+		return;
+	ao_cmd_white();
+	if (ao_cmd_lex_c == 'm') {
+		if(ao_match_word("main"))
+			ao_igniter_fire(ao_igniter_main);
+	} else {
+		if(ao_match_word("drogue"))
+			ao_igniter_fire(ao_igniter_drogue);
+	}
+}
+
+static __code char * __code igniter_status_names[] = {
+	"unknown", "ready", "active", "open"
+};
+
+void
+ao_ignite_print_status(enum ao_igniter igniter, __code char *name) __reentrant
+{
+	enum ao_igniter_status status = ao_igniter_status(igniter);
+	printf("Igniter: %6s Status: %s\n",
+	       name,
+	       igniter_status_names[status]);
+}
+
+void
+ao_ignite_test(void)
+{
+	ao_ignite_print_status(ao_igniter_drogue, "drogue");
+	ao_ignite_print_status(ao_igniter_main, "main");
+}
+
+__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 },
+};
+
+__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);
+}
+
+void
+ao_igniter_init(void)
+{
+	ao_ignite_set_pins();
+	ao_cmd_register(&ao_ignite_cmds[0]);
+	ao_add_task(&ao_igniter_task, ao_igniter, "igniter");
+}
diff --git a/src/core/ao_kalman.c b/src/core/ao_kalman.c
new file mode 100644
index 00000000..59ffd8b2
--- /dev/null
+++ b/src/core/ao_kalman.c
@@ -0,0 +1,298 @@
+/*
+ * 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;
+__pdata int16_t			ao_max_height;
+static __pdata int32_t		ao_avg_height_scaled;
+__pdata 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_ground_accel - 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;
+#ifdef AO_FLIGHT_TEST
+	ao_sample_prev_tick = ao_sample_tick;
+#endif
+}
diff --git a/src/core/ao_lcd.h b/src/core/ao_lcd.h
new file mode 100644
index 00000000..f7e1391a
--- /dev/null
+++ b/src/core/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/core/ao_led.h b/src/core/ao_led.h
new file mode 100644
index 00000000..d9a0914a
--- /dev/null
+++ b/src/core/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/core/ao_log.c b/src/core/ao_log.c
new file mode 100644
index 00000000..7884ec3c
--- /dev/null
+++ b/src/core/ao_log.c
@@ -0,0 +1,283 @@
+/*
+ * 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>
+
+__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_STORAGE_ERASE_LOG;
+}
+
+void
+ao_log_write_erase(uint8_t pos)
+{
+	erase.unused = 0x00;
+	erase.flight = ao_flight_number;
+	ao_storage_write(ao_log_erase_pos(pos),  &erase, sizeof (erase));
+	ao_storage_flush();
+}
+
+static void
+ao_log_read_erase(uint8_t pos)
+{
+	ao_storage_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_config / 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;
+}
+
+static __xdata struct ao_task ao_log_task;
+
+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]);
+
+	/* Create a task to log events to eeprom */
+	ao_add_task(&ao_log_task, ao_log, "log");
+}
diff --git a/src/core/ao_log.h b/src/core/ao_log.h
new file mode 100644
index 00000000..04abeb7e
--- /dev/null
+++ b/src/core/ao_log.h
@@ -0,0 +1,260 @@
+/*
+ * 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_MEGAMETRUM	5	/* 32 byte typed megametrum records */
+#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_POS_NONE		(~0UL)
+
+struct ao_log_record {
+	char			type;
+	uint8_t			csum;
+	uint16_t		tick;
+	union {
+		struct {
+			int16_t		ground_accel;
+			uint16_t	flight;
+		} flight;
+		struct {
+			int16_t		accel;
+			int16_t		pres;
+		} 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 */
+		struct {
+			uint16_t	flight;		/* 4 */
+			int16_t		ground_accel;	/* 6 */
+			uint32_t	ground_pres;	/* 8 */
+			uint32_t	ground_temp;	/* 12 */
+		} flight;				/* 16 */
+		struct {
+			uint16_t	state;
+			uint16_t	reason;
+		} state;
+		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 */
+		struct {
+			int16_t		v_batt;		/* 4 */
+			int16_t		v_pbatt;	/* 6 */
+			int16_t		n_sense;	/* 8 */
+			int16_t		sense[10];	/* 10 */
+		} volt;					/* 30 */
+		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		pad;		/* 21 */
+		} gps;	/* 22 */
+		struct {
+			uint16_t	channels;	/* 4 */
+			struct {
+				uint8_t	svid;
+				uint8_t c_n;
+			} sats[12];			/* 6 */
+		} gps_sat;				/* 30 */
+	} u;
+};
+
+/* 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;
+
+#endif /* _AO_LOG_H_ */
diff --git a/src/core/ao_log_big.c b/src/core/ao_log_big.c
new file mode 100644
index 00000000..db01f46c
--- /dev/null
+++ b/src/core/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/core/ao_log_mega.c b/src/core/ao_log_mega.c
new file mode 100644
index 00000000..ac1590db
--- /dev/null
+++ b/src/core/ao_log_mega.c
@@ -0,0 +1,186 @@
+/*
+ * 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_MEGAMETRUM;
+
+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;
+}
+
+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
+	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];
+				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
+
+		/* 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_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/core/ao_log_single.c b/src/core/ao_log_single.c
new file mode 100644
index 00000000..3f6235a6
--- /dev/null
+++ b/src/core/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/core/ao_log_telem.c b/src/core/ao_log_telem.c
new file mode 100644
index 00000000..18ab85dd
--- /dev/null
+++ b/src/core/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;
+__pdata 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) {
+				memcpy(&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/core/ao_log_telescience.c b/src/core/ao_log_telescience.c
new file mode 100644
index 00000000..002a10bd
--- /dev/null
+++ b/src/core/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/core/ao_log_tiny.c b/src/core/ao_log_tiny.c
new file mode 100644
index 00000000..492658ea
--- /dev/null
+++ b/src/core/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_ring[ao_log_data].adc.pres;
+			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/core/ao_monitor.c b/src/core/ao_monitor.c
new file mode 100644
index 00000000..5876bef7
--- /dev/null
+++ b/src/core/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))
+			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/core/ao_mutex.c b/src/core/ao_mutex.c
new file mode 100644
index 00000000..c82a7d57
--- /dev/null
+++ b/src/core/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);
+	__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);
+	__critical {
+		*mutex = 0;
+		ao_wakeup(mutex);
+	}
+}
diff --git a/src/core/ao_packet.h b/src/core/ao_packet.h
new file mode 100644
index 00000000..f232a878
--- /dev/null
+++ b/src/core/ao_packet.h
@@ -0,0 +1,90 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#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;
+
+void
+ao_packet_send(void);
+
+uint8_t
+ao_packet_recv(void);
+
+void
+ao_packet_flush(void);
+
+void
+ao_packet_putchar(char c) __reentrant;
+
+char
+ao_packet_pollchar(void) __critical;
+
+#if PACKET_HAS_MASTER
+/* ao_packet_master.c */
+
+extern __xdata uint8_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/core/ao_panic.c b/src/core/ao_panic.c
new file mode 100644
index 00000000..52433044
--- /dev/null
+++ b/src/core/ao_panic.c
@@ -0,0 +1,85 @@
+/*
+ * 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
+
+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
+	__critical for (;;) {
+		ao_panic_delay(20);
+		for (n = 0; n < 5; n++) {
+			ao_led_on(AO_LED_RED);
+			ao_beep(AO_BEEP_HIGH);
+			ao_panic_delay(1);
+			ao_led_off(AO_LED_RED);
+			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_RED);
+			ao_beep(AO_BEEP_HIGH);
+			ao_panic_delay(40);
+			ao_led_off(AO_LED_RED);
+			ao_beep(AO_BEEP_OFF);
+			ao_panic_delay(10);
+		}
+		for (n = 0; n < (reason & 0x3f); n++) {
+			ao_led_on(AO_LED_RED);
+			ao_beep(AO_BEEP_MID);
+			ao_panic_delay(10);
+			ao_led_off(AO_LED_RED);
+			ao_beep(AO_BEEP_OFF);
+			ao_panic_delay(10);
+		}
+	}
+}
diff --git a/src/core/ao_product.c b/src/core/ao_product.c
new file mode 100644
index 00000000..ec91b978
--- /dev/null
+++ b/src/core/ao_product.c
@@ -0,0 +1,155 @@
+/*
+ * 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
+#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 */
+	0x32,			/*  bMaxPower */
+
+	/* 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/core/ao_pyro.c b/src/core/ao_pyro.c
new file mode 100644
index 00000000..4f37e979
--- /dev/null
+++ b/src/core/ao_pyro.c
@@ -0,0 +1,434 @@
+/*
+ * 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_pyro.h>
+#include <ao_sample.h>
+#include <ao_flight.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))
+
+/*
+ * 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_ORIENT
+		case ao_pyro_orient_less:
+			if (ao_orient <= pyro->orient_less)
+				continue;
+			break;
+		case ao_pyro_orient_greater:
+			if (ao_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;
+
+		default:
+			continue;
+		}
+		return FALSE;
+	}
+	return TRUE;
+}
+
+#define ao_pyro_fire_port(port, bit, pin) do {	\
+		ao_gpio_set(port, bit, pin, 1);	\
+		ao_delay(AO_MS_TO_TICKS(50));	\
+		ao_gpio_set(port, bit, pin, 0);	\
+	} while (0)
+
+
+static void
+ao_pyro_fire(uint8_t p)
+{
+	switch (p) {
+#if AO_PYRO_NUM > 0
+	case 0: ao_pyro_fire_port(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0); break;
+#endif
+#if AO_PYRO_NUM > 1
+	case 1: ao_pyro_fire_port(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1); break;
+#endif
+#if AO_PYRO_NUM > 2
+	case 2: ao_pyro_fire_port(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2); break;
+#endif
+#if AO_PYRO_NUM > 3
+	case 3: ao_pyro_fire_port(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3); break;
+#endif
+#if AO_PYRO_NUM > 4
+	case 4: ao_pyro_fire_port(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4); break;
+#endif
+#if AO_PYRO_NUM > 5
+	case 5: ao_pyro_fire_port(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5); break;
+#endif
+#if AO_PYRO_NUM > 6
+	case 6: ao_pyro_fire_port(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6); break;
+#endif
+#if AO_PYRO_NUM > 7
+	case 7: ao_pyro_fire_port(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7); break;
+#endif
+	default: break;
+	}
+	ao_delay(AO_MS_TO_TICKS(50));
+}
+
+uint8_t	ao_pyro_wakeup;
+
+static void
+ao_pyro(void)
+{
+	uint8_t		p;
+	struct ao_pyro	*pyro;
+
+	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();
+		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;
+
+			/* Check pyro state to see if it shoule fire
+			 */
+			if (!pyro->delay_done) {
+				if (!ao_pyro_ready(pyro))
+					continue;
+
+				/* If there's a delay set, then remember when
+				 * it expires
+				 */
+				if (pyro->flags & ao_pyro_delay)
+					pyro->delay_done = ao_time() + pyro->delay;
+			}
+
+			/* 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;
+			}
+
+			ao_pyro_fire(p);
+		}
+	}
+}
+
+__xdata struct ao_task ao_pyro_task;
+
+#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_ORIENT
+	{ "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)")  },
+
+	{ "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) },
+};
+
+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 (p < 0 || 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();
+}
+
+static void
+ao_pyro_manual(void)
+{
+	ao_cmd_white();
+	if (!ao_match_word("DoIt"))
+		return;
+	ao_cmd_white();
+	ao_cmd_decimal();
+	if (ao_cmd_lex_i < 0 || AO_PYRO_NUM <= ao_cmd_lex_i)
+		return;
+	ao_pyro_fire(ao_cmd_lex_i);
+
+}
+
+const struct ao_cmds ao_pyro_cmds[] = {
+	{ ao_pyro_manual,	"P DoIt <n>\0Fire igniter" },
+	{ 0, NULL }
+};
+
+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_cmd_register(&ao_pyro_cmds[0]);
+	ao_add_task(&ao_pyro_task, ao_pyro, "pyro");
+}
diff --git a/src/core/ao_pyro.h b/src/core/ao_pyro.h
new file mode 100644
index 00000000..5deb69d0
--- /dev/null
+++ b/src/core/ao_pyro.h
@@ -0,0 +1,71 @@
+/*
+ * 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,
+};
+
+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;
+	int16_t			motor;
+	uint16_t		delay_done;
+	uint8_t			fired;
+};
+
+extern uint8_t	ao_pyro_wakeup;
+
+void
+ao_pyro_set(void);
+
+void
+ao_pyro_show(void);
+
+void
+ao_pyro_init(void);
+
+#endif
diff --git a/src/core/ao_radio_cmac.c b/src/core/ao_radio_cmac.c
new file mode 100644
index 00000000..fc0ca8b1
--- /dev/null
+++ b/src/core/ao_radio_cmac.c
@@ -0,0 +1,169 @@
+/*
+ * 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 __pdata uint8_t ao_radio_cmac_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
+	if (timeout)
+		ao_alarm(timeout);
+
+	i = ao_radio_recv(cmac_data, len + AO_CMAC_KEY_LEN + 2);
+	ao_clear_alarm();
+
+	if (!i) {
+		ao_radio_cmac_rssi = 0;
+		return AO_RADIO_CMAC_TIMEOUT;
+	}
+
+	ao_radio_cmac_rssi = (int8_t) (((int8_t) cmac_data[len + AO_CMAC_KEY_LEN]) >> 1) - 74;
+	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
+{
+	uint8_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/core/ao_radio_cmac.h b/src/core/ao_radio_cmac.h
new file mode 100644
index 00000000..e86f31e9
--- /dev/null
+++ b/src/core/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/core/ao_radio_cmac_cmd.c b/src/core/ao_radio_cmac_cmd.c
new file mode 100644
index 00000000..64410921
--- /dev/null
+++ b/src/core/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/core/ao_radio_cmac_cmd.h b/src/core/ao_radio_cmac_cmd.h
new file mode 100644
index 00000000..6b8782de
--- /dev/null
+++ b/src/core/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/core/ao_report.c b/src/core/ao_report.c
new file mode 100644
index 00000000..1104cd82
--- /dev/null
+++ b/src/core/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/core/ao_rssi.c b/src/core/ao_rssi.c
new file mode 100644
index 00000000..e3964d2d
--- /dev/null
+++ b/src/core/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 volatile uint16_t	ao_rssi_time;
+static __pdata volatile 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/core/ao_sample.c b/src/core/ao_sample.c
new file mode 100644
index 00000000..985c0940
--- /dev/null
+++ b/src/core/ao_sample.c
@@ -0,0 +1,176 @@
+/*
+ * 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
+
+/*
+ * 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
+
+__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
+
+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
+
+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;
+	++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);
+	nsamples = 0;
+	ao_sample_pres_sum = 0;
+}
+
+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();
+}
+
+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 (ao_preflight)
+			ao_sample_preflight();
+		else {
+			if (ao_flight_state < ao_flight_boost)
+				ao_sample_preflight_update();
+			ao_kalman();
+		}
+		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
+	ao_sample_data = ao_data_head;
+	ao_preflight = TRUE;
+}
diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h
new file mode 100644
index 00000000..9336bdf9
--- /dev/null
+++ b/src/core/ao_sample.h
@@ -0,0 +1,143 @@
+/*
+ * 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
+ */
+
+#define GRAVITY 9.80665
+
+/*
+ * 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
+
+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 __pdata int16_t			ao_max_height;	/* max of ao_height */
+extern __pdata 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/core/ao_send_packet.c b/src/core/ao_send_packet.c
new file mode 100644
index 00000000..1a8e74de
--- /dev/null
+++ b/src/core/ao_send_packet.c
@@ -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.
+ */
+
+#include "ao.h"
+
+#define AO_MAX_SEND	128
+
+static __xdata uint8_t ao_send[AO_MAX_SEND];
+
+static uint8_t
+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;
+}
+
+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 = getnibble() << 4;
+		b |= 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/core/ao_send_packet.h b/src/core/ao_send_packet.h
new file mode 100644
index 00000000..526f7b55
--- /dev/null
+++ b/src/core/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/core/ao_serial.h b/src/core/ao_serial.h
new file mode 100644
index 00000000..53aa8a89
--- /dev/null
+++ b/src/core/ao_serial.h
@@ -0,0 +1,86 @@
+/*
+ * 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
+
+#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);
+
+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);
+
+char
+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);
+
+char
+ao_serial2_pollchar(void);
+
+void
+ao_serial2_putchar(char c);
+
+void
+ao_serial2_drain(void);
+
+void
+ao_serial2_set_speed(uint8_t speed);
+#endif
+
+void
+ao_serial_init(void);
+
+#endif /* _AO_SERIAL_H_ */
diff --git a/src/core/ao_sqrt.c b/src/core/ao_sqrt.c
new file mode 100644
index 00000000..09c2e319
--- /dev/null
+++ b/src/core/ao_sqrt.c
@@ -0,0 +1,46 @@
+/*
+ * Copyright © 2011 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "ao.h"
+
+/* 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/core/ao_state.c b/src/core/ao_state.c
new file mode 100644
index 00000000..ed197aa5
--- /dev/null
+++ b/src/core/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/core/ao_stdio.c b/src/core/ao_stdio.c
new file mode 100644
index 00000000..656b23c9
--- /dev/null
+++ b/src/core/ao_stdio.c
@@ -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.
+ */
+
+#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
+
+#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];
+__pdata int8_t ao_cur_stdio;
+__pdata int8_t ao_num_stdios;
+
+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 __critical
+{
+	char c;
+	int8_t stdio = ao_cur_stdio;
+
+	for (;;) {
+		c = ao_stdios[stdio].pollchar();
+		if (c != AO_READ_AGAIN)
+			break;
+		if (++stdio == ao_num_stdios)
+			stdio = 0;
+		if (stdio == ao_cur_stdio)
+			ao_sleep(&ao_stdin_ready);
+	}
+	ao_cur_stdio = stdio;
+	return c;
+}
+
+uint8_t
+ao_echo(void)
+{
+	return ao_stdios[ao_cur_stdio].echo;
+}
+
+int8_t
+ao_add_stdio(char (*pollchar)(void),
+	     void (*putchar)(char),
+	     void (*flush)(void)) __reentrant
+{
+	if (ao_num_stdios == AO_NUM_STDIOS)
+		ao_panic(AO_PANIC_STDIO);
+	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;
+	return ao_num_stdios++;
+}
diff --git a/src/core/ao_storage.c b/src/core/ao_storage.c
new file mode 100644
index 00000000..adf7e4d4
--- /dev/null
+++ b/src/core/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_config; 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/core/ao_storage.h b/src/core/ao_storage.h
new file mode 100644
index 00000000..ea946399
--- /dev/null
+++ b/src/core/ao_storage.h
@@ -0,0 +1,90 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#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;
+
+/* Byte offset of config block. Will be ao_storage_block bytes long */
+extern __pdata ao_pos_t	ao_storage_config;
+
+/* Storage unit size - device reads and writes must be within blocks of this size. Usually 256 bytes. */
+extern __pdata uint16_t ao_storage_unit;
+
+#define AO_STORAGE_ERASE_LOG	(ao_storage_config + AO_CONFIG_MAX_SIZE)
+
+/* Initialize above values. Can only be called once the OS is running */
+void
+ao_storage_setup(void) __reentrant;
+
+/* 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/core/ao_task.c b/src/core/ao_task.c
new file mode 100644
index 00000000..65654731
--- /dev/null
+++ b/src/core/ao_task.c
@@ -0,0 +1,205 @@
+/*
+ * 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>
+
+#define AO_NO_TASK_INDEX	0xff
+
+__xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS];
+__data uint8_t ao_num_tasks;
+__data uint8_t ao_cur_task_index;
+__xdata struct ao_task *__data ao_cur_task;
+
+#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
+
+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;
+	}
+	ao_tasks[ao_num_tasks++] = task;
+	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);
+}
+
+/* Task switching function. This must not use any stack variables */
+void
+ao_yield(void) ao_arch_naked_define
+{
+	ao_arch_save_regs();
+
+	if (ao_cur_task_index == AO_NO_TASK_INDEX)
+		ao_cur_task_index = ao_num_tasks-1;
+	else
+	{
+		ao_arch_save_stack();
+	}
+
+	ao_arch_isr_stack();
+
+#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
+	 */
+	{
+		__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;
+
+			/* Enter lower power mode when there isn't anything to do */
+			if (ao_cur_task_index == ao_last_task_index)
+				ao_arch_cpu_idle();
+		}
+	}
+#if AO_CHECK_STACK
+	cli();
+	in_yield = 0;
+#endif
+	ao_arch_restore_stack();
+}
+
+uint8_t
+ao_sleep(__xdata void *wchan)
+{
+	ao_cur_task->wchan = wchan;
+	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)
+{
+	uint8_t	i;
+
+	ao_check_stack();
+	for (i = 0; i < ao_num_tasks; i++)
+		if (ao_tasks[i]->wchan == wchan)
+			ao_tasks[i]->wchan = NULL;
+}
+
+void
+ao_alarm(uint16_t delay)
+{
+	/* 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
+	 */
+	if (!(ao_cur_task->alarm = ao_time() + delay + 1))
+		ao_cur_task->alarm = 1;
+}
+
+void
+ao_clear_alarm(void)
+{
+	ao_cur_task->alarm = 0;
+}
+
+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)
+{
+	ao_arch_critical(
+		uint8_t	i;
+		ao_num_tasks--;
+		for (i = ao_cur_task_index; i < ao_num_tasks; i++)
+			ao_tasks[i] = ao_tasks[i+1];
+		ao_cur_task_index = AO_NO_TASK_INDEX;
+		ao_yield();
+		);
+	/* we'll never get back here */
+}
+
+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);
+	}
+}
+
+void
+ao_start_scheduler(void)
+{
+	ao_cur_task_index = AO_NO_TASK_INDEX;
+	ao_cur_task = NULL;
+	ao_yield();
+}
diff --git a/src/core/ao_telem.h b/src/core/ao_telem.h
new file mode 100644
index 00000000..1a8da291
--- /dev/null
+++ b/src/core/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/core/ao_telemetry.c b/src/core/ao_telemetry.c
new file mode 100644
index 00000000..52ac9489
--- /dev/null
+++ b/src/core/ao_telemetry.c
@@ -0,0 +1,402 @@
+/*
+ * 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"
+
+static __pdata uint16_t ao_telemetry_interval;
+static __pdata uint8_t ao_rdf = 0;
+static __pdata uint16_t ao_rdf_time;
+
+#if defined(MEGAMETRUM)
+#define AO_SEND_MEGA	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.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_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;
+		telemetry.configuration.flight = ao_log_full() ? 0 : ao_flight_number;
+		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);
+		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_rdf_time = ao_time();
+		while (ao_telemetry_interval) {
+
+
+#ifdef AO_SEND_ALL_BARO
+			ao_send_baro();
+#endif
+#ifdef AO_SEND_MEGA
+			ao_send_mega_sensor();
+			ao_send_mega_data();
+#else
+			ao_send_sensor();
+#endif
+
+#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 (ao_rdf &&
+			    (int16_t) (ao_time() - ao_rdf_time) >= 0)
+			{
+#if HAS_IGNITE_REPORT
+				uint8_t	c;
+#endif
+				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
+					ao_radio_rdf();
+			}
+#endif
+			time += ao_telemetry_interval;
+			delay = time - ao_time();
+			if (delay > 0) {
+				ao_alarm(delay);
+				ao_sleep(&telemetry);
+				ao_clear_alarm();
+			}
+			else
+				time = ao_time();
+		bottom:	;
+		}
+	}
+}
+
+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 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);
+}
+
+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;
+}
+
+__xdata struct ao_task	ao_telemetry_task;
+
+void
+ao_telemetry_init()
+{
+	ao_add_task(&ao_telemetry_task, ao_telemetry, "telemetry");
+}
diff --git a/src/core/ao_telemetry.h b/src/core/ao_telemetry.h
new file mode 100644
index 00000000..32a1668c
--- /dev/null
+++ b/src/core/ao_telemetry.h
@@ -0,0 +1,250 @@
+/*
+ * 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;
+};
+
+struct ao_telemetry_satellite {
+	uint16_t				serial;		/*  0 */
+	uint16_t				tick;		/*  2 */
+	uint8_t					type;		/*  4 */
+	uint8_t					channels;	/*  5 number of reported sats */
+
+	struct ao_telemetry_satellite_info	sats[12];	/* 6 */
+	uint8_t					unused[2];	/* 30 */
+	/* 32 */
+};
+
+#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		pad5;		/*  5 */
+	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_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_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/core/ao_usb.h b/src/core/ao_usb.h
new file mode 100644
index 00000000..e051db93
--- /dev/null
+++ b/src/core/ao_usb.h
@@ -0,0 +1,136 @@
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#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
+ */
+char
+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
+
+#define AO_USB_OUT_EP		4
+#define AO_USB_IN_EP		5
+/*
+ * 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 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;
+} ;
+
+#endif /* _AO_USB_H_ */