AP_GPS: moved iTow handling to GPS_Backend

libraries/AP_GPS/AP_GPS_SBF.cpp

@@ -284,7 +284,7 @@ AP_GPS_SBF::process_message(void)
             state.time_week_ms = (uint32_t)(temp.TOW);
         }
 
-        set_uart_timestamp(sbf_msg.length);
+        check_new_itow(temp.TOW, sbf_msg.length);
         state.last_gps_time_ms = AP_HAL::millis();
 
         // Update velocity state (don't use −2·10^10)
@@ -361,6 +361,7 @@ AP_GPS_SBF::process_message(void)
     case DOP:
     {
         const msg4001 &temp = sbf_msg.data.msg4001u;
+        check_new_itow(temp.TOW, sbf_msg.length);
 
         state.hdop = temp.HDOP;
         state.vdop = temp.VDOP;
@@ -369,6 +370,7 @@ AP_GPS_SBF::process_message(void)
     case ReceiverStatus:
     {
         const msg4014 &temp = sbf_msg.data.msg4014u;
+        check_new_itow(temp.TOW, sbf_msg.length);
         RxState = temp.RxState;
         if ((RxError & RX_ERROR_MASK) != (temp.RxError & RX_ERROR_MASK)) {
             gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF error changed (0x%08x/0x%08x)", state.instance + 1,
@@ -381,6 +383,7 @@ AP_GPS_SBF::process_message(void)
     {
         const msg5908 &temp = sbf_msg.data.msg5908u;
 
+        check_new_itow(temp.TOW, sbf_msg.length);
         // select the maximum variance, as the EKF will apply it to all the columns in it's estimate
         // FIXME: Support returning the covariance matrix to the EKF
         float max_variance_squared = MAX(temp.Cov_VnVn, MAX(temp.Cov_VeVe, temp.Cov_VuVu));

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -1336,10 +1336,8 @@ void AP_GPS_UBLOX::Write_DataFlash_Log_Startup_messages() const
     }
 }
 
+// uBlox specific check_new_itow(), handling message length
 void AP_GPS_UBLOX::_check_new_itow(uint32_t itow)
 {
-    if (itow != _last_itow) {
-        _last_itow = itow;
-        set_uart_timestamp(_payload_length + sizeof(ubx_header) + 2);
-    }
+    check_new_itow(itow, _payload_length + sizeof(ubx_header) + 2);
 }

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -549,9 +549,6 @@ private:
     
     bool havePvtMsg;
 
-    // itow from previous message
-    uint32_t _last_itow;
-
     bool        _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
     void        _configure_rate(void);
     void        _configure_sbas(bool enable);

libraries/AP_GPS/GPS_Backend.cpp

@@ -227,3 +227,12 @@ void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes)
         state.uart_timestamp_ms = port->receive_time_constraint_us(nbytes) / 1000U;
     }
 }
+
+
+void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
+{
+    if (itow != _last_itow) {
+        _last_itow = itow;
+        set_uart_timestamp(msg_length);
+    }
+}

libraries/AP_GPS/GPS_Backend.h

@@ -93,4 +93,10 @@ protected:
       assuming the message started nbytes ago
      */
     void set_uart_timestamp(uint16_t nbytes);
+
+    void check_new_itow(uint32_t itow, uint32_t msg_length);
+    
+private:
+    // itow from previous message
+    uint32_t _last_itow;
 };