bunch of fixes to ubx, particularly for the ublox5. from Henk Fijnvandraat

3 files changed, 262 insertions, 23 deletions

diff --git a/drivers.c b/drivers.c
index ee8f7029..392a54ba 100644
--- a/drivers.c
+++ b/drivers.c
@@ -30,6 +30,12 @@ ssize_t pass_rtcm(struct gps_device_t *session, char *buf, size_t rtcmbytes)
 }
 #endif
 
+#ifdef UBX_ENABLE
+    extern gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t len);
+    extern bool ubx_write(int fd, unsigned char msg_class, unsigned char msg_id, unsigned char *msg, unsigned short data_len);
+    extern void ubx_catch_model(struct gps_device_t *session, unsigned char *buf, size_t len);
+#endif /* UBX_ENABLE */
+
 #ifdef NMEA_ENABLE
 /**************************************************************************
  *
@@ -74,6 +80,14 @@ gps_mask_t nmea_parse_input(struct gps_device_t *session)
 #else
 	return 0;
 #endif /* GARMIN_ENABLE */
+    } else if (session->packet.type == UBX_PACKET) {
+	gpsd_report(LOG_WARN, "UBX packet seen when NMEA expected.\n");
+#ifdef UBX_ENABLE
+	(void)gpsd_switch_driver(session, "uBlox UBX");
+	return ubx_parse(session, session->packet.outbuffer, session->packet.outbuflen);
+#else
+	return 0;
+#endif /* UBX_ENABLE */
     } else if (session->packet.type == NMEA_PACKET) {
 	gps_mask_t st = 0;
 #ifdef GARMINTXT_ENABLE
@@ -91,6 +105,13 @@ gps_mask_t nmea_parse_input(struct gps_device_t *session)
 	    struct gps_type_t **dp;
 
 	    /* maybe this is a trigger string for a driver we know about? */
+#ifdef UBX_ENABLE
+	    if(strncmp((char *)session->packet.outbuffer, "$GPTXT,01,01,02,MOD", 19)==0) {
+		ubx_catch_model(session, session->packet.outbuffer, session->packet.outbuflen);
+		(void)gpsd_switch_driver(session, "uBlox UBX");
+		return 0;
+	    }
+#endif /* UBX_ENABLE */
 	    for (dp = gpsd_drivers; *dp; dp++) {
 		char	*trigger = (*dp)->trigger;
 
@@ -190,6 +211,12 @@ static void nmea_probe_subtype(struct gps_device_t *session, unsigned int seq)
 	(void)nmea_send(session->gpsdata.gps_fd, "$PASHQ,RID");
 	break;
 #endif /* ASHTECH_ENABLE */
+#ifdef UBX_ENABLE
+    case 7:
+	/* probe for UBX -- query software version */
+	ubx_write(session->gpsdata.gps_fd, 0x0a, 0x04, NULL, 0);
+	break;
+#endif /* UBX_ENABLE */
     default:
 	break;
     }
diff --git a/packet.c b/packet.c
index f829ad09..a42593b3 100644
--- a/packet.c
+++ b/packet.c
@@ -272,6 +272,10 @@ static void nextstate(struct gps_packet_t *lexer,
 	    lexer->state = NMEA_DOLLAR;
 	else if (c == '!')
 	    lexer->state = NMEA_BANG;
+#ifdef UBX_ENABLE
+	else if (c == 0xb5) /* LEA-5H can and will output NMEA and UBX back to back */
+	    lexer->state = UBX_LEADER_1;
+#endif
 	else
 	    lexer->state = GROUND_STATE;
 	break;
@@ -668,6 +672,8 @@ static void nextstate(struct gps_packet_t *lexer,
     case UBX_RECOGNIZED:
 	if (c == 0xb5)
 	    lexer->state = UBX_LEADER_1;
+	else if (c == '$') /* LEA-5H can and will output NMEA and UBX back to back */
+	    lexer->state = NMEA_DOLLAR;
 	else
 	    lexer->state = GROUND_STATE;
 	break;
diff --git a/ubx.c b/ubx.c
index 78994095..0c816ee5 100644
--- a/ubx.c
+++ b/ubx.c
@@ -32,12 +32,19 @@
  * see also the FV25 and UBX documents on reference.html
  */
 
-static gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t len);
-static gps_mask_t ubx_msg_nav_sol(struct gps_device_t *session, unsigned char *buf, size_t data_len);
-static gps_mask_t ubx_msg_nav_dop(struct gps_device_t *session, unsigned char *buf, size_t data_len);
-static gps_mask_t ubx_msg_nav_timegps(struct gps_device_t *session, unsigned char *buf, size_t data_len);
-static gps_mask_t ubx_msg_nav_svinfo(struct gps_device_t *session, unsigned char *buf, size_t data_len);
-static void       ubx_msg_inf(unsigned char *buf, size_t data_len);
+static bool have_port_configuration = false;
+static unsigned char original_port_settings[20];
+static unsigned char sbas_in_use;
+
+	bool 		ubx_write(int fd, unsigned char msg_class, unsigned char msg_id, unsigned char *msg, unsigned short data_len);
+	gps_mask_t 	ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t len);
+	void 		ubx_catch_model(struct gps_device_t *session, unsigned char *buf, size_t len);
+static	gps_mask_t 	ubx_msg_nav_sol(struct gps_device_t *session, unsigned char *buf, size_t data_len);
+static	gps_mask_t 	ubx_msg_nav_dop(struct gps_device_t *session, unsigned char *buf, size_t data_len);
+static	gps_mask_t 	ubx_msg_nav_timegps(struct gps_device_t *session, unsigned char *buf, size_t data_len);
+static	gps_mask_t 	ubx_msg_nav_svinfo(struct gps_device_t *session, unsigned char *buf, size_t data_len);
+static	void		ubx_msg_sbas(unsigned char *buf);
+static	void       	ubx_msg_inf(unsigned char *buf, size_t data_len);
 
 /**
  * Navigation solution message
@@ -163,7 +170,7 @@ ubx_msg_nav_timegps(struct gps_device_t *session, unsigned char *buf, size_t dat
 static gps_mask_t
 ubx_msg_nav_svinfo(struct gps_device_t *session, unsigned char *buf, size_t data_len)
 {
-    unsigned int i, tow, nchan, nsv, st;
+    unsigned int i, j, tow, nchan, nsv, st;
 
     if (data_len < 152 ) {
 	gpsd_report(LOG_PROG, "runt svinfo (datalen=%d)\n", data_len);
@@ -174,32 +181,58 @@ ubx_msg_nav_svinfo(struct gps_device_t *session, unsigned char *buf, size_t data
 //				- session->context->leap_seconds;
     /*@ +charint @*/
     nchan = getub(buf, 4);
-    if (nchan > 16){
-	gpsd_report(LOG_WARN, "Invalid NAV SVINFO message, >16 reported");
+    if (nchan > MAXCHANNELS){
+	gpsd_report(LOG_WARN, "Invalid NAV SVINFO message, >%d reported",MAXCHANNELS);
 	return 0;
     }
     /*@ -charint @*/
     gpsd_zero_satellites(&session->gpsdata);
     nsv = 0;
-    for (i = st = 0; i < nchan; i++) {
+    for (i = j = st = 0; i < nchan; i++) {
 	int off = 8 + 12 * i;
-	session->gpsdata.PRN[i]		= (int)getub(buf, off+1);
-	session->gpsdata.ss[i]		= (int)getub(buf, off+4);
-	session->gpsdata.elevation[i]	= (int)getsb(buf, off+5);
-	session->gpsdata.azimuth[i]	= (int)getlesw(buf, off+6);
-	if(session->gpsdata.PRN[i])
+	if((int)getub(buf, off+4) == 0) continue; /* LEA-5H seems to have a bug reporting sats it does not see or hear*/
+	session->gpsdata.PRN[j]		= (int)getub(buf, off+1);
+	session->gpsdata.ss[j]		= (int)getub(buf, off+4);
+	session->gpsdata.elevation[j]	= (int)getsb(buf, off+5);
+	session->gpsdata.azimuth[j]	= (int)getlesw(buf, off+6);
+	if(session->gpsdata.PRN[j])
 		st++;
-
 	/*@ -predboolothers */
 	if (getub(buf, off+2) & 0x01)
-	    session->gpsdata.used[nsv++] = session->gpsdata.PRN[i];
+	    session->gpsdata.used[nsv++] = session->gpsdata.PRN[j];
+	if (session->gpsdata.PRN[j] == sbas_in_use)
+	    session->gpsdata.used[nsv++] = session->gpsdata.PRN[j];
 	/*@ +predboolothers */
+	j++;
     }
     session->gpsdata.satellites = (int)st;
     session->gpsdata.satellites_used = (int)nsv;
     return SATELLITE_SET | USED_SET;
 }
 
+/*
+ * SBAS Info
+ */
+static void
+ubx_msg_sbas(unsigned char *buf)
+{
+#ifdef UBX_SBAS_DEBUG
+    unsigned int i, nsv;
+
+    gpsd_report(LOG_WARN, "SBAS: %d %d %d %d %d\n",
+		(int)getub(buf, 4), (int)getub(buf, 5), (int)getub(buf, 6), (int)getub(buf, 7), (int)getub(buf, 8));
+
+    nsv = (int)getub(buf, 8);
+    for (i = 0; i < nsv; i++) {
+	int off = 12 + 12 * i;
+	gpsd_report(LOG_WARN, "SBAS info on SV: %d\n", (int)getub(buf, off));
+    }
+#endif
+/* really 'in_use' depends on the sats info, EGNOS is still in test */
+/* In WAAS areas one might also check for the type of corrections indicated */
+    sbas_in_use = getub(buf, 4);
+}
+
 static void
 ubx_msg_inf(unsigned char *buf, size_t data_len)
 {
@@ -234,11 +267,12 @@ ubx_msg_inf(unsigned char *buf, size_t data_len)
 }
 
 /*@ +charint @*/
-static gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t len)
+gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t len)
 {
     size_t data_len;
     unsigned short msgid;
     gps_mask_t mask = 0;
+    int i;
 
     if (len < 6)    /* the packet at least contains a head of six bytes */
 	return 0;
@@ -293,6 +327,7 @@ static gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, si
 	    break;
 	case UBX_NAV_SBAS:
 	    gpsd_report(LOG_IO, "UBX_NAV_SBAS\n");
+	    ubx_msg_sbas(&buf[6]);
 	    break;
 	case UBX_NAV_EKFSTATUS:
 	    gpsd_report(LOG_IO, "UBX_NAV_EKFSTATUS\n");
@@ -372,6 +407,23 @@ static gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, si
 	case UBX_TIM_SVIN:
 	    gpsd_report(LOG_IO, "UBX_TIM_SVIN\n");
 	    break;
+
+	case UBX_CFG_PRT:
+	    gpsd_report(LOG_IO, "UBX_CFG_PRT\n");
+	    for(i=6;i<26;i++)
+		original_port_settings[i-6] = buf[i];				/* copy the original port settings */
+	    buf[14+6] &= ~0x02;							/* turn off NMEA output on this port */
+	    ubx_write(session->gpsdata.gps_fd, 0x06, 0x00, &buf[6], 20);	/* send back with all other settings intact */
+	    have_port_configuration = true;
+	    break;
+
+	case UBX_ACK_NAK:
+	    gpsd_report(LOG_IO, "UBX_ACK_NAK, class: %02x, id: %02x\n", buf[6], buf[7]);
+	    break;
+	case UBX_ACK_ACK:
+	    gpsd_report(LOG_IO, "UBX_ACK_ACK, class: %02x, id: %02x\n", buf[6], buf[7]);
+	    break;
+
     default:
 	gpsd_report(LOG_WARN, "UBX: unknown packet id 0x%04hx (length %d) %s\n", 
 	    msgid, len, gpsd_hexdump(buf, len));
@@ -403,26 +455,180 @@ static gps_mask_t parse_input(struct gps_device_t *session)
 	return 0;
 }
 
+void ubx_catch_model(struct gps_device_t *session, unsigned char *buf, size_t len)
+{
+    unsigned char *ip = &buf[19];
+    unsigned char *op = (unsigned char *)session->subtype;
+    int end = ((len - 19) < 63)?(len - 19):63;
+    int i;
+
+    for(i=0;i<end;i++) {
+	if((*ip == 0x00) || (*ip == '*')) {
+	    *op = 0x00;
+	    break;
+	}
+	*(op++) = *(ip++);
+    }
+}
+
+bool ubx_write(int fd, unsigned char msg_class, unsigned char msg_id, unsigned char *msg, unsigned short data_len) {
+   unsigned char CK_A, CK_B;
+   unsigned char head_tail[8];
+   unsigned int i, count;
+   bool      ok;
+
+   head_tail[0] = 0xb5;
+   head_tail[1] = 0x62;
+
+   /* calculate CRC */
+   CK_A = CK_B = 0;
+   head_tail[2] = msg_class;
+   head_tail[3] = msg_id;
+   head_tail[4] = data_len & 0xff;
+   head_tail[5] = (data_len >> 8) & 0xff;
+
+   for (i = 2; i < 6; i++) {
+	CK_A += head_tail[i];
+	CK_B += CK_A;
+   }
+
+   for (i = 0; i < data_len; i++) {
+	CK_A += msg[i];
+	CK_B += CK_A;
+   }
+
+   head_tail[6] = CK_A;
+   head_tail[7] = CK_B;
+
+   gpsd_report(LOG_IO, "=> GPS: UBX class: %02x, id: %02x, len: %d, data:%s, crc: %02x%02x\n",
+                       msg_class, msg_id, data_len, gpsd_hexdump(msg, data_len), CK_A, CK_B);
+
+   count = write(fd, head_tail, 6);
+   (void)tcdrain(fd);
+   if(data_len)
+	count += write(fd, msg, data_len);
+   (void)tcdrain(fd);
+   count += write(fd, &head_tail[6], 2);
+
+   ok = (count == ((unsigned int)data_len + 8));
+   (void)tcdrain(fd);
+   return(0);
+}
+
+#ifdef ALLOW_RECONFIGURE
+static void ubx_configure(struct gps_device_t *session, unsigned int seq)
+{
+    unsigned char msg[32];
+
+    gpsd_report(LOG_IO, "UBX configure: %d\n",seq);
+
+    ubx_write(session->gpsdata.gps_fd, 0x06, 0x00, NULL, 0);	/* get This port's settings */
+
+    msg[0] = 0x03; /* SBAS mode enabled, accept testbed mode */
+    msg[1] = 0x07; /* SBAS usage: range, differential corrections and integrity */
+    msg[2] = 0x03; /* use the maximun search range: 3 channels */
+    msg[3] = 0x00; /* PRN numbers to search for all set to 0 => auto scan */
+    msg[4] = 0x00;
+    msg[5] = 0x00;
+    msg[6] = 0x00;
+    msg[7] = 0x00;
+    ubx_write(session->gpsdata.gps_fd, 0x06, 0x16, msg, 8);
+
+    msg[0] = 0x01; /* class */
+    msg[1] = 0x04; /* msg id  = UBX_NAV_DOP */
+    msg[2] = 0x01; /* rate */
+    ubx_write(session->gpsdata.gps_fd, 0x06, 0x01, msg, 3);
+    msg[0] = 0x01; /* class */
+    msg[1] = 0x06; /* msg id  = NAV-SOL */
+    msg[2] = 0x01; /* rate */
+    ubx_write(session->gpsdata.gps_fd, 0x06, 0x01, msg, 3);
+    msg[0] = 0x01; /* class */
+    msg[1] = 0x20; /* msg id  = UBX_NAV_TIMEGPS */
+    msg[2] = 0x01; /* rate */
+    ubx_write(session->gpsdata.gps_fd, 0x06, 0x01, msg, 3);
+    msg[0] = 0x01; /* class */
+    msg[1] = 0x30; /* msg id  = NAV-SVINFO */
+    msg[2] = 0x0a; /* rate */
+    ubx_write(session->gpsdata.gps_fd, 0x06, 0x01, msg, 3);
+    msg[0] = 0x01; /* class */
+    msg[1] = 0x32; /* msg id  = NAV-SBAS */
+    msg[2] = 0x0a; /* rate */
+    ubx_write(session->gpsdata.gps_fd, 0x06, 0x01, msg, 3);
+
+}
+
+static void ubx_revert(struct gps_device_t *session)
+{
+    unsigned char msg[4] = {
+	0x00, 0x00,	/* hotstart */
+	0x01,		/* controlled software reset */
+	0x00};		/* reserved */
+
+    gpsd_report(LOG_IO, "UBX revert\n");
+
+/* Reverting all in one fast and reliable reset */
+    ubx_write(session->gpsdata.gps_fd, 0x06, 0x04, msg, 4); /* CFG-RST */
+}
+#endif /* ALLOW_RECONFIGURE */
+
+static void ubx_nmea_mode(struct gps_device_t *session, int mode)
+{
+    int i;
+    unsigned char buf[20];
+
+    if(!have_port_configuration)
+	return;
+
+    for(i=0;i<22;i++)
+	buf[i] = original_port_settings[i];	/* copy the original port settings */
+    if(buf[0] == 0x01)				/* set baudrate on serial port only */
+	putlelong(buf, 8, session->gpsdata.baudrate);
+
+    if (mode == 0) {
+	buf[14] &= ~0x01;			/* turn off UBX output on this port */
+	buf[14] |=  0x02;			/* turn on NMEA output on this port */
+    } else {
+	buf[14] &= ~0x02;			/* turn off NMEA output on this port */
+	buf[14] |=  0x01;			/* turn on UBX output on this port */
+    }
+    ubx_write(session->gpsdata.gps_fd, 0x06, 0x00, &buf[6], 20);	/* send back with all other settings intact */
+}
+
+static bool ubx_speed(struct gps_device_t *session, speed_t speed)
+{
+    int i;
+    unsigned char buf[20];
+
+    if((!have_port_configuration) || (buf[0] != 0x01))	/* set baudrate on serial port only */
+	return false;
+
+    for(i=0;i<22;i++)
+	buf[i] = original_port_settings[i];	/* copy the original port settings */
+    putlelong(buf, 8, speed);
+    ubx_write(session->gpsdata.gps_fd, 0x06, 0x00, &buf[6], 20);	/* send back with all other settings intact */
+    return true;
+}
+
 /* This is everything we export */
 struct gps_type_t ubx_binary = {
     .type_name        = "uBlox UBX",    /* Full name of type */
     .trigger          = NULL,           /* Response string that identifies device (not active) */
-    .channels         = 16,             /* Number of satellite channels supported by the device */
+    .channels         = 50,             /* Number of satellite channels supported by the device */
     .probe_detect     = NULL,           /* Startup-time device detector */
     .probe_wakeup     = NULL,           /* Wakeup to be done before each baud hunt */
     .probe_subtype    = NULL,           /* Initialize the device and get subtype */
 #ifdef ALLOW_RECONFIGURE
-    .configurator     = NULL,           /* Enable what reports we need */
+    .configurator     = ubx_configure,  /* Enable what reports we need */
 #endif /* ALLOW_RECONFIGURE */
     .get_packet       = generic_get,    /* Packet getter (using default routine) */
     .parse_packet     = parse_input,    /* Parse message packets */
     .rtcm_writer      = NULL,           /* RTCM handler (using default routine) */
-    .speed_switcher   = NULL,           /* Speed (baudrate) switch */
-    .mode_switcher    = NULL,           /* Switch to NMEA mode */
+    .speed_switcher   = ubx_speed,      /* Speed (baudrate) switch */
+    .mode_switcher    = ubx_nmea_mode,  /* Switch to NMEA mode */
     .rate_switcher    = NULL,           /* Message delivery rate switcher */
     .cycle_chars      = -1,             /* Number of chars per report cycle */
 #ifdef ALLOW_RECONFIGURE
-    .revert           = NULL,           /* Undo the actions of .configurator */
+    .revert           = ubx_revert,     /* Undo the actions of .configurator */
 #endif /* ALLOW_RECONFIGURE */
     .wrapup           = NULL,           /* Puts device back to original settings */
     .cycle            = 1               /* Number of updates per second */