From 0bb7b8e93837535c0e7010f7faaeea46baf4cc41 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 14 Apr 2014 15:23:12 -0700
Subject: [PATCH] AP_GPS: Fix bug that causes permanent lag if we miss a
message
---
libraries/AP_GPS/AP_GPS_UBLOX.cpp | 4 +++-
libraries/AP_GPS/AP_GPS_UBLOX.h | 3 +++
2 files changed, 6 insertions(+), 1 deletion(-)
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
index b2af246d1..e6e8552dd 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp
+++ b/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) {
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h
index 7192e1988..fec842005 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.h
+++ b/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?
--
GitLab