Fix up SiRF parsing and test code so that it actually works

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

1 files changed, 185 insertions, 3 deletions

diff --git a/src/ao_gps_test.c b/src/ao_gps_test.c
index 7489e788..35cce7de 100644
--- a/src/ao_gps_test.c
+++ b/src/ao_gps_test.c
@@ -98,6 +98,145 @@ ao_serial_getchar(void)
 	return c;
 }
 
+static void
+check_sirf_message(char *from, uint8_t *msg, int len)
+{
+	uint16_t	encoded_len, encoded_cksum;
+	uint16_t	cksum;
+	uint8_t		id;
+	int		i;
+
+	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];
+	if (encoded_len != len - 8) {
+		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];
+	if (id == 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 %d\n", week);
+		printf ("\tTOW %d\n", tow);
+		printf ("\tyear %d\n", year);
+		printf ("\tmonth %d\n", month);
+		printf ("\tday %d\n", day);
+		printf ("\thour %d\n", hour);
+		printf ("\tminute %d\n", minute);
+		printf ("\tsecond %g\n", second / 1000.0);
+		printf ("\tsats: %08x\n", sat_list);
+		printf ("\tlat: %g\n", lat / 1.0e7);
+		printf ("\tlon: %g\n", lon / 1.0e7);
+		printf ("\talt_ell: %g\n", alt_ell / 100.0);
+		printf ("\talt_msll: %g\n", alt_msl / 100.0);
+		printf ("\tdatum: %d\n", datum);
+		printf ("\tground speed: %g\n", sog / 100.0);
+		printf ("\tcourse: %g\n", cog / 100.0);
+		printf ("\tclimb: %g\n", climb_rate / 100.0);
+		printf ("\theading rate: %g\n", heading_rate / 100.0);
+		printf ("\th error: %g\n", h_error / 100.0);
+		printf ("\tv error: %g\n", v_error / 100.0);
+		printf ("\tt error: %g\n", t_error / 100.0);
+		printf ("\th vel error: %g\n", h_v_error / 100.0);
+	} else {
+		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	sirf_in_message[4096];
+static int	sirf_in_len;
+
 void *
 ao_gps_input(void *arg)
 {
@@ -109,7 +248,16 @@ ao_gps_input(void *arg)
 		i = read(ao_gps_fd, &c, 1);
 		if (i == 1) {
 			int	v;
+			uint8_t	uc = c;
 
+			if (sirf_in_len || uc == 0xa0) {
+				if (sirf_in_len < 4096)
+					sirf_in_message[sirf_in_len++] = uc;
+				if (uc == 0xb3) {
+					check_sirf_message("recv", sirf_in_message, sirf_in_len);
+					sirf_in_len = 0;
+				}
+			}
 			input_queue[input_tail] = c;
 			input_tail = (input_tail + 1) % QUEUE_LEN;
 			sem_post(&input_semaphore);
@@ -125,16 +273,32 @@ ao_gps_input(void *arg)
 	}
 }
 
+static uint8_t	sirf_message[4096];
+static int	sirf_message_len;
+
 void
 ao_serial_putchar(char c)
 {
 	int	i;
+	uint8_t	uc = (uint8_t) c;
 
-	ao_dbg_char(c);
+	if (sirf_message_len || uc == 0xa0) {
+		if (sirf_message_len < 4096)
+			sirf_message[sirf_message_len++] = uc;
+		if (uc == 0xb3) {
+			check_sirf_message("send", sirf_message, sirf_message_len);
+			sirf_message_len = 0;
+		}
+	}
 	for (;;) {
 		i = write(ao_gps_fd, &c, 1);
-		if (i == 1)
+		if (i == 1) {
+			if ((uint8_t) c == 0xb3 || c == '\r') {
+				tcdrain(ao_gps_fd);
+				usleep (1000 * 100);
+			}
 			break;
+		}
 		if (i < 0 && (errno == EINTR || errno == EAGAIN))
 			continue;
 		perror("putchar");
@@ -161,7 +325,25 @@ ao_serial_set_speed(uint8_t fast)
 void
 ao_dump_state(void)
 {
-	printf ("dump state\n");
+	double	lat, lon;
+	printf ("%02d:%02d:%02d",
+		ao_gps_data.hour, ao_gps_data.minute,
+		ao_gps_data.second);
+	printf (" nsat %d %svalid",
+		ao_gps_data.flags & AO_GPS_NUM_SAT_MASK,
+		ao_gps_data.flags & AO_GPS_VALID ? "" : "not ");
+	printf (" lat %g lon %g alt %d",
+		ao_gps_data.latitude / 1.0e7,
+		ao_gps_data.longitude / 1.0e7,
+		ao_gps_data.altitude);
+	printf (" speed %g climb %g course %d",
+		ao_gps_data.ground_speed / 100.0,
+		ao_gps_data.climb_rate / 100.0,
+		ao_gps_data.course * 2);
+	printf (" hdop %g h_error %d v_error %d",
+		ao_gps_data.hdop / 5.0,
+		ao_gps_data.h_error, ao_gps_data.v_error);
+	printf("\n");
 }
 
 int