Merge branch 'master' into micropeak-logging

18 files changed, 309 insertions, 62 deletions

diff --git a/src/core/ao.h b/src/core/ao.h
index 81d92e72..df5bbf48 100644
--- a/src/core/ao.h
+++ b/src/core/ao.h
@@ -170,6 +170,10 @@ ao_cmd_hex(void);
 void
 ao_cmd_decimal(void);
 
+/* Read a single hex nibble off stdin. */
+uint8_t
+ao_getnibble(void);
+
 uint8_t
 ao_match_word(__code char *word);
 
@@ -525,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:
  *
@@ -595,10 +604,10 @@ ao_monitor_init(void) __reentrant;
  * ao_stdio.c
  */
 
-#define AO_READ_AGAIN	((char) -1)
+#define AO_READ_AGAIN	(-1)
 
 struct ao_stdio {
-	char	(*pollchar)(void);
+	int	(*pollchar)(void);
 	void	(*putchar)(char c) __reentrant;
 	void	(*flush)(void);
 	uint8_t	echo;
@@ -617,7 +626,7 @@ uint8_t
 ao_echo(void);
 
 int8_t
-ao_add_stdio(char (*pollchar)(void),
+ao_add_stdio(int (*pollchar)(void),
 	     void (*putchar)(char) __reentrant,
 	     void (*flush)(void)) __reentrant;
 
@@ -675,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
 
@@ -702,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_cmd.c b/src/core/ao_cmd.c
index 1814cecf..3d086a57 100644
--- a/src/core/ao_cmd.c
+++ b/src/core/ao_cmd.c
@@ -110,6 +110,22 @@ putnibble(uint8_t v)
 		putchar(v + ('a' - 10));
 }
 
+uint8_t
+ao_getnibble(void)
+{
+	char	c;
+
+	c = getchar();
+	if ('0' <= c && c <= '9')
+		return c - '0';
+	if ('a' <= c && c <= 'f')
+		return c - ('a' - 10);
+	if ('A' <= c && c <= 'F')
+		return c - ('A' - 10);
+	ao_cmd_status = ao_cmd_lex_error;
+	return 0;
+}
+
 void
 ao_cmd_put16(uint16_t v)
 {
@@ -249,12 +265,25 @@ ao_reboot(void)
 static void
 version(void)
 {
-	printf("manufacturer     %s\n", ao_manufacturer);
-	printf("product          %s\n", ao_product);
-	printf("serial-number    %u\n", ao_serial_number);
+	printf("manufacturer     %s\n"
+	       "product          %s\n"
+	       "serial-number    %u\n"
+#if HAS_FLIGHT
+	       "current-flight   %u\n"
+#endif
+#if HAS_LOG
+	       "log-format       %u\n"
+#endif
+	       , ao_manufacturer
+	       , ao_product
+	       , ao_serial_number
+#if HAS_FLIGHT
+	       , ao_flight_number
+#endif
 #if HAS_LOG
-	printf("log-format       %u\n", ao_log_format);
+	       , ao_log_format
 #endif
+		);
 #if HAS_MS5607
 	ao_ms5607_info();
 #endif
diff --git a/src/core/ao_config.c b/src/core/ao_config.c
index e85ddcb4..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;
@@ -529,15 +552,15 @@ __code struct ao_config_var ao_config_vars[] = {
 	  ao_config_callsign_set,	ao_config_callsign_show },
 	{ "e <0 disable, 1 enable>\0Enable telemetry and RDF",
 	  ao_config_radio_enable_set, ao_config_radio_enable_show },
+	{ "f <cal>\0Radio calib (cal = rf/(xtal/2^16))",
+	  ao_config_radio_cal_set,  	ao_config_radio_cal_show },
 #endif /* HAS_RADIO */
 #if HAS_ACCEL
 	{ "a <+g> <-g>\0Accel calib (0 for auto)",
 	  ao_config_accel_calibrate_set,ao_config_accel_calibrate_show },
+	{ "o <0 antenna up, 1 antenna down>\0Set pad orientation",
+	  ao_config_pad_orientation_set,ao_config_pad_orientation_show },
 #endif /* HAS_ACCEL */
-#if HAS_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 },
@@ -546,10 +569,6 @@ __code struct ao_config_var ao_config_vars[] = {
 	{ "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 },
@@ -558,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_log_telem.c b/src/core/ao_log_telem.c
index 18ab85dd..23ebf7dd 100644
--- a/src/core/ao_log_telem.c
+++ b/src/core/ao_log_telem.c
@@ -102,9 +102,9 @@ ao_log_single(void)
 		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_xmemcpy(&ao_log_single_write_data.telemetry,
+					   &ao_monitor_ring[ao_log_monitor_pos],
+					   AO_LOG_SINGLE_SIZE);
 				ao_log_single_write();
 				ao_log_monitor_pos = ao_monitor_ring_next(ao_log_monitor_pos);
 				ao_log_telem_track();
diff --git a/src/core/ao_packet.h b/src/core/ao_packet.h
index 0eafd3b2..08b184d6 100644
--- a/src/core/ao_packet.h
+++ b/src/core/ao_packet.h
@@ -62,7 +62,7 @@ ao_packet_flush(void);
 void
 ao_packet_putchar(char c) __reentrant;
 
-char
+int
 ao_packet_pollchar(void);
 
 #if PACKET_HAS_MASTER
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_send_packet.c b/src/core/ao_send_packet.c
index 1a8e74de..66315d22 100644
--- a/src/core/ao_send_packet.c
+++ b/src/core/ao_send_packet.c
@@ -21,22 +21,6 @@
 
 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)
 {
@@ -53,8 +37,8 @@ ao_send_packet(void)
 		return;
 	}
 	for (i = 0; i < count; i++) {
-		b = getnibble() << 4;
-		b |= getnibble();
+		b = ao_getnibble() << 4;
+		b |= ao_getnibble();
 		if (ao_cmd_status != ao_cmd_success)
 			return;
 		ao_send[i] = b;
diff --git a/src/core/ao_serial.h b/src/core/ao_serial.h
index 53aa8a89..a799bf2c 100644
--- a/src/core/ao_serial.h
+++ b/src/core/ao_serial.h
@@ -22,6 +22,7 @@
 #define AO_SERIAL_SPEED_9600	1
 #define AO_SERIAL_SPEED_19200	2
 #define AO_SERIAL_SPEED_57600	3
+#define AO_SERIAL_SPEED_115200	4
 
 #if HAS_SERIAL_0
 extern volatile __xdata struct ao_fifo	ao_serial0_rx_fifo;
@@ -30,6 +31,9 @@ extern volatile __xdata struct ao_fifo	ao_serial0_tx_fifo;
 char
 ao_serial0_getchar(void);
 
+int
+ao_serial0_pollchar(void);
+
 void
 ao_serial0_putchar(char c);
 
@@ -47,7 +51,7 @@ extern volatile __xdata struct ao_fifo	ao_serial1_tx_fifo;
 char
 ao_serial1_getchar(void);
 
-char
+int
 ao_serial1_pollchar(void);
 
 void
@@ -67,7 +71,7 @@ extern volatile __xdata struct ao_fifo	ao_serial2_tx_fifo;
 char
 ao_serial2_getchar(void);
 
-char
+int
 ao_serial2_pollchar(void);
 
 void
@@ -80,6 +84,26 @@ void
 ao_serial2_set_speed(uint8_t speed);
 #endif
 
+#if HAS_SERIAL_3
+extern volatile __xdata struct ao_fifo	ao_serial3_rx_fifo;
+extern volatile __xdata struct ao_fifo	ao_serial3_tx_fifo;
+
+char
+ao_serial3_getchar(void);
+
+int
+ao_serial3_pollchar(void);
+
+void
+ao_serial3_putchar(char c);
+
+void
+ao_serial3_drain(void);
+
+void
+ao_serial3_set_speed(uint8_t speed);
+#endif
+
 void
 ao_serial_init(void);
 
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_stdio.c b/src/core/ao_stdio.c
index 8cf66a23..1748dfe8 100644
--- a/src/core/ao_stdio.c
+++ b/src/core/ao_stdio.c
@@ -98,7 +98,7 @@ __xdata uint8_t ao_stdin_ready;
 char
 getchar(void) __reentrant
 {
-	char c;
+	int c;
 	ao_arch_critical(
 		int8_t stdio = ao_cur_stdio;
 
@@ -123,7 +123,7 @@ ao_echo(void)
 }
 
 int8_t
-ao_add_stdio(char (*pollchar)(void),
+ao_add_stdio(int (*pollchar)(void),
 	     void (*putchar)(char),
 	     void (*flush)(void)) __reentrant
 {
diff --git a/src/core/ao_task.c b/src/core/ao_task.c
index 0411fbdd..9cb074b5 100644
--- a/src/core/ao_task.c
+++ b/src/core/ao_task.c
@@ -305,6 +305,8 @@ ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *nam
 		);
 }
 
+__data uint8_t	ao_task_minimize_latency;
+
 /* Task switching function. This must not use any stack variables */
 void
 ao_yield(void) ao_arch_naked_define
@@ -331,7 +333,12 @@ ao_yield(void) ao_arch_naked_define
 	}
 
 	ao_arch_isr_stack();
-	ao_arch_block_interrupts();
+#if !HAS_TASK_QUEUE
+	if (ao_task_minimize_latency)
+		ao_arch_release_interrupts();
+	else
+#endif
+		ao_arch_block_interrupts();
 
 #if AO_CHECK_STACK
 	in_yield = 1;
@@ -374,7 +381,7 @@ ao_yield(void) ao_arch_naked_define
 				break;
 
 			/* Wait for interrupts when there's nothing ready */
-			if (ao_cur_task_index == ao_last_task_index)
+			if (ao_cur_task_index == ao_last_task_index && !ao_task_minimize_latency)
 				ao_arch_wait_interrupt();
 		}
 	}
diff --git a/src/core/ao_task.h b/src/core/ao_task.h
index 049f69a7..50bfb220 100644
--- a/src/core/ao_task.h
+++ b/src/core/ao_task.h
@@ -47,6 +47,7 @@ struct ao_task {
 extern __xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS];
 extern __data uint8_t ao_num_tasks;
 extern __xdata struct ao_task *__data ao_cur_task;
+extern __data uint8_t ao_task_minimize_latency;	/* Reduce IRQ latency */
 
 /*
  ao_task.c
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/core/ao_usb.h b/src/core/ao_usb.h
index e051db93..4476ee6b 100644
--- a/src/core/ao_usb.h
+++ b/src/core/ao_usb.h
@@ -33,7 +33,7 @@ ao_usb_getchar(void);
 /* Poll for a charcter on the USB input queue.
  * returns AO_READ_AGAIN if none are available
  */
-char
+int
 ao_usb_pollchar(void);
 
 /* Flush the USB output queue */