Add support for the SkyTraq GPS unit

This is a build-time option selected by hacking the Makefile at present.

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

7 files changed, 864 insertions, 6 deletions

diff --git a/src/Makefile b/src/Makefile
index 828c48bd..892635cb 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -59,7 +59,7 @@ TELE_RECEIVER_SRC =\
 
 TELE_DRIVER_SRC = \
 	ao_convert.c \
-	ao_gps.c \
+	ao_gps_skytraq.c \
 	ao_serial.c
 
 #
@@ -189,7 +189,7 @@ SYM=$(REL:.rel=.sym)
 PROGS=	telemetrum.ihx tidongle.ihx \
 	teleterra.ihx teledongle.ihx
 
-HOST_PROGS=ao_flight_test ao_gps_test
+HOST_PROGS=ao_flight_test ao_gps_test ao_gps_test_skytraq
 
 PCDB=$(PROGS:.ihx=.cdb)
 PLNK=$(PROGS:.ihx=.lnk)
@@ -271,5 +271,8 @@ install:
 ao_flight_test: ao_flight.c ao_flight_test.c ao_host.h
 	cc -g -o $@ ao_flight_test.c
 
-ao_gps_test: ao_gps.c ao_gps_test.c ao_gps_print.c ao_host.h
+ao_gps_test: ao_gps_sirf.c ao_gps_test.c ao_gps_print.c ao_host.h
 	cc -g -o $@ ao_gps_test.c
+
+ao_gps_test_skytraq: ao_gps_skytraq.c ao_gps_test_skytraq.c ao_gps_print.c ao_host.h
+	cc -g -o $@ ao_gps_test_skytraq.c
diff --git a/src/ao.h b/src/ao.h
index 4116be65..0f3f0ea7 100644
--- a/src/ao.h
+++ b/src/ao.h
@@ -668,7 +668,8 @@ void
 ao_serial_putchar(char c) __critical;
 
 #define AO_SERIAL_SPEED_4800	0
-#define AO_SERIAL_SPEED_57600	1
+#define AO_SERIAL_SPEED_9600	1
+#define AO_SERIAL_SPEED_57600	2
 
 void
 ao_serial_set_speed(uint8_t speed);
diff --git a/src/ao_gps.c b/src/ao_gps_sirf.c
index 2b3a5178..2b3a5178 100644
--- a/src/ao_gps.c
+++ b/src/ao_gps_sirf.c
diff --git a/src/ao_gps_skytraq.c b/src/ao_gps_skytraq.c
new file mode 100644
index 00000000..b397d975
--- /dev/null
+++ b/src/ao_gps_skytraq.c
@@ -0,0 +1,369 @@
+/*
+ * 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
+
+#define AO_GPS_LEADER		3
+
+static const char ao_gps_header[] = "GPG";
+
+__xdata uint8_t ao_gps_mutex;
+static __xdata char ao_gps_char;
+static __xdata uint8_t ao_gps_cksum;
+static __xdata uint8_t ao_gps_error;
+
+__xdata struct ao_gps_data	ao_gps_data;
+__xdata struct ao_gps_tracking_data	ao_gps_tracking_data;
+
+static __xdata struct ao_gps_data		ao_gps_next;
+static __xdata struct ao_gps_tracking_data	ao_gps_tracking_next;
+
+static const char ao_gps_config[] = {
+	0xa0, 0xa1, 0x00, 0x09,		/* length 9 bytes */
+	0x08,				/* configure nmea */
+	1,				/* gga interval */
+	1,				/* gsa interval */
+	1,				/* gsv interval */
+	1,				/* gll interval */
+	1,				/* rmc interval */
+	1,				/* vtg interval */
+	1,				/* zda interval */
+	0,				/* attributes (0 = update to sram, 1 = update flash too) */
+	0x09, 0x0d, 0x0a,
+};
+
+static void
+ao_gps_lexchar(void)
+{
+	if (ao_gps_error)
+		ao_gps_char = '\n';
+	else
+		ao_gps_char = ao_serial_getchar();
+	ao_gps_cksum ^= ao_gps_char;
+}
+
+void
+ao_gps_skip(void)
+{
+	while (ao_gps_char >= '0')
+		ao_gps_lexchar();
+}
+
+void
+ao_gps_skip_field(void)
+{
+	while (ao_gps_char != ',' && ao_gps_char != '*' && ao_gps_char != '\n')
+		ao_gps_lexchar();
+}
+
+void
+ao_gps_skip_sep(void)
+{
+	if (ao_gps_char == ',' || ao_gps_char == '.' || ao_gps_char == '*')
+		ao_gps_lexchar();
+}
+
+__xdata static uint8_t ao_gps_num_width;
+
+static int16_t
+ao_gps_decimal(uint8_t max_width)
+{
+	int16_t	v;
+	__xdata uint8_t	neg = 0;
+
+	ao_gps_skip_sep();
+	if (ao_gps_char == '-') {
+		neg = 1;
+		ao_gps_lexchar();
+	}
+	v = 0;
+	ao_gps_num_width = 0;
+	while (ao_gps_num_width < max_width) {
+		if (ao_gps_char < '0' || '9' < ao_gps_char)
+			break;
+		v = v * (int16_t) 10 + ao_gps_char - '0';
+		ao_gps_num_width++;
+		ao_gps_lexchar();
+	}
+	if (neg)
+		v = -v;
+	return v;
+}
+
+static uint8_t
+ao_gps_hex(uint8_t max_width)
+{
+	uint8_t	v, d;
+
+	ao_gps_skip_sep();
+	v = 0;
+	ao_gps_num_width = 0;
+	while (ao_gps_num_width < max_width) {
+		if ('0' <= ao_gps_char && ao_gps_char <= '9')
+			d = ao_gps_char - '0';
+		else if ('A' <= ao_gps_char && ao_gps_char <= 'F')
+			d = ao_gps_char - 'A' + 10;
+		else if ('a' <= ao_gps_char && ao_gps_char <= 'f')
+			d = ao_gps_char - 'a' + 10;
+		else
+			break;
+		v = (v << 4) | d;
+		ao_gps_num_width++;
+		ao_gps_lexchar();
+	}
+	return v;
+}
+
+static int32_t
+ao_gps_parse_pos(uint8_t deg_width) __reentrant
+{
+	int32_t	d;
+	int32_t	m;
+	int32_t	f;
+
+	d = ao_gps_decimal(deg_width);
+	m = ao_gps_decimal(2);
+	if (ao_gps_char == '.') {
+		f = ao_gps_decimal(4);
+		while (ao_gps_num_width < 4) {
+			f *= 10;
+			ao_gps_num_width++;
+		}
+	} else {
+		f = 0;
+		if (ao_gps_char != ',')
+			ao_gps_error = 1;
+	}
+	d = d * 10000000l;
+	m = m * 10000l + f;
+	d = d + m * 50 / 3;
+	return d;
+}
+
+static uint8_t
+ao_gps_parse_flag(char no_c, char yes_c) __reentrant
+{
+	uint8_t	ret = 0;
+	ao_gps_skip_sep();
+	if (ao_gps_char == yes_c)
+		ret = 1;
+	else if (ao_gps_char == no_c)
+		ret = 0;
+	else
+		ao_gps_error = 1;
+	ao_gps_lexchar();
+	return ret;
+}
+
+
+void
+ao_gps(void) __reentrant
+{
+	char	c;
+	uint8_t	i;
+
+	ao_serial_set_speed(AO_SERIAL_SPEED_9600);
+	for (i = 0; i < sizeof (ao_gps_config); i++)
+		ao_serial_putchar(ao_gps_config[i]);
+	for (;;) {
+		/* Locate the begining of the next record */
+		for (;;) {
+			c = ao_serial_getchar();
+			if (c == '$')
+				break;
+		}
+
+		ao_gps_cksum = 0;
+		ao_gps_error = 0;
+
+		/* Skip anything other than GPG */
+		for (i = 0; i < AO_GPS_LEADER; i++) {
+			ao_gps_lexchar();
+			if (ao_gps_char != ao_gps_header[i])
+				break;
+		}
+		if (i != AO_GPS_LEADER)
+			continue;
+
+		/* pull the record identifier characters off the link */
+		ao_gps_lexchar();
+		c = ao_gps_char;
+		ao_gps_lexchar();
+		i = ao_gps_char;
+		ao_gps_lexchar();
+		if (ao_gps_char != ',')
+			continue;
+
+		if (c == (uint8_t) 'G' && i == (uint8_t) 'A') {
+			/* Now read the data into the gps data record
+			 *
+			 * $GPGGA,025149.000,4528.1723,N,12244.2480,W,1,05,2.0,103.5,M,-19.5,M,,0000*66
+			 *
+			 * Essential fix data
+			 *
+			 *	   025149.000	time (02:51:49.000 GMT)
+			 *	   4528.1723,N	Latitude 45°28.1723' N
+			 *	   12244.2480,W	Longitude 122°44.2480' W
+			 *	   1		Fix quality:
+			 *				   0 = invalid
+			 *				   1 = GPS fix (SPS)
+			 *				   2 = DGPS fix
+			 *				   3 = PPS fix
+			 *				   4 = Real Time Kinematic
+			 *				   5 = Float RTK
+			 *				   6 = estimated (dead reckoning)
+			 *				   7 = Manual input mode
+			 *				   8 = Simulation mode
+			 *	   05		Number of satellites (5)
+			 *	   2.0		Horizontal dilution
+			 *	   103.5,M		Altitude, 103.5M above msl
+			 *	   -19.5,M		Height of geoid above WGS84 ellipsoid
+			 *	   ?		time in seconds since last DGPS update
+			 *	   0000		DGPS station ID
+			 *	   *66		checksum
+			 */
+
+			ao_gps_next.flags = AO_GPS_RUNNING;
+			ao_gps_next.hour = ao_gps_decimal(2);
+			ao_gps_next.minute = ao_gps_decimal(2);
+			ao_gps_next.second = ao_gps_decimal(2);
+			ao_gps_skip_field();	/* skip seconds fraction */
+
+			ao_gps_next.latitude = ao_gps_parse_pos(2);
+			if (ao_gps_parse_flag('N', 'S'))
+				ao_gps_next.latitude = -ao_gps_next.latitude;
+			ao_gps_next.longitude = ao_gps_parse_pos(3);
+			if (ao_gps_parse_flag('E', 'W'))
+				ao_gps_next.longitude = -ao_gps_next.longitude;
+
+			i = ao_gps_decimal(0xff);
+			if (i == 1)
+				ao_gps_next.flags |= AO_GPS_VALID;
+
+			i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT;
+			if (i > AO_GPS_NUM_SAT_MASK)
+				i = AO_GPS_NUM_SAT_MASK;
+			ao_gps_next.flags |= i;
+
+			ao_gps_lexchar();
+			ao_gps_skip_field();	/* Horizontal dilution */
+
+			ao_gps_next.altitude = ao_gps_decimal(0xff);
+			ao_gps_skip_field();	/* skip any fractional portion */
+
+			/* Skip remaining fields */
+			while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
+				ao_gps_lexchar();
+				ao_gps_skip_field();
+			}
+			if (ao_gps_char == '*') {
+				uint8_t cksum = ao_gps_cksum ^ '*';
+				if (cksum != ao_gps_hex(2))
+					ao_gps_error = 1;
+			} else
+				ao_gps_error = 1;
+			if (!ao_gps_error) {
+				ao_mutex_get(&ao_gps_mutex);
+				memcpy(&ao_gps_data, &ao_gps_next, sizeof (struct ao_gps_data));
+				ao_mutex_put(&ao_gps_mutex);
+				ao_wakeup(&ao_gps_data);
+			}
+		} else if (c == (uint8_t) 'S' && i == (uint8_t) 'V') {
+			uint8_t	done;
+			/* Now read the data into the GPS tracking data record
+			 *
+			 * $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72<CR><LF>
+			 *
+			 * Satellites in view data
+			 *
+			 *	3		Total number of GSV messages
+			 *	1		Sequence number of current GSV message
+			 *	12		Total sats in view (0-12)
+			 *	05		SVID
+			 *	54		Elevation
+			 *	069		Azimuth
+			 *	45		C/N0 in dB
+			 *	...		other SVIDs
+			 *	72		checksum
+			 */
+			c = ao_gps_decimal(1);	/* total messages */
+			i = ao_gps_decimal(1);	/* message sequence */
+			if (i == 1) {
+				ao_gps_tracking_next.channels = 0;
+			}
+			done = (uint8_t) c == i;
+			ao_gps_lexchar();
+			ao_gps_skip_field();	/* sats in view */
+			while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
+				i = ao_gps_tracking_next.channels;
+				ao_gps_tracking_next.sats[i].svid = ao_gps_decimal(2);	/* SVID */
+				ao_gps_lexchar();
+				ao_gps_skip_field();	/* elevation */
+				ao_gps_lexchar();
+				ao_gps_skip_field();	/* azimuth */
+				if (ao_gps_tracking_next.sats[i].c_n_1 = ao_gps_decimal(2))	/* C/N0 */
+					ao_gps_tracking_next.sats[i].state = 0xbf;
+				else
+					ao_gps_tracking_next.sats[i].state = 0;
+				ao_gps_tracking_next.channels = i + 1;
+			}
+			if (ao_gps_char == '*') {
+				uint8_t cksum = ao_gps_cksum ^ '*';
+				if (cksum != ao_gps_hex(2))
+					ao_gps_error = 1;
+			}
+			else
+				ao_gps_error = 1;
+			if (ao_gps_error)
+				ao_gps_tracking_next.channels = 0;
+			else if (done) {
+				ao_mutex_get(&ao_gps_mutex);
+				memcpy(&ao_gps_tracking_data, &ao_gps_tracking_next,
+				       sizeof(ao_gps_tracking_data));
+				ao_mutex_put(&ao_gps_mutex);
+				ao_wakeup(&ao_gps_tracking_data);
+			}
+		}
+	}
+}
+
+__xdata struct ao_task ao_gps_task;
+
+static void
+gps_dump(void) __reentrant
+{
+	ao_mutex_get(&ao_gps_mutex);
+	ao_gps_print(&ao_gps_data);
+	putchar('\n');
+	ao_gps_tracking_print(&ao_gps_tracking_data);
+	putchar('\n');
+	ao_mutex_put(&ao_gps_mutex);
+}
+
+__code struct ao_cmds ao_gps_cmds[] = {
+	{ 'g', gps_dump,	"g                                  Display current GPS values" },
+	{ 0, gps_dump, NULL },
+};
+
+void
+ao_gps_init(void)
+{
+	ao_add_task(&ao_gps_task, ao_gps, "gps");
+	ao_cmd_register(&ao_gps_cmds[0]);
+}
diff --git a/src/ao_gps_test.c b/src/ao_gps_test.c
index c94128d9..366bca71 100644
--- a/src/ao_gps_test.c
+++ b/src/ao_gps_test.c
@@ -398,7 +398,7 @@ ao_serial_set_speed(uint8_t speed)
 }
 
 #include "ao_gps_print.c"
-#include "ao_gps.c"
+#include "ao_gps_sirf.c"
 
 void
 ao_dump_state(void *wchan)
diff --git a/src/ao_gps_test_skytraq.c b/src/ao_gps_test_skytraq.c
new file mode 100644
index 00000000..510bc419
--- /dev/null
+++ b/src/ao_gps_test_skytraq.c
@@ -0,0 +1,478 @@
+/*
+ * 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 AO_GPS_TEST
+#include "ao_host.h"
+#include <termios.h>
+#include <errno.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#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)
+
+struct ao_gps_data {
+	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 */
+};
+
+#define SIRF_SAT_STATE_ACQUIRED			(1 << 0)
+#define SIRF_SAT_STATE_CARRIER_PHASE_VALID	(1 << 1)
+#define SIRF_SAT_BIT_SYNC_COMPLETE		(1 << 2)
+#define SIRF_SAT_SUBFRAME_SYNC_COMPLETE		(1 << 3)
+#define SIRF_SAT_CARRIER_PULLIN_COMPLETE	(1 << 4)
+#define SIRF_SAT_CODE_LOCKED			(1 << 5)
+#define SIRF_SAT_ACQUISITION_FAILED		(1 << 6)
+#define SIRF_SAT_EPHEMERIS_AVAILABLE		(1 << 7)
+
+struct ao_gps_sat_data {
+	uint8_t		svid;
+	uint8_t		state;
+	uint8_t		c_n_1;
+};
+
+struct ao_gps_tracking_data {
+	uint8_t			channels;
+	struct ao_gps_sat_data	sats[12];
+};
+
+void
+ao_mutex_get(uint8_t *mutex)
+{
+}
+
+void
+ao_mutex_put(uint8_t *mutex)
+{
+}
+
+static int
+ao_gps_fd;
+
+static void
+ao_dbg_char(char c)
+{
+	char	line[128];
+	line[0] = '\0';
+	if (c < ' ') {
+		if (c == '\n')
+			sprintf (line, "\n");
+		else
+			sprintf (line, "\\%02x", ((int) c) & 0xff);
+	} else {
+		sprintf (line, "%c", c);
+	}
+	write(1, line, strlen(line));
+}
+
+#define QUEUE_LEN	4096
+
+static char	input_queue[QUEUE_LEN];
+int		input_head, input_tail;
+
+#include <sys/time.h>
+
+int
+get_millis(void)
+{
+	struct timeval	tv;
+	gettimeofday(&tv, NULL);
+	return tv.tv_sec * 1000 + tv.tv_usec / 1000;
+}
+
+static void
+check_skytraq_message(char *from, uint8_t *msg, int len)
+{
+	uint16_t	encoded_len, encoded_cksum;
+	uint16_t	cksum;
+	uint8_t		id;
+	int		i;
+
+//	fwrite(msg, 1, len, stdout);
+	return;
+	if (msg[0] != 0xa0 || msg[1] != 0xa2) {
+		printf ("bad header\n");
+		return;
+	}
+	if (len < 7) {
+		printf("short\n");
+		return;
+	}
+	if (msg[len-1] != 0xb3 || msg[len-2] != 0xb0) {
+		printf ("bad trailer\n");
+		return;
+	}
+	encoded_len = (msg[2] << 8) | msg[3];
+	id = msg[4];
+/*	printf ("%9d: %3d\n", get_millis(), id); */
+	if (encoded_len != len - 8) {
+		if (id != 52)
+			printf ("length mismatch (got %d, wanted %d)\n",
+				len - 8, encoded_len);
+		return;
+	}
+	encoded_cksum = (msg[len - 4] << 8) | msg[len-3];
+	cksum = 0;
+	for (i = 4; i < len - 4; i++)
+		cksum = (cksum + msg[i]) & 0x7fff;
+	if (encoded_cksum != cksum) {
+		printf ("cksum mismatch (got %04x wanted %04x)\n",
+			cksum, encoded_cksum);
+		return;
+	}
+	id = msg[4];
+	switch (id) {
+	case 41:{
+		int	off = 4;
+
+		uint8_t		id;
+		uint16_t	nav_valid;
+		uint16_t	nav_type;
+		uint16_t	week;
+		uint32_t	tow;
+		uint16_t	year;
+		uint8_t		month;
+		uint8_t		day;
+		uint8_t		hour;
+		uint8_t		minute;
+		uint16_t	second;
+		uint32_t	sat_list;
+		int32_t		lat;
+		int32_t		lon;
+		int32_t		alt_ell;
+		int32_t		alt_msl;
+		int8_t		datum;
+		uint16_t	sog;
+		uint16_t	cog;
+		int16_t		mag_var;
+		int16_t		climb_rate;
+		int16_t		heading_rate;
+		uint32_t	h_error;
+		uint32_t	v_error;
+		uint32_t	t_error;
+		uint16_t	h_v_error;
+
+#define get_u8(u)	u = (msg[off]); off+= 1
+#define get_u16(u)	u = (msg[off] << 8) | (msg[off + 1]); off+= 2
+#define get_u32(u)	u = (msg[off] << 24) | (msg[off + 1] << 16) | (msg[off+2] << 8) | (msg[off+3]); off+= 4
+
+		get_u8(id);
+		get_u16(nav_valid);
+		get_u16(nav_type);
+		get_u16(week);
+		get_u32(tow);
+		get_u16(year);
+		get_u8(month);
+		get_u8(day);
+		get_u8(hour);
+		get_u8(minute);
+		get_u16(second);
+		get_u32(sat_list);
+		get_u32(lat);
+		get_u32(lon);
+		get_u32(alt_ell);
+		get_u32(alt_msl);
+		get_u8(datum);
+		get_u16(sog);
+		get_u16(cog);
+		get_u16(mag_var);
+		get_u16(climb_rate);
+		get_u16(heading_rate);
+		get_u32(h_error);
+		get_u32(v_error);
+		get_u32(t_error);
+		get_u16(h_v_error);
+
+
+		printf ("Geodetic Navigation Data (41):\n");
+		printf ("\tNav valid %04x\n", nav_valid);
+		printf ("\tNav type %04x\n", nav_type);
+		printf ("\tWeek %5d", week);
+		printf (" TOW %9d", tow);
+		printf (" %4d-%2d-%2d %02d:%02d:%07.4f\n",
+			year, month, day,
+			hour, minute, second / 1000.0);
+		printf ("\tsats: %08x\n", sat_list);
+		printf ("\tlat: %g", lat / 1.0e7);
+		printf (" lon: %g", lon / 1.0e7);
+		printf (" alt_ell: %g", alt_ell / 100.0);
+		printf (" alt_msll: %g", alt_msl / 100.0);
+		printf (" datum: %d\n", datum);
+		printf ("\tground speed: %g", sog / 100.0);
+		printf (" course: %g", cog / 100.0);
+		printf (" climb: %g", climb_rate / 100.0);
+		printf (" heading rate: %g\n", heading_rate / 100.0);
+		printf ("\th error: %g", h_error / 100.0);
+		printf (" v error: %g", v_error / 100.0);
+		printf (" t error: %g", t_error / 100.0);
+		printf (" h vel error: %g\n", h_v_error / 100.0);
+		break;
+	}
+	case 4: {
+		int off = 4;
+		uint8_t		id;
+		int16_t		gps_week;
+		uint32_t	gps_tow;
+		uint8_t		channels;
+		int		j, k;
+
+		get_u8(id);
+		get_u16(gps_week);
+		get_u32(gps_tow);
+		get_u8(channels);
+
+		printf ("Measured Tracker Data (4):\n");
+		printf ("GPS week: %d\n", gps_week);
+		printf ("GPS time of week: %d\n", gps_tow);
+		printf ("channels: %d\n", channels);
+		for (j = 0; j < 12; j++) {
+			uint8_t	svid, azimuth, elevation;
+			uint16_t state;
+			uint8_t	c_n[10];
+			get_u8(svid);
+			get_u8(azimuth);
+			get_u8(elevation);
+			get_u16(state);
+			for (k = 0; k < 10; k++) {
+				get_u8(c_n[k]);
+			}
+			printf ("Sat %3d:", svid);
+			printf (" aziumuth: %6.1f", azimuth * 1.5);
+			printf (" elevation: %6.1f", elevation * 0.5);
+			printf (" state: 0x%02x", state);
+			printf (" c_n:");
+			for (k = 0; k < 10; k++)
+				printf(" %3d", c_n[k]);
+			if (state & SIRF_SAT_STATE_ACQUIRED)
+				printf(" acq,");
+			if (state & SIRF_SAT_STATE_CARRIER_PHASE_VALID)
+				printf(" car,");
+			if (state & SIRF_SAT_BIT_SYNC_COMPLETE)
+				printf(" bit,");
+			if (state & SIRF_SAT_SUBFRAME_SYNC_COMPLETE)
+				printf(" sub,");
+			if (state & SIRF_SAT_CARRIER_PULLIN_COMPLETE)
+				printf(" pullin,");
+			if (state & SIRF_SAT_CODE_LOCKED)
+				printf(" code,");
+			if (state & SIRF_SAT_ACQUISITION_FAILED)
+				printf(" fail,");
+			if (state & SIRF_SAT_EPHEMERIS_AVAILABLE)
+				printf(" ephem,");
+			printf ("\n");
+		}
+		break;
+	}
+	default:
+		return;
+		printf ("%s %4d:", from, encoded_len);
+		for (i = 4; i < len - 4; i++) {
+			if (((i - 4) & 0xf) == 0)
+				printf("\n   ");
+			printf (" %3d", msg[i]);
+		}
+		printf ("\n");
+	}
+}
+
+static uint8_t	skytraq_message[4096];
+static int	skytraq_message_len;
+static uint8_t	skytraq_in_message[4096];
+static int	skytraq_in_len;
+
+char
+ao_serial_getchar(void)
+{
+	char	c;
+	uint8_t	uc;
+
+	while (input_head == input_tail) {
+		for (;;) {
+			input_tail = read(ao_gps_fd, input_queue, QUEUE_LEN);
+			if (input_tail < 0) {
+				if (errno == EINTR || errno == EAGAIN)
+					continue;
+				perror ("getchar");
+				exit (1);
+			}
+			input_head = 0;
+			break;
+		}
+	}
+	c = input_queue[input_head];
+	input_head = (input_head + 1) % QUEUE_LEN;
+	uc = c;
+//	printf ("c: %02x %c\n", uc, uc);
+	if (skytraq_in_len || uc == '$') {
+		if (skytraq_in_len < 4096)
+			skytraq_in_message[skytraq_in_len++] = uc;
+		if (uc == 0x0a) {
+			check_skytraq_message("recv", skytraq_in_message, skytraq_in_len);
+			skytraq_in_len = 0;
+		}
+	}
+	return c;
+}
+
+
+void
+ao_serial_putchar(char c)
+{
+	int	i;
+	uint8_t	uc = (uint8_t) c;
+
+	if (skytraq_message_len || uc == 0xa0) {
+		if (skytraq_message_len < 4096)
+			skytraq_message[skytraq_message_len++] = uc;
+		if (uc == 0x0a) {
+			check_skytraq_message("send", skytraq_message, skytraq_message_len);
+			skytraq_message_len = 0;
+		}
+	}
+	for (;;) {
+		i = write(ao_gps_fd, &c, 1);
+		if (i == 1) {
+			if ((uint8_t) c == 0xb3 || c == '\r') {
+				static const struct timespec delay = {
+					.tv_sec = 0,
+					.tv_nsec = 100 * 1000 * 1000
+				};
+				tcdrain(ao_gps_fd);
+//				nanosleep(&delay, NULL);
+			}
+			break;
+		}
+		if (i < 0 && (errno == EINTR || errno == EAGAIN))
+			continue;
+		perror("putchar");
+		exit(1);
+	}
+}
+
+#define AO_SERIAL_SPEED_4800	0
+#define AO_SERIAL_SPEED_9600	1
+#define AO_SERIAL_SPEED_57600	2
+
+static void
+ao_serial_set_speed(uint8_t speed)
+{
+	int	fd = ao_gps_fd;
+	struct termios	termios;
+
+	tcdrain(fd);
+	tcgetattr(fd, &termios);
+	switch (speed) {
+	case AO_SERIAL_SPEED_4800:
+		cfsetspeed(&termios, B4800);
+		break;
+	case AO_SERIAL_SPEED_9600:
+		cfsetspeed(&termios, B38400);
+		break;
+	case AO_SERIAL_SPEED_57600:
+		cfsetspeed(&termios, B57600);
+		break;
+	}
+	tcsetattr(fd, TCSAFLUSH, &termios);
+	tcflush(fd, TCIFLUSH);
+}
+
+#include "ao_gps_print.c"
+#include "ao_gps_skytraq.c"
+
+void
+ao_dump_state(void *wchan)
+{
+	double	lat, lon;
+	int	i;
+	if (wchan == &ao_gps_data)
+		ao_gps_print(&ao_gps_data);
+	else
+		ao_gps_tracking_print(&ao_gps_tracking_data);
+	putchar('\n');
+	return;
+}
+
+int
+ao_gps_open(const char *tty)
+{
+	struct termios	termios;
+	int fd;
+
+	fd = open (tty, O_RDWR);
+	if (fd < 0)
+		return -1;
+
+	tcgetattr(fd, &termios);
+	cfmakeraw(&termios);
+	cfsetspeed(&termios, B4800);
+	tcsetattr(fd, TCSAFLUSH, &termios);
+
+	tcdrain(fd);
+	tcflush(fd, TCIFLUSH);
+	return fd;
+}
+
+#include <getopt.h>
+
+static const struct option options[] = {
+	{ .name = "tty", .has_arg = 1, .val = 'T' },
+	{ 0, 0, 0, 0},
+};
+
+static void usage(char *program)
+{
+	fprintf(stderr, "usage: %s [--tty <tty-name>]\n", program);
+	exit(1);
+}
+
+int
+main (int argc, char **argv)
+{
+	char	*tty = "/dev/ttyUSB0";
+	int	c;
+
+	while ((c = getopt_long(argc, argv, "T:", options, NULL)) != -1) {
+		switch (c) {
+		case 'T':
+			tty = optarg;
+			break;
+		default:
+			usage(argv[0]);
+			break;
+		}
+	}
+	ao_gps_fd = ao_gps_open(tty);
+	if (ao_gps_fd < 0) {
+		perror (tty);
+		exit (1);
+	}
+	ao_gps();
+}
diff --git a/src/ao_serial.c b/src/ao_serial.c
index 59110354..1e3ea3e3 100644
--- a/src/ao_serial.c
+++ b/src/ao_serial.c
@@ -60,7 +60,10 @@ ao_serial_getchar(void) __critical
 		ao_sleep(&ao_usart1_rx_fifo);
 	ao_fifo_remove(ao_usart1_rx_fifo, c);
 	if (serial_echo) {
-		printf("%02x\n", ((int) c) & 0xff);
+		printf("%02x ", ((int) c) & 0xff);
+		if (c >= ' ')
+			putchar(c);
+		putchar('\n');
 		flush();
 	}
 	return c;
@@ -121,6 +124,10 @@ static const struct {
 		/* .baud = */ 163,
 		/* .gcr  = */ (7 << UxGCR_BAUD_E_SHIFT) | UxGCR_ORDER_LSB
 	},
+	/* [AO_SERIAL_SPEED_9600] = */ {
+		/* .baud = */ 163,
+		/* .gcr  = */ (8 << UxGCR_BAUD_E_SHIFT) | UxGCR_ORDER_LSB
+	},
 	/* [AO_SERIAL_SPEED_57600] = */ {
 		/* .baud = */ 59,
 		/* .gcr =  */ (11 << UxGCR_BAUD_E_SHIFT) | UxGCR_ORDER_LSB