AP_GPS_UBLOX: add support for TIMEGPS message. used to get gps week

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -88,6 +88,11 @@ AP_GPS_UBLOX::_request_next_config(void)
             _next_message--;
         }
         break;
+    case STEP_TIMEGPS:
+        if(!_request_message_rate(CLASS_NAV, MSG_TIMEGPS)) {
+            _next_message--;
+        }
+        break;
     case STEP_PORT:
         _request_port();
         break;
@@ -229,10 +234,11 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
             }
             break;
         case MSG_SOL:
-            if(rate == RATE_SOL) {
+            desired_rate = havePvtMsg ? 0 : RATE_SOL;
+            if(rate == desired_rate) {
                 _unconfigured_messages &= ~CONFIG_RATE_SOL;
             } else {
-                _configure_message_rate(msg_class, msg_id, RATE_SOL);
+                _configure_message_rate(msg_class, msg_id, desired_rate);
                 _unconfigured_messages |= CONFIG_RATE_SOL;
                 _cfg_needs_save = true;
             }
@@ -246,6 +252,15 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
                 _cfg_needs_save = true;
             }
             break;
+        case MSG_TIMEGPS:
+            if(rate == RATE_TIMEGPS) {
+                _unconfigured_messages &= ~CONFIG_RATE_TIMEGPS;
+            } else {
+                _configure_message_rate(msg_class, msg_id, RATE_TIMEGPS);
+                _unconfigured_messages |= CONFIG_RATE_TIMEGPS;
+                _cfg_needs_save = true;
+            }
+            break;
         case MSG_VELNED:
             desired_rate = havePvtMsg ? 0 : RATE_VELNED;
             if(rate == desired_rate) {
@@ -1072,6 +1087,12 @@ AP_GPS_UBLOX::_parse_gps(void)
         next_fix = state.status;
 #endif
         break;
+    case MSG_TIMEGPS:
+        Debug("MSG_TIMEGPS");
+        _check_new_itow(_buffer.timegps.itow);
+        if (_buffer.timegps.valid & UBX_TIMEGPS_VALID_WEEK_MASK)
+            state.time_week = _buffer.timegps.week;
+        break;
     case MSG_VELNED:
         Debug("MSG_VELNED");
         if (havePvtMsg) {
@@ -1336,7 +1357,8 @@ static const char *reasons[] = {"navigation rate",
                                 "GNSS settings",
                                 "SBAS settings",
                                 "PVT rate",
-                                "time pulse settings"};
+                                "time pulse settings",
+                                "TIMEGPS rate"};
 
 static_assert((1 << ARRAY_SIZE(reasons)) == CONFIG_LAST, "UBLOX: Missing configuration description");
 

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -47,11 +47,14 @@
 #define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7
 #define UBX_MSG_TYPES 2
 
+#define UBX_TIMEGPS_VALID_WEEK_MASK 0x2
+
 #define UBLOX_MAX_PORTS 6
 
 #define RATE_POSLLH 1
 #define RATE_STATUS 1
 #define RATE_SOL 1
+#define RATE_TIMEGPS 5
 #define RATE_PVT 1
 #define RATE_VELNED 1
 #define RATE_DOP 1
@@ -73,7 +76,8 @@
 #define CONFIG_SBAS          (1<<12)
 #define CONFIG_RATE_PVT      (1<<13)
 #define CONFIG_TP5           (1<<14)
-#define CONFIG_LAST          (1<<15) // this must always be the last bit
+#define CONFIG_RATE_TIMEGPS  (1<<15)
+#define CONFIG_LAST          (1<<16) // this must always be the last bit
 
 #define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED)
 
@@ -289,6 +293,15 @@ private:
         uint32_t heading_accuracy;
     };
 
+    struct PACKED ubx_nav_timegps {
+        uint32_t itow;
+        int32_t ftow;
+        uint16_t week;
+        int8_t leapS;
+        uint8_t valid; //  leapsvalid | weekvalid | tow valid;
+        uint32_t tAcc;
+    };
+
     // Lea6 uses a 60 byte message
     struct PACKED ubx_mon_hw_60 {
         uint32_t pinSel;
@@ -414,6 +427,7 @@ private:
         ubx_nav_dop dop;
         ubx_nav_solution solution;
         ubx_nav_pvt pvt;
+        ubx_nav_timegps timegps;
         ubx_nav_velned velned;
         ubx_cfg_msg_rate msg_rate;
         ubx_cfg_msg_rate_6 msg_rate_6;
@@ -452,6 +466,7 @@ private:
         MSG_DOP = 0x4,
         MSG_SOL = 0x6,
         MSG_PVT = 0x7,
+        MSG_TIMEGPS = 0x20,
         MSG_VELNED = 0x12,
         MSG_CFG_CFG = 0x09,
         MSG_CFG_RATE = 0x08,
@@ -507,6 +522,7 @@ private:
         STEP_STATUS,
         STEP_POSLLH,
         STEP_VELNED,
+        STEP_TIMEGPS,
         STEP_POLL_SVINFO, // poll svinfo
         STEP_POLL_SBAS, // poll SBAS
         STEP_POLL_NAV, // poll NAV settings