Convert GPS to SiRF binary protocol.

This switches the GPS unit from NMEA to SiRF protocol at startup and then
parses the binary data. The binary data uses a different encoding of lat/lon
than the NMEA strings, and so the telemetry and eeprom data formats change
with this switch.

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

1 files changed, 231 insertions, 186 deletions

diff --git a/src/ao_gps.c b/src/ao_gps.c
index cbde8b48..147b665c 100644
--- a/src/ao_gps.c
+++ b/src/ao_gps.c
@@ -17,237 +17,282 @@
 
 #include "ao.h"
 
-#define AO_GPS_LEADER		6
-
-static const char ao_gps_header[] = "GPGGA,";
 __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;
-static __xdata struct ao_gps_data	ao_gps_next;
 
-const char ao_gps_config[] =
-	"$PSRF103,00,00,01,01*25\r\n"	/* GGA 1 per sec */
-	"$PSRF103,01,00,00,01*25\r\n"	/* GLL disable */
-	"$PSRF103,02,00,00,01*26\r\n"	/* GSA disable */
-	"$PSRF103,03,00,00,01*27\r\n"	/* GSV disable */
-	"$PSRF103,04,00,00,01*20\r\n"	/* RMC disable */
-	"$PSRF103,05,00,00,01*21\r\n";	/* VTG disable */
+const char ao_gps_config[] = {
+	/* Serial param - binary, 4800 baud, 8 bits, 1 stop, no parity */
+	'$', 'P', 'S', 'R', 'F', '1', '0', '0', ',', '0', ',',
+	'4', '8', '0', '0', ',', '8', ',', '1', ',', '0', '*',
+	'0', 'F', '\r','\n',
+
+	0xa0, 0xa2, 0x00, 0x0e,	/* length: 14 bytes */
+	136,			/* mode control */
+	0, 0,			/* reserved */
+	4,			/* degraded mode (disabled) */
+	0, 0,			/* reserved */
+	0, 0,			/* user specified altitude */
+	2,			/* alt hold mode (disabled, require 3d fixes) */
+	0,			/* alt hold source (use last computed altitude) */
+	0,			/* reserved */
+	0,			/* Degraded time out (disabled) */
+	0,			/* Dead Reckoning time out (disabled) */
+	0,			/* Track smoothing (disabled) */
+	0x00, 0x8e, 0xb0, 0xb3,
+
+	0xa0, 0xa2, 0x00, 0x02,	/* length: 2 bytes */
+	143,			/* static navigation */
+	0,			/* disable */
+	0x00, 0x8f, 0xb0, 0xb3,
+
+	0xa0, 0xa2, 0x00, 0x08,	/* length: 8 bytes */
+	166,			/* Set message rate */
+	2,			/* enable/disable all messages */
+	0,			/* message id (ignored) */
+	0,			/* update rate (0 = disable) */
+	0, 0, 0, 0,		/* reserved */
+	0x00, 0xa8, 0xb0, 0xb3,
+
+	0xa0, 0xa2, 0x00, 0x08,	/* length: 8 bytes */
+	166,			/* Set message rate */
+	0,			/* enable/disable one message */
+	41,			/* message 41 */
+	1,			/* once per second */
+	0, 0, 0, 0,		/* reserved */
+	0x00, 0xd0, 0xb0, 0xb3,
+};
 
-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;
-}
+#define NAV_TYPE_GPS_FIX_TYPE_MASK			(7 << 0)
+#define NAV_TYPE_NO_FIX					(0 << 0)
+#define NAV_TYPE_SV_KF					(1 << 0)
+#define NAV_TYPE_2_SV_KF				(2 << 0)
+#define NAV_TYPE_3_SV_KF				(3 << 0)
+#define NAV_TYPE_4_SV_KF				(4 << 0)
+#define NAV_TYPE_2D_LEAST_SQUARES			(5 << 0)
+#define NAV_TYPE_3D_LEAST_SQUARES			(6 << 0)
+#define NAV_TYPE_DR					(7 << 0)
+#define NAV_TYPE_TRICKLE_POWER				(1 << 3)
+#define NAV_TYPE_ALTITUDE_HOLD_MASK			(3 << 4)
+#define NAV_TYPE_ALTITUDE_HOLD_NONE			(0 << 4)
+#define NAV_TYPE_ALTITUDE_HOLD_KF			(1 << 4)
+#define NAV_TYPE_ALTITUDE_HOLD_USER			(2 << 4)
+#define NAV_TYPE_ALTITUDE_HOLD_ALWAYS			(3 << 4)
+#define NAV_TYPE_DOP_LIMIT_EXCEEDED			(1 << 6)
+#define NAV_TYPE_DGPS_APPLIED				(1 << 7)
+#define NAV_TYPE_SENSOR_DR				(1 << 8)
+#define NAV_TYPE_OVERDETERMINED				(1 << 9)
+#define NAV_TYPE_DR_TIMEOUT_EXCEEDED			(1 << 10)
+#define NAV_TYPE_FIX_MI_EDIT				(1 << 11)
+#define NAV_TYPE_INVALID_VELOCITY			(1 << 12)
+#define NAV_TYPE_ALTITUDE_HOLD_DISABLED			(1 << 13)
+#define NAV_TYPE_DR_ERROR_STATUS_MASK			(3 << 14)
+#define NAV_TYPE_DR_ERROR_STATUS_GPS_ONLY		(0 << 14)
+#define NAV_TYPE_DR_ERROR_STATUS_DR_FROM_GPS		(1 << 14)
+#define NAV_TYPE_DR_ERROR_STATUS_DR_SENSOR_ERROR	(2 << 14)
+#define NAV_TYPE_DR_ERROR_STATUS_DR_IN_TEST		(3 << 14)
+
+struct sirf_geodetic_nav_data {
+	uint16_t	nav_type;
+	uint16_t	utc_year;
+	uint8_t 	utc_month;
+	uint8_t 	utc_day;
+	uint8_t 	utc_hour;
+	uint8_t 	utc_minute;
+	uint16_t 	utc_second;
+	int32_t		lat;
+	int32_t		lon;
+	int32_t		alt_msl;
+	uint16_t	ground_speed;
+	uint16_t	course;
+	int16_t		climb_rate;
+	uint32_t	h_error;
+	uint32_t	v_error;
+	uint8_t		num_sv;
+	uint8_t		hdop;
+};
 
-void
-ao_gps_skip(void)
-{
-	while (ao_gps_char >= '0')
-		ao_gps_lexchar();
-}
+static __xdata struct sirf_geodetic_nav_data	ao_sirf_data;
 
-void
-ao_gps_skip_field(void)
+static __pdata uint16_t ao_sirf_cksum;
+static __pdata uint16_t ao_sirf_len;
+
+#define ao_sirf_byte()	((uint8_t) ao_serial_getchar())
+
+static uint8_t data_byte(void)
 {
-	while (ao_gps_char != ',' && ao_gps_char != '*' && ao_gps_char != '\n')
-		ao_gps_lexchar();
+	uint8_t	c = ao_sirf_byte();
+	--ao_sirf_len;
+	ao_sirf_cksum += c;
+	return c;
 }
 
-void
-ao_gps_skip_sep(void)
+static void sirf_u16(uint8_t offset)
 {
-	while (ao_gps_char == ',' || ao_gps_char == '.' || ao_gps_char == '*')
-		ao_gps_lexchar();
-}
+	uint16_t __xdata *ptr = (uint16_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset);
+	uint16_t val;
 
-__xdata static uint8_t ao_gps_num_width;
+	val = data_byte() << 8;
+	val |= data_byte ();
+	*ptr = val;
+}
 
-static int16_t
-ao_gps_decimal(uint8_t max_width)
+static void sirf_u8(uint8_t offset)
 {
-	int16_t	v;
-	__xdata uint8_t	neg = 0;
+	uint8_t __xdata *ptr = (uint8_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset);
+	uint8_t val;
 
-	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;
+	val = data_byte ();
+	*ptr = val;
 }
 
-static uint8_t
-ao_gps_hex(uint8_t max_width)
+static void sirf_u32(uint8_t offset)
 {
-	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;
+	uint32_t __xdata *ptr = (uint32_t __xdata *) (((char __xdata *) &ao_sirf_data) + offset);
+	uint32_t val;
+
+	val = ((uint32_t) data_byte ()) << 24;
+	val |= ((uint32_t) data_byte ()) << 16;
+	val |= ((uint32_t) data_byte ()) << 8;
+	val |= ((uint32_t) data_byte ());
+	*ptr = val;
 }
 
-static void
-ao_gps_parse_pos(__xdata struct ao_gps_pos * pos, uint8_t deg_width) __reentrant
+static void sirf_discard(uint8_t len)
 {
-	pos->degrees = ao_gps_decimal(deg_width);
-	pos->minutes = ao_gps_decimal(2);
-	if (ao_gps_char == '.') {
-		pos->minutes_fraction = ao_gps_decimal(4);
-		while (ao_gps_num_width < 4) {
-			pos->minutes_fraction *= 10;
-			ao_gps_num_width++;
-		}
-	} else {
-		pos->minutes_fraction = 0;
-		if (ao_gps_char != ',')
-			ao_gps_error = 1;
-	}
+	while (len--)
+		data_byte();
 }
 
+#define SIRF_END	0
+#define SIRF_DISCARD	1
+#define SIRF_U8		2
+#define SIRF_U16	3
+#define SIRF_U32	4
+
+struct sirf_packet_parse {
+	uint8_t	type;
+	uint8_t	offset;
+};
+
+static const struct sirf_packet_parse geodetic_nav_data_packet[] = {
+	{ SIRF_DISCARD, 2 },							/* 1 nav valid */
+	{ SIRF_U16, offsetof(struct sirf_geodetic_nav_data, nav_type) },	/* 3 */
+	{ SIRF_DISCARD, 6 },							/* 5 week number, time of week */
+	{ SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_year) },	/* 11 */
+	{ SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_month) },	/* 13 */
+	{ SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_day) },		/* 14 */
+	{ SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_hour) },		/* 15 */
+	{ SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_minute) },	/* 16 */
+	{ SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_second) },	/* 17 */
+	{ SIRF_DISCARD, 4 },	/* satellite id list */				/* 19 */
+	{ SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lat) },		/* 23 */
+	{ SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lon) },		/* 27 */
+	{ SIRF_DISCARD, 4 },	/* altitude from ellipsoid */			/* 31 */
+	{ SIRF_U32, offsetof(struct sirf_geodetic_nav_data, alt_msl) },		/* 35 */
+	{ SIRF_DISCARD, 1 },	/* map datum */					/* 39 */
+	{ SIRF_U16, offsetof(struct sirf_geodetic_nav_data, ground_speed) },	/* 40 */
+	{ SIRF_U16, offsetof(struct sirf_geodetic_nav_data, course) },		/* 42 */
+	{ SIRF_DISCARD, 2 },	/* magnetic variation */			/* 44 */
+	{ SIRF_U16, offsetof(struct sirf_geodetic_nav_data, climb_rate) },	/* 46 */
+	{ SIRF_DISCARD, 2 },	/* turn rate */					/* 48 */
+	{ SIRF_U32, offsetof(struct sirf_geodetic_nav_data, h_error) },		/* 50 */
+	{ SIRF_U32, offsetof(struct sirf_geodetic_nav_data, v_error) },		/* 54 */
+	{ SIRF_DISCARD, 30 },	/* time error, h_vel error, clock_bias,
+				   clock bias error, clock drift,
+				   clock drift error, distance,
+				   distance error, heading error */		/* 58 */
+	{ SIRF_U8, offsetof(struct sirf_geodetic_nav_data, num_sv) },		/* 88 */
+	{ SIRF_U8, offsetof(struct sirf_geodetic_nav_data, hdop) },		/* 89 */
+	{ SIRF_DISCARD, 1 },	/* additional mode info */			/* 90 */
+	{ SIRF_END, 0 },							/* 91 */
+};
+
 static void
-ao_gps_parse_flag(char yes_c, uint8_t yes, char no_c, uint8_t no) __reentrant
+ao_sirf_parse_41(void)
 {
-	ao_gps_skip_sep();
-	if (ao_gps_char == yes_c)
-		ao_gps_next.flags |= yes;
-	else if (ao_gps_char == no_c)
-		ao_gps_next.flags |= no;
-	else
-		ao_gps_error = 1;
-	ao_gps_lexchar();
+	uint8_t	i, offset;
+
+	for (i = 0; ; i++) {
+		offset = geodetic_nav_data_packet[i].offset;
+		switch (geodetic_nav_data_packet[i].type) {
+		case SIRF_END:
+			return;
+		case SIRF_DISCARD:
+			sirf_discard(offset);
+			break;
+		case SIRF_U8:
+			sirf_u8(offset);
+			break;
+		case SIRF_U16:
+			sirf_u16(offset);
+			break;
+		case SIRF_U32:
+			sirf_u32(offset);
+			break;
+		}
+	}
 }
 
-
 void
 ao_gps(void) __reentrant
 {
 	char	c;
 	uint8_t	i;
+	uint16_t cksum;
 
 	for (i = 0; (c = ao_gps_config[i]); i++)
 		ao_serial_putchar(c);
 	for (;;) {
 		/* Locate the begining of the next record */
-		for (;;) {
-			c = ao_serial_getchar();
-			if (c == '$')
-				break;
-		}
+		while (ao_sirf_byte() != 0xa0)
+			;
+		if (ao_sirf_byte() != 0xa2)
+			continue;
+
+		/* Length */
+		ao_sirf_len = ao_sirf_byte() << 8;
+		ao_sirf_len |= ao_sirf_byte();
+		if (ao_sirf_len > 1023)
+			continue;
+
+		ao_sirf_cksum = 0;
 
-		ao_gps_cksum = 0;
-		ao_gps_error = 0;
+		/* message ID */
+		i = data_byte ();							/* 0 */
 
-		/* Skip anything other than GGA */
-		for (i = 0; i < AO_GPS_LEADER; i++) {
-			ao_gps_lexchar();
-			if (ao_gps_char != ao_gps_header[i])
+		switch (i) {
+		case 41:
+			if (ao_sirf_len < 91)
 				break;
+			ao_sirf_parse_41();
+			break;
 		}
-		if (i != AO_GPS_LEADER)
+		if (ao_sirf_len != 0)
 			continue;
 
-		/* 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 = 0;
-		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_parse_pos(&ao_gps_next.latitude, 2);
-		ao_gps_parse_flag('N', AO_GPS_LATITUDE_NORTH, 'S', AO_GPS_LATITUDE_SOUTH);
-		ao_gps_parse_pos(&ao_gps_next.longitude, 3);
-		ao_gps_parse_flag('W', AO_GPS_LONGITUDE_WEST, 'E', AO_GPS_LONGITUDE_EAST);
-
-		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) {
+		/* verify checksum and end sequence */
+		ao_sirf_cksum &= 0x7fff;
+		cksum = ao_sirf_byte() << 8;
+		cksum |= ao_sirf_byte();
+		if (ao_sirf_cksum != cksum)
+			continue;
+		if (ao_sirf_byte() != 0xb0)
+			continue;
+		if (ao_sirf_byte() != 0xb3)
+			continue;
+
+		switch (i) {
+		case 41:
 			ao_mutex_get(&ao_gps_mutex);
-			memcpy(&ao_gps_data, &ao_gps_next, sizeof (struct ao_gps_data));
+			ao_gps_data.hour = ao_sirf_data.utc_hour;
+			ao_gps_data.minute = ao_sirf_data.utc_minute;
+			ao_gps_data.second = ao_sirf_data.utc_second / 1000;
+			ao_gps_data.flags = (ao_sirf_data.num_sv << AO_GPS_NUM_SAT_SHIFT) & AO_GPS_NUM_SAT_MASK;
+			if ((ao_sirf_data.nav_type & NAV_TYPE_GPS_FIX_TYPE_MASK) >= NAV_TYPE_4_SV_KF)
+				ao_gps_data.flags |= AO_GPS_VALID;
 			ao_mutex_put(&ao_gps_mutex);
 			ao_wakeup(&ao_gps_data);
+			break;
 		}
 	}
 }