AP_InertialNav: updates for new GPS API

This commit is contained in:
Andrew Tridgell 2013-10-23 22:14:33 +11:00
parent b7b9efd120
commit 19ba07a3f9

View File

@ -125,13 +125,13 @@ void AP_InertialNav::check_gps()
uint32_t now = hal.scheduler->millis();
// compare gps time to previous reading
if( _gps != NULL && _gps->time != _gps_last_time ) {
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->time;
_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) {
@ -414,4 +414,4 @@ void AP_InertialNav::set_position_xy(float pos_x, float pos_y)
_historic_xy_counter = 0;
_hist_position_estimate_x.add(_position_base.x);
_hist_position_estimate_y.add(_position_base.y);
}
}