Leap second fix for U-Blox message UBX_NAV_TIMELS

2 files changed, 203 insertions, 1 deletions

diff --git a/recipes-navigation/gpsd/gpsd/0006-ubxtimelps.patch b/recipes-navigation/gpsd/gpsd/0006-ubxtimelps.patch
new file mode 100644
index 0000000..e715847
--- /dev/null
+++ b/recipes-navigation/gpsd/gpsd/0006-ubxtimelps.patch
@@ -0,0 +1,200 @@
+diff -Naur old/driver_ubx.c new/driver_ubx.c
+--- old/driver_ubx.c	2017-01-19 14:12:25.099231346 -0600
++++ new/driver_ubx.c	2017-01-19 15:13:58.552152991 -0600
+@@ -57,6 +57,29 @@
+ #define UBX_CFG_LEN		20
+ #define outProtoMask		14
+ 
++#ifdef UBLOXTIMELS_ENABLE
++/* UBX-NAV-TIMELS support */
++static char *srcOfCurrLs[] = {
++  "firmware",
++  "GPS GLONASS difference",
++  "GPS",
++  "SBAS",
++  "BeiDou",
++  "Galileo",
++  "Aided data",
++  "Configured"
++};
++static char *srcOfLsChange[] = {
++  "No Source",
++  "Undefined",
++  "GPS",
++  "SBAS",
++  "BeiDou",
++  "Galileo",
++  "GLONOSS",
++};
++#endif  /* UBLOXTIMELS_ENABLE */
++
+ static gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf,
+ 			    size_t len);
+ static gps_mask_t ubx_msg_nav_dop(struct gps_device_t *session,
+@@ -237,6 +260,89 @@
+     return mask;
+ }
+ 
++#ifdef UBLOXTIMELS_ENABLE
++/**
++ * Navigation time to leap second
++ * 
++ * Sets leap_notify if leap second is < 23 hours away.
++ */
++static void
++ubx_msg_nav_timels(struct gps_device_t *session, unsigned char *buf,
++		size_t data_len)
++{
++    int version;
++    unsigned int flags;
++    int valid_curr_ls;
++    int valid_time_to_ls_event;
++    
++    if (data_len != 24) {
++        gpsd_log(&session->context->errout, LOG_WARN, 
++	  "UBX-NAV-TIMELS: unexpected length %d, expecting 24\n",
++	  data_len);
++	return;
++    }
++    version = getsb(buf,4);
++    /* Only version 0 is defined so far. */
++    flags = (unsigned int)getub(buf, 23);
++    gpsd_log(&session->context->errout, LOG_DATA,
++        "UBX-NAV-TIMELS: flags 0x%x message version %d\n",flags, version);
++    valid_curr_ls = flags & UBX_TIMELS_VALID_CURR_LS;
++    valid_time_to_ls_event = flags & UBX_TIMELS_VALID_TIME_LS_EVT;
++    if(valid_curr_ls) {
++      unsigned int src_of_curr_ls = getub(buf,8);
++      int curr_ls = getsb(buf,9);
++      char *src = "Unknown";
++      
++      if(src_of_curr_ls < (sizeof srcOfCurrLs/(sizeof srcOfCurrLs[0])))
++	src = srcOfCurrLs[src_of_curr_ls];
++	
++      gpsd_log(&session->context->errout, LOG_DATA,
++	     "UBX-NAV-TIMELS: source_of_current_leapsecond=%u:%s curr_ls=%d\n",
++	     src_of_curr_ls,src,curr_ls);
++      session->context->leap_seconds = curr_ls;
++      session->context->valid |= LEAP_SECOND_VALID;
++    } /* Valid current leap second */
++    if(valid_time_to_ls_event) {
++      char *src="Unknown";
++      unsigned int src_of_ls_change;
++      unsigned short dateOfLSGpsWn, dateOfLSGpsDn;
++      int lsChange = getsb(buf,11);
++      int timeToLsEvent = getles32(buf,12);
++      src_of_ls_change = getub(buf,10);
++      if(src_of_ls_change < (sizeof srcOfLsChange/(sizeof srcOfLsChange[0])))
++	src = srcOfLsChange[src_of_ls_change];
++      dateOfLSGpsWn = getles16(buf,16);
++      dateOfLSGpsDn = getles16(buf,18);
++      gpsd_log(&session->context->errout, LOG_DATA,
++	 "UBX_NAV_TIMELS: source_of_leapsecond_change=%u:%s "
++	 "leapSecondChage=%d timeToLsEvent=%d\n",
++	 src_of_ls_change,src,lsChange,timeToLsEvent);
++      gpsd_log(&session->context->errout, LOG_DATA,
++	       "UBX_NAV_TIMELS: dateOfLSGpsWn=%d dateOfLSGpsDn=%d\n",
++	       dateOfLSGpsWn,dateOfLSGpsDn);
++      if(timeToLsEvent < 60*60*23 && timeToLsEvent > 0) {
++	if(lsChange == 0)
++	   session->context->leap_notify = LEAP_NOWARNING;
++	else if (lsChange == 1)
++	  session->context->leap_notify = LEAP_ADDSECOND;
++	else if (lsChange == -1)
++	  session->context->leap_notify = LEAP_DELSECOND;
++      } else
++	session->context->leap_notify = LEAP_NOWARNING;
++
++      if (session->context->leap_notify == LEAP_ADDSECOND)
++	gpsd_log(&session->context->errout,LOG_INF,
++		"UBX_NAV_TIMELS: Add leap second today\n");
++      else if (session->context->leap_notify == LEAP_DELSECOND)
++	gpsd_log(&session->context->errout,LOG_INF,
++		 "UBX_NAV_TIMELS: Remove leap second today\n");
++      
++      gpsd_log(&session->context->errout, LOG_DATA,
++	       "UBX_NAV_TIMELS: leaving: leap_notify=%d\n",
++	session->context->leap_notify);
++    }
++}
++#endif  /* UBLOXTIMELS_ENABLE */
+  /**
+  * Geodetic position solution message
+  */
+@@ -523,13 +629,22 @@
+     case UBX_NAV_EKFSTATUS:
+ 	gpsd_log(&session->context->errout, LOG_DATA, "UBX_NAV_EKFSTATUS\n");
+ 	break;
+-
++    case UBX_NAV_TIMELS:
++    gpsd_log(&session->context->errout, LOG_DATA, "UBX_NAV_TIMELS\n");
++#ifdef UBLOXTIMELS_ENABLE
++	ubx_msg_nav_timels(session, &buf[UBX_PREFIX_LEN],data_len);
++#endif // UBLOXTIMELS_ENABLE
++	break;
+     case UBX_RXM_RAW:
+ 	gpsd_log(&session->context->errout, LOG_DATA, "UBX_RXM_RAW\n");
+ 	break;
+     case UBX_RXM_SFRB:
++	gpsd_log(&session->context->errout, LOG_DATA, "UBX_RXM_SFRB\n");
+ 	mask = ubx_msg_sfrb(session, &buf[UBX_PREFIX_LEN]);
+ 	break;
++    case UBX_RXM_SFRBX:
++	gpsd_log(&session->context->errout, LOG_DATA, "UBX_RXM_SFRBX\n");
++	break;
+     case UBX_RXM_SVSI:
+ 	gpsd_log(&session->context->errout, LOG_PROG, "UBX_RXM_SVSI\n");
+ 	break;
+@@ -955,6 +1070,12 @@
+ 	msg[1] = 0x20;		/* msg id  = UBX_NAV_TIMEGPS */
+ 	msg[2] = 0x01;		/* rate */
+ 	(void)ubx_write(session, 0x06u, 0x01, msg, 3);
++#ifdef UBLOXTIMELS_ENABLE
++	msg[0] = 0x01;		/* class */
++	msg[1] = 0x26;		/* msg id  = UBX_NAV_TIMELS */
++	msg[2] = 0xff;		/* rate */
++#endif /* UBLOXTIMELS_ENABLE */
++	(void)ubx_write(session, 0x06u, 0x01, msg, 3);
+ 	msg[0] = 0x01;		/* class */
+ 	msg[1] = 0x30;		/* msg id  = NAV-SVINFO */
+ 	msg[2] = 0x0a;		/* rate */
+diff -Naur old/driver_ubx.h new/driver_ubx.h
+--- old/driver_ubx.h	2017-01-19 14:12:25.099231346 -0600
++++ new/driver_ubx.h	2017-01-19 14:01:48.177653001 -0600
+@@ -35,6 +35,7 @@
+     UBX_NAV_TIMEGPS	= UBX_MSGID(UBX_CLASS_NAV, 0x20),
+     UBX_NAV_TIMEUTC	= UBX_MSGID(UBX_CLASS_NAV, 0x21),
+     UBX_NAV_CLOCK	= UBX_MSGID(UBX_CLASS_NAV, 0x22),
++    UBX_NAV_TIMELS	= UBX_MSGID(UBX_CLASS_NAV, 0x26),
+     UBX_NAV_SVINFO	= UBX_MSGID(UBX_CLASS_NAV, 0x30),
+     UBX_NAV_DGPS	= UBX_MSGID(UBX_CLASS_NAV, 0x31),
+     UBX_NAV_SBAS	= UBX_MSGID(UBX_CLASS_NAV, 0x32),
+@@ -42,6 +43,7 @@
+ 
+     UBX_RXM_RAW		= UBX_MSGID(UBX_CLASS_RXM, 0x10),
+     UBX_RXM_SFRB	= UBX_MSGID(UBX_CLASS_RXM, 0x11),
++    UBX_RXM_SFRBX	= UBX_MSGID(UBX_CLASS_RXM, 0x13),
+     UBX_RXM_SVSI	= UBX_MSGID(UBX_CLASS_RXM, 0x20),
+     UBX_RXM_ALM		= UBX_MSGID(UBX_CLASS_RXM, 0x30),
+     UBX_RXM_EPH		= UBX_MSGID(UBX_CLASS_RXM, 0x31),
+@@ -102,6 +104,9 @@
+ #define UBX_SOL_VALID_WEEK 0x04
+ #define UBX_SOL_VALID_TIME 0x08
+ 
++#define UBX_TIMELS_VALID_CURR_LS 0x01
++#define UBX_TIMELS_VALID_TIME_LS_EVT 0x01
++
+ /* from UBX_NAV_SVINFO */
+ #define UBX_SAT_USED 0x01
+ #define UBX_SAT_DGPS 0x02
+diff -Naur old/SConstruct new/SConstruct
+--- old/SConstruct	2017-01-19 14:51:35.638466569 -0600
++++ new/SConstruct	2017-01-19 15:20:02.488159765 -0600
+@@ -113,6 +113,7 @@
+     ("tripmate",      True,  "DeLorme TripMate support"),
+     ("tsip",          True,  "Trimble TSIP support"),
+     ("ublox",         True,  "u-blox Protocol support"),
++    ("ubloxtimels",   False, "u-blox UBX-NAV-TIMELS support (leap second)"),
+     ("fury",          True,  "Jackson Labs Fury and Firefly support"),
+     ("nmea2000",      True,  "NMEA2000/CAN support"),
+     # Non-GPS protocols
diff --git a/recipes-navigation/gpsd/gpsd_3.16.bb b/recipes-navigation/gpsd/gpsd_3.16.bb
index 7e573f8..52aa9a9 100644
--- a/recipes-navigation/gpsd/gpsd_3.16.bb
+++ b/recipes-navigation/gpsd/gpsd_3.16.bb
@@ -4,7 +4,7 @@ LICENSE = "BSD"
 LIC_FILES_CHKSUM = "file://COPYING;md5=d217a23f408e91c94359447735bc1800"
 DEPENDS = "dbus dbus-glib ncurses python libusb1 chrpath-replacement-native pps-tools"
 PROVIDES = "virtual/gpsd"
-PR="m1"
+PR="m2"
 
 EXTRANATIVEPATH += "chrpath-native"
 
@@ -14,6 +14,7 @@ SRC_URI = "${SAVANNAH_GNU_MIRROR}/${BPN}/${BP}.tar.gz \
     file://0004-SConstruct-disable-html-and-man-docs-building-becaus.patch \
     file://0001-include-sys-ttydefaults.h.patch \
     file://0005-suppress-text-in-binary.patch \
+    file://0006-ubxtimelps.patch \
     file://gpsd-default \
     file://gpsd \
     file://gpsd_ubx_fixed.sh \
@@ -58,6 +59,7 @@ EXTRA_OESCONS = " \
     systemd='${SYSTEMD_OESCONS}' \
     libdir='${libdir}' \
     ublox='yes' \
+    ubloxtimels='yes' \
     pps='yes' \
     gpsd_group='gps' \
     ntpshm='yes' \