The start of a UBX driver, from Andreas Stricker. Not yet

functional but committed to allow for in-tree development.

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
diff --git a/drivers.c b/drivers.c
index 5a9a3eb0..4f90c39c 100644
--- a/drivers.c
+++ b/drivers.c
@@ -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,
diff --git a/gpsd.h b/gpsd.h
index 50e2c369..809f4be9 100644
--- a/gpsd.h
+++ b/gpsd.h
@@ -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");
diff --git a/packet.c b/packet.c
index 56b9c03e..abe4c020 100644
--- a/packet.c
+++ b/packet.c
@@ -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
diff --git a/ubx.c b/ubx.c
new file mode 100644
index 00000000..291dcc87
--- /dev/null
+++ b/ubx.c
@@ -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) */
diff --git a/ubx.h b/ubx.h
new file mode 100644
index 00000000..e9c06e85
--- /dev/null
+++ b/ubx.h
@@ -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
+