// check_gps - check if new gps readings have arrived and use them to correct position estimates
void AP_InertialNav::check_gps()
{
const uint32_t now = hal.scheduler->millis();
// compare gps time to previous reading
const AP_GPS &gps = _ahrs.get_gps();
if(gps.last_fix_time_ms() != _gps_last_time ) {
// call position correction method
correct_with_gps(now, gps.location().lng, gps.location().lat);
// record gps time and system time of this update
_gps_last_time = gps.last_fix_time_ms();
}else{
// if GPS updates stop arriving degrade position error to 10% over 2 seconds (assumes 100hz update rate)
if (now - _gps_last_update > AP_INTERTIALNAV_GPS_TIMEOUT_MS) {
_position_error.x *= 0.9886;
_position_error.y *= 0.9886;
// increment error count
if (_flags.ignore_error == 0 && _error_count < 255 && _xy_enabled) {
_error_count++;
}
}
}
}
// check_gps - check if new gps readings have arrived and use them to correct position estimates
void AP_InertialNav::check_gps()
{
const uint32_t now = hal.scheduler->millis();
// compare gps time to previous reading
if( _gps != NULL && _gps->last_fix_time != _gps_last_time ) {
// call position correction method
correct_with_gps(now, _gps->longitude, _gps->latitude);
// record gps time and system time of this update
_gps_last_time = _gps->last_fix_time;
}else{
// clear position error if GPS updates stop arriving
if (now - _gps_last_update > AP_INTERTIALNAV_GPS_TIMEOUT_MS) {
_position_error.x = 0;
_position_error.y = 0;
}
}
}
