AP_GPS: Fix bug that causes permanent lag if we miss a message

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -394,6 +394,7 @@ AP_GPS_UBLOX::_parse_gps(void)
     switch (_msg_id) {
     case MSG_POSLLH:
         Debug("MSG_POSLLH next_fix=%u", next_fix);
+        _last_pos_time        = _buffer.posllh.time;
         state.location.lng    = _buffer.posllh.longitude;
         state.location.lat    = _buffer.posllh.latitude;
         state.location.alt    = _buffer.posllh.altitude_msl / 10;
@@ -469,6 +470,7 @@ AP_GPS_UBLOX::_parse_gps(void)
         break;
     case MSG_VELNED:
         Debug("MSG_VELNED");
+        _last_vel_time         = _buffer.velned.time;
         state.ground_speed     = _buffer.velned.speed_2d*0.01f;          // m/s
         state.ground_course_cd = _buffer.velned.heading_2d / 1000;       // Heading 2D deg * 100000 rescaled to deg * 100
         state.have_vertical_velocity = true;
@@ -488,7 +490,7 @@ AP_GPS_UBLOX::_parse_gps(void)
 
     // we only return true when we get new position and speed data
     // this ensures we don't use stale data
-    if (_new_position && _new_speed) {
+    if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {
         _new_speed = _new_position = false;
 		_fix_count++;
         if ((hal.scheduler->millis() - _last_5hz_time) > 15000U && !need_rate_update) {

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -247,6 +247,9 @@ private:
     uint8_t			_fix_count;
     uint8_t         _class;
 
+    uint32_t        _last_vel_time;
+    uint32_t        _last_pos_time;
+
     // do we have new position information?
     bool            _new_position:1;
     // do we have new speed information?