diff options
Diffstat (limited to 'monitor_garmin.c')
-rw-r--r-- | monitor_garmin.c | 296 |
1 files changed, 296 insertions, 0 deletions
diff --git a/monitor_garmin.c b/monitor_garmin.c new file mode 100644 index 00000000..5f155f18 --- /dev/null +++ b/monitor_garmin.c @@ -0,0 +1,296 @@ +/* + * Garmin binary object for the GPS packet monitor. + * + * This file is Copyright (c) 2011 by the GPSD project + * BSD terms apply: see the file COPYING in the distribution root for details. + */ + +#include <assert.h> +#include <math.h> + +#include "gpsd.h" +#include "bits.h" +#include "gpsmon.h" + +#if defined(GARMIN_ENABLE) && defined(BINARY_ENABLE) +extern const struct gps_type_t garmin_ser_binary; + +static WINDOW *miscwin, *mid51win, *mid114win; + +#define display (void)mvwprintw + +#define GARMIN_CHANNELS 12 + +#define GPSD_LE16TOH(x) getles16((char *)(&(x)), 0) +#define GPSD_LE32TOH(x) getles32((char *)(&(x)), 0) + +#pragma pack(1) +/* Satellite Data Record */ +typedef struct +{ + uint8_t svid; + uint16_t snr; + uint8_t elev; + uint16_t azmth; + uint8_t status; +} cpo_sat_data; + +/* Position Record */ +typedef struct +{ + float alt; + float epe; + float eph; + float epv; + int16_t fix; + double gps_tow; + double lat; + double lon; + float lon_vel; + float lat_vel; + float alt_vel; + float msl_hght; + int16_t leap_sec; + int32_t grmn_days; +} cpo_pvt_data; + +/* Receiver Measurement Record */ +typedef struct +{ + uint32_t cycles; + double pr; + uint16_t phase; + int8_t slp_dtct; + uint8_t snr_dbhz; + uint8_t svid; + int8_t valid; +} cpo_rcv_sv_data; +typedef struct +{ + double rcvr_tow; + int16_t rcvr_wn; + cpo_rcv_sv_data sv[GARMIN_CHANNELS]; +} cpo_rcv_data; + +/* description of the status */ +static char *fixdesc[] = { + "no fix", + "no fix", + "2D", + "3D", + "2D dif", + "3D dif", +}; + +/* check range of an unsigned quantity */ +#define CHECK_RANGE(vec, i) ((i) < sizeof(vec)/sizeof(vec[0])) + +static bool garmin_bin_initialize(void) +{ + /*@-globstate@*/ + unsigned int i; + +#ifndef CONTROLSEND_ENABLE + if(serial) { + monitor_complain("Direct mode doesn't supported."); + return false; + } +#endif + + /*@ -onlytrans @*/ + miscwin = subwin(devicewin, 1, 80, 1, 0); + mid51win = subwin(devicewin, 12, 18, 2, 0); + mid114win = subwin(devicewin, GARMIN_CHANNELS + 3, 23, 2, 18); + if (miscwin == NULL || mid51win == NULL || mid114win == NULL) + return false; + + (void)syncok(miscwin, true); + (void)syncok(mid51win, true); + (void)syncok(mid114win, true); + + /*@ -nullpass @*/ + (void)wattrset(miscwin, A_BOLD); + display(miscwin, 0, 0, "Time:"); + (void)wattrset(miscwin, A_NORMAL); + + (void)wborder(mid51win, 0, 0, 0, 0, 0, 0, 0, 0), + (void)wattrset(mid51win, A_BOLD); + display(mid51win, 0, 4, " Position "); + display(mid51win, 1, 2, "Fix:"); + display(mid51win, 2, 2, "Lat:"); + (void)mvwaddch(mid51win, 2, 16, ACS_DEGREE); + display(mid51win, 3, 2, "Lon:"); + (void)mvwaddch(mid51win, 3, 16, ACS_DEGREE); + display(mid51win, 4, 2, "Alt: m"); + display(mid51win, 5, 2, "Speed: m/s"); + display(mid51win, 6, 2, "Climb: m/s"); + display(mid51win, 7, 2, "Leap: sec"); + display(mid51win, 8, 2, "epe: m"); + display(mid51win, 9, 2, "eph: m"); + display(mid51win, 10, 2, "epv: m"); + display(mid51win, 11, 3, " ID 51 (0x33) "); + (void)wattrset(mid51win, A_NORMAL); + + (void)wborder(mid114win, 0, 0, 0, 0, 0, 0, 0, 0), + (void)wattrset(mid114win, A_BOLD); + display(mid114win, 1, 1, "Ch PRN Az El SNR ST"); + for (i = 0; i < GARMIN_CHANNELS; i++) { + display(mid114win, (int)i + 2, 1, "%2d", i); + } + display(mid114win, 0, 5, " Satellite "); + display(mid114win, 14, 4, " ID 114 (0x72) "); + (void)wattrset(mid114win, A_NORMAL); + + /* initialize the GPS context's time fields */ + gpsd_time_init(session.context, time(NULL)); + + return true; + /*@+globstate@*/ +} + +/*@ -globstate */ +static void garmin_bin_update(uint16_t pkt_id, uint32_t pkt_size UNUSED, unsigned char *pkt_data) +{ + int i; + cpo_sat_data *sats = NULL; + cpo_pvt_data *pvt = NULL; + cpo_rcv_data *rmd = NULL; + char tbuf[JSON_DATE_MAX+1]; + + switch (pkt_id) { + case 0x29: /* Receiver Measurement Record */ + case 0x34: + rmd = (cpo_rcv_data *)pkt_data; /* for future use */ + monitor_log("RMD 0x%02x=", pkt_id); + break; + + case 0x33: /* Position Record */ + display(miscwin, 0, 6, "%-24s", + unix_to_iso8601(session.gpsdata.fix.time, tbuf, sizeof(tbuf))); + pvt = (cpo_pvt_data *)pkt_data; + display(mid51win, 1, 7, "%s", + (CHECK_RANGE(fixdesc, (uint8_t)GPSD_LE16TOH(pvt->fix)) ? \ + fixdesc[GPSD_LE16TOH(pvt->fix)] : "unknown")); + display(mid51win, 2, 8, "%3.5f", pvt->lat * RAD_2_DEG); + display(mid51win, 3, 8, "%3.5f", pvt->lon * RAD_2_DEG); + display(mid51win, 4, 8, "%8.2f", pvt->alt + pvt->msl_hght); + display(mid51win, 5, 9, "%5.1f", hypot(pvt->lon_vel, pvt->lat_vel)); + display(mid51win, 6, 9, "%5.1f", pvt->alt_vel); + display(mid51win, 7, 8, "%d", (int)GPSD_LE16TOH(pvt->leap_sec)); + display(mid51win, 8, 7, "%6.2f", pvt->epe); + display(mid51win, 9, 7, "%6.2f", pvt->eph); + display(mid51win, 10, 7, "%6.2f", pvt->epv); + monitor_log("PVT 0x%02x=", pkt_id); + break; + + case 0x72: /* Satellite Data Record */ + sats = (cpo_sat_data *)pkt_data; + for (i = 0; i < GARMIN_CHANNELS; i++, sats++) { + display(mid114win, i + 2, 3, " %3u %3u %2u %4.1f %2x", + sats->svid, + GPSD_LE16TOH(sats->azmth), + sats->elev, + (float)GPSD_LE16TOH(sats->snr) / 100.0, + sats->status); + } + monitor_log("SAT 0x%02x=", pkt_id); + break; + + case 0xff: /* Product Data Record */ + monitor_log("PDR 0x%02x=", pkt_id); + break; + + default: + monitor_log("UNK 0x%02x=", pkt_id); + break; + } +} + +static void garmin_bin_ser_update(void) +{ + unsigned char *buf; + size_t len; + uint16_t pkt_id; + uint32_t pkt_size; + unsigned char pkt_data[255]; + int i, j; + unsigned char c; + unsigned char chksum; + bool pkt_good = false, got_dle = false; + + buf = session.packet.outbuffer; + len = session.packet.outbuflen; + + if (!(buf[0] == (unsigned char)0x10 && /* DLE */ + buf[len-2] == (unsigned char)0x10 && /* DLE */ + buf[len-1] == (unsigned char)0x03)) /* ETX */ + goto end; /* bad pkt */ + + if (buf[1] == (unsigned char)0x10 && buf[2] != (unsigned char)0x10) + goto end; /* bad pkt */ + pkt_id = (uint16_t)buf[1]; + + if (buf[2] == (unsigned char)0x10 && buf[3] != (unsigned char)0x10) + goto end; /* bad pkt */ + pkt_size = (uint32_t)buf[2]; + + chksum = buf[1] + buf[2]; /* pkt_id + pkt_size */ + + j = 0; + for (i = 0; i <= 255; i++) { + if (pkt_size == (uint32_t)j) + break; + c = buf[i+3]; + if (got_dle) { + got_dle = false; + if (c != (unsigned char)0x10) + goto end; /* bad pkt */ + } else { + pkt_data[j++] = c; + chksum += c; + if (c == (unsigned char)0x10) + got_dle = true; + } + } + + /* check pkt chksum */ + if ((unsigned char)(-chksum) == buf[len-3]) + pkt_good = true; + + end: + if (pkt_good) { +#ifdef CONTROLSEND_ENABLE + if(serial) + /* good packet, send ACK */ + (void)monitor_control_send((unsigned char *)"\x10\x06\x00\xfa\x10\x03", 6); +#endif + garmin_bin_update(pkt_id, pkt_size, pkt_data); + } else { +#ifdef CONTROLSEND_ENABLE + if(serial) + /* bad packet, send NAK */ + (void)monitor_control_send((unsigned char *)"\x10\x15\x00\xeb\x10\x03", 6); +#endif + monitor_log("BAD 0x%02x=", buf[1]); + } +} +/*@ +globstate */ + +static void garmin_bin_wrap(void) +{ + (void)delwin(miscwin); + (void)delwin(mid51win); + (void)delwin(mid114win); +} + +const struct monitor_object_t garmin_bin_ser_mmt = { + .initialize = garmin_bin_initialize, + .update = garmin_bin_ser_update, + .command = NULL, + .wrap = garmin_bin_wrap, + .min_y = 16,.min_x = 80, + .driver = &garmin_ser_binary, +}; + +#endif /* defined(GARMIN_ENABLE) && defined(BINARY_ENABLE) */ + |