From 16cad7154b8abd0da9e1efb1101ac70486e2e5d6 Mon Sep 17 00:00:00 2001 From: Kai Harrekilde-Petersen Date: Wed, 28 Mar 2018 21:26:59 +0200 Subject: [PATCH 4/5] Update Skytraq driver to support Venus8 modules, change modes (NMEA/Native, cycletime, baudrates) Signed-off-by: Kai Harrekilde-Petersen --- driver_nmea0183.c | 40 ++- driver_skytraq.c | 828 ++++++++++++++++++++++++++++++++++++++++++++++++------ driver_skytraq.h | 386 +++++++++++++++++++++++++ drivers.c | 13 + 4 files changed, 1164 insertions(+), 103 deletions(-) create mode 100644 driver_skytraq.h diff --git a/driver_nmea0183.c b/driver_nmea0183.c index eb3d881c..8930a543 100644 --- a/driver_nmea0183.c +++ b/driver_nmea0183.c @@ -1524,6 +1524,8 @@ static gps_mask_t processMTK3301(int c UNUSED, char *field[], #endif /* MTK3301_ENABLE */ #ifdef SKYTRAQ_ENABLE +/* Use this, rather than duplicate the functionality here (multiple times) */ +extern bool sky_detect(struct gps_device_t *); static gps_mask_t processPSTI030(int count, char *field[], struct gps_device_t *session) @@ -1634,10 +1636,8 @@ static gps_mask_t processPSTI(int count, char *field[], /* set something, so it won't look like an unknown sentence */ mask = ONLINE_SET; - if ( 0 != strncmp(session->subtype, "Skytraq", 7) ) { - /* this is skytraq, but marked yet, so probe for Skytraq */ - (void)gpsd_write(session, "\xA0\xA1\x00\x02\x02\x01\x03\x0d\x0a",9); - } + /* Probe for Skytraq if not already detected */ + (void)sky_detect(session); if (0 == strcmp("00", field[1]) ) { if ( 4 != count ) @@ -1651,17 +1651,34 @@ static gps_mask_t processPSTI(int count, char *field[], if (0 == strcmp("001", field[1])) { /* Active Antenna Status Report */ gpsd_log(&session->context->errout, LOG_DATA, - "PSTI,001: Count: %d\n", count); + "PSTI,001: Status: %s\n", field[2]); + return mask; + } + if (0 == strcmp("003", field[1])) { + /* Undocumented message sent by S1216/Venus8 FW v2.2.4 */ + gpsd_log(&session->context->errout, LOG_DATA, + "PSTI,003: Value: %s\n", field[2]); return mask; } if (0 == strcmp("005", field[1])) { /* GPIO 10 event-triggered time & position stamp. */ + /* Time error < 100nsec, max pos error < 0.278mm/kph * speed + * $PSTI,005,hhmmss.sssssss,xx,xx,xxxx,ddmm.mmmmmmm,a,dddmm.mmmmmmm,a,x.x*hh + * Field 2: UTC Time (zero filled) + * Field 3-5: UTC Day/month/Year (DDMMYYYY) + * Field 6,7: Latitude, N/S + * Field 8,9: Longitude, E/W + * Field 10: Altitude + */ + gpsd_log(&session->context->errout, LOG_DATA, "PSTI,005: Count: %d\n", count); return mask; } if (0 == strcmp("030", field[1])) { /* Recommended Minimum 3D GNSS Data */ + /* $PSTI,030,hhmmss.sss,A,dddmm.mmmmmmm,a,dddmm.mmmmmmm,a,x.x,x.x,x.x,x.x,ddmmyy,a.x.x,x.x*hh + */ return processPSTI030( count, field, session); } if (0 == strcmp("032", field[1])) { @@ -1669,6 +1686,7 @@ static gps_mask_t processPSTI(int count, char *field[], if ( 16 != count ) return 0; /* RTK Baseline */ + /* $PSTI,032,hhmmss.sss,ddmmyy,A,R,x.xxx,x.xxx,x.xxx,x.xxx,x.xx,,,,,*hh */ if ( 0 == strcmp(field[4], "A")) { /* Status Valid */ if (field[2][0] != '\0' && field[3][0] != '\0') { @@ -1706,10 +1724,8 @@ static gps_mask_t processSTI(int count, char *field[], /* set something, so it won't look like an unknown sentence */ mask = ONLINE_SET; - if ( 0 != strncmp(session->subtype, "Skytraq", 7) ) { - /* this is skytraq, but marked yet, so probe for Skytraq */ - (void)gpsd_write(session, "\xA0\xA1\x00\x02\x02\x01\x03\x0d\x0a",9); - } + /* Probe for Skytraq if not already detected */ + (void)sky_detect(session); if ( 0 == strcmp( field[1], "IC") ) { /* $STI,IC,error=XX, this is always very bad, but undocumented */ @@ -1778,7 +1794,7 @@ gps_mask_t nmea_parse(char *sentence, struct gps_device_t * session) #endif /* ASHTECH_ENABLE */ #ifdef MTK3301_ENABLE {"PMTK", 3, false, processMTK3301}, - /* for some reason thhe parser no longer triggering on leading chars */ + /* for some reason the parser no longer triggers on leading chars */ {"PMTK001", 3, false, processMTK3301}, {"PMTK424", 3, false, processMTK3301}, {"PMTK705", 3, false, processMTK3301}, @@ -1795,6 +1811,8 @@ gps_mask_t nmea_parse(char *sentence, struct gps_device_t * session) {"TXT", 5, false, processTXT}, {"ZDA", 4, false, processZDA}, {"VTG", 0, false, NULL}, /* ignore Velocity Track made Good */ + {"DTM", 0, false, NULL}, /* ignore Datum */ + {"GRS", 0, false, NULL}, /* ignore GPS Range Residuals */ }; int count; @@ -1830,7 +1848,7 @@ gps_mask_t nmea_parse(char *sentence, struct gps_device_t * session) if (*p == '*') *p++ = ','; /* otherwise we drop the last field */ #ifdef SKYTRAQ_ENABLE_UNUSED - /* $STI is special, no trailing *, or chacksum */ + /* $STI is special, no trailing *, or checksum */ if ( 0 != strncmp( "STI,", sentence, 4) ) { skytraq_sti = true; *p++ = ','; /* otherwise we drop the last field */ diff --git a/driver_skytraq.c b/driver_skytraq.c index 2861d39b..42e7fc4d 100644 --- a/driver_skytraq.c +++ b/driver_skytraq.c @@ -1,8 +1,10 @@ /* * This is the gpsd driver for Skytraq GPSes operating in binary mode. * - * This file is Copyright (c) 2016 by the GPSD project + * This file is Copyright (c) 2016, 2018 by the GPSD project * SPDX-License-Identifier: BSD-2-clause + * + * Handles Skytraq Venus638 & Venus838 GNSS Modules */ #include @@ -13,24 +15,38 @@ #include #include #include +#include #include "gpsd.h" #include "bits.h" #include "strfuncs.h" +#include "driver_skytraq.h" #if defined(SKYTRAQ_ENABLE) -#define HI(n) ((n) >> 8) -#define LO(n) ((n) & 0xff) - /* * No ACK/NAK? Just rety after 6 seconds */ #define SKY_RETRY_TIME 6 #define SKY_CHANNELS 48 /* max channels allowed in format */ -static gps_mask_t sky_parse(struct gps_device_t *, unsigned char *, size_t); +bool sky_detect(struct gps_device_t *); /* called from driver_nmea when receiving a Skytraq-only sentence */ +static gps_mask_t sky_dispatch(struct gps_device_t *, unsigned char *, size_t); +static void sky_configure(struct gps_device_t *); +static void sky_deactivate(struct gps_device_t *); +static ssize_t sky_control_send(struct gps_device_t *, char *, size_t); +static bool sky_write(struct gps_device_t *, unsigned char *); +static bool sky_speed(struct gps_device_t *session, speed_t speed, char parity, int stopbits); +static void sky_mode(struct gps_device_t *session, int mode); +static bool sky_rate(struct gps_device_t *session, double cycletime); +static void sky_init_query(struct gps_device_t *); + +static gps_mask_t sky_parse_input(struct gps_device_t *); + +static gps_mask_t sky_resp_sw_ver(struct gps_device_t *, unsigned char *, size_t); +static gps_mask_t sky_msg_nav_data(struct gps_device_t *, unsigned char *, size_t); +static gps_mask_t sky_resp_gnss_time(struct gps_device_t *, unsigned char *, size_t); +static gps_mask_t sky_resp_gps_utc_time(struct gps_device_t *, unsigned char *, size_t); -static gps_mask_t sky_msg_80(struct gps_device_t *, unsigned char *, size_t); static gps_mask_t sky_msg_DC(struct gps_device_t *, unsigned char *, size_t); static gps_mask_t sky_msg_DD(struct gps_device_t *, unsigned char *, size_t); static gps_mask_t sky_msg_DE(struct gps_device_t *, unsigned char *, size_t); @@ -39,62 +55,425 @@ static gps_mask_t sky_msg_E0(struct gps_device_t *, unsigned char *, size_t); static gps_mask_t sky_msg_E2(struct gps_device_t *, unsigned char *, size_t); static gps_mask_t sky_msg_E3(struct gps_device_t *, unsigned char *, size_t); -#ifdef __UNUSED -/* Poll Software Version MID 2 */ -static unsigned char versionprobe[] = { - 0xa0, 0xa1, 0x00, 0x02, - 0x02, /* MID 2 */ - 0x01, /* System */ - 0x00, 0x0d, 0x0a -}; -// cppcheck-suppress unusedFunction +#define SKY_DETECTED(ses) (0==strncmp(session->subtype, "Skytraq", 7)) +#define SKY_SID_CMD(mid) ((mid>(SKY_MSGID_62-1)) && (mid<(SKY_MSGID_6A+1))) + +/* Return true if we've previously detected a Skytraq + * If not, probe for it and return false (subtype is set by response) */ +bool sky_detect(struct gps_device_t *session) +{ + if (SKY_DETECTED(session)) return true; + sky_init_query(session); + return false; /* We can't receive a response in time to answer here */ +} + +/* Ask for FW/OEM version of the module, without modifying state/settings */ +static void sky_init_query(struct gps_device_t *session) +{ + unsigned char msg[] = { + SKY_START_1, SKY_START_2, + 0x00, 0x02, + SKY_QUERY_SW_VER, 0x01, /* Query system SW version */ + 0x00, SKY_END_1, SKY_END_2 + }; + gpsd_log(&session->context->errout, LOG_PROG, "sky_init_query() l=%d\n", + session->lexer.counter); + if (SKY_DETECTED(session)) return; + // nmea_send(session, "$Skytraq"); + (void)sky_write(session, msg); +} + +/* + * sky_write(): send binary message to Skytraq GNSS + * The header (A0,A1), checksum, and trailer (0D,0A) is filled in by function. + */ static bool sky_write(struct gps_device_t *session, unsigned char *msg) { - unsigned int crc; + unsigned int csum; size_t i, len; - bool ok; - unsigned int type = (unsigned int)msg[4]; - + ssize_t sent; + char hex[80]; + unsigned char mid = msg[4]; + unsigned char sid = msg[5]; len = (size_t) ((msg[2] << 8) | msg[3]); + if (0 == len) { + gpsd_log(&session->context->errout, LOG_PROG, + "Skytraq: sky_write() Packet length zero\n"); + return false; + } + + /* Fill in header/trailer values */ + msg[0] = SKY_START_1; + msg[1] = SKY_START_2; + msg[len+5] = SKY_END_1; + msg[len+6] = SKY_END_2; /* calculate Checksum */ - crc = 0; + csum = 0; /* coverity_submit[tainted_data] */ for (i = 0; i < len; i++) - crc ^= (int)msg[4 + i]; + csum ^= (int)msg[4 + i]; + + /* enter CSUM after payload */ + msg[len + 4] = (unsigned char)(csum & 0x00ff); + + len += 7; /* length incl header/csum/trailer needed from now on */ + if (true == SKY_SID_CMD(mid)) { + gpsd_log(&session->context->errout, LOG_RAW, + "=> GPS: Skytraq sending MID/SID=0x%02x/0x%02x: %s\n", + mid, sid, gpsd_packetdump(hex, sizeof(hex)-1, (char *)msg, len)); + } else { + gpsd_log(&session->context->errout, LOG_RAW, + "=> GPS: Skytraq sending MID=0x%02x: %s\n", mid, + gpsd_packetdump(hex, sizeof(hex)-1, (char *)msg, len)); + } + + /* Copy to public assembly buffer for gpsmon snopping */ + session->msgbuflen = len; + (void)memcpy(session->msgbuf, msg, len); + sent = gpsd_write(session, (const char *)msg, len); + + return sent == (ssize_t) len; +} + +static bool sky_send_msgs(struct gps_device_t *session, struct sky_config_t *msgs) +{ + bool res = true; + int i=0; - /* enter CRC after payload */ - msg[len + 4] = (unsigned char)(crc & 0x00ff); + if (NULL==msgs) return true; + + while (msgs[i].len>0) { + if (NULL!= msgs[i].log) gpsd_log(&session->context->errout, LOG_PROG, msgs[i].log); + msgs[i].msg[2] = 0x0FF & (msgs[i].len >> 8); + msgs[i].msg[3] = 0x0FF & (msgs[i].len >> 0); + res &= sky_write(session, msgs[i].msg); + i++; + } + return res; +} + +#ifdef CONTROLSEND_ENABLE +/* not used by gpsd, it's for gpsctl and friends */ +static ssize_t sky_control_send(struct gps_device_t *session, char *msg, size_t len) +{ + char hex[80]; + gpsd_log(&session->context->errout, LOG_RAW, + "sky_control_send('%s', %u)\n", + gpsd_packetdump(hex, sizeof(hex)-1, (char *)msg, len), len); + return sky_write(session, (unsigned char *)msg); +} +#endif /* CONTROLSEND_ENABLE */ + +#ifdef TIMEHINT_ENABLE +static double sky_time_offset(struct gps_device_t *session UNUSED) +{ + /* Measured 1PPS to start/end of UART message and cgps reported Time Offset + * on a S1216F8 (all times in msec), with a 1Hz update rate in NMEA mode: + * Baud | first | last | Time + * 9600 78.50 153.46 180 + * 19200 84.90 122.38 130 + * 38400 99.60 118.40 100 + * 57600 63.26 75.82 100 + * 115200 100.59 107.17 100 + * 230400 88.39 91.80 100 + * 460800 97.36 99.26 100 + * 921600 94.50 95.70 90 + */ + switch(session->gpsdata.dev.baudrate) { + case 9600: + return 0.18; + case 19200: + return 0.13; + } + return 0.10; +} +#endif /* TIMEHINT_ENABLE */ + +/* Main event handler */ +static void sky_event_hook(struct gps_device_t *session, event_t event) +{ + unsigned *counter = &session->lexer.counter; + if (session->context->readonly) + return; + + if (event == event_wakeup) { + /* No wakeup action required */ + gpsd_log(&session->context->errout, LOG_PROG, "sky_event_hook(event_wakeup)\n"); + return; + } + + if (event == event_triggermatch) { + /* If NMEA sees the .trigger string */ + gpsd_log(&session->context->errout, LOG_PROG, "sky_event_hook(event_triggermatch)\n"); + return; + } + + if (event == event_identified) { + /* First full packet from Skytraq GNSS receiver positively detected + * session.lexer.counter = 0. + * If (device_default_cycle_time != 1 sec), set session->device->gpsdata.cycle here. + * If possible, get the software version and store it in session->subtype. + */ + gpsd_log(&session->context->errout, LOG_PROG, + "sky_event_hook(event_identified) lexer=%d\n", (int) counter); + sky_init_query(session); + sky_configure(session); + return; + } + if (event == event_configure) { + /* Configure device */ + gpsd_log(&session->context->errout, LOG_PROG, "sky_event_hook(event_configure) l=%d\n", + (int) counter); + return; + } + + if (event == event_driver_switch) { + /* Auto-baud event */ + *counter = 0; + gpsd_log(&session->context->errout, LOG_PROG, "sky_event_hook(event_driver_switch)\n"); + return; + } + + if (event == event_deactivate) { + gpsd_log(&session->context->errout, LOG_PROG, "sky_event_hook(event_deactivate)\n"); + sky_deactivate(session); + return; + } + + if (event == event_reactivate) { + /* Driver reactivation/reset */ + gpsd_log(&session->context->errout, LOG_PROG, "sky_event_hook(event_reactivate)\n"); + sky_speed(session, 115200, 'N', 8); + sky_mode(session, MODE_NMEA); + return; + } +} + +static void sky_configure(struct gps_device_t *session) +{ + struct sky_config_t msgs[] = { + { 2, {0, 0, 0, 0, SKY_MSGID_64, SKY_64_QUERY_BOOT_STATUS, 0, 0, 0}, + "SKY_64_QUERY_BOOT_STATUS"}, + { 2, {0, 0, 0, 0, SKY_MSGID_64, SKY_64_QUERY_GPS_UTC_TIME, 0, 0, 0}, + "SKY_64_QUERY_GPS_UTC_TIME"}, + { 2, {0, 0, 0, 0, SKY_MSGID_64, SKY_64_QUERY_GNSS_TIME, 0, 0, 0}, + "SKY_64_QUERY_GNSS_TIME"}, +#if RECONFIGURE_ENABLE + { 3, {0, 0, 0, 0, SKY_CONFIG_POWER_MODE, 0x01, 0x00, 0, 0, 0}, + "SKY_CONFIG_POWER_MODE=1,0"}, + { 9, {0, 0, 0, 0, SKY_MSGID_62, SKY_62_CONFIG_SBAS, 1, 2, 8, 1, 1, 7, 0, 0, 0, 0}, + "SKY_62_CONFIG_SBAS=1,2,8,1,1,7,0"}, + { 5, {0, 0, 0, 0, SKY_MSGID_62, SKY_62_CONFIG_QZSS, 1, 1, 0, 0, 0, 0}, + "SKY_62_CONFIG_QZSS=1,1,0"}, + { 4, {0, 0, 0, 0, SKY_MSGID_63, SKY_63_CONFIG_SAEE, 1, 0, 0, 0, 0}, + "SKY_63_CONFIG_SAEE=1,0"}, + /* CONFIG_NMEA order: GGA GSA GSV GLL RMC VTG ZDA */ + { 9, {0, 0, 0, 0, SKY_CONFIG_NMEA, SKY_NMEA_NORM, SKY_NMEA_SLOW, + SKY_NMEA_SLOW, SKY_NMEA_SLOW, SKY_NMEA_NORM, 0, SKY_NMEA_SLOW, 0, 0, 0, 0}, + "SKY_CONFIG_NMEA=5,5,5,5,1,0,5,0"}, + /* CONFIG_EXT_NMEA_INTVL order: GGA GSA GSV GLL RMC VTG ZDA GNS GBS GRS DTM GST */ + {15, {0, 0, 0, 0, SKY_MSGID_64, SKY_64_CONFIG_EXT_NMEA_INTVL, + SKY_NMEA_NORM, SKY_NMEA_SLOW, SKY_NMEA_SLOW, 0, SKY_NMEA_NORM, + 0, SKY_NMEA_SLOW, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + "SKY_64_CONFIG_EXT_NMEA_INTVL=5,5,5,0,1,0,5,0,0,0,0,0,0"}, + { 3, {0, 0, 0, 0, SKY_MSGID_64, SKY_64_CONFIG_PSTI_INTVL, 0, 0, 0, 0}, + "SKY_64_CONFIG_PSTI_INTVL=0"}, + { 4, {0, 0, 0, 0, SKY_MSGID_64, SKY_64_CONFIG_INTERFER_DET, 1, 0, 0, 0, 0}, + "SKY_64_CONFIG_INTERFER_DET=1,0"}, + { 4, {0, 0, 0, 0, SKY_MSGID_64, SKY_64_CONFIG_NAV_MODE, 0, 0, 0, 0, 0}, + "SKY_64_CONFIG_NAV_MODE=automatic,0"}, +#endif /* RECONFIGURE_ENABLE */ + { 0, {0, }, NULL}, /* End marker */ + }; + gpsd_log(&session->context->errout, LOG_PROG, "Skytraq: sky_configure()\n"); + sky_send_msgs(session, msgs); +#if 0 + int i = 0; + + while (msgs[i].len>0) { + gpsd_log(&session->context->errout, LOG_PROG, "Skytraq: sky_configure() l=%d i=%d %s\n", + counter, i, msgs[i].log); + msgs[i].msg[2] = 0x0FF & (msgs[i].len >> 8); + msgs[i].msg[3] = 0x0FF & (msgs[i].len >> 0); + sky_write(session, msgs[i].msg); + i++; + } +#endif +#if RECONFIGURE_ENABLE + sky_mode(session, session->mode == O_OPTIMIZE ? MODE_BINARY : MODE_NMEA); +#endif /* RECONFIGURE_ENABLE */ + + return; +} + +static void sky_deactivate(struct gps_device_t *session) +{ + unsigned char msg[40]; + time_t now; + struct tm *tm; + unsigned year; + + gpsd_log(&session->context->errout, LOG_PROG, "Skytraq deactivate() called\n"); + + sky_speed(session, 115200, 'N', 8); + sky_mode(session, MODE_NMEA); + /* SKY_RESTART message */ + msg[ 2] = 0; + msg[ 3] = 9; + msg[ 4] = SKY_RESTART; + msg[ 5] = 2; /* 1=Hot start, 2=Warm start, 3=Cold start, Others=reserved */ + now = time((time_t *)NULL); + tm = gmtime(&now); + year = tm->tm_year + 1900; + msg[ 6] = 0x00FF & (year >> 8); + msg[ 7] = 0x00FF & (year >> 0); + msg[ 8] = (unsigned char) (tm->tm_mon + 1); /* tm_mon is 0..11, Skytraq wants 1..12 */ + msg[ 9] = 0x00FF & tm->tm_mday; + msg[10] = tm->tm_hour; + msg[11] = tm->tm_min; + msg[12] = tm->tm_sec; + if (session->gpsdata.fix.mode >= MODE_2D) { + int lat, lon; + lat = (int) 100 * session->gpsdata.fix.latitude; + lon = (int) 100 * session->gpsdata.fix.longitude; + msg[13] = 0x00ff & (lat >> 8); + msg[14] = 0x00ff & (lat >> 0); + msg[15] = 0x00ff & (lon >> 8); + msg[16] = 0x00ff & (lon >> 0); + } + if (session->gpsdata.fix.mode == MODE_3D) { + unsigned int alt = (unsigned int) session->gpsdata.fix.altitude; + msg[17] = 0x00ff & (alt >> 8); + msg[18] = 0x00ff & (alt >> 0); + } + (void)sky_write(session, msg); +} + +static void sky_mode(struct gps_device_t *session, int mode) +{ + unsigned char msg[] = { + SKY_START_1, SKY_START_2, 0x00, 0x03, + SKY_CONFIG_MSG_TYPE, 0x00, 0x00, + 0x00, SKY_END_1, SKY_END_2}; + if (mode == MODE_NMEA) msg[5] = 0x01; + if (mode == MODE_BINARY) msg[5] = 0x02; + session->back_to_nmea = (mode == MODE_NMEA); gpsd_log(&session->context->errout, LOG_PROG, - "Skytraq: Writing control type %02x:\n", type); - ok = (gpsd_write(session, (const char *)msg, len+7) == (ssize_t) (len+7)); + "Skytraq: setting MODE=%s\n", (mode==MODE_BINARY) ? "Binary" : "NMEA"); + (void)sky_write(session, msg); + return; +} - return (ok); +/* Note: + * Can only change the baud rate; rest is locked at 8N1 + */ +static bool sky_speed(struct gps_device_t *session, + speed_t speed, char parity, int stopbits) +{ + unsigned char msg[] = { + SKY_START_1, SKY_START_2, + 0x00, 0x04, + SKY_CONFIG_SERIAL, 0x00, 0x00, 0x00, + 0x00, SKY_END_1, SKY_END_2}; + unsigned spd = 5; /* default to 115200 baud */ +#define N_BAUD 9 + unsigned baud[N_BAUD] = {4800, 9600, 19200, 38400, 57600, 115200, 230400, 460800, 921600}; + int i; + + gpsd_log(&session->context->errout, LOG_PROG, + "Skytraq: sky_speed(%d, %d%c%d) called\n",speed, 8, parity, stopbits); + /* Bail out on anything but 8N1 format */ + if (parity != 'N' || stopbits != 1) + return false; + + /* Check for accepted baudrates */ + for(i=0;icontext->errout, LOG_INF, + "Skytraq: unknown baud rate (%d), defaulting to %d\n", + speed, baud[spd]); + return false; + } + msg[6] = spd; + return sky_write(session,msg); +} + +static bool sky_rate(struct gps_device_t *session, double cycletime) +/* change the sample rate of the GPS */ +{ + struct sky_config_t msgs[] = { + { 3, {0, 0, 0, 0, SKY_CONFIG_POS_UPD_RATE, 0, 0, 0, 0, 0}, NULL}, + { 9, {0, 0, 0, 0, SKY_CONFIG_NMEA, SKY_NMEA_SLOW, SKY_NMEA_SLOW, + SKY_NMEA_SLOW, SKY_NMEA_SLOW, SKY_NMEA_NORM, 0, + SKY_NMEA_SLOW, 0, 0, 0, 0}, NULL}, + { 0, {0, }, NULL }, + }; +#define ACC 0.05 +#define SKY_RATES 9 + unsigned rates[SKY_RATES] = {1, 2, 4, 5, 8, 10, 20, 40, 50}; + unsigned rate = (unsigned) (100/cycletime); + int i; + + /* gpsd like to have a cycletime rather than an update rate + * so flip it and ensure we dont get rounding errors */ + gpsd_log(&session->context->errout, LOG_PROG, + "Skytraq: sky_rate() called with cycletime = %.2f\n", cycletime); + if (cycletime > 255.0) { + gpsd_log(&session->context->errout, LOG_INF, + "Skytraq: Can't set a cycletime above 255sec, ignoring.\n"); + return false; + } + if (cycletime > 1.0) { + /* GNSS update rate can't be set below 1Hz, but setting EXIT_NMEA_INTVL can fake it */ + msgs[0].msg[ 5] = rate = 1; + msgs[1].msg[10] = (unsigned char) cycletime; + } else { + for(i=0;i=(rates[i]*100*(1-ACC)) && + rate<=(rates[i]*100*(1+ACC))) { + msgs[0].msg[5] = rates[i]; + break; + } + } + } + if(0 == msgs[0].msg[5]) { + /* We didn't find a matching rate: complain and don't try to update */ + gpsd_log(&session->context->errout, LOG_INF, + "Skytraq: cycletime doesn't match a valid rate (%f %d)\n", + cycletime, rate); + return false; + } + gpsd_log(&session->context->errout, LOG_PROG, + "Skytraq: Setting update rate to %d Hz\n", rate/100); + return sky_send_msgs(session, msgs); } -#endif /* __UNUSED */ + /* * decode MID 0x80, Software Version * - * 10 bytes + * 14 bytes */ -static gps_mask_t sky_msg_80(struct gps_device_t *session, +static gps_mask_t sky_resp_sw_ver(struct gps_device_t *session, unsigned char *buf, size_t len) { - unsigned int kver_x; /* kernel version */ - unsigned int kver_y; /* kernel version */ - unsigned int kver_z; /* kernel version */ - unsigned int over_x; /* ODM version */ - unsigned int over_y; /* ODM version */ - unsigned int over_z; /* ODM version */ - unsigned int rev_yy; /* revision */ - unsigned int rev_mm; /* revision */ - unsigned int rev_dd; /* revision */ - - if ( 14 != len) + unsigned int kver_x, kver_y, kver_z; /* kernel version x.y.z */ + unsigned int over_x, over_y, over_z; /* ODM version x.y.z */ + unsigned int rev_yy, rev_mm, rev_dd; /* revision yy.mm.dd */ + + if (14 != len) { + gpsd_log(&session->context->errout, LOG_INF, + "Skytraq: SKY_RESP_SW_VER returned illegal length %d\n", len); return 0; + } kver_x = getbeu16(buf, 2); kver_y = getub(buf, 4); @@ -107,19 +486,205 @@ static gps_mask_t sky_msg_80(struct gps_device_t *session, rev_dd = getub(buf, 13); (void)snprintf(session->subtype, sizeof(session->subtype) - 1, - "kver=%3u.%2u,%2u, over=%3u.%2u,%2u, rev=%02u.%02u.%02u", + "Skytraq: kver=%u.%u.%u, over=%u.%u.%u, rev=%02u.%02u.%02u", kver_x, kver_y, kver_z, over_x, over_y, over_z, rev_yy, rev_mm, rev_dd); gpsd_log(&session->context->errout, LOG_DATA, - "Skytraq: MID 0x80: kver=%u.%u,%u, over=%u.%u,%u, rev=%u.%u.%u\n", + "Skytraq: Software version: kver=%u.%u.%u, over=%u.%u.%u, rev=%02u.%02u.%02u\n", kver_x, kver_y, kver_z, over_x, over_y, over_z, rev_yy, rev_mm, rev_dd); + return ONLINE_SET; +} + + +static gps_mask_t sky_resp_boot_status(struct gps_device_t *session, + unsigned char *buf, size_t len UNUSED) +{ + int status; + int flash; + status = getub(buf, 2); + flash = getub(buf, 3); + gpsd_log(&session->context->errout, LOG_DATA, + "Skytraq: booted from %s. Flash type 0x%02x\n", + (status==0) ? "FLASH" : "ROM" , flash); + return ONLINE_SET; +} + + +/* + * Binary Navigation Data Message (0xA8) + * Returns 59 bytes; + * Fix Mode, SV, Week, TOW, Latitude, Longitude, ellipsoid altitude, + * mean sea level altitude, gdop, pdop, hdop, vdop, tdop, + * ecef.{x,y,z}, ecef_v.{x,y,z} + */ +static gps_mask_t sky_msg_nav_data(struct gps_device_t *session, + unsigned char *buf, size_t len) +{ + unsigned char navmode; /* Navigation fix mode (0: No fix, 1: 2D, 2: 3D, 3: 3D+DGNSS */ + unsigned short week; /* GNSS week number */ + double ftow; /* Time of week */ + int *mode = &session->newdata.mode; + int *status = &session->gpsdata.status; + gps_mask_t mask = 0; + + if (59 != len) { + gpsd_log(&session->context->errout, LOG_INF, "Skytraq: " + "Navigation Data Message has incorrect length %d\n", len); + return 0; + } + + navmode = getub(buf, 1); + switch (navmode) { + case SKY_MODE_2D: /* 2D fix */ + if (*mode != MODE_2D) { + *mode = MODE_2D; + mask |= MODE_SET; + } + if (*status != STATUS_FIX) { + *status = STATUS_FIX; + mask |= STATUS_SET; + } + break; + case SKY_MODE_3D: + case SKY_MODE_DGPS: /* 3D fix */ + if (*mode != MODE_3D) { + *mode = MODE_3D; + mask |= MODE_SET; + } + if (*status != STATUS_FIX) { + *status = STATUS_FIX; + mask |= STATUS_SET; + } + break; + default: /* Includes SKY_MODE_NONE */ + if (*mode != MODE_NO_FIX) { + *mode = MODE_NO_FIX; + mask |= MODE_SET; + } + if (*status != STATUS_NO_FIX) { + *status = STATUS_NO_FIX; + mask |= STATUS_SET; + } + break; + } + session->gpsdata.satellites_used = getub(buf, 2); + mask |= ONLINE_SET | SATELLITE_SET; + week = getbeu16(buf, 3); + ftow = getbeu32(buf, 5)/100.0; + session->newdata.time = gpsd_gpstime_resolve(session, week, ftow); + mask |= TIME_SET | NTPTIME_IS; + session->newdata.ecef.x = getbes32(buf, 35) / 100.0; + session->newdata.ecef.y = getbes32(buf, 39) / 100.0; + session->newdata.ecef.z = getbes32(buf, 43) / 100.0; + session->newdata.ecef.vx = getbes32(buf, 47) / 100.0; + session->newdata.ecef.vy = getbes32(buf, 51) / 100.0; + session->newdata.ecef.vz = getbes32(buf, 55) / 100.0; + mask |= ECEF_SET | VECEF_SET; + /* Skytraq do return lat/long, but not speeds/climb rates, so use ecef_to_wgs84fix() anyway*/ + session->newdata.latitude = 1e-7 * getbes32(buf, 9); + session->newdata.longitude = 1e-7 * getbes32(buf, 13); + session->newdata.altitude = 1e-2 * getbes32(buf, 21); + mask |= LATLON_SET | ALTITUDE_SET; + ecef_to_wgs84fix(&session->newdata, &session->gpsdata.separation, + session->newdata.ecef.x, session->newdata.ecef.y, session->newdata.ecef.z, + session->newdata.ecef.vx, session->newdata.ecef.vy, session->newdata.ecef.vz); + mask |= SPEED_SET | TRACK_SET | CLIMB_SET; + session->gpsdata.dop.gdop = getbeu16(buf, 25); + session->gpsdata.dop.pdop = getbeu16(buf, 27); + session->gpsdata.dop.hdop = getbeu16(buf, 29); + session->gpsdata.dop.vdop = getbeu16(buf, 31); + session->gpsdata.dop.tdop = getbeu16(buf, 33); + mask |= DOP_SET | CLEAR_IS | REPORT_IS; + gpsd_log(&session->context->errout, LOG_DATA, + "Skytraq: NAVDATA time=%.3f, lat=%.6f lon=%.6f alt=%.2f mode=%d status=%d\n", + session->newdata.time, + session->newdata.latitude, + session->newdata.longitude, + session->newdata.altitude, + session->newdata.mode, + session->gpsdata.status); + return mask; +} + + +/* + * GNSS Time response (0x64,0x8E) + */ +static gps_mask_t sky_resp_gnss_time(struct gps_device_t *session, + unsigned char *buf, size_t len) +{ + unsigned tow; + unsigned nsec; + unsigned week; + char dleap; /* default GPS/UTC leap seconds (compiled in?) */ + char leap; /* current GPS/UTC leap seconds */ + unsigned char mask; +#define TOW_VALID (1<<0) +#define WEEK_VALID (1<<1) +#define LEAP_VALID (1<<2) + + if (15 != len) { + gpsd_log(&session->context->errout, LOG_INF, + "Skytraq: SKY_RESP_GNSS_TIME returned illegal length %d\n", len); + return 0; + } + + tow = getbeu32(buf, 2); + nsec = getbeu32(buf, 6); + week = getbeu16(buf, 10); + dleap = getsb(buf, 14); + leap = getsb(buf, 15); + mask = getub(buf, 16); + if (0 != (mask & TOW_VALID)) { + tow = 0; + } + if (0 != (mask & WEEK_VALID)) { + week = 0; + } + if (0 != (mask & LEAP_VALID)) { + leap = 0; + } + gpsd_log(&session->context->errout, LOG_DATA, + "Skytraq: received GNSS time: %.4f%06d week=%d leap=%i default=%i\n", + tow/1000.0, nsec, week, leap, dleap); return 0; } +static gps_mask_t sky_resp_gps_utc_time(struct gps_device_t *session, + unsigned char *buf, size_t len) +{ + unsigned inval, year, month, day; + + if (7 != len) { + gpsd_log(&session->context->errout, LOG_INF, + "Skytraq: SKY_RESP_GPS_UTC_REF_TIME returned illegal length %d\n", len); + return 0; + } + inval = getub(buf, 2); + year = getbeu16(buf, 3); + month = getub(buf, 5); + day = getub(buf, 6); + if (!inval && year && month && day) { + /* Only update if we got good data */ + gpsd_century_update(session, year - (year % 100)); + gpsd_log(&session->context->errout, LOG_DATA, + "Skytraq: received GPS UTC date: %04d-%02d-%02d\n", + year, month, day); + } + return ONLINE_SET; +} + + +/* + * The following messages (0xDC-0xE5) are emitted by the modules + * supporting the Binary Messages Raw Measurements Data Extension in + * AN0030 + */ + /* * decode MID 0xDC, Measurement Time * @@ -445,66 +1010,137 @@ static gps_mask_t sky_msg_E3(struct gps_device_t *session, } -static gps_mask_t sky_parse(struct gps_device_t * session, unsigned char *buf, - size_t len) +static gps_mask_t sky_dispatch(struct gps_device_t * session, unsigned char *buf, + size_t len) { + char hex[80]; gps_mask_t mask = 0; + unsigned int csum = 0; + int pl, i; + unsigned char mid, sid; if (len == 0) return mask; + session->cycle_end_reliable = true; + + /* check the checksum */ + pl = getbeu16(buf, 2); + for(i=0; icontext->errout, LOG_PROG, + "Skytraq: invalid csum in MID 0x%02x (expected %02x, got %02x)\n", + buf[4],buf[4+i],csum); + return 0; + } + if (buf[4]!=SKY_MSG_NAV_DATA_MSG) + gpsd_log(&session->context->errout, LOG_PROG, "<= Skytraq: Received %s\n", + gpsd_packetdump(hex,sizeof(hex)-1,(char *)buf, len)); + buf += 4; /* skip the leaders and length */ len -= 7; /* don't count the leaders, length, csum and terminators */ - // session->driver.sirf.lastid = buf[0]; - /* check the checksum?? */ + mid = buf[0]; + sid = buf[1]; + if (false == SKY_SID_CMD(mid)) { + gpsd_log(&session->context->errout, LOG_RAW, + "<= Skytraq Mid=%02x len=%3d %s\n", + mid, (buf[-2]<<8)|buf[-1], + gpsd_packetdump(hex,sizeof(hex)-1,(char *)buf, len)); + } else { + gpsd_log(&session->context->errout, LOG_RAW, + "<= Skytraq Mid/Sid=%02x/0x%02x len=%3d %s\n", + mid, sid, (buf[-2]<<8)|buf[-1], + gpsd_packetdump(hex,sizeof(hex)-1,(char *)buf, len)); + } - /* could change if the set of messages we enable does */ - /* session->cycle_end_reliable = true; */ + switch (mid) { + case SKY_RESP_SW_VER: + return sky_resp_sw_ver(session, buf, len); + + case SKY_RESP_ACK: + if (!SKY_SID_CMD(buf[1])) { + gpsd_log(&session->context->errout, LOG_PROG, + "<= Skytraq: ACK to MID=0x%02x\n", buf[1]); + } else { + gpsd_log(&session->context->errout, LOG_PROG, + "<= Skytraq: ACK to MID/SID=0x%02x/0x%02x\n", buf[1], buf[2]); + } + return mask; - switch (buf[0]) { - case 0x80: - /* 128 */ - return sky_msg_80(session, buf, len); + case SKY_RESP_NACK: + if (!SKY_SID_CMD(buf[1])) { + gpsd_log(&session->context->errout, LOG_PROG, + "<= Skytraq: NACK to MID=0x%02x\n", buf[1]); + } else { + gpsd_log(&session->context->errout, LOG_PROG, + "<= Skytraq: NACK to MID/SID=0x%02x/0x%02x\n", buf[1], buf[2]); + } + return mask; - case 0x83: - /* 131 - ACK */ - gpsd_log(&session->context->errout, LOG_PROG, - "Skytraq: ACK to MID %#02x\n", buf[1]); - break; - case 0x84: - /* 132 - NACK */ - gpsd_log(&session->context->errout, LOG_INF, - "Skytraq: NACK to MID %#02x\n", buf[1]); - break; - case 0xDC: - /* 220 */ + case SKY_MSG_NAV_DATA_MSG: + return sky_msg_nav_data(session, buf, len); + + case SKY_MSGID_62: + /* FALL-THROUGH */ + case SKY_MSGID_63: + /* FALL-THROUGH */ + case SKY_MSGID_65: + /* FALL-THROUGH */ + case SKY_MSGID_67: + /* FALL-THROUGH */ + case SKY_MSGID_6A: + gpsd_log(&session->context->errout, LOG_INF, + "Skytraq: Not handling MID/SID=0x%02x/0x%02x yet\n",mid,sid); + return mask; + + case SKY_MSGID_64: + switch(sid) { + case SKY_64_RESP_BOOT_STATUS: + return sky_resp_boot_status(session, buf, len); + case SKY_64_RESP_GPS_UTC_REF_TIME: + return sky_resp_gps_utc_time(session, buf, len); + case SKY_64_RESP_GNSS_TIME: + return sky_resp_gnss_time(session, buf, len); + default: + gpsd_log(&session->context->errout, LOG_INF, + "Skytraq: Unable to handle MID/SID=0x%02x/0x%02x\n",mid,sid); + return mask; + } + + case SKY_MSG_MEAS_TIME: return sky_msg_DC(session, buf, len); - case 0xDD: - /* 221 */ + case SKY_MSG_MEAS_RAW: return sky_msg_DD(session, buf, len); - case 0xDE: - /* 222 */ + case SKY_MSG_SV_CH_STATUS: return sky_msg_DE(session, buf, len); - case 0xDF: - /* 223 - Nave status (PVT) */ + case SKY_MSG_NAV_PVT: return sky_msg_DF(session, buf, len); - case 0xE0: - /* 224 */ + case SKY_MSG_GPS_SUBFRAME: return sky_msg_E0(session, buf, len); - case 0xE2: + case SKY_MSG_BD_SUBFRAME_D1: /* 226 - Beidou2 D1 Subframe data */ return sky_msg_E2(session, buf, len); - case 0xE3: - /* 227 - Beidou2 D2 Subframe data */ + case SKY_MSG_BD_SUBFRAME_D2: return sky_msg_E3(session, buf, len); + case SKY_RESP_UNKNOWN_85: + /* Undocumented RTK Module response also seen from S1216F8: + * <00 05> <85> 6a 06 00 05 <0d 0a> + */ + gpsd_log(&session->context->errout, LOG_INF, + "<=Skytraq: RESP 0x85: len=%zd %s\n", len, + gpsd_packetdump(hex, sizeof(hex)-1, (char *)buf, len)); + return mask; + default: gpsd_log(&session->context->errout, LOG_PROG, "Skytraq: Unknown packet id %#02x length %zd\n", @@ -513,35 +1149,43 @@ static gps_mask_t sky_parse(struct gps_device_t * session, unsigned char *buf, return mask; } -static gps_mask_t skybin_parse_input(struct gps_device_t *session) +static gps_mask_t sky_parse_input(struct gps_device_t *session) { if (session->lexer.type == SKY_PACKET) { - return sky_parse(session, session->lexer.outbuffer, - session->lexer.outbuflen); -#ifdef NMEA0183_ENABLE - } else if (session->lexer.type == NMEA_PACKET) { - return nmea_parse((char *)session->lexer.outbuffer, session); -#endif /* NMEA0183_ENABLE */ - } else - return 0; + return sky_dispatch(session, session->lexer.outbuffer, + session->lexer.outbuflen); + } else { + /* Also handles Skytraq-proprietary messages ala $PSTI */ + return generic_parse_input(session); + } } /* this is everything we export */ /* *INDENT-OFF* */ const struct gps_type_t driver_skytraq = { - .type_name = "Skytraq", /* full name of type */ + .type_name = "Skytraq", /* full name of type */ .packet_type = SKY_PACKET, /* associated lexer packet type */ .flags = DRIVER_STICKY, /* remember this */ - .trigger = NULL, /* no trigger */ - .channels = SKY_CHANNELS, /* consumer-grade GPS */ - .probe_detect = NULL, /* no probe */ + .trigger = "$Skytraq", /* Emitted on power-on and warm/cold reset */ + .channels = SKY_CHANNELS, /* Survey-grade GPS */ + .probe_detect = sky_detect, /* Wrapper around sky_init_query */ .get_packet = generic_get, /* be prepared for Skytraq or NMEA */ - .parse_packet = skybin_parse_input,/* parse message packets */ + .parse_packet = sky_parse_input, /* parse message packets */ .rtcm_writer = gpsd_write, /* send RTCM data straight */ - .init_query = NULL, /* non-perturbing initial qury */ - .event_hook = NULL, /* lifetime event handler */ + .init_query = sky_init_query, /* non-perturbing initial qury */ + .event_hook = sky_event_hook, /* lifetime event handler */ +#ifdef RECONFIGURE_ENABLE + .speed_switcher = sky_speed, /* Speed (baudrate) switch */ + .mode_switcher = sky_mode, /* Mode switcher */ + .rate_switcher = sky_rate, /* Message delivery rate switcher */ +#endif /* RECONFIGURE_ENABLE */ +#ifdef CONTROLSEND_ENABLE + .control_send = sky_control_send,/* how to send a control string */ +#endif /* CONTROLSEND_ENABLE */ +#ifdef TIMEHINT_ENABLE + .time_offset = sky_time_offset, +#endif /* TIMEHINT_ENABLE */ }; /* *INDENT-ON* */ #endif /* defined( SKYTRAQ_ENABLE) && defined(BINARY_ENABLE) */ - diff --git a/driver_skytraq.h b/driver_skytraq.h new file mode 100644 index 00000000..f30788d7 --- /dev/null +++ b/driver_skytraq.h @@ -0,0 +1,386 @@ +/* + * This file is Copyright (c) 2018 by the GPSD project + * SPDX-License-Identifier: BSD-2-clause + */ +#ifndef _GPSD_SKY_H_ +#define _GPSD_SKY_H_ + +/* + * Binary commands accepted by the Skytraq Venus638 & Venus838 Modules + * References: + * Venus6: AN0003, v1.4.24, June 18, 2013 + * Venus8: https://navspark.mybigcommerce.com/content/AN0028_1.4.42.pdf + * S1216: http://www.skytraq.com.tw/datasheet/S1216V8_v0.10.pdf + * Venus8 Binary Messages Raw Measurements Data Extension: + * https://navspark.mybigcommerce.com/content/AN0030_1.4.33.pdf + * S2525F8-RTK_v5.pdf, June 24, 2016 + * S2525DC8_v0.2.pdf, November 28, 2014 + * + * Message Format: + *
+ * All fields are encoded in Big Endian. + * Header: <0xA0, 0xA1> + * PL: Payload length in bytes + * CS: Checksum is XOR of all bytes + * End of Sequence: "\n\r" <0x0D, 0x0A> + * Minimum packet length is 7 + payload. + * + * The below data is as per AN0003_v1.4.19, AN0028_1.4.42, & AN0030_v1.4.33 + * + * Legend: + * 6/8 : Accepted/used by Venus638/Venus838 firmware (R= Raw Meas, K=RTK, D=Disciplined Clock) + * MID : MessageID in first payload byte + * SID : Sub-Message ID (optional) in second payload byte + * Len : Payload length (not including header, checksum and End of Sequence) + * Dir : Direction (input/output). Outputs have bit 7 set on MID/SID (if present). + * Name: Name of request/response + * | 6 | 8 | MID | SID | Dir | Len | Name | + * |---|---|------|------|-----|-----|------------------------------------| + * |--- Input System Messages --------------------------------------------| + * | 6 | 8 | 0x01 | | In | 15 | System restart + * | 6 | 8 | 0x02 | | In | 2 | Query software version + * | 6 | 8 | 0x03 | | In | 2 | Query software CRC + * | 6 | 8 | 0x04 | | In | 2 | Reset to factory defaults + * | 6 | 8 | 0x05 | | In | 4 | Configure serial port + * | | | 0x06 | | In | | Reserved + * | | | 0x07 | | In | | Reserved + * | 6 | 8 | 0x08 | | In | 9 | Configure NMEA + * | 6 | 8 | 0x09 | | In | 3 | Configure message type + * | | 8 | 0x0B | | In | 6 | Software image download + * | 6 | 8 | 0x0C | | In | 3 | Configure power mode + * | 6 | 8 | 0x0E | | In | 3 | Configure position update rate + * | 6 | 8 | 0x10 | | In | 1 | Query position update rate + * | 6 | 8 | 0x11 | | In | 3 | Configure navigation data message interval + * | | 8 | 0x15 | | In | 1 | Query power mode + * | | R | 0x1E | | In | 9 | Configure Binary Measurement Data Output + * | | R | 0x1F | | In | 1 | Query Binary Measurement Data Output + * | | R | 0x20 | | In | 17 | Configure Binary RTCM Data Output + * | | R | 0x21 | | In | 1 | Query Binary RTCM Data Output + * |--- Input GNSS Messages ----------------------------------------------| + * | 6 | 8 | 0x29 | | In | 19 | Configure datum + * | 6 | 8 | 0x2A | | In | 9 | Configure DOP mask + * | | 8 | 0x2B | | In | 5 | Configure elevation and CNR mask + * | 6 | 8 | 0x2D | | In | 1 | Query datum + * | 6 | 8 | 0x2E | | In | 1 | Query DOP mask + * | | 8 | 0x2F | | In | 1 | Query elevation and CNR mask + * | 6 | 8 | 0x30 | | In | 2 | Get GPS ephemeris + * | 6 | | 0x31 | | In | 87 | Set GPS ephemeris (see 0x41 for Venus838 cmd) + * | 6 | | 0x37 | | In | 3 | Configure WAAS + * | 6 | | 0x38 | | In | 1 | Query WAAS status + * | 6 | 8 | 0x39 | | In | 2/3 | Configure position pinning (V6: 2B) + * | 6 | 8 | 0x3A | | In | 1 | Query position pinning + * | 6 | 8 | 0x3B | | In | 11 | Configure position pinning parameters + * | 6 | | 0x3C | | In | 3 | Configuration navigation mode + * | 6 | | 0x3D | | In | 1 | Query navigation mode + * | 6 | | 0x3E | | In | 3 | Configure 1PPS mode + * | 6 | | 0x3F | | In | 1 | Query 1PPS mode + * | | 8 | 0x41 | | In | 87 | Set GPS ephemeris (see 0x31 for Venus638 cmd) + * | | 8 | 0x44 | | In | 2 | Query 1PPS timing + * | | 8 | 0x45 | | In | 6 | Configure 1PPS cable delay + * | | 8 | 0x46 | | In | 1 | Query 1PPS cable delay + * | | 8 | 0x4B | | In | 3 | Configure NMEA TalkerID + * | | 8 | 0x4F | | In | 1 | Query NMEA TalkerID + * | | 8 | 0x50 | | In | 2 | Get GPS Almanac + * | | 8 | 0x51 | | In | 52 | Set GPS Almanac + * | | 8 | 0x54 | | In | 31 | Configure 1PPS timing + * | | 8 | 0x5B | | In | 2 | Get GLONASS Ephemeris + * | | 8 | 0x5C | | In | 43 | Set GLONASS Ephemeris + * | | 8 | 0x5D | | In | 2 | Get GLONASS Almanac + * | | 8 | 0x5E | | In | 26 | Set GLONASS Almanac + * | | 8 | 0x5F | | In | 2 | Get GLONASS Time Correction Parameters + * | | 8 | 0x60 | | In | 10 | Set GLONASS Time Correction Parameters + * |--- Messages with Sub-IDs --------------------------------------------| + * | | 8 | 0x62 | 0x01 | In | 9 | Configure SBAS + * | | 8 | 0x62 | 0x02 | In | 2 | Query SBAS status + * | | 8 | 0x62 | 0x03 | In | 5 | Configure QZSS + * | | 8 | 0x62 | 0x04 | In | 2 | Query QZSS status + * | | 8 | 0x62 | 0x80 | Out | 8 | SBAS status + * | | 8 | 0x62 | 0x81 | Out | 4 | QZSS status + * |---|---|------|------|-----|-----|------------------------------------| + * | | 8 | 0x63 | 0x01 | In | 4 | Configure SAEE + * | | 8 | 0x63 | 0x02 | In | 2 | Query SAEE + * | | 8 | 0x63 | 0x80 | Out | 3 | SAEE status + * |---|---|------|------|-----|-----|------------------------------------| + * | | 8 | 0x64 | 0x01 | In | 2 | Query boot status + * | | 8 | 0x64 | 0x02 | In | 15 | Configure extended NMEA message interval + * | | 8 | 0x64 | 0x03 | In | 2 | Query extended NMEA message interval + * | | 8 | 0x64 | 0x06 | In | 4 | Configure interference detection + * | | 8 | 0x64 | 0x07 | In | 2 | Query interference detection status + * | | 8 | 0x64 | 0x0A | In | 4 | Configure GPS parameter search engine number + * | | 8 | 0x64 | 0x0B | In | 2 | Query GPS parameter search engine number + * | | 8 | 0x64 | 0x11 | In | 5 | Configure Position Fix Navigation Mask + * | | 8 | 0x64 | 0x12 | In | 2 | Query Position Fix Navigation Mask + * | | 8 | 0x64 | 0x15 | In | 8 | Configure UTC Reference Time Sync to GPS Time + * | | 8 | 0x64 | 0x16 | In | 2 | Query UTC Reference Time Sync to GPS Time + * | | 8 | 0x64 | 0x17 | In | 4 | Configure GNSS navigation mode + * | | 8 | 0x64 | 0x18 | In | 2 | Query GNSS navigation mode + * | | 8 | 0x64 | 0x19 | In | 5 | Configure GNSS constellation type for navigation solution + * | | 8 | 0x64 | 0x1A | In | 2 | Query GNSS constellation type for navigation solution + * | | 8 | 0x64 | 0x1F | In | 4 | Configure GPS/UTC leap seconds + * | | 8 | 0x64 | 0x20 | In | 2 | Query GPS time + * | | 8 | 0x64 | 0x21 | In | 5 | Configure PSTI Message Interval + * | | 8 | 0x64 | 0x22 | In | 3 | Query PTSI Message Interval + * | | 8 | 0x64 | 0x27 | In | 5 | Configure GNSS datum index + * | | 8 | 0x64 | 0x28 | In | 2 | Query GNSS datum index + * | | 8 | 0x64 | 0x2F | In | var | Configure GNSS Geo-Fencing Data PL=4+n*16 (n=1..10) + * | | 8 | 0x64 | 0x30 | In | 2 | Query GNSS Geo-Fencing Data + * | | 8 | 0x64 | 0x31 | In | 2 | Query GNSS Geo-Fencing Result + * | | 8 | 0x64 | 0x7D | In | 2 | Query Version Extension String + * | | 8 | 0x64 | 0x80 | Out | 4 | GNSS boot status + * | | 8 | 0x64 | 0x81 | Out | 14 | Extended NMEA message interval + * | | 8 | 0x64 | 0x83 | Out | 4 | Interference detection status + * | | 8 | 0x64 | 0x85 | Out | 3 | GPS parameter search engine number + * | | 8 | 0x64 | 0x88 | Out | 5 | Position Fix Navigation + * | | 8 | 0x64 | 0x8A | Out | 7 | GPS UTC Reference Time + * | | 8 | 0x64 | 0x8B | Out | 3 | GNSS navigation mode + * | | 8 | 0x64 | 0x8C | Out | 4 | GNSS constellation type for navigation solution + * | | 8 | 0x64 | 0x8E | Out | 15 | GPS time + * | | 8 | 0x64 | 0x8F | Out | 3 | PSTI Message Interval + * | | 8 | 0x64 | 0x92 | Out | 5 | GNSS datum index + * | | 8 | 0x64 | 0x96 | Out | var | GNSS Geo-Fencing Data PL=3+n*16 (n=1..10) + * | | 8 | 0x64 | 0x97 | Out | 19 | GNSS Geo-Fencing Result + * | | 8 | 0x64 | 0xFE | Out | 34 | Version Extension String + * |---|---|------|------|-----|-----|------------------------------------| + * | | 8 | 0x65 | 0x01 | In | 7 | Configure 1PPS pulse width + * | | 8 | 0x65 | 0x02 | In | 3 | Query 1PPS pulse width + * | | 8 | 0x65 | 0x03 | In | 7 | Configure 1PPS frequency output + * | | 8 | 0x65 | 0x04 | In | 2 | Query 1PPS frequency output + * | | 8 | 0x65 | 0x80 | Out | 6 | 1PPS pulse width + * | | 8 | 0x65 | 0x81 | Out | 6 | 1PPS frequency output + * |---|---|------|------|-----|-----|------------------------------------| + * | | 8 | 0x67 | 0x01 | In | var | Set Beidou Ephemeris Data PL=126/87 + * | | 8 | 0x67 | 0x02 | In | 3 | Get Beidou Ephemeris Data + * | | 8 | 0x67 | 0x03 | In | 53 | Set Beidou Almanac Data + * | | 8 | 0x67 | 0x04 | In | 3 | Get Beidou Almanac Data + * | | 8 | 0x67 | 0x80 | Out | var | Beidou Ephemeris Data PL=126/87 + * | | 8 | 0x67 | 0x81 | Out | 53 | Beidou Almanac Data + * |---|---|------|------|-----|-----|------------------------------------| + * | | 8 | 0x6A | 0x01 | In | 4 | Configure RTK Mode + * | | 8 | 0x6A | 0x02 | In | 3 | Query RTK Mode + * | | K | 0x6A | 0x04 | In | | Reset/re-calc GLONASS IFB (undocumented) + * | | 8 | 0x6A | 0x80 | Out | 3 | RTK Mode + * |--- Output System Messages -------------------------------------------| + * | 6 | 8 | 0x80 | | Out | 14 | Software version + * | 6 | 8 | 0x81 | | Out | 4 | Software CRC + * | | | 0x82 | | Out | | Reserved + * | 6 | 8 | 0x83 | | Out | 2 | ACK + * | 6 | 8 | 0x84 | | Out | 2/3 | NACK (3 bytes if MID/SID resquest) + * | | 8 | 0x85 | | Out | 5 | + * | 6 | 8 | 0x86 | | Out | 2 | Position update rate + * | | R | 0x89 | | Out | 8 | Binary Measurement Data Output Status + * | | R | 0x8A | | Out | 16 | Binary RTCM Data Output Status + * | | 8 | 0x90 | | Out | 43 | GLONASS Ephemeris Data + * | | 8 | 0x91 | | Out | 25 | GLONASS Almanac Data + * | | 8 | 0x92 | | Out | 9 | Beidou Time Correction Parameters + * | | 8 | 0x93 | | Out | 2 | GNSS NMEA TalkerID + * |--- Output GNSS Messages ---------------------------------------------| + * | 6 | 8 | 0xA8 | | Out | 59 | Navigation data message (periodic) + * | 6 | 8 | 0xAE | | Out | 3 | GNSS datum + * | 6 | 8 | 0xAF | | Out | 8 | GNSS DOP mask + * | | 8 | 0xB0 | | Out | 4 | Elevation and CNR mask + * | 6 | 8 | 0xB1 | | Out | 87 | GPS Ephemeris Data + * | 6 | | 0xB3 | | Out | 2 | GNSS WAAS status + * | 6 | 8 | 0xB4 | | Out | 12 | GNSS position pinning status + * | 6 | | 0xB5 | | Out | 2 | GPS navigation mode + * | 6 | | 0xB6 | | Out | 2 | GPS 1PPS mode + * | | 8 | 0xB9 | | Out | 2 | GNSS power mode + * | | 8 | 0xBB | | Out | 5 | GNSS 1PPS cable delay + * | | 8 | 0xBE | | Out | 52 | GPS Almanac Data + * | | 8 | 0xC2 | | Out | 35 | GNSS 1PPS timing (only flash-based receivers) + * | | R | 0xDC | | Out | 10 | MEAS_TIME (periodic) + * | | R | 0xDD | | Out | var | RAW_MEAS (periodic) PL=3+n*23 (n=1..16) + * | | R | 0xDE | | Out | var | SV_CH_STATUS (periodic) PL=3+n*10 (n=1..16) + * | | R | 0xDF | | Out | 81 | RCV_STATE (periodic) + * | | R | 0xE0 | | Out | 33 | GPS Subframe (periodic) + * | | R | 0xE1 | | Out | 12 | GLONASS String (periodic) + * | | R | 0xE2 | | Out | 31 | Beidou D1 Subframe (periodic) + * | | R | 0xE3 | | Out | 31 | Beidou D2 Subframe (periodic) + * | | R | 0xE5 | | Out | var | EXT_RAW_MEAS (periodic) PL=14+n*31 (n=1..17) + */ + +typedef enum { + /* Input System Messages */ + SKY_RESTART = 0x01, + SKY_QUERY_SW_VER = 0x02, + SKY_QUERY_SW_CRC = 0x03, + SKY_FACTORY_RESET = 0x04, + SKY_CONFIG_SERIAL = 0x05, + SKY_CONFIG_NMEA = 0x08, + SKY_CONFIG_MSG_TYPE = 0x09, + SKY_DOWNLOAD_SW_IMG = 0x0B, + SKY_CONFIG_POWER_MODE = 0x0C, + SKY_CONFIG_POS_UPD_RATE = 0x0E, + SKY_QUERY_POS_UPD_RATE = 0x10, + SKY_CONFIG_NAV_MSG_INTVL = 0x11, + SKY_QUERY_POWER_MODE = 0x15, + SKY_CONFIG_MEAS_DATA = 0x1E, + SKY_QUERY_MEAS_DATA = 0x1F, + SKY_CONFIG_RTCM_DATA = 0x20, + SKY_QUERY_RTCM_DATA = 0x21, + /* Input GNSS Messages */ + SKY_CONFIG_DATUM = 0x29, + SKY_CONFIG_DOP = 0x2A, + SKY_CONFIG_ELEV_CNR = 0x2B, + SKY_QUERY_DATUM = 0x2D, + SKY_QUERY_DOP = 0x2E, + SKY_QUERY_ELEV_CNR = 0x2F, + SKY_GET_EPHEMERIS = 0x30, + SKY_SET_EPHEMERIS_V6 = 0x31, + SKY_CONFIG_WAAS = 0x37, + SKY_QUERY_WAAS = 0x38, + SKY_CONFIG_POSITION_PIN = 0x39, + SKY_QUERY_POSITION_PIN = 0x3A, + SKY_CONFIG_POS_PIN_PARAM = 0x3B, + SKY_CONFIG_NAV_MODE = 0x3C, + SKY_QUERY_NAV_MODE = 0x3D, + SKY_CONFIG_PPS_MODE = 0x3E, + SKY_QUERY_PPS_MODE = 0x3F, + SKY_SET_EPHEMERIS_V8 = 0x41, + SKY_QUERY_PPS_TIMING = 0x44, + SKY_CONFIG_PPS_CABLE_DEL = 0x45, + SKY_QUERY_PPS_CABLE_DEL = 0x46, + SKY_CONFIG_NMEA_TALKER = 0x4B, + SKY_QUERY_NMEA_TALKER = 0x4F, + SKY_CONFIG_PPS_TIMING = 0x54, + SKY_GET_GLONASS_EPHEM = 0x5B, + SKY_SET_GLONASS_EPHEM = 0x5C, + /* Messages with Sub-IDs */ + SKY_MSGID_62 = 0x62, + SKY_MSGID_63 = 0x63, + SKY_MSGID_64 = 0x64, + SKY_MSGID_65 = 0x65, + SKY_MSGID_67 = 0x67, + SKY_MSGID_6A = 0x6A, + /* Output System Messages */ + SKY_RESP_SW_VER = 0x80, + SKY_RESP_SW_CRC = 0x81, + SKY_RESP_ACK = 0x83, + SKY_RESP_NACK = 0x84, + SKY_RESP_UNKNOWN_85 = 0x85, + SKY_RESP_POS_UPD_RATE = 0x86, + SKY_RESP_MEAS_DATA = 0x89, + SKY_RESP_RTCM_DATA = 0x8A, + SKY_RESP_GLONASS_EPHEM = 0x90, + SKY_RESP_NMEA_TALKER = 0x93, + /* Output GNSS Messages */ + SKY_MSG_NAV_DATA_MSG = 0xA8, + SKY_RESP_DATUM = 0xAE, + SKY_RESP_DOP = 0xAF, + SKY_RESP_ELEV_CNR = 0xB0, + SKY_RESP_GPS_EPHEM = 0xB1, + SKY_RESP_WAAS = 0xB3, + SKY_RESP_POS_PIN_STATUS = 0xB4, + SKY_RESP_NAV_MODE = 0xB5, + SKY_RESP_PPS_MODE = 0xB6, + SKY_RESP_POWER_MODE = 0xB9, + SKY_RESP_PPS_CABLE_DEL = 0xBB, + SKY_RESP_PPS_TIMING = 0xC2, + SKY_MSG_MEAS_TIME = 0xDC, + SKY_MSG_MEAS_RAW = 0xDD, + SKY_MSG_SV_CH_STATUS = 0xDE, + SKY_MSG_NAV_PVT = 0xDF, + SKY_MSG_GPS_SUBFRAME = 0xE0, + SKY_MSG_GLONASS_STRING = 0xE1, + SKY_MSG_BD_SUBFRAME_D1 = 0xE2, + SKY_MSG_BD_SUBFRAME_D2 = 0xE3, + SKY_MSG_EXT_RAW_MEAS = 0xE5, +} sky_mid_t; + +typedef enum { + SKY_SID_NONE = 0x00, + SKY_62_CONFIG_SBAS = 0x01, + SKY_62_QUERY_SBAS = 0x02, + SKY_62_CONFIG_QZSS = 0x03, + SKY_62_QUERY_QZSS = 0x04, + SKY_62_RESP_SBAS = 0x80, + SKY_62_RESP_QZSS = 0x81, + SKY_63_CONFIG_SAEE = 0x01, + SKY_63_QUERY_SAEE = 0x02, + SKY_63_RESP_SAEE = 0x80, + SKY_64_QUERY_BOOT_STATUS = 0x01, + SKY_64_CONFIG_EXT_NMEA_INTVL = 0x02, + SKY_64_QUERY_EXT_NMEA_INTVL = 0x03, + SKY_64_CONFIG_INTERFER_DET = 0x06, + SKY_64_QUERY_INTERFER_DET = 0x07, + SKY_64_CONFIG_PARAM_ENG_NUM = 0x0A, + SKY_64_QUERY_PARAM_ENG_NUM = 0x0B, + SKY_64_CONFIG_GPS_UTC_TIME = 0x15, + SKY_64_QUERY_GPS_UTC_TIME = 0x16, + SKY_64_CONFIG_NAV_MODE = 0x17, + SKY_64_QUERY_NAV_MODE = 0x18, + SKY_64_CONFIG_CONSTEL_TYPE = 0x19, + SKY_64_QUERY_CONSTEL_TYPE = 0x1A, + SKY_64_CONFIG_LEAP_SEC = 0x1F, + SKY_64_QUERY_GNSS_TIME = 0x20, + SKY_64_CONFIG_PSTI_INTVL = 0x21, + SKY_64_QUERY_PSTI_INTVL = 0x22, + SKY_64_CONFIG_DATUM_INDEX = 0x27, + SKY_64_QUERY_DATUM_INDEX = 0x28, + SKY_64_QUERY_VERSION_EXT = 0x7D, + SKY_64_RESP_BOOT_STATUS = 0x80, + SKY_64_RESP_EXT_NMEA_INTVL = 0x81, + SKY_64_RESP_INTERFER_DET = 0x83, + SKY_64_RESP_PARAM_ENG_NUM = 0x85, + SKY_64_RESP_GPS_UTC_REF_TIME = 0x8A, + SKY_64_RESP_NAV_TYPE = 0x8B, + SKY_64_RESP_CONSTEL_TYPE = 0x8C, + SKY_64_RESP_GNSS_TIME = 0x8E, + SKY_64_RESP_PSTI_INTVL = 0x8F, + SKY_64_RESP_DATUM_INDEX = 0x92, + SKY_64_RESP_GEOFENCE_DATA = 0x96, + SKY_64_RESP_GEOFENCE_RESULT = 0x97, + SKY_64_RESP_VERSION_EXT = 0xFE, + SKY_65_CONFIG_PPS_WIDTH = 0x01, + SKY_65_QUERY_PPS_WIDTH = 0x02, + SKY_65_CONFIG_PPS_FREQ = 0x03, + SKY_65_QUERY_PPS_FREQ = 0x04, + SKY_65_RESP_PPS_WIDTH = 0x80, + SKY_65_RESP_PPS_FREQ = 0x81, + SKY_67_SET_BD_EPHEMERIS = 0x01, + SKY_67_GET_BD_EPHEMERIS = 0x02, + SKY_67_SET_BD_ALMANAC = 0x03, + SKY_67_GET_BD_ALMANAC = 0x04, + SKY_67_RESP_BD_EPHEMERIS = 0x80, + SKY_67_RESP_BD_ALMANAC = 0x81, + SKY_6A_CONFIG_RTK_MODE = 0x01, + SKY_6A_QUERY_RTK_MODE = 0x02, + SKY_6A_RESET_GLONASS_IFB = 0x04, + SKY_6A_RESP_RTK_MODE = 0x80 +} sky_sid_t; + +typedef enum { + VENUS6 = 1, + VENUS8 = 2, + V_ALL = 0xFF, +} sky_supp_t; + +typedef struct { + sky_supp_t chip; + sky_mid_t mid; + sky_sid_t sid; +} sky_cmd_supp_t; + +/* Start-of- and End-of- sentence markers for binary messages */ +#define SKY_START_1 0xA0 +#define SKY_START_2 0xA1 +#define SKY_END_1 0x0D +#define SKY_END_2 0x0A + +/* Fix Mode constants from Navigation Data Message, 0xA8, and possibly others */ +#define SKY_MODE_NONE 0 +#define SKY_MODE_2D 1 +#define SKY_MODE_3D 2 +#define SKY_MODE_DGPS 3 + +/* NMEA update rates. Set messages not needed every second to SKY_NMEA_SLOW */ +#define SKY_NMEA_SLOW 10 +#define SKY_NMEA_NORM 1 + +struct sky_config_t { + unsigned len; /* Message length */ + unsigned char msg[40]; /* Data to be sent. sky_write() fills out header, csum and trailer */ + char *log; /* Log message to gpsd_log() */ +}; + + +#endif /* _GPSD_SKY_H_ */ diff --git a/drivers.c b/drivers.c index eda1fd61..6f342913 100644 --- a/drivers.c +++ b/drivers.c @@ -19,6 +19,11 @@ #include "bits.h" /* for getbeu16(), to extract big-endian words */ #include "strfuncs.h" +#ifdef SKYTRAQ_ENABLE +/* Use this instead of duplicating the code here */ +extern bool sky_detect(struct gps_device_t *); +#endif /* SKYTRAQ_ENABLE */ + ssize_t generic_get(struct gps_device_t *session) { return packet_get(session->gpsdata.gps_fd, &session->lexer); @@ -242,6 +247,14 @@ static void nmea_event_hook(struct gps_device_t *session, event_t event) (void)nmea_send(session, "$PMTK605"); break; #endif /* MTK3301_ENABLE */ +#ifdef SKYTRAQ_ENABLE + case 9: + /* probe for Skytraq Venus6/8 -- query firmware version */ + gpsd_log(&session->context->errout, LOG_PROG, + "=> Probing for Skytraq\n"); + (void)sky_detect(session); + break; +#endif /* SKYTRAQ_ENABLE */ default: break; } -- 2.11.0