From: Keith Packard Date: Tue, 30 Jun 2009 06:03:58 +0000 (-0700) Subject: Add GPS speed and error data to telemetry and aoview X-Git-Tag: 0.5~45 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=527d7c803ed9597b210634018cb2eb9d048d9846 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 --- 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 #include #include @@ -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();