Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

This commit is contained in:
Jonathan Challinger 2014-04-14 15:23:12 -07:00 committed by Andrew Tridgell
parent 416e9457ce
commit 0bb7b8e938
2 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -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) {

3
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -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?