Add GPS speed and error data to telemetry and aoview

Having switched to the SiRF binary GPS format, the velocity and error data
can now be displayed.

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

7 files changed, 130 insertions, 11 deletions

diff --git a/aoview/aoview.h b/aoview/aoview.h
index 4eb4cd80..6fb5e098 100644
--- a/aoview/aoview.h
+++ b/aoview/aoview.h
@@ -20,6 +20,10 @@
 
 #define _GNU_SOURCE
 
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
@@ -70,9 +74,15 @@ struct aostate {
 		int minute;
 		int second;
 	} gps_time;
-	double	lat;
-	double	lon;
-	int	alt;
+	double	lat;		/* degrees (+N -S) */
+	double	lon;		/* degrees (+E -W) */
+	int	alt;		/* m */
+	double	ground_speed;	/* m/s */
+	int	course;		/* degrees */
+	double	climb_rate;	/* m/s */
+	double	hdop;		/* unitless? */
+	int	h_error;	/* m */
+	int	v_error;	/* m */
 };
 
 void
diff --git a/aoview/aoview_monitor.c b/aoview/aoview_monitor.c
index e2ba75ba..d01ca771 100644
--- a/aoview/aoview_monitor.c
+++ b/aoview/aoview_monitor.c
@@ -117,6 +117,21 @@ aoview_monitor_parse(char *line)
 		state.lat = state.lon = 0;
 		state.alt = 0;
 	}
+	if (nword >= 46) {
+		sscanf(words[40], "%lfm/s", &state.ground_speed);
+		sscanf(words[41], "%d", &state.course);
+		sscanf(words[42], "%lfm/s", &state.climb_rate);
+		sscanf(words[43], "%lf", &state.hdop);
+		sscanf(words[44], "%d", &state.h_error);
+		sscanf(words[45], "%d", &state.v_error);
+	} else {
+		state.ground_speed = 0;
+		state.course = 0;
+		state.climb_rate = 0;
+		state.hdop = 0;
+		state.h_error = 0;
+		state.v_error = 0;
+	}
 	aoview_state_notify(&state);
 }
 
diff --git a/aoview/aoview_state.c b/aoview/aoview_state.c
index 356828c6..52f581ff 100644
--- a/aoview/aoview_state.c
+++ b/aoview/aoview_state.c
@@ -151,6 +151,13 @@ aoview_state_notify(struct aostate *state)
 				     state->gps_time.hour,
 				     state->gps_time.minute,
 				     state->gps_time.second);
+		aoview_table_add_row("GPS ground speed", "%fm/s %d°",
+				     state->ground_speed,
+				     state->course);
+		aoview_table_add_row("GPS climb rate", "%fm/s",
+				     state->climb_rate);
+		aoview_table_add_row("GPS precision", "%f(hdop) %dm(h) %dm(v)\n",
+				     state->hdop, state->h_error, state->v_error);
 		aoview_great_circle(pad_lat, pad_lon, state->lat, state->lon,
 				    &dist, &bearing);
 		aoview_table_add_row("Distance from pad", "%gm", dist * 1000);
diff --git a/src/ao.h b/src/ao.h
index d4df1595..30511c8c 100644
--- a/src/ao.h
+++ b/src/ao.h
@@ -678,9 +678,15 @@ struct ao_gps_data {
 	uint8_t			minute;
 	uint8_t			second;
 	uint8_t			flags;
-	int32_t			latitude;
-	int32_t			longitude;
-	int16_t			altitude;
+	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 */
 };
 
 extern __xdata uint8_t ao_gps_mutex;
diff --git a/src/ao_gps.c b/src/ao_gps.c
index 147b665c..e7f0693c 100644
--- a/src/ao_gps.c
+++ b/src/ao_gps.c
@@ -290,6 +290,22 @@ ao_gps(void) __reentrant
 			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_gps_data.latitude = ao_sirf_data.lat;
+			ao_gps_data.longitude = ao_sirf_data.lon;
+			ao_gps_data.altitude = ao_sirf_data.alt_msl / 100;
+			ao_gps_data.ground_speed = ao_sirf_data.ground_speed;
+			ao_gps_data.course = ao_sirf_data.course / 200;
+			ao_gps_data.hdop = ao_sirf_data.hdop;
+			ao_gps_data.climb_rate = ao_sirf_data.climb_rate;
+			if (ao_sirf_data.h_error > 6553500)
+				ao_gps_data.h_error = 65535;
+			else
+				ao_gps_data.h_error = ao_sirf_data.h_error / 100;
+			if (ao_sirf_data.v_error > 6553500)
+				ao_gps_data.v_error = 65535;
+			else
+				ao_gps_data.v_error = ao_sirf_data.v_error / 100;
+			ao_gps_data.h_error = ao_sirf_data.h_error;
 			ao_mutex_put(&ao_gps_mutex);
 			ao_wakeup(&ao_gps_data);
 			break;
diff --git a/src/ao_gps_print.c b/src/ao_gps_print.c
index bef87aea..46521b10 100644
--- a/src/ao_gps_print.c
+++ b/src/ao_gps_print.c
@@ -44,15 +44,19 @@ void
 ao_gps_print(__xdata struct ao_gps_data *gps_data) __reentrant
 {
 	printf("GPS %2d sat",
-	       (gps_data->flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT);;
+	       (gps_data->flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT);
 	if (gps_data->flags & AO_GPS_VALID) {
 		static __xdata struct ao_gps_split	lat, lon;
+		int16_t climb;
+		uint8_t climb_sign;
+
 		ao_gps_split(gps_data->latitude, &lat);
 		ao_gps_split(gps_data->longitude, &lon);
-		printf(" %2d:%02d:%02d %2d°%02d.%04d'%c %2d°%02d.%04d'%c %5dm\n",
+		printf(" %2d:%02d:%02d",
 		       gps_data->hour,
 		       gps_data->minute,
-		       gps_data->second,
+		       gps_data->second);
+		printf(" %2d°%02d.%04d'%c %2d°%02d.%04d'%c %5dm",
 		       lat.degrees,
 		       lat.minutes,
 		       lat.minutes_fraction,
@@ -61,8 +65,25 @@ ao_gps_print(__xdata struct ao_gps_data *gps_data) __reentrant
 		       lon.minutes,
 		       lon.minutes_fraction,
 		       lon.positive ? 'E' : 'W',
-		       gps_data->altitude,
-		       (gps_data->flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT);
+		       gps_data->altitude);
+		if (gps_data->climb_rate >= 0) {
+			climb_sign = ' ';
+			climb = gps_data->climb_rate;
+		} else {
+			climb_sign = '-';
+			climb = -gps_data->climb_rate;
+		}
+		printf(" %5u.%02dm/s(H) %d° %c%5d.%02dm/s(V)",
+		       gps_data->ground_speed / 100,
+		       gps_data->ground_speed % 100,
+		       gps_data->course * 2,
+		       climb_sign,
+		       climb / 100,
+		       climb % 100);
+		printf(" %d.%d(hdop) %5d(herr) %5d(verr)\n",
+		       gps_data->hdop,
+		       gps_data->h_error,
+		       gps_data->v_error);
 	} else {
 		printf(" unlocked\n");
 	}
diff --git a/src/sirf-cksum b/src/sirf-cksum
new file mode 100755
index 00000000..b905f318
--- /dev/null
+++ b/src/sirf-cksum
@@ -0,0 +1,44 @@
+#!/usr/bin/env nickle
+
+int checksum(int[] msg)
+{
+	int sum = 0;
+	for (int i = 0; i < dim(msg); i++) {
+		sum += msg[i];
+		sum &= 0x7fff;
+	}
+	return sum;
+}
+
+void main()
+{
+	string[...]	input;
+	int[...]	msg;
+
+	setdim(input, 0);
+	while (!File::end(stdin)) {
+		input[dim(input)] = gets();
+	}
+
+	setdim(msg, 0);
+	for (int i = 0; i < dim(input); i++) {
+		string[*] words = String::wordsplit(input[i], " ,\t");
+		for (int j = 0; j < dim(words); j++) {
+			if (words[j] == "/" + "*")
+				break;
+			if (String::length(words[j]) > 0 &&
+			    Ctype::isdigit(words[j][0])) {
+				msg[dim(msg)] = string_to_integer(words[j]);
+			}
+		 }
+	}
+	printf("\t0xa0, 0xa2, 0x%02x, 0x%02x,\t/* length: %d bytes */\n",
+	       dim(msg) >> 8, dim(msg) & 0xff, dim(msg));
+	for (int i = 0; i < dim(input); i++)
+		printf("%s\n", input[i]);
+	int csum = checksum(msg);
+	printf ("\t0x%02x, 0x%02x, 0xb0, 0xb3,\n",
+		csum >> 8, csum & 0xff);
+}
+
+main();