diff options
-rw-r--r-- | Makefile.am | 3 | ||||
-rw-r--r-- | configure.ac | 15 | ||||
-rw-r--r-- | drivers.c | 4 | ||||
-rw-r--r-- | gpsd.h | 23 | ||||
-rw-r--r-- | libgpsd_core.c | 5 | ||||
-rw-r--r-- | packet.c | 78 | ||||
-rw-r--r-- | packet_states.h | 12 | ||||
-rw-r--r-- | ubx.c | 190 | ||||
-rw-r--r-- | ubx.h | 58 |
9 files changed, 386 insertions, 2 deletions
diff --git a/Makefile.am b/Makefile.am index 8e86e636..738b3a9e 100644 --- a/Makefile.am +++ b/Makefile.am @@ -111,6 +111,7 @@ libgps_c_sources = \ serial.c \ drivers.c \ zodiac.c \ + ubx.c \ garmin.c \ tsip.c \ evermore.c \ @@ -243,7 +244,7 @@ gpsflash.1: gpsflash.xml endif -include_HEADERS = gps.h gpsd.h libgpsmm.h +include_HEADERS = gps.h gpsd.h libgpsmm.h ubx.h EXTRA_DIST = \ autogen.sh \ diff --git a/configure.ac b/configure.ac index 559f793f..36677d6d 100644 --- a/configure.ac +++ b/configure.ac @@ -307,6 +307,19 @@ else AC_MSG_RESULT([no]) fi +dnl check for UBX support +AC_ARG_ENABLE(ubx, + AC_HELP_STRING([--enable-ubx], + [enable UBX Protocol support]), + [ac_ubx=$enableval], [ac_ubx=no]) +AC_MSG_CHECKING([for UBX support]) +if test x"$ac_ubx" = "xyes"; then + AC_MSG_RESULT([yes]) + AC_DEFINE([UBX_ENABLE], 1, [UBX Protocol support]) +else + AC_MSG_RESULT([no]) +fi + dnl check for EverMore support AC_ARG_ENABLE(evermore, AC_HELP_STRING([--disable-evermore], @@ -594,6 +607,7 @@ echo "iTrax : $ac_itrax" echo "iTalk : $ac_italk" echo "Navcom : $ac_navcom" echo "Garmin : $ac_garmin" +echo "UBX : $ac_ubx" echo "True North : $ac_tnt" echo "EverMore : $ac_evermore" echo "RTCM104 support : $ac_rtcm104" @@ -622,6 +636,7 @@ if test x"$ac_nmea" = "xno" -a \ x"$ac_italk" = "xno" -a \ x"$ac_navcom" = "xno" -a \ x"$ac_garmin" = "xno" -a \ + x"$ac_ubx" = "xno" -a \ x"$ac_evermore" = "xno"; then AC_MSG_ERROR(Can't build gpsd with no protocols enabled) fi @@ -14,6 +14,7 @@ #include "gpsd.h" extern struct gps_type_t zodiac_binary; +extern struct gps_type_t ubx_binary; ssize_t generic_get(struct gps_device_t *session) { @@ -860,6 +861,9 @@ static struct gps_type_t *gpsd_driver_array[] = { #ifdef NAVCOM_ENABLE &navcom_binary, #endif /* NAVCOM_ENABLE */ +#ifdef UBX_ENABLE + &ubx_binary, +#endif /* UBX_ENABLE */ #ifdef GARMIN_ENABLE &garmin_usb_binary, &garmin_ser_binary, @@ -18,7 +18,7 @@ #ifdef EARTHMATE_ENABLE #define ZODIAC_ENABLE #endif -#if defined(ZODIAC_ENABLE) || defined(SIRF_ENABLE) || defined(GARMIN_ENABLE) || defined(TSIP_ENABLE) || defined(EVERMORE_ENABLE) || defined(ITALK_ENABLE) +#if defined(ZODIAC_ENABLE) || defined(SIRF_ENABLE) || defined(GARMIN_ENABLE) || defined(TSIP_ENABLE) || defined(EVERMORE_ENABLE) || defined(ITALK_ENABLE) || defined(UBX_ENABLE) #define BINARY_ENABLE #endif #if defined(TRIPMATE_ENABLE) || defined(BINARY_ENABLE) @@ -71,6 +71,7 @@ struct gps_packet_t { #define RTCM_PACKET 7 #define GARMIN_PACKET 8 #define NAVCOM_PACKET 9 +#define UBX_PACKET 10 unsigned int state; size_t length; unsigned char inbuffer[MAX_PACKET_LENGTH*2+1]; @@ -297,6 +298,26 @@ struct gps_device_t { unsigned int Zv[ZODIAC_CHANNELS]; /* signal values (0-7) */ } zodiac; #endif /* ZODIAC_ENABLE */ +#ifdef UBX_ENABLE +#if 0 + struct { + /* XXX insert user fields here */ + } ubx; +#endif +#endif /* UBX_ENABLE */ + /* + * This is not conditionalized on RTCM104_ENABLE because we need to + * be able to build rtcmdecode even when RTCM support is not + * configured in the daemon. It doesn't take up extra space. + */ + struct { + /* ISGPS200 decoding */ + bool locked; + int curr_offset; + isgps30bits_t curr_word; + isgps30bits_t buf[RTCM_WORDS_MAX]; + unsigned int bufindex; + } isgps; #endif /* BINARY_ENABLE */ } driver; }; diff --git a/libgpsd_core.c b/libgpsd_core.c index ae0704d1..0e8b15d1 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 UBX_ENABLE + case UBX_PACKET: + (void)gpsd_switch_driver(session, "uBlox UBX"); + break; +#endif /* UBX_ENABLE */ #ifdef NAVCOM_ENABLE case NAVCOM_PACKET: (void)gpsd_switch_driver(session, "Navcom binary"); @@ -138,6 +138,12 @@ static void nextstate(struct gps_packet_t *lexer, break; } #endif /* ZODIAC_ENABLE */ +#ifdef UBX_ENABLE + if (c == 0xb5) { + lexer->state = UBX_LEADER_1; + break; + } +#endif /* UBX_ENABLE */ #ifdef ITALK_ENABLE if (c == '<') { lexer->state = ITALK_LEADER_1; @@ -542,6 +548,48 @@ static void nextstate(struct gps_packet_t *lexer, lexer->state = ZODIAC_RECOGNIZED; break; #endif /* ZODIAC_ENABLE */ +#ifdef UBX_ENABLE + case UBX_LEADER_1: + if (c == 0x62) + lexer->state = UBX_LEADER_2; + else + lexer->state = GROUND_STATE; + break; + case UBX_LEADER_2: + lexer->state = UBX_CLASS_ID; + break; + case UBX_CLASS_ID: + lexer->state = UBX_MESSAGE_ID; + break; + case UBX_MESSAGE_ID: + lexer->length = (size_t)c; + lexer->state = UBX_LENGTH_1; + break; + case UBX_LENGTH_1: + lexer->length += (c << 8); + if (lexer->length <= MAX_PACKET_LENGTH) + lexer->state = UBX_LENGTH_2; + else + lexer->state = GROUND_STATE; + break; + case UBX_LENGTH_2: + lexer->state = UBX_PAYLOAD; + break; + case UBX_PAYLOAD: + if (--lexer->length == 0) + lexer->state = UBX_CHECKSUM_A; + /* else stay in payload state */ + break; + case UBX_CHECKSUM_A: + lexer->state = UBX_RECOGNIZED; + break; + case UBX_RECOGNIZED: + if (c == 0xb5) + lexer->state = UBX_LEADER_1; + else + lexer->state = GROUND_STATE; + break; +#endif /* UBX_ENABLE */ #ifdef EVERMORE_ENABLE case EVERMORE_LEADER_1: if (c == STX) @@ -954,6 +1002,36 @@ ssize_t packet_parse(struct gps_packet_t *lexer, size_t fix) break; } #endif /* ZODIAC_ENABLE */ +#ifdef UBX_ENABLE + else if (lexer->state == UBX_RECOGNIZED) { + /* UBX use a TCP like checksum */ + short n, len; + unsigned char ck_a = 0; + unsigned char ck_b = 0; + len = lexer->inbufptr - lexer->inbuffer; + gpsd_report(LOG_IO, "len: %d\n", len); + for (n = 2; n < (len-2); n++) { + ck_a += lexer->inbuffer[n]; + ck_b += ck_a; + } + if (ck_a == lexer->inbuffer[len-2] && + ck_b == lexer->inbuffer[len-1]) + packet_accept(lexer, UBX_PACKET); + else { + gpsd_report(LOG_IO, + "UBX checksum 0x%02hhx%02hhx over length %hd,"\ + " expecting 0x%02hhx%02hhx\n", + ck_a, + ck_b, + len, + lexer->inbuffer[len-2], + lexer->inbuffer[len-1]); + lexer->state = GROUND_STATE; + } + packet_discard(lexer); + break; + } +#endif /* UBX_ENABLE */ #ifdef EVERMORE_ENABLE else if (lexer->state == EVERMORE_RECOGNIZED) { unsigned int n, crc, checksum, len; diff --git a/packet_states.h b/packet_states.h index 66d88a52..fb2ea8ef 100644 --- a/packet_states.h +++ b/packet_states.h @@ -95,6 +95,18 @@ NAVCOM_RECOGNIZED, /* found end of the Navcom packet */ #endif /* NAVCOM_ENABLE */ +#ifdef UBX_ENABLE + UBX_LEADER_1, /* first constant leader byte found */ + UBX_LEADER_2, /* second constant leader byte found */ + UBX_CLASS_ID, /* classid read */ + UBX_MESSAGE_ID, /* message id read */ + UBX_LENGTH_1, /* first length byte read (le) */ + UBX_LENGTH_2, /* second length byte read (le) */ + UBX_PAYLOAD, /* payload eating */ + UBX_CHECKSUM_A, /* checksum A byte (tcp checksum) */ + UBX_RECOGNIZED, /* this is also UBX_CHECKSUM_B */ +#endif + /* * Packet formats without checksums start here. We list them last so * that if a format with a conflicting structure *and* a checksum can @@ -0,0 +1,190 @@ +/* $Id$ + * + * UBX driver + */ + +#include <sys/types.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <math.h> +#include <ctype.h> +#include <unistd.h> +#include <time.h> +#include <stdio.h> + +#include "gpsd_config.h" +#include "gpsd.h" +#if defined(UBX_ENABLE) && defined(BINARY_ENABLE) +#include "ubx.h" + +#define LITTLE_ENDIAN_PROTOCOL +#include "bits.h" + +gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t len); +gps_mask_t ubx_package_nav_sol(struct gps_device_t *session, unsigned char *buf, size_t data_len); + +/** + * Navigation solution package + */ +gps_mask_t +ubx_package_nav_sol(struct gps_device_t *session, unsigned char *buf, size_t data_len) +{ + unsigned short gps_week; + unsigned int tow; + + if (data_len < 52) + return 0; + + gps_week = getsw(buf, 8); + tow = getul(buf, 4); + session->gpsdata.sentence_time = gpstime_to_unix(gps_week, tow) + - session->context->leap_seconds; +#ifdef NTPSHM_ENABLE + if (session->context->enable_ntpshm) + (void)ntpshm_put(session, session->gpsdata.sentence_time); /* TODO overhead */ +#endif + + if (buf[10] == UBX_MODE_3D) + session->gpsdata.fix.mode = MODE_3D; + if (buf[10] == UBX_MODE_2D || + buf[10] == UBX_MODE_DR || /* consider this too as 2D */ + buf[10] == UBX_MODE_GPSDR) /* conservative */ + session->gpsdata.fix.mode = MODE_2D; + else + session->gpsdata.fix.mode = MODE_NO_FIX; + + return 0; +} + +/*@ +charint @*/ +gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + unsigned short data_len; + unsigned short msgid; + + if (len < 6) /* the packet at least contains a head of six bytes */ + return 0; + + /* we may need to dump the raw packet */ + gpsd_report(LOG_PROG, "UBX packet type %02hhx:%02hhx length %d: %s\n", + buf[2], buf[3], len, gpsd_hexdump(buf, len)); + + /* extract message id and length */ + msgid = (buf[2] << 8) | buf[3]; + data_len = getsw(buf, 4); + switch (msgid) + { + case UBX_NAV_X: + gpsd_report(LOG_PROG, "UBX_NAV_X\n"); + break; + case UBX_NAV_SOL: + gpsd_report(LOG_PROG, "UBX_NAV_SOL\n"); + ubx_package_nav_sol(session, &buf[6], data_len); + break; + case UBX_NAV_POSLLH: + gpsd_report(LOG_PROG, "UBX_NAV_POSLLH\n"); + break; + case UBX_NAV_STATUS: + gpsd_report(LOG_PROG, "UBX_NAV_STATUS\n"); + break; + case UBX_NAV_SVINFO: + gpsd_report(LOG_PROG, "UBX_NAV_SVINFO\n"); + break; + case UBX_MON_SCHED: + gpsd_report(LOG_PROG, "UBX_MON_SCHED\n"); + break; + case UBX_MON_IO: + gpsd_report(LOG_PROG, "UBX_MON_IO\n"); + break; + case UBX_MON_TXBUF: + gpsd_report(LOG_PROG, "UBX_MON_TXBUF\n"); + break; + case UBX_INF_NOTICE: + gpsd_report(LOG_PROG, "UBX_INF_NOTICE\n"); + break; + case UBX_INF_WARNING: + gpsd_report(LOG_PROG, "UBX_INF_WARNING\n"); + break; + default: + gpsd_report(LOG_WARN, "UBX: unknown packet id %04hx (length: %d)\n", + msgid, len); + } + return 0; +} +/*@ -charint @*/ + +static gps_mask_t parse_input(struct gps_device_t *session) +{ + gps_mask_t st; + + if (session->packet.type == UBX_PACKET){ + st = ubx_parse(session, session->packet.outbuffer, session->packet.outbuflen); + session->gpsdata.driver_mode = 1; + 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; + return st; +#endif /* NMEA_ENABLE */ + } else + return 0; +} + +/* The methods in this code take parameters and have */ +/* return values that conform to the requirements AT */ +/* THE TIME THE CODE WAS WRITTEN. */ +/* */ +/* These values may well have changed by the time */ +/* you read this and methods could have been added */ +/* or deleted. */ +/* */ +/* The latest situation can be found by inspecting */ +/* the contents of struct gps_type_t in gpsd.h. */ +/* */ +/* This always contains the correct definitions that */ +/* any driver must use to compile. */ + +/* This is everything we export */ +struct gps_type_t ubx_binary = { + /* Full name of type */ + .typename = "uBlox UBX", + /* Response string that identifies device (not active) */ + .trigger = NULL, + /* Number of satellite channels supported by the device */ + .channels = 12, + /* Startup-time device detector */ + .probe_detect = /*probe_detect*/ NULL, + /* Wakeup to be done before each baud hunt */ + .probe_wakeup = /*probe_wakeup*/ NULL, + /* Initialize the device and get subtype */ + .probe_subtype = /*probe_subtype*/ NULL, +#ifdef ALLOW_RECONFIGURE + /* Enable what reports we need */ + .configurator = /*configurator*/ NULL, +#endif /* ALLOW_RECONFIGURE */ + /* Packet getter (using default routine) */ + .get_packet = generic_get, + /* Parse message packets */ + .parse_packet = parse_input, + /* RTCM handler (using default routine) */ + .rtcm_writer = /*pass_rtcm*/ NULL, + /* Speed (baudrate) switch */ + .speed_switcher = /*set_speed*/ NULL, + /* Switch to NMEA mode */ + .mode_switcher = /* set_mode */ NULL, + /* Message delivery rate switcher */ + .rate_switcher = NULL, + /* Number of chars per report cycle */ + .cycle_chars = -1, +#ifdef ALLOW_RECONFIGURE + /* Undo the actions of .configurator */ + .revert = /*ubx_revert*/ NULL, +#endif /* ALLOW_RECONFIGURE */ + /* Puts device back to original settings */ + .wrapup = /*ubx_wrapup*/ NULL, + /* Number of updates per second */ + .cycle = 1 +}; +#endif /* defined(UBX_ENABLE) && defined(BINARY_ENABLE) */ @@ -0,0 +1,58 @@ +/* $Id */ + +#define UBX_MESSAGE_BASE_SIZE 6 +#define UBX_MESSAGE_DATA_OFFSET UBX_MESSAGE_BASE_SIZE + +typedef enum { + UBX_CLASS_ACK = 0x05, /**< (Not) Acknowledges for cfg messages */ + UBX_CLASS_AID = 0x0b, /**< AGPS */ + UBX_CLASS_CFG = 0x06, /**< Configuration requests */ + UBX_CLASS_INF = 0x04, /**< Informative text messages */ + UBX_CLASS_MON = 0x0a, /**< System monitoring */ + UBX_CLASS_NAV = 0x01, /**< Navigation */ + UBX_CLASS_RXM = 0x02, /**< Receiver Manager */ + UBX_CLASS_TIM = 0x0d, /**< Time */ + UBX_CLASS_UPD = 0x09, /**< Firmware updates */ +} ubx_classes_t; + +#define UBX_MSGID(cls_, id_) (((cls_)<<8)|(id_)) + +typedef enum { + UBX_ACK_NAK = UBX_MSGID(UBX_CLASS_ACK, 0x00), + UBX_ACK_ACK = UBX_MSGID(UBX_CLASS_ACK, 0x01), + UBX_AID_REQ = UBX_MSGID(UBX_CLASS_AID, 0x00), + UBX_AID_DATA = UBX_MSGID(UBX_CLASS_AID, 0x10), + UBX_AID_INI = UBX_MSGID(UBX_CLASS_AID, 0x01), + UBX_AID_HUI = UBX_MSGID(UBX_CLASS_AID, 0x02), + UBX_AID_ALM = UBX_MSGID(UBX_CLASS_AID, 0x30), + UBX_AID_EPH = UBX_MSGID(UBX_CLASS_AID, 0x31), + + UBX_NAV_SOL = UBX_MSGID(UBX_CLASS_NAV, 0x06), + UBX_NAV_POSLLH = UBX_MSGID(UBX_CLASS_NAV, 0x02), + UBX_NAV_STATUS = UBX_MSGID(UBX_CLASS_NAV, 0x03), + UBX_NAV_SVINFO = UBX_MSGID(UBX_CLASS_NAV, 0x30), + UBX_NAV_X = UBX_MSGID(UBX_CLASS_NAV, 0x40), + + UBX_MON_SCHED = UBX_MSGID(UBX_CLASS_MON, 0x01), + UBX_MON_IO = UBX_MSGID(UBX_CLASS_MON, 0x02), + UBX_MON_TXBUF = UBX_MSGID(UBX_CLASS_MON, 0x08), + + UBX_INF_WARNING = UBX_MSGID(UBX_CLASS_INF, 0X01), + UBX_INF_NOTICE = UBX_MSGID(UBX_CLASS_INF, 0x02), + + UBX_CFG_PRT = UBX_MSGID(UBX_CLASS_CFG, 0x00), +} ubx_message_t; + +typedef enum { + UBX_MODE_NOFIX = 0x00, /* no fix available */ + UBX_MODE_DR = 0x01, /* Dead reckoning */ + UBX_MODE_2D = 0x02, /* 2D fix */ + UBX_MODE_3D = 0x03, /* 3D fix */ + UBX_MODE_GPSDR = 0x04, /* GPS + dead reckoning */ +} ubx_mode_t; + +#define UBX_SOL_FLAG_GPS_FIX_OK 0x01 +#define UBX_SOL_FLAG_DGPS 0x02 +#define UBX_SOL_VALID_WEEK 0x04 +#define UBX_SOL_VALID_TIME 0x08 + |