// 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; } } }