aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS: try harder to get 5Hz updates from a uBlox

Browse Source 
check for getting at least one 5Hz update every 15s
This commit is contained in:
Andrew Tridgell 2013-11-05 15:18:25 +11:00 committed by Randy Mackay
parent 522f248b63
commit 01a32d0632
2 changed files with 12 additions and 6 deletions

16
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -326,9 +326,16 @@ AP_GPS_UBLOX::_parse_gps(void)
num_sats = _buffer.solution.satellites;
hdop = _buffer.solution.position_DOP;
if (next_fix >= GPS::FIX_2D) {
_last_gps_time = hal.scheduler->millis();
if (time_week == _buffer.solution.week &&
time_week_ms + 200 == _buffer.solution.time) {
// we got a 5Hz update. This relies on the way
// that uBlox gives timestamps that are always
// multiples of 200 for 5Hz
_last_5hz_time = _last_gps_time;
}
time_week_ms = _buffer.solution.time;
time_week = _buffer.solution.week;
_last_gps_time = hal.scheduler->millis();
}
#if UBLOX_FAKE_3DLOCK
next_fix = fix;
@ -364,16 +371,14 @@ AP_GPS_UBLOX::_parse_gps(void)
if (_new_position && _new_speed) {
_new_speed = _new_position = false;
_fix_count++;
uint32_t new_fix_time = hal.scheduler->millis();
if (!need_rate_update && new_fix_time - _last_fix_time > 300 && _fix_count % 20 == 0) {
if ((hal.scheduler->millis() - _last_5hz_time) > 15000U && !need_rate_update) {
// the GPS is running slow. It possibly browned out and
// restarted with incorrect parameters. We will slowly
// send out new parameters to fix it
need_rate_update = true;
rate_update_step = 0;
_last_5hz_time = hal.scheduler->millis();
}
_last_fix_time = new_fix_time;
if (_fix_count == 100) {
// ask for nav settings every 20 seconds
@ -475,6 +480,7 @@ AP_GPS_UBLOX::_configure_gps(void)
// start the process of updating the GPS rates
need_rate_update = true;
_last_5hz_time = hal.scheduler->millis();
rate_update_step = 0;
// ask for the current navigation settings

2
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -223,7 +223,7 @@ private:
bool need_rate_update;
uint8_t rate_update_step;
uint32_t _last_fix_time;
uint32_t _last_5hz_time;
void _configure_navigation_rate(uint16_t rate_ms);
void _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);