5 files changed, 53 insertions, 10 deletions

diff --git a/src/drivers/ao_btm.c b/src/drivers/ao_btm.c
index f3816047..c862200a 100644
--- a/src/drivers/ao_btm.c
+++ b/src/drivers/ao_btm.c
@@ -120,7 +120,7 @@ uint8_t
 ao_btm_get_line(void)
 {
 	uint8_t ao_btm_reply_len = 0;
-	char c;
+	int c;
 
 	for (;;) {
 
diff --git a/src/drivers/ao_companion.c b/src/drivers/ao_companion.c
index c749adea..0ebe8429 100644
--- a/src/drivers/ao_companion.c
+++ b/src/drivers/ao_companion.c
@@ -118,10 +118,13 @@ ao_companion_status(void) __reentrant
 	printf("Companion running: %d\n", ao_companion_running);
 	if (!ao_companion_running)
 		return;
-	printf("device: %d\n", ao_companion_setup.board_id);
-	printf("update period: %d\n", ao_companion_setup.update_period);
-	printf("channels: %d\n", ao_companion_setup.channels);
-	printf("data:");
+	printf("device: %d\n"
+	       "update period: %d\n"
+	       "channels: %d\n"
+	       "data:",
+	       ao_companion_setup.board_id,
+	       ao_companion_setup.update_period,
+	       ao_companion_setup.channels);
 	for(i = 0; i < ao_companion_setup.channels; i++)
 		printf(" %5u", ao_companion_data[i]);
 	printf("\n");
diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c
index d80da97c..d2f67e6b 100644
--- a/src/drivers/ao_gps_skytraq.c
+++ b/src/drivers/ao_gps_skytraq.c
@@ -21,6 +21,7 @@
 
 #ifndef ao_gps_getchar
 #define ao_gps_getchar	ao_serial1_getchar
+#define ao_gps_fifo	ao_serial1_rx_fifo
 #endif
 
 #ifndef ao_gps_putchar
@@ -453,6 +454,8 @@ ao_gps_nmea_parse(void)
 	}
 }
 
+static uint8_t	ao_gps_updating;
+
 void
 ao_gps(void) __reentrant
 {
@@ -468,6 +471,13 @@ ao_gps(void) __reentrant
 		if (ao_gps_getchar() == '$') {
 			ao_gps_nmea_parse();
 		}
+#ifndef AO_GPS_TEST
+		while (ao_gps_updating) {
+			ao_usb_putchar(ao_gps_getchar());
+			if (ao_fifo_empty(ao_gps_fifo))
+				flush();
+		}
+#endif
 	}
 }
 
@@ -492,8 +502,38 @@ gps_dump(void) __reentrant
 	ao_mutex_put(&ao_gps_mutex);
 }
 
+static __code uint8_t ao_gps_115200[] = {
+	SKYTRAQ_MSG_3(5,0,5,0)	/* Set to 115200 baud */
+};
+
+static void
+ao_gps_set_speed_delay(uint8_t speed) {
+	ao_delay(AO_MS_TO_TICKS(500));
+	ao_gps_set_speed(speed);
+	ao_delay(AO_MS_TO_TICKS(500));
+}
+
+static void
+gps_update(void) __reentrant
+{
+	ao_gps_updating = 1;
+	ao_task_minimize_latency = 1;
+#if HAS_ADC
+	ao_timer_set_adc_interval(0);
+#endif
+	ao_skytraq_sendstruct(ao_gps_115200);
+	ao_gps_set_speed_delay(AO_SERIAL_SPEED_4800);
+	ao_skytraq_sendstruct(ao_gps_115200);
+	ao_gps_set_speed_delay(AO_SERIAL_SPEED_115200);
+
+	/* It's a binary protocol; abandon attempts to escape */
+	for (;;)
+		ao_gps_putchar(ao_usb_getchar());
+}
+
 __code struct ao_cmds ao_gps_cmds[] = {
 	{ gps_dump, 	"g\0Display GPS" },
+	{ gps_update,	"U\0Update GPS firmware" },
 	{ 0, NULL },
 };
 
diff --git a/src/drivers/ao_packet.c b/src/drivers/ao_packet.c
index 3c1e7a18..91319923 100644
--- a/src/drivers/ao_packet.c
+++ b/src/drivers/ao_packet.c
@@ -21,8 +21,8 @@ __xdata struct ao_packet_recv ao_rx_packet;
 __xdata struct ao_packet ao_tx_packet;
 __pdata uint8_t ao_packet_rx_len, ao_packet_rx_used, ao_packet_tx_used;
 
-static __xdata char tx_data[AO_PACKET_MAX];
-static __xdata char rx_data[AO_PACKET_MAX];
+static __xdata uint8_t tx_data[AO_PACKET_MAX];
+static __xdata uint8_t rx_data[AO_PACKET_MAX];
 static __pdata uint8_t rx_seq;
 
 __xdata struct ao_task	ao_packet_task;
@@ -169,7 +169,7 @@ ao_packet_putchar(char c) __reentrant
 		tx_data[ao_packet_tx_used++] = c;
 }
 
-char
+int
 ao_packet_pollchar(void)
 {
 	/* No need to block interrupts, all variables here
diff --git a/src/drivers/ao_packet_master.c b/src/drivers/ao_packet_master.c
index 481232df..023c788b 100644
--- a/src/drivers/ao_packet_master.c
+++ b/src/drivers/ao_packet_master.c
@@ -20,7 +20,7 @@
 static char
 ao_packet_getchar(void)
 {
-	char c;
+	int c;
 	while ((c = ao_packet_pollchar()) == AO_READ_AGAIN) {
 		if (!ao_packet_enable)
 			break;
@@ -35,7 +35,7 @@ ao_packet_getchar(void)
 static void
 ao_packet_echo(void) __reentrant
 {
-	char	c;
+	int	c;
 	while (ao_packet_enable) {
 		c = ao_packet_getchar();
 		if (c != AO_READ_AGAIN)