diff options
-rw-r--r-- | Makefile.am | 1 | ||||
-rw-r--r-- | configure.ac | 17 | ||||
-rw-r--r-- | drivers.c | 16 | ||||
-rw-r--r-- | gpsd.h | 2 | ||||
-rw-r--r-- | gpsfake.py | 12 | ||||
-rw-r--r-- | libgpsd_core.c | 5 | ||||
-rw-r--r-- | navcom.c | 445 | ||||
-rw-r--r-- | packet.c | 72 | ||||
-rw-r--r-- | packet_states.h | 13 |
9 files changed, 580 insertions, 3 deletions
diff --git a/Makefile.am b/Makefile.am index 654db6bf..3dd1d1a3 100644 --- a/Makefile.am +++ b/Makefile.am @@ -115,6 +115,7 @@ libgps_c_sources = \ tsip.c \ evermore.c \ italk.c \ + navcom.c \ libgpsd_core.c \ subframe.c \ ntpshm.c \ diff --git a/configure.ac b/configure.ac index 2ddd5a02..cac42e88 100644 --- a/configure.ac +++ b/configure.ac @@ -266,6 +266,19 @@ else AC_MSG_RESULT([no]) fi +dnl check for Navcom support +AC_ARG_ENABLE(navcom, + AC_HELP_STRING([--disable-navcom], + [disable Navcom support]), + [ac_navcom=$enableval], [ac_navcom=yes]) +AC_MSG_CHECKING([for Navcom support]) +if test x"$ac_navcom" = "xyes"; then + AC_MSG_RESULT([yes]) + AC_DEFINE([NAVCOM_ENABLE], 1, [Navcom support]) +else + AC_MSG_RESULT([no]) +fi + dnl check for Garmin support AC_ARG_ENABLE(garmin, AC_HELP_STRING([--disable-garmin], @@ -313,7 +326,7 @@ AC_ARG_ENABLE(rtcm104, [disable rtcm104 support]), [ac_rtcm104=$enableval], [ac_rtcm104=yes]) AC_MSG_CHECKING([for rtcm104 support]) -if test x"$ac_earthmate" = "xno" -a x"$ac_evermore" = "xno" -a x"$ac_garmin" = "xno" -a x"$ac_italk" = "xno" -a x"$ac_sirf" = "xno" -a x"$ac_tsip" = "xno"; then +if test x"$ac_earthmate" = "xno" -a x"$ac_evermore" = "xno" -a x"$ac_garmin" = "xno" -a x"$ac_italk" = "xno" -a x"$ac_sirf" = "xno" -a x"$ac_tsip" = "xno" -a x"$ac_navcom" = "xno"; then ac_rtcm104=no fi if test x"$ac_rtcm104" = "xyes"; then @@ -566,6 +579,7 @@ echo "Tripmate : $ac_tripmate" echo "Earthmate : $ac_earthmate" echo "iTrax : $ac_itrax" echo "iTalk : $ac_italk" +echo "Navcom : $ac_navcom" echo "Garmin : $ac_garmin" echo "True North : $ac_tnt" echo "EverMore : $ac_evermore" @@ -593,6 +607,7 @@ if test x"$ac_nmea" = "xno" -a \ x"$ac_earthmate" = "xno" -a \ x"$ac_itrax" = "xno" -a \ x"$ac_italk" = "xno" -a \ + x"$ac_navcom" = "xno" -a \ x"$ac_garmin" = "xno" -a \ x"$ac_evermore" = "xno"; then AC_MSG_ERROR(Can't build gpsd with no protocols enabled) @@ -20,7 +20,7 @@ ssize_t generic_get(struct gps_device_t *session) return packet_get(session->gpsdata.gps_fd, &session->packet); } -#if defined(NMEA_ENABLE) || defined(SIRF_ENABLE) || defined(EVERMORE_ENABLE) || defined(ITALK_ENABLE) +#if defined(NMEA_ENABLE) || defined(SIRF_ENABLE) || defined(EVERMORE_ENABLE) || defined(ITALK_ENABLE) || defined(NAVCOM_ENABLE) ssize_t pass_rtcm(struct gps_device_t *session, char *buf, size_t rtcmbytes) /* most GPSes take their RTCM corrections straight up */ { @@ -55,7 +55,15 @@ gps_mask_t nmea_parse_input(struct gps_device_t *session) #else return 0; #endif /* EVERMORE_ENABLE */ - } else if (session->packet.type == GARMIN_PACKET) { + } else if (session->packet.type == NAVCOM_PACKET) { + gpsd_report(LOG_WARN, "Navcom packet seen when NMEA expected.\n"); +#ifdef NAVCOM_ENABLE + (void)gpsd_switch_driver(session, "Navcom binary"); + return navcom_parse(session, session->packet.outbuffer, session->packet.outbuflen); +#else + return 0; +#endif /* NAVCOM_ENABLE */ +} else if (session->packet.type == GARMIN_PACKET) { gpsd_report(LOG_WARN, "Garmin packet seen when NMEA expected.\n"); #ifdef GARMIN_ENABLE /* we might never see a trigger, have this as a backstop */ @@ -823,6 +831,7 @@ static struct gps_type_t rtcm104 = { extern struct gps_type_t garmin_usb_binary, garmin_ser_binary; extern struct gps_type_t sirf_binary, tsip_binary; extern struct gps_type_t evermore_binary, italk_binary; +extern struct gps_type_t navcom_binary; /*@ -nullassign @*/ /* the point of this rigamarole is to not have to export a table size */ @@ -848,6 +857,9 @@ static struct gps_type_t *gpsd_driver_array[] = { #ifdef ZODIAC_ENABLE &zodiac_binary, #endif /* ZODIAC_ENABLE */ +#ifdef NAVCOM_ENABLE + &navcom_binary, +#endif /* NAVCOM_ENABLE */ #ifdef GARMIN_ENABLE &garmin_usb_binary, &garmin_ser_binary, @@ -70,6 +70,7 @@ struct gps_packet_t { #define ITALK_PACKET 6 #define RTCM_PACKET 7 #define GARMIN_PACKET 8 +#define NAVCOM_PACKET 9 unsigned int state; size_t length; unsigned char inbuffer[MAX_PACKET_LENGTH*2+1]; @@ -328,6 +329,7 @@ ssize_t pass_rtcm(struct gps_device_t *, char *, size_t); extern gps_mask_t sirf_parse(struct gps_device_t *, unsigned char *, size_t); extern gps_mask_t evermore_parse(struct gps_device_t *, unsigned char *, size_t); +extern gps_mask_t navcom_parse(struct gps_device_t *, unsigned char *, size_t); extern gps_mask_t garmin_ser_parse(struct gps_device_t *); extern bool dgnss_url(char *); @@ -140,6 +140,10 @@ class TestLoad: self.legend = "gpsfake: packet %d: " self.idoffset = 3 self.textual = False + elif self.sentences[0][0] == '\x02': + self.packtype = "Navcom binary" + self.legend = "gpsfake: packet %d" + self.textual = False elif self.sentences[0][0] == '\x10': self.packtype = "TSIP binary" self.legend = "gpsfake: packet %d: " @@ -180,6 +184,14 @@ class TestLoad: id = ord(third) | (ord(fourth) << 8) ndata = ord(fifth) | (ord(sixth) << 8) return "\xff\x81" + third + fourth + fifth + sixth + self.logfp.read(2*ndata+6) + elif first == '\x02' and second == '\x99': # Navcom + third = self.logfp.read(1) + fourth = self.logfp.read(1) + fifth = self.logfp.read(1) + sixth = self.logfp.read(1) + id = ord(fourth) + ndata = ord(fifth) | (ord(sixth) << 8) + return "\x02\x99\x66" + fourth + fifth + sixth + self.logfp.read(ndata-2) elif first == '\x10': # TSIP packet = first + second delcnt = 0 diff --git a/libgpsd_core.c b/libgpsd_core.c index 81fc03b0..ae0704d1 100644 --- a/libgpsd_core.c +++ b/libgpsd_core.c @@ -658,6 +658,11 @@ gps_mask_t gpsd_poll(struct gps_device_t *session) (void)gpsd_switch_driver(session, "Zodiac binary"); break; #endif /* ZODIAC_ENABLE */ +#ifdef NAVCOM_ENABLE + case NAVCOM_PACKET: + (void)gpsd_switch_driver(session, "Navcom binary"); + break; +#endif /* NAVCOM_ENABLE */ #ifdef EVERMORE_ENABLE case EVERMORE_PACKET: (void)gpsd_switch_driver(session, "EverMore binary"); diff --git a/navcom.c b/navcom.c new file mode 100644 index 00000000..8d12e1c5 --- /dev/null +++ b/navcom.c @@ -0,0 +1,445 @@ +/* + * Driver for Navcom receivers using propietary NCT messages, a binary protocol. + * + * Vendor website: http://www.navcomtech.com/ + * Technical references: http://www.navcomtech.com/support/docs.cfm + * + * Tested with an SF-2040G model + * + * At this stage, this driver implements the following commands: + * + * 0x20: Data Request (tell the unit which responses you want) + * 0x3f: LED Configuration (controls the front panel LEDs -- for testing) + * 0x1c: Test Support Block (again, blinks the front panel lights) + * + * and it understands the following responses: + * + * 0xb1: PVT Block (pos., vel., time., DOPs) + * 0x86: Channel Status (satellites visible + tracked) + * + * FIXME - Position errors theoretically are being reported at the one-sigma level. + * However, field tests suggest the values to be more consistent with + * two-sigma. Need to clear this up. + * FIXME - I'm not too sure of the way I have computed the vertical positional error + * I have used FOM as a scaling factor for VDOP, thusly VRMS = FOM/HDOP*VDOP + * TODO - Read 0x83 blocks (Ionosphere and UTC data) for transforming GPS time to UTC + * TODO - Lots of other things in mind, but the important stuff seems to be there. + * + * By Diego Berge. Contact via web form at http://www.nippur.net/survey/xuc/contact + * (the form is in Catalan, but you'll figure it out) + * + */ + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <math.h> +#include <ctype.h> +#include <unistd.h> +#include <time.h> +#include <sys/types.h> +#include <inttypes.h> +#include <stdio.h> +#include "gpsd_config.h" +#include "gpsd.h" + +#if defined(NAVCOM_ENABLE) && defined(BINARY_ENABLE) +#define LITTLE_ENDIAN_PROTOCOL +#include "bits.h" + +/* Have data which is 24 bits long */ +#define getsl24(buf,off) ((int32_t)((u_int32_t)getub((buf), (off)+2)<<24 | (u_int32_t)getub((buf), (off)+1)<<16 | (u_int32_t)getub((buf), (off))<<8)>>8) + +#define NAVCOM_CHANNELS 26 + +static u_int8_t checksum(unsigned char *buf, size_t len) +{ + size_t n; + u_int8_t csum = 0x00; + for(n = 0; n < len; n++) + csum ^= buf[n]; + return csum; +} + +static bool navcom_send_cmd(struct gps_device_t *session, unsigned char *cmd, size_t len) +{ + gpsd_report(LOG_RAW, "Sending Navcom command 0x%02x: %s\n", + cmd[3], gpsd_hexdump(cmd, len)); + return gpsd_write(session, cmd, len); +} + +/* Data Request */ +static void navcom_cmd_0x20(struct gps_device_t *session, u_int8_t block_id, u_int16_t rate) +{ + unsigned char msg[14]; + putbyte(msg, 0, 0x02); + putbyte(msg, 1, 0x99); + putbyte(msg, 2, 0x66); + putbyte(msg, 3, 0x20); /* Cmd ID */ + putword(msg, 4, 0x000a); /* Length */ + putbyte(msg, 6, 0x00); /* Action */ + putbyte(msg, 7, 0x00); /* Count of blocks */ + putbyte(msg, 8, block_id); /* Data Block ID */ + putbyte(msg, 9, 0x02); /* Logical Ports */ + putword(msg, 10, rate); /* Data rate */ + putbyte(msg, 12, checksum(msg+3, 9)); + putbyte(msg, 13, 0x03); + navcom_send_cmd(session, msg, 14); +} + +/* Changes the LED settings in the receiver */ +static void navcom_cmd_0x3f(struct gps_device_t *session) +{ + unsigned char msg[12]; + putbyte(msg, 0, 0x02); + putbyte(msg, 1, 0x99); + putbyte(msg, 2, 0x66); + putbyte(msg, 3, 0x3f); /* Cmd ID */ + putword(msg, 4, 0x0008); + putbyte(msg, 6, 0x01); /* Action */ + putbyte(msg, 7, 0x00); /* Reserved */ + putbyte(msg, 8, 0x02); /* Link LED setting */ + putbyte(msg, 9, 0x0a); /* Battery LED setting */ + putbyte(msg, 10, checksum(msg+3, 7)); + putbyte(msg, 11, 0x03); + navcom_send_cmd(session, msg, 12); +} + +/* Test Support Block - Blinks the LEDs */ +static void navcom_cmd_0x1c(struct gps_device_t *session, u_int8_t mode) +{ + unsigned char msg[12]; + putbyte(msg, 0, 0x02); + putbyte(msg, 1, 0x99); + putbyte(msg, 2, 0x66); + putbyte(msg, 3, 0x1c); /* Cmd ID */ + putword(msg, 4, 0x0008); + putbyte(msg, 6, 0x00); + putbyte(msg, 7, mode); /* 0x01 or 0x02 */ + putbyte(msg, 8, mode); + putbyte(msg, 9, 0x00); + putbyte(msg, 10, checksum(msg+3, 7)); + putbyte(msg, 11, 0x03); + navcom_send_cmd(session, msg, 12); +} + + +static void navcom_probe_subtype(struct gps_device_t *session, unsigned int seq) +{ + /* Request the following messages: 0x83, 0x84, 0x86, 0xb0, 0xb1 */ + if (!seq) { + navcom_cmd_0x3f(session); + navcom_cmd_0x1c(session, 0x02); + navcom_cmd_0x20(session, 0xb1, 0x000a); + navcom_cmd_0x20(session, 0xb0, 0x000a); + navcom_cmd_0x20(session, 0x86, 0x000a); + } +#ifdef __UNUSED__ + if ((seq % 20) == 0) + navcom_cmd_0x1c(session, 0x01); + else if ((seq % 10) == 0) + navcom_cmd_0x1c(session, 0x02); +#endif /* __UNUSED__ */ +} + +/* PVT Block */ +static gps_mask_t handle_0xb1(struct gps_device_t *session) +{ + unsigned char *buf = session->packet.outbuffer + 3; + uint16_t week; + uint32_t tow; + uint32_t sats_used; + int32_t lat, lon; + /* Resolution of lat/lon values (2^-11) */ + #define LL_RES (0.00048828125) + uint8_t lat_fraction, lon_fraction; + /* Resolution of lat/lon fractions (2^-15) */ + #define LL_FRAC_RES (0.000030517578125) + uint8_t nav_mode; + int32_t ellips_height, altitude; + /* Resolution of height and altitude values (2.0^-10) */ + #define EL_RES (0.0009765625) + long vel_north, vel_east, vel_up; + /* Resolution of velocity values (2.0^-10) */ + #define VEL_RES (0.0009765625) + double track; + uint8_t fom, gdop, pdop, hdop, vdop, tdop; + /* This value means "undefined" */ + #define DOP_UNDEFINED (255) + +#ifdef __UNUSED__ + uint16_t max_dgps_age; + uint8_t dgps_conf; + uint8_t ext_nav_mode; + int16_t ant_height_adj; + int32_t set_delta_north, set_delta_east, set_delta_up; + uint8_t nav_failure_code; +#endif /* __UNUSED__ */ + + /* FIXME - Need to read block 0x86 to get up-to-date leap seconds */ + /* Timestamp */ + week = getuw(buf, 3); + tow = getul(buf, 5); + session->gpsdata.fix.time = session->gpsdata.sentence_time = gpstime_to_unix(week, tow/1000.0) - session->context->leap_seconds; + gpsd_report(LOG_RAW+1, "Navcom packet type 0xb1 - week = %d tow=%f unixtime=%f\n", + week, tow/1000.0, session->gpsdata.fix.time); + + /* Satellites used */ + unsigned char n; + sats_used = getul(buf, 9); + session->gpsdata.satellites_used = 0; + for(n = 0; n < 31; n++) { + if (sats_used & (0x01 << n)) + session->gpsdata.used[session->gpsdata.satellites_used++] = n+1; + } + + /* Get latitude, longitude */ + lat = getsl(buf, 13); + lon = getsl(buf, 17); + lat_fraction = (getub(buf, 21) >> 4); + lon_fraction = (getub(buf, 21) & 0x0f); + + session->gpsdata.fix.latitude = (double)(lat * LL_RES + lat_fraction * LL_FRAC_RES ) / 3600; + session->gpsdata.fix.longitude = (double)(lon * LL_RES + lon_fraction * LL_FRAC_RES ) / 3600; + gpsd_report(LOG_RAW, "Navcom packet type 0xb1 - lat = %f (%d, %08x), lon = %f (%d, %08x)\n", + session->gpsdata.fix.latitude, lat, lat, session->gpsdata.fix.longitude, lon, lon); + + /* Nav mode */ + nav_mode = getub(buf, 22); + if ((nav_mode & 0xc0) == 0xc0) { + session->gpsdata.fix.mode = MODE_3D; + if (nav_mode & 0x03) + session->gpsdata.status = STATUS_DGPS_FIX; + else + session->gpsdata.status = STATUS_FIX; + } + else if (nav_mode & 0x80) { + session->gpsdata.fix.mode = MODE_2D; + if (nav_mode & 0x03) + session->gpsdata.status = STATUS_DGPS_FIX; + else + session->gpsdata.status = STATUS_FIX; + } + else { + session->gpsdata.fix.mode = MODE_NO_FIX; + session->gpsdata.status = STATUS_NO_FIX; + } + + /* Height Data */ + ellips_height = getsl(buf, 23); + altitude = getsl(buf, 27); + + session->gpsdata.fix.altitude = (double)(altitude * EL_RES); + session->gpsdata.separation = (double)(ellips_height - altitude)*EL_RES; + + /* Speed Data */ + vel_north = getsl24(buf, 31); + vel_east = getsl24(buf, 34); + /* vel_up = getsl24(buf, 37); */ + vel_up = getsl24(buf, 37); + + track = atan2(vel_north, vel_east); + if (track < 0) + track += 2 * PI; + session->gpsdata.fix.track = track * RAD_2_DEG; + /* FIXME Confirm what the tech spec means by (2^-10 m/s) +/- 8192m/s */ + session->gpsdata.fix.speed = sqrt(pow(vel_east,2) + pow(vel_north,2)) * VEL_RES; + session->gpsdata.fix.climb = vel_up * VEL_RES; + gpsd_report(LOG_RAW+1, "Navcom packet type 0xb1 - velocities - track = %f, speed = %f, climb = %f\n", + session->gpsdata.fix.track, + session->gpsdata.fix.speed, + session->gpsdata.fix.climb); + + /* Quality indicators */ + fom = getub(buf, 40); + gdop = getub(buf, 41); + pdop = getub(buf, 42); + hdop = getub(buf, 43); + vdop = getub(buf, 44); + tdop = getub(buf, 45); + + session->gpsdata.fix.eph = fom/100.0; + /* FIXME This cannot possibly be right */ + /* I cannot find where to get VRMS from in the Navcom output, though */ + session->gpsdata.fix.epv = (double)fom/(double)hdop*(double)vdop/100.0; + + if (gdop == DOP_UNDEFINED) + session->gpsdata.gdop = NAN; + else + session->gpsdata.gdop = gdop/10.0; + if (pdop == DOP_UNDEFINED) + session->gpsdata.pdop = NAN; + else + session->gpsdata.pdop = pdop/10.0; + if (hdop == DOP_UNDEFINED) + session->gpsdata.hdop = NAN; + else + session->gpsdata.hdop = hdop/10.0; + if (vdop == DOP_UNDEFINED) + session->gpsdata.vdop = NAN; + else + session->gpsdata.vdop = vdop/10.0; + if (tdop == DOP_UNDEFINED) + session->gpsdata.tdop = NAN; + else + session->gpsdata.tdop = tdop/10.0; + + gpsd_report(LOG_RAW+1, "hrms = %f, gdop = %f, pdop = %f, hdop = %f, vdop = %f, tdop = %f\n", + session->gpsdata.fix.eph, session->gpsdata.gdop, session->gpsdata.pdop, + session->gpsdata.hdop, session->gpsdata.vdop, session->gpsdata.tdop); + + return LATLON_SET | ALTITUDE_SET | CLIMB_SET | SPEED_SET | TRACK_SET | TIME_SET + | STATUS_SET | MODE_SET | USED_SET | HERR_SET | VERR_SET | DOP_SET | CYCLE_START_SET; +} + +/* Channel Status */ +static gps_mask_t handle_0x86(struct gps_device_t *session) +{ + size_t n, i; + u_int8_t prn, tracking_status, ele, ca_snr, p2_snr, channel; + u_int16_t azm; + unsigned char *buf = session->packet.outbuffer + 3; + size_t msg_len = getuw(buf, 1); + u_int16_t week = getuw(buf, 3); + u_int32_t tow = getul(buf, 5); + u_int16_t status = getuw(buf, 10); + u_int8_t sats_visible = getub(buf, 12); + u_int8_t sats_tracked = getub(buf, 13); + u_int8_t sats_used = getub(buf, 14); + u_int8_t pdop = getub(buf, 15); + + /* Timestamp and PDOP */ + session-> gpsdata.fix.time = gpstime_to_unix(week, tow/1000.0) - session->context->leap_seconds; + session->gpsdata.pdop = pdop / 10.0; + + /* Satellite count */ + session->gpsdata.satellites = sats_visible; + session->gpsdata.satellites_used = sats_used; + + /* Fix mode */ + switch(status & 0x05) + { + case 0x05: + session->gpsdata.status = STATUS_DGPS_FIX; + break; + case 0x01: + session->gpsdata.status = STATUS_FIX; + break; + default: + session->gpsdata.status = STATUS_NO_FIX; + } + + gpsd_report(LOG_RAW, "Navcom packet type 0x86 - satellites: visible = %u, tracked = %u, used = %u\n", + sats_visible, sats_tracked, sats_used); + + /* Satellite details */ + i = 0; + for(n = 17; n < msg_len; n += 14) { + if(i >= MAXCHANNELS) { + gpsd_report(LOG_ERROR, "internal error - too many satellites!\n"); + gpsd_zero_satellites(&session->gpsdata); + return ERROR_SET; + } + prn = getub(buf, n); + tracking_status = getub(buf, n+1); + ele = getub(buf, n+5); + azm = getuw(buf, n+6); + ca_snr = getub(buf, n+8); + p2_snr = getub(buf, n+10); + channel = getub(buf, n+13); + if (tracking_status != 0x00) { + session->gpsdata.PRN[i] = (int)prn; + session->gpsdata.elevation[i] = ele; + session->gpsdata.azimuth[i] = azm; + session->gpsdata.ss[i++] = (p2_snr ? p2_snr : ca_snr) / 4.0; + gpsd_report(LOG_RAW+1, "prn = %02x, ele = %02x, azm = %04x, ss = %d\n", + prn, ele, azm, session->gpsdata.ss); + } + } + + return TIME_SET | PDOP_SET | SATELLITE_SET | STATUS_SET; +} + + +/*@ +charint @*/ +gps_mask_t navcom_parse(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + unsigned char cmd_id; + unsigned char *payload; + unsigned int msg_len; + + if (len == 0) + return 0; + + cmd_id = getub(buf, 3); + payload = &buf[6]; + msg_len = getuw(buf, 4); + + /*@ -usedef -compdef @*/ + gpsd_report(LOG_RAW, "Navcom packet type 0x%02x, length %d: %s\n", + cmd_id, msg_len, gpsd_hexdump(buf, len)); + /*@ +usedef +compdef @*/ + + (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), + "0x%02x",cmd_id); + + switch (cmd_id) + { + case 0xb1: + return handle_0xb1(session); + case 0x86: + return handle_0x86(session); + default: + gpsd_report(LOG_IO, "Unknown or unimplemented Navcom packet id 0x%02x, length %d\n", + cmd_id, msg_len); + return 0; + } +} +/*@ -charint @*/ + +static gps_mask_t navcom_parse_input(struct gps_device_t *session) +{ + gps_mask_t st; + + if (session->packet.type == NAVCOM_PACKET){ + st = navcom_parse(session, session->packet.outbuffer, session->packet.outbuflen); + session->gpsdata.driver_mode = 1; /* binary */ + return st; +#ifdef NMEA_ENABLE + } else if (session->packet.type == NMEA_PACKET) { + st = nmea_parse((char *)session->packet.outbuffer, session); + session->gpsdata.driver_mode = 0; /* NMEA */ + return st; +#endif /* NMEA_ENABLE */ + } else + return 0; +} + + +/* this is everything we export */ +struct gps_type_t navcom_binary = +{ + .typename = "Navcom binary", /* full name of type */ + .trigger = "\x02\x99\x66", + .channels = NAVCOM_CHANNELS, /* 12 L1 + 12 L2 + 2 L-Band */ + .probe_wakeup = NULL, /* no wakeup to be done before hunt */ + .probe_detect = NULL, /* no probe */ + .probe_subtype = navcom_probe_subtype, /* subtype probing */ +#ifdef ALLOW_RECONFIGURE + .configurator = NULL, /* no reconfigure */ +#endif /* ALLOW_RECONFIGURE */ + .get_packet = generic_get, /* use generic one */ + .parse_packet = navcom_parse_input, /* parse message packets */ + .rtcm_writer = pass_rtcm, /* send RTCM data straight */ + .speed_switcher = NULL, /* we can change baud rates */ + .mode_switcher = NULL, /* there is a mode switcher */ + .rate_switcher = NULL, /* no sample-rate switcher */ + .cycle_chars = -1, /* ignore, no rate switch */ +#ifdef ALLOW_RECONFIGURE + .revert = NULL, /* no reversion code */ +#endif /* ALLOW_RECONFIGURE */ + .wrapup = NULL, /* ignore, no wrapup */ + .cycle = 1, /* updates every second */ +}; + +#endif /* defined(NAVCOM_ENABLE) && defined(BINARY_ENABLE) */ @@ -144,6 +144,12 @@ static void nextstate(struct gps_packet_t *lexer, break; } #endif /* ITALK_ENABLE */ +#ifdef NAVCOM_ENABLE + if (c == 0x02) { + lexer->state = NAVCOM_LEADER_1; + break; + } +#endif /* NAVCOM_ENABLE */ #ifdef RTCM104_ENABLE if ((isgpsstat = rtcm_decode(lexer, c)) == ISGPS_SYNC) { lexer->state = RTCM_SYNC_STATE; @@ -400,6 +406,64 @@ static void nextstate(struct gps_packet_t *lexer, lexer->state = EVERMORE_LEADER_2; else #endif /* EVERMORE_ENABLE */ +#ifdef NAVCOM_ENABLE + case NAVCOM_LEADER_1: + if (c == 0x99) + lexer->state = NAVCOM_LEADER_2; + else + lexer->state = GROUND_STATE; + break; + case NAVCOM_LEADER_2: + if (c == 0x66) + lexer->state = NAVCOM_LEADER_3; + else + lexer->state = GROUND_STATE; + break; + case NAVCOM_LEADER_3: + lexer->state = NAVCOM_ID; + break; + case NAVCOM_ID: + lexer->length = (size_t)c - 4; + lexer->state = NAVCOM_LENGTH_1; + break; + case NAVCOM_LENGTH_1: + lexer->length += (c << 8); + lexer->state = NAVCOM_LENGTH_2; + break; + case NAVCOM_LENGTH_2: + if (--lexer->length == 0) + lexer->state = NAVCOM_PAYLOAD; + break; + case NAVCOM_PAYLOAD: + { + unsigned int n; + unsigned char csum = lexer->inbuffer[3]; + for(n=4; (unsigned char *)(lexer->inbuffer + n) < lexer->inbufptr - 1; n++) + csum ^= lexer->inbuffer[n]; + if(csum != c) { + gpsd_report(LOG_INF, "Navcom packet type 0x%hx bad checksum 0x%hx, expecting 0x%hx\n", + lexer->inbuffer[3], csum, c); + gpsd_report(LOG_RAW, "Navcom packet dump: %s\n", + gpsd_hexdump(lexer->inbuffer, lexer->inbuflen)); + lexer->state = GROUND_STATE; + break; + } + } + lexer->state = NAVCOM_CSUM; + break; + case NAVCOM_CSUM: + if (c == 0x03) + lexer->state = NAVCOM_RECOGNIZED; + else + lexer->state = GROUND_STATE; + break; + case NAVCOM_RECOGNIZED: + if (c == 0x02) + lexer->state = NAVCOM_LEADER_1; + else + lexer->state = GROUND_STATE; + break; +#endif /* NAVCOM_ENABLE */ #if defined(TSIP_ENABLE) || defined(GARMIN_ENABLE) /* garmin is special case of TSIP */ /* check last because there's no checksum */ @@ -960,6 +1024,14 @@ ssize_t packet_parse(struct gps_packet_t *lexer, size_t fix) break; } #endif /* ITALK_ENABLE */ +#ifdef NAVCOM_ENABLE + else if (lexer->state == NAVCOM_RECOGNIZED) { + /* By the time we got here we know checksum is OK */ + packet_accept(lexer, NAVCOM_PACKET); + packet_discard(lexer); + break; + } +#endif /* NAVCOM_ENABLE */ #ifdef RTCM104_ENABLE else if (lexer->state == RTCM_RECOGNIZED) { /* diff --git a/packet_states.h b/packet_states.h index d2ea8d85..66d88a52 100644 --- a/packet_states.h +++ b/packet_states.h @@ -82,6 +82,19 @@ ITALK_RECOGNIZED, /* found end of the iTalk packet */ #endif /* ITALK_ENABLE */ +#ifdef NAVCOM_ENABLE + NAVCOM_EXPECTED, /* expecting Navcom packet */ + NAVCOM_LEADER_1, /* saw leading 0x02 */ + NAVCOM_LEADER_2, /* saw leading 0x99 */ + NAVCOM_LEADER_3, /* saw leading 0x66 */ + NAVCOM_ID, /* saw message ID */ + NAVCOM_LENGTH_1, /* saw first byte of Navcom packet length */ + NAVCOM_LENGTH_2, /* saw second byte of Navcom packet length */ + NAVCOM_PAYLOAD, /* we're in a Navcom payload */ + NAVCOM_CSUM, /* saw checksum */ + NAVCOM_RECOGNIZED, /* found end of the Navcom packet */ +#endif /* NAVCOM_ENABLE */ + /* * Packet formats without checksums start here. We list them last so * that if a format with a conflicting structure *and* a checksum can |