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>

diff --git a/aoview/aoview.h b/aoview/aoview.h
index 4eb4cd80b8a06d8ef267b0050c37e3f1e8209b24..6fb5e0988c691918fff4ee9172df94596668270f 100644 (file)
--- 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 e2ba75bab0f8f9d92e7a3c47fe43ad697aca3c6b..d01ca77160993bcd154b683f3ebb9fc1faa40530 100644 (file)
--- 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 356828c65a3b0bc1e58e063974e11bbe0e6a10a8..52f581ff63cdb3d8bf161929065d0964953d3213 100644 (file)
--- 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 d4df159553352b5da135e973b696ebb5d077042a..30511c8c2f7c49326e73bb9c02412ccb4bf237dc 100644 (file)
--- 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 147b665c363de0cdc63a929254ea96f9f07f1c9f..e7f0693c3320d994915010475bcf3a1dbd771a3f 100644 (file)
--- 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 bef87aeadaf2c83c62ea14d2a285e945181e6fd3..46521b107207b4a1cc8bd59a6d4b101f17a28b5f 100644 (file)
--- 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 (executable)
index 0000000..b905f31
--- /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();