Merge branch 'master' of ssh://git.gag.com/scm/git/fw/altos

20 files changed, 1327 insertions, 96 deletions

diff --git a/src/core/ao.h b/src/core/ao.h
index 54018b37..df5bbf48 100644
--- a/src/core/ao.h
+++ b/src/core/ao.h
@@ -529,6 +529,11 @@ ao_radio_recv_abort(void);
 void
 ao_radio_test(uint8_t on);
 
+typedef int16_t (*ao_radio_fill_func)(uint8_t *buffer, int16_t len);
+
+void
+ao_radio_send_lots(ao_radio_fill_func fill);
+
 /*
  * Compute the packet length as follows:
  *
@@ -679,7 +684,7 @@ extern __xdata uint8_t ao_force_freq;
 #endif
 
 #define AO_CONFIG_MAJOR	1
-#define AO_CONFIG_MINOR	12
+#define AO_CONFIG_MINOR	13
 
 #define AO_AES_LEN 16
 
@@ -706,12 +711,17 @@ struct ao_config {
 #if AO_PYRO_NUM
 	struct ao_pyro	pyro[AO_PYRO_NUM];	/* minor version 12 */
 #endif
+	uint16_t	aprs_interval;		/* minor version 13 */
 };
 
 #define AO_IGNITE_MODE_DUAL		0
 #define AO_IGNITE_MODE_APOGEE		1
 #define AO_IGNITE_MODE_MAIN		2
 
+#define AO_RADIO_ENABLE_CORE		1
+#define AO_RADIO_DISABLE_TELEMETRY	2
+#define AO_RADIO_DISABLE_RDF		4
+
 #define AO_PAD_ORIENTATION_ANTENNA_UP	0
 #define AO_PAD_ORIENTATION_ANTENNA_DOWN	1
 
diff --git a/src/core/ao_config.c b/src/core/ao_config.c
index 797fe7ec..0aac16a6 100644
--- a/src/core/ao_config.c
+++ b/src/core/ao_config.c
@@ -128,7 +128,7 @@ _ao_config_get(void)
 		if (minor < 6)
 			ao_config.pad_orientation = AO_CONFIG_DEFAULT_PAD_ORIENTATION;
 		if (minor < 8)
-			ao_config.radio_enable = TRUE;
+			ao_config.radio_enable = AO_RADIO_ENABLE_CORE;
 		if (minor < 9)
 			ao_xmemset(&ao_config.aes_key, '\0', AO_AES_LEN);
 		if (minor < 10)
@@ -139,6 +139,8 @@ _ao_config_get(void)
 		if (minor < 12)
 			memset(&ao_config.pyro, '\0', sizeof (ao_config.pyro));
 #endif
+		if (minor < 13)
+			ao_config.aprs_interval = 0;
 		ao_config.minor = AO_CONFIG_MINOR;
 		ao_config_dirty = 1;
 	}
@@ -498,6 +500,27 @@ ao_config_key_set(void) __reentrant
 }
 #endif
 
+#if HAS_APRS
+
+void
+ao_config_aprs_show(void)
+{
+	printf ("APRS interval: %d\n", ao_config.aprs_interval);
+}
+
+void
+ao_config_aprs_set(void)
+{
+	ao_cmd_decimal();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	_ao_config_edit_start();
+	ao_config.aprs_interval = ao_cmd_lex_i;
+	_ao_config_edit_finish();
+}
+
+#endif /* HAS_APRS */
+
 struct ao_config_var {
 	__code char	*str;
 	void		(*set)(void) __reentrant;
@@ -554,6 +577,10 @@ __code struct ao_config_var ao_config_vars[] = {
 	{ "P <n,?>\0Configure pyro channels",
 	  ao_pyro_set, ao_pyro_show },
 #endif
+#if HAS_APRS
+	{ "A <secs>\0APRS packet interval (0 disable)",
+	  ao_config_aprs_set, ao_config_aprs_show },
+#endif
 	{ "s\0Show",
 	  ao_config_show,		0 },
 #if HAS_EEPROM
diff --git a/src/core/ao_data.h b/src/core/ao_data.h
index 6fdd19cb..7e2f85d8 100644
--- a/src/core/ao_data.h
+++ b/src/core/ao_data.h
@@ -288,4 +288,25 @@ typedef int16_t accel_t;
 
 #endif
 
+#if !HAS_GYRO && HAS_MPU6000
+
+#define HAS_GYRO	1
+
+typedef int16_t	gyro_t;
+typedef int32_t angle_t;
+
+/* Y axis is aligned with the direction of motion (along) */
+/* X axis is aligned in the other board axis (across) */
+/* Z axis is aligned perpendicular to the board (through) */
+
+#define ao_data_along(packet)	((packet)->mpu6000.accel_y)
+#define ao_data_across(packet)	((packet)->mpu6000.accel_x)
+#define ao_data_through(packet)	((packet)->mpu6000.accel_z)
+
+#define ao_data_roll(packet)	((packet)->mpu6000.gyro_y)
+#define ao_data_pitch(packet)	((packet)->mpu6000.gyro_x)
+#define ao_data_yaw(packet)	((packet)->mpu6000.gyro_z)
+
+#endif
+
 #endif /* _AO_DATA_H_ */
diff --git a/src/core/ao_log.h b/src/core/ao_log.h
index 4eaed420..036d6f2d 100644
--- a/src/core/ao_log.h
+++ b/src/core/ao_log.h
@@ -199,10 +199,16 @@ struct ao_log_mega {
 	union {						/* 4 */
 		/* AO_LOG_FLIGHT */
 		struct {
-			uint16_t	flight;		/* 4 */
-			int16_t		ground_accel;	/* 6 */
-			uint32_t	ground_pres;	/* 8 */
-		} flight;				/* 12 */
+			uint16_t	flight;			/* 4 */
+			int16_t		ground_accel;		/* 6 */
+			uint32_t	ground_pres;		/* 8 */
+			int16_t		ground_accel_along;	/* 16 */
+			int16_t		ground_accel_across;	/* 12 */
+			int16_t		ground_accel_through;	/* 14 */
+			int16_t		ground_roll;		/* 18 */
+			int16_t		ground_pitch;		/* 20 */
+			int16_t		ground_yaw;		/* 22 */
+		} flight;					/* 24 */
 		/* AO_LOG_STATE */
 		struct {
 			uint16_t	state;
diff --git a/src/core/ao_log_mega.c b/src/core/ao_log_mega.c
index ac1590db..e03687ad 100644
--- a/src/core/ao_log_mega.c
+++ b/src/core/ao_log_mega.c
@@ -95,6 +95,14 @@ ao_log(void)
 #if HAS_ACCEL
 	log.u.flight.ground_accel = ao_ground_accel;
 #endif
+#if HAS_GYRO
+	log.u.flight.ground_accel_along = ao_ground_accel_along;
+	log.u.flight.ground_accel_across = ao_ground_accel_across;
+	log.u.flight.ground_accel_through = ao_ground_accel_through;
+	log.u.flight.ground_pitch = ao_ground_pitch;
+	log.u.flight.ground_yaw = ao_ground_yaw;
+	log.u.flight.ground_roll = ao_ground_roll;
+#endif
 	log.u.flight.ground_pres = ao_ground_pres;
 	log.u.flight.flight = ao_flight_number;
 	ao_log_mega(&log);
diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c
index 985c0940..dec44f9f 100644
--- a/src/core/ao_sample.c
+++ b/src/core/ao_sample.c
@@ -37,6 +37,16 @@ __pdata alt_t		ao_sample_height;
 #if HAS_ACCEL
 __pdata accel_t		ao_sample_accel;
 #endif
+#if HAS_GYRO
+__pdata accel_t		ao_sample_accel_along;
+__pdata accel_t		ao_sample_accel_across;
+__pdata accel_t		ao_sample_accel_through;
+__pdata gyro_t		ao_sample_roll;
+__pdata gyro_t		ao_sample_pitch;
+__pdata gyro_t		ao_sample_yaw;
+__pdata angle_t		ao_sample_angle;
+__pdata angle_t		ao_sample_roll_angle;
+#endif
 
 __data uint8_t		ao_sample_data;
 
@@ -53,6 +63,15 @@ __pdata accel_t		ao_accel_2g;		/* factory accel calibration */
 __pdata int32_t		ao_accel_scale;		/* sensor to m/s² conversion */
 #endif
 
+#if HAS_GYRO
+__pdata accel_t		ao_ground_accel_along;
+__pdata accel_t		ao_ground_accel_across;
+__pdata accel_t		ao_ground_accel_through;
+__pdata gyro_t		ao_ground_pitch;
+__pdata gyro_t		ao_ground_yaw;
+__pdata gyro_t		ao_ground_roll;
+#endif
+
 static __pdata uint8_t	ao_preflight;		/* in preflight mode */
 
 static __pdata uint16_t	nsamples;
@@ -60,6 +79,14 @@ __pdata int32_t ao_sample_pres_sum;
 #if HAS_ACCEL
 __pdata int32_t ao_sample_accel_sum;
 #endif
+#if HAS_GYRO
+__pdata int32_t ao_sample_accel_along_sum;
+__pdata int32_t ao_sample_accel_across_sum;
+__pdata int32_t	ao_sample_accel_through_sum;
+__pdata int32_t ao_sample_pitch_sum;
+__pdata int32_t ao_sample_yaw_sum;
+__pdata int32_t	ao_sample_roll_sum;
+#endif
 
 static void
 ao_sample_preflight_add(void)
@@ -68,6 +95,14 @@ ao_sample_preflight_add(void)
 	ao_sample_accel_sum += ao_sample_accel;
 #endif
 	ao_sample_pres_sum += ao_sample_pres;
+#if HAS_GYRO
+	ao_sample_accel_along_sum += ao_sample_accel_along;
+	ao_sample_accel_across_sum += ao_sample_accel_across;
+	ao_sample_accel_through_sum += ao_sample_accel_through;
+	ao_sample_pitch_sum += ao_sample_pitch;
+	ao_sample_yaw_sum += ao_sample_yaw;
+	ao_sample_roll_sum += ao_sample_roll;
+#endif
 	++nsamples;
 }
 
@@ -80,8 +115,23 @@ ao_sample_preflight_set(void)
 #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;
+#if HAS_GYRO
+	ao_ground_accel_along = ao_sample_accel_along_sum >> 9;
+	ao_ground_accel_across = ao_sample_accel_across_sum >> 9;
+	ao_ground_accel_through = ao_sample_accel_through_sum >> 9;
+	ao_ground_pitch = ao_sample_pitch_sum >> 9;
+	ao_ground_yaw = ao_sample_yaw_sum >> 9;
+	ao_ground_roll = ao_sample_roll_sum >> 9;
+	ao_sample_accel_along_sum = 0;
+	ao_sample_accel_across_sum = 0;
+	ao_sample_accel_through_sum = 0;
+	ao_sample_pitch_sum = 0;
+	ao_sample_yaw_sum = 0;
+	ao_sample_roll_sum = 0;
+	ao_sample_angle = 0;
+#endif	
+	nsamples = 0;
 }
 
 static void
@@ -122,6 +172,25 @@ ao_sample_preflight_update(void)
 		ao_sample_preflight_set();
 }
 
+#if 0
+#if HAS_GYRO
+static int32_t	p_filt;
+static int32_t	y_filt;
+
+static gyro_t inline ao_gyro(void) {
+	gyro_t	p = ao_sample_pitch - ao_ground_pitch;
+	gyro_t	y = ao_sample_yaw - ao_ground_yaw;
+
+	p_filt = p_filt - (p_filt >> 6) + p;
+	y_filt = y_filt - (y_filt >> 6) + y;
+
+	p = p_filt >> 6;
+	y = y_filt >> 6;
+	return ao_sqrt(p*p + y*y);
+}
+#endif
+#endif
+
 uint8_t
 ao_sample(void)
 {
@@ -147,6 +216,14 @@ ao_sample(void)
 			ao_sample_accel = ao_data_accel_invert(ao_sample_accel);
 		ao_data_set_accel(ao_data, ao_sample_accel);
 #endif
+#if HAS_GYRO
+		ao_sample_accel_along = ao_data_along(ao_data);
+		ao_sample_accel_across = ao_data_across(ao_data);
+		ao_sample_accel_through = ao_data_through(ao_data);
+		ao_sample_pitch = ao_data_pitch(ao_data);
+		ao_sample_yaw = ao_data_yaw(ao_data);
+		ao_sample_roll = ao_data_roll(ao_data);
+#endif
 
 		if (ao_preflight)
 			ao_sample_preflight();
@@ -154,6 +231,9 @@ ao_sample(void)
 			if (ao_flight_state < ao_flight_boost)
 				ao_sample_preflight_update();
 			ao_kalman();
+#if HAS_GYRO
+			/* do quaternion stuff here... */
+#endif
 		}
 		ao_sample_data = ao_data_ring_next(ao_sample_data);
 	}
@@ -171,6 +251,21 @@ ao_sample_init(void)
 	ao_sample_accel_sum = 0;
 	ao_sample_accel = 0;
 #endif
+#if HAS_GYRO
+	ao_sample_accel_along_sum = 0;
+	ao_sample_accel_across_sum = 0;
+	ao_sample_accel_through_sum = 0;
+	ao_sample_accel_along = 0;
+	ao_sample_accel_across = 0;
+	ao_sample_accel_through = 0;
+	ao_sample_pitch_sum = 0;
+	ao_sample_yaw_sum = 0;
+	ao_sample_roll_sum = 0;
+	ao_sample_pitch = 0;
+	ao_sample_yaw = 0;
+	ao_sample_roll = 0;
+	ao_sample_angle = 0;
+#endif
 	ao_sample_data = ao_data_head;
 	ao_preflight = TRUE;
 }
diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h
index 9336bdf9..a2dac979 100644
--- a/src/core/ao_sample.h
+++ b/src/core/ao_sample.h
@@ -111,6 +111,14 @@ extern __pdata accel_t	ao_ground_accel;	/* startup acceleration */
 extern __pdata accel_t 	ao_accel_2g;		/* factory accel calibration */
 extern __pdata int32_t	ao_accel_scale;		/* sensor to m/s² conversion */
 #endif
+#if HAS_GYRO
+extern __pdata accel_t	ao_ground_accel_along;
+extern __pdata accel_t	ao_ground_accel_across;
+extern __pdata accel_t	ao_ground_accel_through;
+extern __pdata gyro_t	ao_ground_pitch;
+extern __pdata gyro_t	ao_ground_yaw;
+extern __pdata gyro_t	ao_ground_roll;
+#endif
 
 void ao_sample_init(void);
 
diff --git a/src/core/ao_sqrt.c b/src/core/ao_sqrt.c
index 09c2e319..3a550eaa 100644
--- a/src/core/ao_sqrt.c
+++ b/src/core/ao_sqrt.c
@@ -15,7 +15,9 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  */
 
+#ifndef AO_FLIGHT_TEST
 #include "ao.h"
+#endif
 
 /* Adapted from int_sqrt.c in the linux kernel, which is licensed GPLv2 */
 /**
diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c
index 52ac9489..8d440e15 100644
--- a/src/core/ao_telemetry.c
+++ b/src/core/ao_telemetry.c
@@ -22,6 +22,12 @@ static __pdata uint16_t ao_telemetry_interval;
 static __pdata uint8_t ao_rdf = 0;
 static __pdata uint16_t ao_rdf_time;
 
+#if HAS_APRS
+static __pdata uint16_t ao_aprs_time;
+
+#include <ao_aprs.h>
+#endif
+
 #if defined(MEGAMETRUM)
 #define AO_SEND_MEGA	1
 #endif
@@ -288,30 +294,40 @@ ao_telemetry(void)
 		while (ao_telemetry_interval == 0)
 			ao_sleep(&telemetry);
 		time = ao_rdf_time = ao_time();
+#if HAS_APRS
+		ao_aprs_time = time;
+#endif
 		while (ao_telemetry_interval) {
 
-
+#if HAS_APRS
+			if (!(ao_config.radio_enable & AO_RADIO_DISABLE_TELEMETRY))
+#endif
+			{
 #ifdef AO_SEND_ALL_BARO
-			ao_send_baro();
+				ao_send_baro();
 #endif
 #ifdef AO_SEND_MEGA
-			ao_send_mega_sensor();
-			ao_send_mega_data();
+				ao_send_mega_sensor();
+				ao_send_mega_data();
 #else
-			ao_send_sensor();
+				ao_send_sensor();
 #endif
 
 #if HAS_COMPANION
-			if (ao_companion_running)
-				ao_send_companion();
+				if (ao_companion_running)
+					ao_send_companion();
 #endif
-			ao_send_configuration();
+				ao_send_configuration();
 #if HAS_GPS
-			ao_send_location();
-			ao_send_satellite();
+				ao_send_location();
+				ao_send_satellite();
 #endif
+			}
 #ifndef AO_SEND_ALL_BARO
 			if (ao_rdf &&
+#if HAS_APRS
+			    !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) &&
+#endif
 			    (int16_t) (ao_time() - ao_rdf_time) >= 0)
 			{
 #if HAS_IGNITE_REPORT
@@ -325,6 +341,14 @@ ao_telemetry(void)
 #endif
 					ao_radio_rdf();
 			}
+#if HAS_APRS
+			if (ao_config.aprs_interval != 0 &&
+			    (int16_t) (ao_time() - ao_aprs_time) >= 0)
+			{
+				ao_aprs_time = ao_time() + AO_SEC_TO_TICKS(ao_config.aprs_interval);
+				ao_aprs_send();
+			}
+#endif
 #endif
 			time += ao_telemetry_interval;
 			delay = time - ao_time();
@@ -389,8 +413,9 @@ ao_rdf_set(uint8_t rdf)
 	ao_rdf = rdf;
 	if (rdf == 0)
 		ao_radio_rdf_abort();
-	else
+	else {
 		ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS;
+	}
 }
 
 __xdata struct ao_task	ao_telemetry_task;
diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c
new file mode 100644
index 00000000..03bcab05
--- /dev/null
+++ b/src/drivers/ao_aprs.c
@@ -0,0 +1,599 @@
+/** 
+ * http://ad7zj.net/kd7lmo/aprsbeacon_code.html
+ *
+ * @mainpage Pico Beacon
+ *
+ * @section overview_sec Overview
+ *
+ * The Pico Beacon is an APRS based tracking beacon that operates in the UHF 420-450MHz band.  The device utilizes a 
+ * Microchip PIC 18F2525 embedded controller, Motorola M12+ GPS engine, and Analog Devices AD9954 DDS.  The device is capable
+ * of generating a 1200bps A-FSK and 9600 bps FSK AX.25 compliant APRS (Automatic Position Reporting System) message.
+
+
+ *
+ * @section history_sec Revision History
+ *
+ * @subsection v305 V3.05
+ * 23 Dec 2006, Change include; (1) change printf format width to conform to ANSI standard when new CCS 4.xx compiler released.
+ *
+ *
+ * @subsection v304 V3.04
+ * 10 Jan 2006, Change include; (1) added amplitude control to engineering mode,
+ *                                     (2) corrected number of bytes reported in log,
+ *                                     (3) add engineering command to set high rate position reports (5 seconds), and
+ *                                     (4) corrected size of LOG_COORD block when searching for end of log.
+ *
+ * @subsection v303 V3.03
+ * 15 Sep 2005, Change include; (1) removed AD9954 setting SDIO as input pin, 
+ *                                     (2) additional comments and Doxygen tags,
+ *                                     (3) integration and test code calculates DDS FTW,
+ *                                     (4) swapped bus and reference analog input ports (hardware change),
+ *                                     (5) added message that indicates we are reading flash log and reports length,
+ *                                     (6) report bus voltage in 10mV steps, and
+ *                                     (7) change log type enumerated values to XORed nibbles for error detection.
+ *
+ *
+ * @subsection v302 V3.02
+ * 6 Apr 2005, Change include; (1) corrected tracked satellite count in NMEA-0183 $GPGGA message,
+ *                                    (2) Doxygen documentation clean up and additions, and
+ *                                    (3) added integration and test code to baseline.
+ *
+ * 
+ * @subsection v301 V3.01
+ * 13 Jan 2005, Renamed project and files to Pico Beacon.
+ *
+ *
+ * @subsection v300 V3.00
+ * 15 Nov 2004, Change include; (1) Micro Beacon extreme hardware changes including integral transmitter,
+ *                                     (2) PIC18F2525 processor,
+ *                                     (3) AD9954 DDS support functions,
+ *                                     (4) added comments and formatting for doxygen,
+ *                                     (5) process GPS data with native Motorola protocol,
+ *                                     (6) generate plain text $GPGGA and $GPRMC messages,
+ *                                     (7) power down GPS 5 hours after lock,
+ *                                     (8) added flight data recorder, and
+ *                                     (9) added diagnostics terminal mode.
+ *
+ * 
+ * @subsection v201 V2.01
+ * 30 Jan 2004, Change include; (1) General clean up of in-line documentation, and 
+ *                                     (2) changed temperature resolution to 0.1 degrees F.
+ *
+ * 
+ * @subsection v200 V2.00
+ * 26 Oct 2002, Change include; (1) Micro Beacon II hardware changes including PIC18F252 processor,
+ *                                     (2) serial EEPROM, 
+ *                                     (3) GPS power control, 
+ *                                     (4) additional ADC input, and 
+ *                                     (5) LM60 temperature sensor.                            
+ *
+ *
+ * @subsection v101 V1.01
+ * 5 Dec 2001, Change include; (1) Changed startup message, and 
+ *                                    (2) applied SEPARATE pragma to several methods for memory usage.
+ *
+ *
+ * @subsection v100 V1.00
+ * 25 Sep 2001, Initial release.  Flew ANSR-3 and ANSR-4.
+ * 
+
+
+ *
+ *
+ * @section copyright_sec Copyright
+ *
+ * Copyright (c) 2001-2009 Michael Gray, KD7LMO
+
+
+ *
+ *
+ * @section gpl_sec GNU General Public License
+ *
+ *  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; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  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
+ *  
+
+
+ * 
+ * 
+ * @section design Design Details
+ *
+ * Provides design details on a variety of the components that make up the Pico Beacon.
+ *
+ *  @subpage power
+ */
+
+/**
+ *  @page power Power Consumption
+ *
+ *  Measured DC power consumption.
+ * 
+ *  3VDC prime power current 
+
+ *
+ *    7mA Held in reset 
+
+ *   18mA Processor running, all I/O off 
+
+ *  110mA GPS running 
+
+ *  120mA GPS running w/antenna 
+
+ *  250mA DDS running and GPS w/antenna 
+
+ *  420mA DDS running, GPS w/antenna, and PA chain on with no RF 
+
+ *  900mA Transmit 
+
+ *
+ */
+
+#ifndef AO_APRS_TEST
+#include <ao.h>
+#endif
+
+#include <ao_aprs.h>
+
+// Public methods, constants, and data structures for each class.
+
+static void timeInit(void);
+
+static void tncInit(void);
+static void tnc1200TimerTick(void);
+
+/** @} */
+
+/**
+ *  @defgroup sys System Library Functions
+ *
+ *  Generic system functions similiar to the run-time C library.
+ *
+ *  @{
+ */
+
+/**
+ *    Calculate the CRC-16 CCITT of buffer that is length bytes long.
+ *    The crc parameter allow the calculation on the CRC on multiple buffers.
+ *
+ *    @param buffer Pointer to data buffer.
+ *    @param length number of bytes in data buffer
+ *    @param crc starting value
+ *
+ *    @return CRC-16 of buffer[0 .. length]
+ */
+static uint16_t sysCRC16(const uint8_t *buffer, uint8_t length, uint16_t crc)
+{
+    uint8_t i, bit, value;
+
+    for (i = 0; i < length; ++i) 
+    {
+        value = buffer[i];
+
+        for (bit = 0; bit < 8; ++bit) 
+        {
+            crc ^= (value & 0x01);
+            crc = ( crc & 0x01 ) ? ( crc >> 1 ) ^ 0x8408 : ( crc >> 1 );
+            value = value >> 1;
+        } // END for
+    } // END for
+
+    return crc ^ 0xffff;
+}
+
+/** @} */
+
+/**
+ *  @defgroup rtc Real Time Interrupt tick
+ *
+ *  Manage the built-in real time interrupt.  The interrupt clock PRI is 104uS (9600 bps).
+ *
+ *  @{
+ */
+
+/// 16-bit NCO where the upper 8-bits are used to index into the frequency generation table.
+static uint16_t timeNCO;
+
+/// Audio tone NCO update step (phase).
+static uint16_t timeNCOFreq;
+
+/**
+ *   Initialize the real-time clock.
+ */
+static void timeInit()
+{
+    timeNCO = 0x00;
+    timeNCOFreq = 0x2000;
+}
+
+/** @} */
+
+/**
+ *  @defgroup tnc TNC (Terminal Node Controller)
+ *
+ *  Functions that provide a subset of the TNC functions.
+ *
+ *  @{
+ */
+
+/// The number of start flag bytes to send before the packet message.  (360bits * 1200bps = 300mS)
+#define TNC_TX_DELAY 45
+
+/// The size of the TNC output buffer.
+#define TNC_BUFFER_SIZE 40
+
+/// States that define the current mode of the 1200 bps (A-FSK) state machine.
+typedef enum
+{
+    /// Stand by state ready to accept new message.
+    TNC_TX_READY,
+
+    /// 0x7E bit stream pattern used to define start of APRS message.
+    TNC_TX_SYNC,
+
+    /// Transmit the AX.25 header that contains the source/destination call signs, APRS path, and flags.
+    TNC_TX_HEADER,
+
+    /// Transmit the message data.
+    TNC_TX_DATA,
+
+    /// Transmit the end flag sequence.
+    TNC_TX_END
+} TNC_TX_1200BPS_STATE;
+
+/// AX.25 compliant packet header that contains destination, station call sign, and path.
+/// 0x76 for SSID-11, 0x78 for SSID-12
+static uint8_t TNC_AX25_HEADER[] = { 
+    'A' << 1, 'P' << 1, 'A' << 1, 'M' << 1, ' ' << 1, ' ' << 1, 0x60, \
+    'N' << 1, '0' << 1, 'C' << 1, 'A' << 1, 'L' << 1, 'L' << 1, 0x78, \
+    'W' << 1, 'I' << 1, 'D' << 1, 'E' << 1, '2' << 1, ' ' << 1, 0x65, \
+    0x03, 0xf0 };
+
+#define TNC_CALLSIGN_OFF	7
+#define TNC_CALLSIGN_LEN	6
+
+static void
+tncSetCallsign(void)
+{
+#ifndef AO_APRS_TEST
+	uint8_t	i;
+
+	for (i = 0; i < TNC_CALLSIGN_LEN; i++) {
+		if (!ao_config.callsign[i])
+			break;
+		TNC_AX25_HEADER[TNC_CALLSIGN_OFF + i] = ao_config.callsign[i] << 1;
+	}
+	for (; i < TNC_CALLSIGN_LEN; i++)
+		TNC_AX25_HEADER[TNC_CALLSIGN_OFF + i] = ' ' << 1;
+#endif
+}
+
+/// The next bit to transmit.
+static uint8_t tncTxBit;
+
+/// Current mode of the 1200 bps state machine.
+static TNC_TX_1200BPS_STATE tncMode;
+
+/// Counter for each bit (0 - 7) that we are going to transmit.
+static uint8_t tncBitCount;
+
+/// A shift register that holds the data byte as we bit shift it for transmit.
+static uint8_t tncShift;
+
+/// Index into the APRS header and data array for each byte as we transmit it.
+static uint8_t tncIndex;
+
+/// The number of bytes in the message portion of the AX.25 message.
+static uint8_t tncLength;
+
+/// A copy of the last 5 bits we've transmitted to determine if we need to bit stuff on the next bit.
+static uint8_t tncBitStuff;
+
+/// Buffer to hold the message portion of the AX.25 packet as we prepare it.
+static uint8_t tncBuffer[TNC_BUFFER_SIZE];
+
+/** 
+ *   Initialize the TNC internal variables.
+ */
+static void tncInit()
+{
+    tncTxBit = 0;
+    tncMode = TNC_TX_READY;
+}
+
+/**
+ *   Method that is called every 833uS to transmit the 1200bps A-FSK data stream.
+ *   The provides the pre and postamble as well as the bit stuffed data stream.
+ */
+static void tnc1200TimerTick()
+{
+    // Set the A-FSK frequency.
+    if (tncTxBit == 0x00)
+        timeNCOFreq = 0x2000;
+    else
+        timeNCOFreq = 0x3aab;
+
+    switch (tncMode) 
+    {
+        case TNC_TX_READY:
+            // Generate a test signal alteranting between high and low tones.
+            tncTxBit = (tncTxBit == 0 ? 1 : 0);
+            break;
+
+        case TNC_TX_SYNC:
+            // The variable tncShift contains the lastest data byte.
+            // NRZI enocde the data stream.
+            if ((tncShift & 0x01) == 0x00) {
+                if (tncTxBit == 0)
+                    tncTxBit = 1;
+                else
+                    tncTxBit = 0;
+	    }
+                    
+            // When the flag is done, determine if we need to send more or data.
+            if (++tncBitCount == 8) 
+            {
+                tncBitCount = 0;
+                tncShift = 0x7e;
+
+                // Once we transmit x mS of flags, send the data.
+                // txDelay bytes * 8 bits/byte * 833uS/bit = x mS
+                if (++tncIndex == TNC_TX_DELAY) 
+                {
+                    tncIndex = 0;
+                    tncShift = TNC_AX25_HEADER[0];
+                    tncBitStuff = 0;
+                    tncMode = TNC_TX_HEADER;
+                } // END if
+            } else
+                tncShift = tncShift >> 1;
+            break;
+
+        case TNC_TX_HEADER:
+            // Determine if we have sent 5 ones in a row, if we have send a zero.
+            if (tncBitStuff == 0x1f) 
+            {
+                if (tncTxBit == 0)
+                    tncTxBit = 1;
+                else
+                    tncTxBit = 0;
+
+                tncBitStuff = 0x00;
+                return;
+            }    // END if
+
+            // The variable tncShift contains the lastest data byte.
+            // NRZI enocde the data stream.
+            if ((tncShift & 0x01) == 0x00) {
+                if (tncTxBit == 0)
+                    tncTxBit = 1;
+                else
+                    tncTxBit = 0;
+	    }
+
+            // Save the data stream so we can determine if bit stuffing is 
+            // required on the next bit time.
+            tncBitStuff = ((tncBitStuff << 1) | (tncShift & 0x01)) & 0x1f;
+
+            // If all the bits were shifted, get the next byte.
+            if (++tncBitCount == 8) 
+            {
+                tncBitCount = 0;
+
+                // After the header is sent, then send the data.
+                if (++tncIndex == sizeof(TNC_AX25_HEADER)) 
+                {
+                    tncIndex = 0;
+                    tncShift = tncBuffer[0];
+                    tncMode = TNC_TX_DATA;
+                } else
+                    tncShift = TNC_AX25_HEADER[tncIndex];
+
+            } else
+                tncShift = tncShift >> 1;
+
+            break;
+
+        case TNC_TX_DATA:
+            // Determine if we have sent 5 ones in a row, if we have send a zero.
+            if (tncBitStuff == 0x1f) 
+            {
+                if (tncTxBit == 0)
+                    tncTxBit = 1;
+                else
+                    tncTxBit = 0;
+
+                tncBitStuff = 0x00;
+                return;
+            }    // END if
+
+            // The variable tncShift contains the lastest data byte.
+            // NRZI enocde the data stream.
+            if ((tncShift & 0x01) == 0x00) {
+                if (tncTxBit == 0)
+                    tncTxBit = 1;
+                else
+                    tncTxBit = 0;
+	    }
+
+            // Save the data stream so we can determine if bit stuffing is 
+            // required on the next bit time.
+            tncBitStuff = ((tncBitStuff << 1) | (tncShift & 0x01)) & 0x1f;
+
+            // If all the bits were shifted, get the next byte.
+            if (++tncBitCount == 8) 
+            {
+                tncBitCount = 0;
+
+                // If everything was sent, transmit closing flags.
+                if (++tncIndex == tncLength) 
+                {
+                    tncIndex = 0;
+                    tncShift = 0x7e;
+                    tncMode = TNC_TX_END;
+                } else
+                    tncShift = tncBuffer[tncIndex];
+
+            } else
+                tncShift = tncShift >> 1;
+
+            break;
+
+        case TNC_TX_END:
+            // The variable tncShift contains the lastest data byte.
+            // NRZI enocde the data stream. 
+            if ((tncShift & 0x01) == 0x00) {
+                if (tncTxBit == 0)
+                    tncTxBit = 1;
+                else
+                    tncTxBit = 0;
+	    }
+
+            // If all the bits were shifted, get the next one.
+            if (++tncBitCount == 8) 
+            {
+                tncBitCount = 0;
+                tncShift = 0x7e;
+    
+                // Transmit two closing flags.
+                if (++tncIndex == 2) 
+                {
+                    tncMode = TNC_TX_READY;
+
+                    return;
+                } // END if
+            } else
+                tncShift = tncShift >> 1;
+
+            break;
+    } // END switch
+}
+
+/**
+ *   Generate the plain text position packet.
+ */
+static int tncPositionPacket(void)
+{
+    int32_t	latitude = ao_gps_data.latitude;
+    int32_t	longitude = ao_gps_data.longitude;
+    int32_t	altitude = ao_gps_data.altitude;
+
+    uint16_t	lat_deg;
+    uint16_t	lon_deg;
+    uint16_t	lat_min;
+    uint16_t	lat_frac;
+    uint16_t	lon_min;
+    uint16_t	lon_frac;
+
+    char	lat_sign = 'N', lon_sign = 'E';
+
+    if (latitude < 0) {
+	lat_sign = 'S';
+	latitude = -latitude;
+    }
+
+    if (longitude < 0) {
+	lon_sign = 'W';
+	longitude = -longitude;
+    }
+
+    /* Round latitude and longitude by 0.005 minutes */
+    latitude = latitude + 833;
+    if (latitude > 900000000)
+	latitude = 900000000;
+    longitude = longitude + 833;
+    if (longitude > 1800000000)
+	    longitude = 1800000000;
+
+    lat_deg = latitude / 10000000;
+    latitude -= lat_deg * 10000000;
+    latitude *= 60;
+    lat_min = latitude / 10000000;
+    latitude -= lat_min * 10000000;
+    lat_frac = latitude / 100000;
+
+    lon_deg = longitude / 10000000;
+    longitude -= lon_deg * 10000000;
+    longitude *= 60;
+    lon_min = longitude / 10000000;
+    longitude -= lon_min * 10000000;
+    lon_frac = longitude / 100000;
+
+    if (altitude < 0)
+	altitude = 0;
+
+    altitude = (altitude * (int32_t) 10000 + (3048/2)) / (int32_t) 3048;
+    
+    return sprintf ((char *) tncBuffer, "=%02u%02u.%02u%c\\%03u%02u.%02u%cO /A=%06u\015",
+		    lat_deg, lat_min, lat_frac, lat_sign,
+		    lon_deg, lon_min, lon_frac, lon_sign,
+		    altitude);
+}
+
+static int16_t
+tncFill(uint8_t *buf, int16_t len)
+{
+    int16_t	l = 0;
+    uint8_t	b;
+    uint8_t	bit;
+
+    while (tncMode != TNC_TX_READY && l < len) {
+	b = 0;
+	for (bit = 0; bit < 8; bit++) {
+	    b = b << 1 | (timeNCO >> 15);
+	    timeNCO += timeNCOFreq;
+	}
+	*buf++ = b;
+	l++;
+	tnc1200TimerTick();
+    }
+    if (tncMode == TNC_TX_READY)
+	l = -l;
+    return l;
+}
+
+/** 
+ *    Prepare an AX.25 data packet.  Each time this method is called, it automatically
+ *    rotates through 1 of 3 messages.
+ *
+ *    @param dataMode enumerated type that specifies 1200bps A-FSK or 9600bps FSK
+ */
+void ao_aprs_send(void)
+{
+    uint16_t crc;
+
+    timeInit();
+    tncInit();
+    tncSetCallsign();
+
+    tncLength = tncPositionPacket();
+
+    // Calculate the CRC for the header and message.
+    crc = sysCRC16(TNC_AX25_HEADER, sizeof(TNC_AX25_HEADER), 0xffff);
+    crc = sysCRC16(tncBuffer, tncLength, crc ^ 0xffff);
+
+    // Save the CRC in the message.
+    tncBuffer[tncLength++] = crc & 0xff;
+    tncBuffer[tncLength++] = (crc >> 8) & 0xff;
+
+    // Prepare the variables that are used in the real-time clock interrupt.
+    tncBitCount = 0;
+    tncShift = 0x7e;
+    tncTxBit = 0;
+    tncIndex = 0;
+    tncMode = TNC_TX_SYNC;
+
+    ao_radio_send_lots(tncFill);
+}
+
+/** @} */
diff --git a/src/drivers/ao_aprs.h b/src/drivers/ao_aprs.h
new file mode 100644
index 00000000..a033fa0b
--- /dev/null
+++ b/src/drivers/ao_aprs.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_APRS_H_
+#define _AO_APRS_H_
+
+void
+ao_aprs_send(void);
+
+#endif /* _AO_APRS_H_ */
diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c
index f27958f9..63d2f955 100644
--- a/src/drivers/ao_cc1120.c
+++ b/src/drivers/ao_cc1120.c
@@ -24,10 +24,12 @@
 #define AO_RADIO_MAX_RECV	sizeof(struct ao_packet)
 #define AO_RADIO_MAX_SEND	sizeof(struct ao_packet)
 
-uint8_t ao_radio_wake;
-uint8_t ao_radio_mutex;
-uint8_t ao_radio_abort;
-uint8_t ao_radio_in_recv;
+static uint8_t ao_radio_mutex;
+
+static uint8_t ao_radio_wake;		/* radio ready. Also used as sleep address */
+static uint8_t ao_radio_abort;		/* radio operation should abort */
+static uint8_t ao_radio_mcu_wake;	/* MARC status change */
+static uint8_t ao_radio_marc_status;	/* Last read MARC status value */
 
 #define CC1120_DEBUG	AO_FEC_DEBUG
 #define CC1120_TRACE	0
@@ -218,13 +220,32 @@ ao_radio_recv_abort(void)
 #define ao_radio_rdf_value 0x55
 
 static uint8_t
-ao_radio_marc_status(void)
+ao_radio_get_marc_status(void)
 {
 	return ao_radio_reg_read(CC1120_MARC_STATUS1);
 }
 
 static void
-ao_radio_tx_isr(void)
+ao_radio_mcu_wakeup_isr(void)
+{
+	ao_radio_mcu_wake = 1;
+	ao_wakeup(&ao_radio_wake);
+}
+
+
+static void
+ao_radio_check_marc_status(void)
+{
+	ao_radio_mcu_wake = 0;
+	ao_radio_marc_status = ao_radio_get_marc_status();
+	
+	/* Anyt other than 'tx/rx finished' means an error occurred */
+	if (ao_radio_marc_status & ~(CC1120_MARC_STATUS1_TX_FINISHED|CC1120_MARC_STATUS1_RX_FINISHED))
+		ao_radio_abort = 1;
+}
+
+static void
+ao_radio_isr(void)
 {
 	ao_exti_disable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
 	ao_radio_wake = 1;
@@ -234,8 +255,9 @@ ao_radio_tx_isr(void)
 static void
 ao_radio_start_tx(void)
 {
-	ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_tx_isr);
+	ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_isr);
 	ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
+	ao_exti_enable(AO_CC1120_MCU_WAKEUP_PORT, AO_CC1120_MCU_WAKEUP_PIN);
 	ao_radio_strobe(CC1120_STX);
 }
 
@@ -247,6 +269,8 @@ ao_radio_idle(void)
 		if ((state >> CC1120_STATUS_STATE) == CC1120_STATUS_STATE_IDLE)
 			break;
 	}
+	/* Flush any pending TX bytes */
+	ao_radio_strobe(CC1120_SFTX);
 }
 
 /*
@@ -261,7 +285,7 @@ ao_radio_idle(void)
 #define PACKET_DEV_M	80
 
 /*
- * For our packet data, set the symbol rate to 38360 Baud
+ * For our packet data, set the symbol rate to 38400 Baud
  *
  *              (2**20 + DATARATE_M) * 2 ** DATARATE_E
  *	Rdata = -------------------------------------- * fosc
@@ -294,18 +318,19 @@ static const uint16_t packet_setup[] = {
 				 (0 << CC1120_PKT_CFG0_PKG_BIT_LEN) |
 				 (0 << CC1120_PKT_CFG0_UART_MODE_EN) |
 				 (0 << CC1120_PKT_CFG0_UART_SWAP_EN)),
+	AO_CC1120_MARC_GPIO_IOCFG,		CC1120_IOCFG_GPIO_CFG_MARC_MCU_WAKEUP,
 };
 
 static const uint16_t packet_tx_setup[] = {
 	CC1120_PKT_CFG2,	((CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR << CC1120_PKT_CFG2_CCA_MODE) |
 				 (CC1120_PKT_CFG2_PKT_FORMAT_NORMAL << CC1120_PKT_CFG2_PKT_FORMAT)),
-	CC1120_IOCFG2, 		CC1120_IOCFG_GPIO_CFG_RX0TX1_CFG,
+	AO_CC1120_INT_GPIO_IOCFG, 		CC1120_IOCFG_GPIO_CFG_RX0TX1_CFG,
 };
 
 static const uint16_t packet_rx_setup[] = {
 	CC1120_PKT_CFG2,	((CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR << CC1120_PKT_CFG2_CCA_MODE) |
 				 (CC1120_PKT_CFG2_PKT_FORMAT_SYNCHRONOUS_SERIAL << CC1120_PKT_CFG2_PKT_FORMAT)),
-	CC1120_IOCFG2, 		CC1120_IOCFG_GPIO_CFG_CLKEN_SOFT,
+	AO_CC1120_INT_GPIO_IOCFG, 		CC1120_IOCFG_GPIO_CFG_CLKEN_SOFT,
 };
 
 /*
@@ -323,12 +348,12 @@ static const uint16_t packet_rx_setup[] = {
 /*
  * For our RDF beacon, set the symbol rate to 2kBaud (for a 1kHz tone)
  *
- *              (2**20 - DATARATE_M) * 2 ** DATARATE_E
+ *              (2**20 + DATARATE_M) * 2 ** DATARATE_E
  *	Rdata = -------------------------------------- * fosc
  *		             2 ** 39
  *
- *	DATARATE_M = 511705
- *	DATARATE_E = 6
+ *	DATARATE_M = 25166
+ *	DATARATE_E = 5
  *
  * To make the tone last for 200ms, we need 2000 * .2 = 400 bits or 50 bytes
  */
@@ -358,7 +383,64 @@ static const uint16_t rdf_setup[] = {
 				 (0 << CC1120_PKT_CFG0_UART_SWAP_EN)),
 };
 
-static uint8_t ao_radio_mode;
+/*
+ * APRS deviation is 5kHz
+ *
+ *	fdev = fosc >> 24 * (256 + dev_m) << dev_e
+ *
+ *     	32e6Hz / (2 ** 24) * (256 + 71) * (2 ** 3) = 4989
+ */
+
+#define APRS_DEV_E	3
+#define APRS_DEV_M	71
+#define APRS_PACKET_LEN	50
+
+/*
+ * For our APRS beacon, set the symbol rate to 9.6kBaud (8x oversampling for 1200 baud data rate)
+ *
+ *              (2**20 + DATARATE_M) * 2 ** DATARATE_E
+ *	Rdata = -------------------------------------- * fosc
+ *		             2 ** 39
+ *
+ *	DATARATE_M = 239914
+ *	DATARATE_E = 7
+ *
+ *	Rdata = 9599.998593330383301
+ *
+ */
+#define APRS_DRATE_E	7
+#define APRS_DRATE_M	239914
+
+static const uint16_t aprs_setup[] = {
+	CC1120_DEVIATION_M,	APRS_DEV_M,
+	CC1120_MODCFG_DEV_E,	((CC1120_MODCFG_DEV_E_MODEM_MODE_NORMAL << CC1120_MODCFG_DEV_E_MODEM_MODE) |
+				 (CC1120_MODCFG_DEV_E_MOD_FORMAT_2_GFSK << CC1120_MODCFG_DEV_E_MOD_FORMAT) |
+				 (APRS_DEV_E << CC1120_MODCFG_DEV_E_DEV_E)),
+	CC1120_DRATE2,		((APRS_DRATE_E << CC1120_DRATE2_DATARATE_E) |
+				 (((APRS_DRATE_M >> 16) & CC1120_DRATE2_DATARATE_M_19_16_MASK) << CC1120_DRATE2_DATARATE_M_19_16)),
+	CC1120_DRATE1,		((APRS_DRATE_M >> 8) & 0xff),
+	CC1120_DRATE0,		((APRS_DRATE_M >> 0) & 0xff),
+	CC1120_PKT_CFG2,	((CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR << CC1120_PKT_CFG2_CCA_MODE) |
+				 (CC1120_PKT_CFG2_PKT_FORMAT_NORMAL << CC1120_PKT_CFG2_PKT_FORMAT)),
+	CC1120_PKT_CFG1,	((0 << CC1120_PKT_CFG1_WHITE_DATA) |
+				 (CC1120_PKT_CFG1_ADDR_CHECK_CFG_NONE << CC1120_PKT_CFG1_ADDR_CHECK_CFG) |
+				 (CC1120_PKT_CFG1_CRC_CFG_DISABLED << CC1120_PKT_CFG1_CRC_CFG) |
+				 (0 << CC1120_PKT_CFG1_APPEND_STATUS)),
+};
+
+#define AO_PKT_CFG0_INFINITE ((0 << CC1120_PKT_CFG0_RESERVED7) |	\
+			      (CC1120_PKT_CFG0_LENGTH_CONFIG_INFINITE << CC1120_PKT_CFG0_LENGTH_CONFIG) | \
+			      (0 << CC1120_PKT_CFG0_PKG_BIT_LEN) |	\
+			      (0 << CC1120_PKT_CFG0_UART_MODE_EN) |	\
+			      (0 << CC1120_PKT_CFG0_UART_SWAP_EN))
+
+#define AO_PKT_CFG0_FIXED ((0 << CC1120_PKT_CFG0_RESERVED7) |		\
+			   (CC1120_PKT_CFG0_LENGTH_CONFIG_FIXED << CC1120_PKT_CFG0_LENGTH_CONFIG) | \
+			   (0 << CC1120_PKT_CFG0_PKG_BIT_LEN) |		\
+			   (0 << CC1120_PKT_CFG0_UART_MODE_EN) |	\
+			   (0 << CC1120_PKT_CFG0_UART_SWAP_EN))
+
+static uint16_t ao_radio_mode;
 
 #define AO_RADIO_MODE_BITS_PACKET	1
 #define AO_RADIO_MODE_BITS_PACKET_TX	2
@@ -366,17 +448,23 @@ static uint8_t ao_radio_mode;
 #define AO_RADIO_MODE_BITS_TX_FINISH	8
 #define AO_RADIO_MODE_BITS_PACKET_RX	16
 #define AO_RADIO_MODE_BITS_RDF		32
+#define AO_RADIO_MODE_BITS_APRS		64
+#define AO_RADIO_MODE_BITS_INFINITE	128
+#define AO_RADIO_MODE_BITS_FIXED	256
 
 #define AO_RADIO_MODE_NONE		0
 #define AO_RADIO_MODE_PACKET_TX_BUF	(AO_RADIO_MODE_BITS_PACKET | AO_RADIO_MODE_BITS_PACKET_TX | AO_RADIO_MODE_BITS_TX_BUF)
 #define AO_RADIO_MODE_PACKET_TX_FINISH	(AO_RADIO_MODE_BITS_PACKET | AO_RADIO_MODE_BITS_PACKET_TX | AO_RADIO_MODE_BITS_TX_FINISH)
 #define AO_RADIO_MODE_PACKET_RX		(AO_RADIO_MODE_BITS_PACKET | AO_RADIO_MODE_BITS_PACKET_RX)
 #define AO_RADIO_MODE_RDF		(AO_RADIO_MODE_BITS_RDF | AO_RADIO_MODE_BITS_TX_FINISH)
+#define AO_RADIO_MODE_APRS_BUF		(AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_INFINITE | AO_RADIO_MODE_BITS_TX_BUF)
+#define AO_RADIO_MODE_APRS_LAST_BUF	(AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_FIXED | AO_RADIO_MODE_BITS_TX_BUF)
+#define AO_RADIO_MODE_APRS_FINISH	(AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_FIXED | AO_RADIO_MODE_BITS_TX_FINISH)
 
 static void
-ao_radio_set_mode(uint8_t new_mode)
+ao_radio_set_mode(uint16_t new_mode)
 {
-	uint8_t	changes;
+	uint16_t changes;
 	int i;
 
 	if (new_mode == ao_radio_mode)
@@ -392,10 +480,10 @@ ao_radio_set_mode(uint8_t new_mode)
 			ao_radio_reg_write(packet_tx_setup[i], packet_tx_setup[i+1]);
 		
 	if (changes & AO_RADIO_MODE_BITS_TX_BUF)
-		ao_radio_reg_write(CC1120_IOCFG2, CC1120_IOCFG_GPIO_CFG_TXFIFO_THR);
+		ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_TXFIFO_THR);
 
 	if (changes & AO_RADIO_MODE_BITS_TX_FINISH)
-		ao_radio_reg_write(CC1120_IOCFG2, CC1120_IOCFG_GPIO_CFG_RX0TX1_CFG);
+		ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_RX0TX1_CFG);
 
 	if (changes & AO_RADIO_MODE_BITS_PACKET_RX)
 		for (i = 0; i < sizeof (packet_rx_setup) / sizeof (packet_rx_setup[0]); i += 2)
@@ -404,6 +492,17 @@ ao_radio_set_mode(uint8_t new_mode)
 	if (changes & AO_RADIO_MODE_BITS_RDF)
 		for (i = 0; i < sizeof (rdf_setup) / sizeof (rdf_setup[0]); i += 2)
 			ao_radio_reg_write(rdf_setup[i], rdf_setup[i+1]);
+
+	if (changes & AO_RADIO_MODE_BITS_APRS)
+		for (i = 0; i < sizeof (aprs_setup) / sizeof (aprs_setup[0]); i += 2)
+			ao_radio_reg_write(aprs_setup[i], aprs_setup[i+1]);
+
+	if (changes & AO_RADIO_MODE_BITS_INFINITE)
+		ao_radio_reg_write(CC1120_PKT_CFG0, AO_PKT_CFG0_INFINITE);
+
+	if (changes & AO_RADIO_MODE_BITS_FIXED)
+		ao_radio_reg_write(CC1120_PKT_CFG0, AO_PKT_CFG0_FIXED);
+
 	ao_radio_mode = new_mode;
 }
 
@@ -431,10 +530,20 @@ ao_radio_setup(void)
 }
 
 static void
+ao_radio_set_len(uint8_t len)
+{
+	static uint8_t	last_len;
+
+	if (len != last_len) {
+		ao_radio_reg_write(CC1120_PKT_LEN, len);
+		last_len = len;
+	}
+}
+
+static void
 ao_radio_get(uint8_t len)
 {
 	static uint32_t	last_radio_setting;
-	static uint8_t	last_len;
 
 	ao_mutex_get(&ao_radio_mutex);
 	if (!ao_radio_configured)
@@ -445,10 +554,7 @@ ao_radio_get(uint8_t len)
 		ao_radio_reg_write(CC1120_FREQ0, ao_config.radio_setting);
 		last_radio_setting = ao_config.radio_setting;
 	}
-	if (len != last_len) {
-		ao_radio_reg_write(CC1120_PKT_LEN, len);
-		last_len = len;
-	}
+	ao_radio_set_len(len);
 }
 
 #define ao_radio_put()	ao_mutex_put(&ao_radio_mutex)
@@ -470,9 +576,11 @@ ao_rdf_run(void)
 	ao_radio_start_tx();
 
 	ao_arch_block_interrupts();
-	while (!ao_radio_wake && !ao_radio_abort)
+	while (!ao_radio_wake && !ao_radio_abort && !ao_radio_mcu_wake)
 		ao_sleep(&ao_radio_wake);
 	ao_arch_release_interrupts();
+	if (ao_radio_mcu_wake)
+		ao_radio_check_marc_status();
 	if (!ao_radio_wake)
 		ao_radio_idle();
 	ao_radio_put();
@@ -562,6 +670,31 @@ ao_radio_test_cmd(void)
 	}
 }
 
+static void
+ao_radio_wait_isr(void)
+{
+	ao_arch_block_interrupts();
+	while (!ao_radio_wake && !ao_radio_mcu_wake && !ao_radio_abort)
+		ao_sleep(&ao_radio_wake);
+	ao_arch_release_interrupts();
+	if (ao_radio_mcu_wake)
+		ao_radio_check_marc_status();
+}
+
+static uint8_t
+ao_radio_wait_tx(uint8_t wait_fifo)
+{
+	uint8_t	fifo_space = 0;
+
+	do {
+		ao_radio_wait_isr();
+		if (!wait_fifo)
+			return 0;
+		fifo_space = ao_radio_tx_fifo_space();
+	} while (!fifo_space && !ao_radio_abort);
+	return fifo_space;
+}
+
 static uint8_t	tx_data[(AO_RADIO_MAX_SEND + 4) * 2];
 
 void
@@ -583,6 +716,7 @@ ao_radio_send(const void *d, uint8_t size)
 	while (encode_len) {
 		this_len = encode_len;
 
+		ao_radio_wake = 0;
 		if (this_len > fifo_space) {
 			this_len = fifo_space;
 			ao_radio_set_mode(AO_RADIO_MODE_PACKET_TX_BUF);
@@ -601,16 +735,81 @@ ao_radio_send(const void *d, uint8_t size)
 			ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
 		}
 			
-		do {
-			ao_radio_wake = 0;
-			ao_arch_block_interrupts();
-			while (!ao_radio_wake)
-				ao_sleep(&ao_radio_wake);
-			ao_arch_release_interrupts();
-			if (!encode_len)
+		fifo_space = ao_radio_wait_tx(encode_len != 0);
+		if (ao_radio_abort) {
+			ao_radio_idle();
+			break;
+		}
+	}
+	ao_radio_put();
+}
+
+#define AO_RADIO_LOTS	64
+
+void
+ao_radio_send_lots(ao_radio_fill_func fill)
+{
+	uint8_t	buf[AO_RADIO_LOTS], *b;
+	int	cnt;
+	int	total = 0;
+	uint8_t	done = 0;
+	uint8_t	started = 0;
+	uint8_t	fifo_space;
+
+	ao_radio_get(0xff);
+	fifo_space = CC1120_FIFO_SIZE;
+	while (!done) {
+		cnt = (*fill)(buf, sizeof(buf));
+		if (cnt < 0) {
+			done = 1;
+			cnt = -cnt;
+		}
+		total += cnt;
+
+		/* At the last buffer, set the total length */
+		if (done)
+			ao_radio_set_len(total & 0xff);
+
+		b = buf;
+		while (cnt) {
+			uint8_t	this_len = cnt;
+
+			/* Wait for some space in the fifo */
+			while (!ao_radio_abort && (fifo_space = ao_radio_tx_fifo_space()) == 0) {
+				ao_radio_wake = 0;
+				ao_radio_wait_isr();
+			}
+			if (ao_radio_abort)
 				break;
-			fifo_space = ao_radio_tx_fifo_space();
-		} while (!fifo_space);
+			if (this_len > fifo_space)
+				this_len = fifo_space;
+
+			cnt -= this_len;
+
+			if (done) {
+				if (cnt)
+					ao_radio_set_mode(AO_RADIO_MODE_APRS_LAST_BUF);
+				else
+					ao_radio_set_mode(AO_RADIO_MODE_APRS_FINISH);
+			} else
+				ao_radio_set_mode(AO_RADIO_MODE_APRS_BUF);
+
+			ao_radio_fifo_write(b, this_len);
+			b += this_len;
+
+			if (!started) {
+				ao_radio_start_tx();
+				started = 1;
+			} else
+				ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
+		}
+		if (ao_radio_abort) {
+			ao_radio_idle();
+			break;
+		}
+		/* Wait for the transmitter to go idle */
+		ao_radio_wake = 0;
+		ao_radio_wait_isr();
 	}
 	ao_radio_put();
 }
@@ -660,14 +859,21 @@ ao_radio_rx_isr(void)
 static uint16_t
 ao_radio_rx_wait(void)
 {
-	ao_arch_block_interrupts();
-	rx_waiting = 1;
-	while (rx_data_cur - rx_data_consumed < AO_FEC_DECODE_BLOCK &&
-	       !ao_radio_abort) {
-		ao_sleep(&ao_radio_wake);
-	}
-	rx_waiting = 0;
-	ao_arch_release_interrupts();
+	do {
+		if (ao_radio_mcu_wake)
+			ao_radio_check_marc_status();
+		ao_arch_block_interrupts();
+		rx_waiting = 1;
+		while (rx_data_cur - rx_data_consumed < AO_FEC_DECODE_BLOCK &&
+		       !ao_radio_abort &&
+		       !ao_radio_mcu_wake)
+		{
+			if (ao_sleep(&ao_radio_wake))
+				ao_radio_abort = 1;
+		}
+		rx_waiting = 0;
+		ao_arch_release_interrupts();
+	} while (ao_radio_mcu_wake);
 	if (ao_radio_abort)
 		return 0;
 	rx_data_consumed += AO_FEC_DECODE_BLOCK;
@@ -703,30 +909,53 @@ ao_radio_recv(__xdata void *d, uint8_t size)
 	rx_data_consumed = 0;
 	rx_ignore = 2;
 
+	/* Must be set before changing the frequency; any abort
+	 * after the frequency is set needs to terminate the read
+	 * so that the registers can be reprogrammed
+	 */
 	ao_radio_abort = 0;
-	ao_radio_in_recv = 1;
+
 	/* configure interrupt pin */
 	ao_radio_get(len);
 	ao_radio_set_mode(AO_RADIO_MODE_PACKET_RX);
 
 	ao_radio_wake = 0;
+	ao_radio_mcu_wake = 0;
 
 	stm_spi2.cr2 = 0;
 
 	/* clear any RXNE */
 	(void) stm_spi2.dr;
 
-	ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_rx_isr);
+	/* Have the radio signal when the preamble quality goes high */
+	ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_PQT_REACHED);
+	ao_exti_set_mode(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN,
+			 AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_HIGH);
+	ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_isr);
 	ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
+	ao_exti_enable(AO_CC1120_MCU_WAKEUP_PORT, AO_CC1120_MCU_WAKEUP_PIN);
 
 	ao_radio_strobe(CC1120_SRX);
 
+	/* Wait for the preamble to appear */
+	ao_radio_wait_isr();
+	if (ao_radio_abort)
+		goto abort;
+
+	ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_CLKEN_SOFT);
+	ao_exti_set_mode(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN,
+			 AO_EXTI_MODE_FALLING|AO_EXTI_PRIORITY_HIGH);
+
+	ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_rx_isr);
+	ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
+
 	ao_radio_burst_read_start(CC1120_SOFT_RX_DATA_OUT);
 
 	ret = ao_fec_decode(rx_data, rx_data_count, d, size + 2, ao_radio_rx_wait);
 
 	ao_radio_burst_read_stop();
 
+abort:
 	ao_radio_strobe(CC1120_SIDLE);
 
 	/* Convert from 'real' rssi to cc1111-style values */
@@ -739,11 +968,6 @@ ao_radio_recv(__xdata void *d, uint8_t size)
 
 	((uint8_t *) d)[size] = (uint8_t) rssi;
 
-	ao_radio_in_recv = 0;
-
-	if (ao_radio_abort)
-		ao_delay(1);
-
 #if AO_PROFILE
 	rx_last_done_tick = rx_done_tick;
 	rx_done_tick = ao_profile_tick();
@@ -963,7 +1187,7 @@ static void ao_radio_show(void) {
 	printf ("Status:   %02x\n", status);
 	printf ("CHIP_RDY: %d\n", (status >> CC1120_STATUS_CHIP_RDY) & 1);
 	printf ("STATE:    %s\n", cc1120_state_name[(status >> CC1120_STATUS_STATE) & CC1120_STATUS_STATE_MASK]);
-	printf ("MARC:     %02x\n", ao_radio_marc_status());
+	printf ("MARC:     %02x\n", ao_radio_get_marc_status());
 
 	for (i = 0; i < AO_NUM_CC1120_REG; i++)
 		printf ("\t%02x %-20.20s\n", ao_radio_reg_read(ao_cc1120_reg[i].addr), ao_cc1120_reg[i].name);
@@ -971,7 +1195,7 @@ static void ao_radio_show(void) {
 }
 
 static void ao_radio_beep(void) {
-	ao_radio_rdf(RDF_PACKET_LEN);
+	ao_radio_rdf();
 }
 
 static void ao_radio_packet(void) {
@@ -1007,11 +1231,25 @@ ao_radio_test_recv()
 	}
 }
 
+#if HAS_APRS
+#include <ao_aprs.h>
+
+static void
+ao_radio_aprs()
+{
+	ao_packet_slave_stop();
+	ao_aprs_send();
+}
+#endif
+
 #endif
 
 static const struct ao_cmds ao_radio_cmds[] = {
 	{ ao_radio_test_cmd,	"C <1 start, 0 stop, none both>\0Radio carrier test" },
 #if CC1120_DEBUG
+#if HAS_APRS
+	{ ao_radio_aprs,	"G\0Send APRS packet" },
+#endif
 	{ ao_radio_show,	"R\0Show CC1120 status" },
 	{ ao_radio_beep,	"b\0Emit an RDF beacon" },
 	{ ao_radio_packet,	"p\0Send a test packet" },
@@ -1043,7 +1281,13 @@ ao_radio_init(void)
 	ao_enable_port(AO_CC1120_INT_PORT);
 	ao_exti_setup(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN,
 		      AO_EXTI_MODE_FALLING|AO_EXTI_PRIORITY_HIGH,
-		      ao_radio_tx_isr);
+		      ao_radio_isr);
+
+	/* Enable the hacked up GPIO3 pin */
+	ao_enable_port(AO_CC1120_MCU_WAKEUP_PORT);
+	ao_exti_setup(AO_CC1120_MCU_WAKEUP_PORT, AO_CC1120_MCU_WAKEUP_PIN,
+		      AO_EXTI_MODE_FALLING|AO_EXTI_PRIORITY_MED,
+		      ao_radio_mcu_wakeup_isr);
 
 	ao_cmd_register(&ao_radio_cmds[0]);
 }
diff --git a/src/drivers/ao_m25.c b/src/drivers/ao_m25.c
index 9603c1de..9f696ace 100644
--- a/src/drivers/ao_m25.c
+++ b/src/drivers/ao_m25.c
@@ -99,18 +99,7 @@ static __xdata uint8_t ao_m25_mutex;
 
 static __xdata uint8_t	ao_m25_instruction[4];
 
-#if HAS_BOOT_RADIO
-extern uint8_t ao_radio_in_recv;
-
-static void ao_boot_radio(void) {
-	if (ao_radio_in_recv)
-		ao_radio_recv_abort();
-}
-#else
-#define ao_boot_radio()
-#endif
-
-#define M25_SELECT(cs)		do { ao_boot_radio(); ao_spi_get_mask(AO_M25_SPI_CS_PORT,cs,AO_M25_SPI_BUS, AO_SPI_SPEED_FAST); } while (0)
+#define M25_SELECT(cs)		ao_spi_get_mask(AO_M25_SPI_CS_PORT,cs,AO_M25_SPI_BUS, AO_SPI_SPEED_FAST)
 #define M25_DESELECT(cs)	ao_spi_put_mask(AO_M25_SPI_CS_PORT,cs,AO_M25_SPI_BUS)
 
 #define M25_BLOCK_SHIFT			16
diff --git a/src/megadongle-v0.1/ao_pins.h b/src/megadongle-v0.1/ao_pins.h
index cabe9ee2..5a5eaa30 100644
--- a/src/megadongle-v0.1/ao_pins.h
+++ b/src/megadongle-v0.1/ao_pins.h
@@ -132,15 +132,21 @@
 #define AO_RADIO_CAL_DEFAULT 	0x6ca333
 
 #define AO_FEC_DEBUG		0
-#define AO_CC1120_SPI_CS_PORT	(&stm_gpioc)
-#define AO_CC1120_SPI_CS_PIN	5
+#define AO_CC1120_SPI_CS_PORT	(&stm_gpioa)
+#define AO_CC1120_SPI_CS_PIN	0
 #define AO_CC1120_SPI_BUS	AO_SPI_2_PB13_PB14_PB15
 
 #define AO_CC1120_INT_PORT	(&stm_gpioc)
 #define AO_CC1120_INT_PIN	14
 
+#define AO_CC1120_MCU_WAKEUP_PORT	(&stm_gpioc)
+#define AO_CC1120_MCU_WAKEUP_PIN	(0)
+
 #define AO_CC1120_INT_GPIO	2
-#define HAS_BOOT_RADIO		1
+#define AO_CC1120_INT_GPIO_IOCFG	CC1120_IOCFG2
+
+#define AO_CC1120_MARC_GPIO	3
+#define AO_CC1120_MARC_GPIO_IOCFG	CC1120_IOCFG3
 
 /*
  * Profiling Viterbi decoding
diff --git a/src/megametrum-v0.1/Makefile b/src/megametrum-v0.1/Makefile
index 7d6c7388..a5fdcbb2 100644
--- a/src/megametrum-v0.1/Makefile
+++ b/src/megametrum-v0.1/Makefile
@@ -37,12 +37,12 @@ INC = \
 #PROFILE=ao_profile.c
 #PROFILE_DEF=-DAO_PROFILE=1
 
-SAMPLE_PROFILE=ao_sample_profile.c \
-	ao_sample_profile_timer.c
-SAMPLE_PROFILE_DEF=-DHAS_SAMPLE_PROFILE=1
+#SAMPLE_PROFILE=ao_sample_profile.c \
+#	ao_sample_profile_timer.c
+#SAMPLE_PROFILE_DEF=-DHAS_SAMPLE_PROFILE=1
 
-STACK_GUARD=ao_mpu_stm.c
-STACK_GUARD_DEF=-DHAS_STACK_GUARD=1
+#STACK_GUARD=ao_mpu_stm.c
+#STACK_GUARD_DEF=-DHAS_STACK_GUARD=1
 
 ALTOS_SRC = \
 	ao_interrupt.c \
@@ -90,6 +90,7 @@ ALTOS_SRC = \
 	ao_packet.c \
 	ao_companion.c \
 	ao_pyro.c \
+	ao_aprs.c \
 	$(PROFILE) \
 	$(SAMPLE_PROFILE) \
 	$(STACK_GUARD)
diff --git a/src/megametrum-v0.1/ao_megametrum.c b/src/megametrum-v0.1/ao_megametrum.c
index cb1eb417..fbdab64a 100644
--- a/src/megametrum-v0.1/ao_megametrum.c
+++ b/src/megametrum-v0.1/ao_megametrum.c
@@ -53,7 +53,9 @@ main(void)
 	ao_exti_init();
 
 	ao_adc_init();
+#if HAS_BEEP
 	ao_beep_init();
+#endif
 	ao_cmd_init();
 
 #if HAS_MS5607
diff --git a/src/megametrum-v0.1/ao_pins.h b/src/megametrum-v0.1/ao_pins.h
index f07dc26e..b1a70ea2 100644
--- a/src/megametrum-v0.1/ao_pins.h
+++ b/src/megametrum-v0.1/ao_pins.h
@@ -70,6 +70,7 @@
 #define HAS_BEEP		1
 #define HAS_RADIO		1
 #define HAS_TELEMETRY		1
+#define HAS_APRS		1
 
 #define HAS_SPI_1		1
 #define SPI_1_PA5_PA6_PA7	1	/* Barometer */
@@ -281,11 +282,19 @@ struct ao_adc {
 #define AO_CC1120_SPI_CS_PIN	5
 #define AO_CC1120_SPI_BUS	AO_SPI_2_PB13_PB14_PB15
 
-#define AO_CC1120_INT_PORT	(&stm_gpioc)
-#define AO_CC1120_INT_PIN	14
+#define AO_CC1120_INT_PORT		(&stm_gpioc)
+#define AO_CC1120_INT_PIN		14
+#define AO_CC1120_MCU_WAKEUP_PORT	(&stm_gpioc)
+#define AO_CC1120_MCU_WAKEUP_PIN	(0)
 
 #define AO_CC1120_INT_GPIO	2
-#define HAS_BOOT_RADIO		1
+#define AO_CC1120_INT_GPIO_IOCFG	CC1120_IOCFG2
+
+#define AO_CC1120_MARC_GPIO	3
+#define AO_CC1120_MARC_GPIO_IOCFG	CC1120_IOCFG3
+
+
+#define HAS_BOOT_RADIO		0
 
 /*
  * Mag sensor (hmc5883)
diff --git a/src/test/Makefile b/src/test/Makefile
index 0dcdc949..092bf360 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -1,7 +1,8 @@
 vpath % ..:../core:../drivers:../util
 
 PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \
-	ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_test
+	ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_test \
+	ao_aprs_test
 
 INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h
 
@@ -9,7 +10,7 @@ KALMAN=make-kalman
 
 CFLAGS=-I.. -I. -I../core -I../drivers -O0 -g -Wall
 
-all: $(PROGS)
+all: $(PROGS) ao_aprs_data.wav
 
 clean:
 	rm -f $(PROGS) run-out.baro run-out.full
@@ -49,5 +50,14 @@ ao_kalman.h: $(KALMAN)
 ao_fec_test: ao_fec_test.c ao_fec_tx.c ao_fec_rx.c
 	cc $(CFLAGS) -DAO_FEC_DEBUG=1 -o $@ ao_fec_test.c ../core/ao_fec_tx.c ../core/ao_fec_rx.c -lm
 
+ao_aprs_test: ao_aprs_test.c ao_aprs.c
+	cc $(CFLAGS) -o $@ ao_aprs_test.c
+
+SOX_INPUT_ARGS=--type raw --encoding unsigned-integer -b 8 -c 1 -r 9600
+SOX_OUTPUT_ARGS=--type wav
+
+ao_aprs_data.wav: ao_aprs_test
+	./ao_aprs_test | sox $(SOX_INPUT_ARGS) - $(SOX_OUTPUT_ARGS) $@
+
 check: ao_fec_test ao_flight_test ao_flight_test_baro run-tests
 	./ao_fec_test && ./run-tests
\ No newline at end of file
diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c
new file mode 100644
index 00000000..3b31f2d3
--- /dev/null
+++ b/src/test/ao_aprs_test.c
@@ -0,0 +1,132 @@
+/*
+ * 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 <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+#include <stdarg.h>
+
+#include <ao_telemetry.h>
+
+struct ao_telemetry_location ao_gps_data;
+
+#define AO_APRS_TEST
+
+typedef int16_t (*ao_radio_fill_func)(uint8_t *buffer, int16_t len);
+
+#define DEBUG 0
+#if DEBUG
+void
+ao_aprs_bit(uint8_t bit)
+{
+	static int	seq = 0;
+	printf ("%6d %d\n", seq++, bit ? 1 : 0);
+}
+#else
+void
+ao_aprs_bit(uint8_t bit)
+{
+	putchar (bit ? 0xc0 : 0x40);
+}
+#endif
+
+void
+ao_radio_send_lots(ao_radio_fill_func fill);
+
+#include <ao_aprs.c>
+
+/*
+ * @section copyright_sec Copyright
+ *
+ * Copyright (c) 2001-2009 Michael Gray, KD7LMO
+
+
+ *
+ *
+ * @section gpl_sec GNU General Public License
+ *
+ *  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; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  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
+ *  
+
+ */
+
+static void
+audio_gap(int secs)
+{
+#if !DEBUG
+	int	samples = secs * 9600;
+
+	while (samples--)
+		ao_aprs_bit(0);
+#endif
+}
+
+// This is where we go after reset.
+int main(int argc, char **argv)
+{
+    audio_gap(1);
+
+    ao_gps_data.latitude = (45.0 + 28.25 / 60.0) * 10000000;
+    ao_gps_data.longitude = (-(122 + 44.2649 / 60.0)) * 10000000;
+    ao_gps_data.altitude = 84;
+
+    /* Transmit one packet */
+    ao_aprs_send();
+
+    tncBuffer[strlen((char *) tncBuffer) - 2] = '\0';
+    fprintf(stderr, "packet: %s\n", tncBuffer);
+
+    exit(0);
+}
+
+void
+ao_radio_send_lots(ao_radio_fill_func fill)
+{
+	int16_t	len;
+	uint8_t	done = 0;
+	uint8_t	buf[16], *b, c;
+	uint8_t bit;
+
+	while (!done) {
+		len = (*fill)(buf, sizeof (buf));
+		if (len < 0) {
+			done = 1;
+			len = -len;
+		}
+		b = buf;
+		while (len--) {
+			c = *b++;
+			for (bit = 0; bit < 8; bit++) {
+				ao_aprs_bit(c & 0x80);
+				c <<= 1;
+			}
+		}
+	}
+}
diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index acdf4d92..cdd1f236 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -236,10 +236,14 @@ extern int32_t	ao_accel_scale;
 extern alt_t	ao_ground_height;
 extern alt_t	ao_sample_alt;
 
+double ao_sample_qangle;
+
 int ao_sample_prev_tick;
 uint16_t	prev_tick;
 
+
 #include "ao_kalman.c"
+#include "ao_sqrt.c"
 #include "ao_sample.c"
 #include "ao_flight.c"
 
@@ -309,7 +313,7 @@ ao_mpu6000_accel(int16_t sensor)
 }
 
 static double
-ao_mpu6000_gyro(int16_t sensor)
+ao_mpu6000_gyro(int32_t sensor)
 {
 	return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE;
 }
@@ -370,6 +374,7 @@ ao_insert(void)
 		if (!ao_summary) {
 			printf("%7.2f height %8.2f accel %8.3f "
 #if MEGAMETRUM
+			       "roll %8.3f angle %8.3f qangle %8.3f "
 			       "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f "
 #endif
 			       "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n",
@@ -377,6 +382,9 @@ ao_insert(void)
 			       height,
 			       accel,
 #if MEGAMETRUM
+			       ao_mpu6000_gyro(ao_sample_roll_angle) / 100.0,
+			       ao_mpu6000_gyro(ao_sample_angle) / 100.0,
+			       ao_sample_qangle,
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_x),
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_y),
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_z),
@@ -715,12 +723,14 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick)
 	q_dot.q2 =  0.5 * (ao_orient.q0 * rate_y + ao_orient.q3 * rate_x - ao_orient.q1 * rate_z) + lambda * ao_orient.q2;
 	q_dot.q3 =  0.5 * (ao_orient.q0 * rate_z + ao_orient.q1 * rate_y - ao_orient.q2 * rate_x) + lambda * ao_orient.q3;
 
+#if 0
 	printf ("update_orientation %g %g %g (%g s)\n", rate_x, rate_y, rate_z, dt);
 	printf ("q_dot (%g) %g %g %g\n",
 		q_dot.q0,
 		q_dot.q1,
 		q_dot.q2,
 		q_dot.q3);
+#endif
 
 	ao_orient.q0 += q_dot.q0 * dt;
 	ao_orient.q1 += q_dot.q1 * dt;
@@ -731,6 +741,8 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick)
 
 	ao_quat_rot(&ao_current, &ao_up, &ao_orient);
 
+	ao_sample_qangle = 180 / M_PI * acos(ao_current.q3 * sqrt(2));
+#if 0
 	printf ("orient (%g) %g %g %g current (%g) %g %g %g\n",
 		ao_orient.q0,
 		ao_orient.q1,
@@ -740,6 +752,7 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick)
 		ao_current.q1,
 		ao_current.q2,
 		ao_current.q3);
+#endif
 }
 #endif
 
@@ -845,7 +858,7 @@ ao_sleep(void *wchan)
 						double		rate_y = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y);
 						double		rate_z = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z);
 
-						update_orientation(rate_x, rate_z, rate_y, tick);
+						update_orientation(rate_x * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick);
 					}
 					ao_records_read++;
 					ao_insert();