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>

4 files changed, 95 insertions, 8 deletions

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();