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

AP_InertialNav: bug fix for gps delay handling

Browse Source
This commit is contained in:
Randy Mackay 2013-10-25 17:49:43 +09:00
parent 01074f9839
commit 16baa7557c
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -175,9 +175,9 @@ void AP_InertialNav::correct_with_gps(uint32_t now, int32_t lon, int32_t lat)
}else{
// ublox gps positions are delayed by 400ms
// we store historical position at 10hz so 4 iterations ago
if( _hist_position_estimate_x.size() >= AP_INTERTIALNAV_GPS_LAG_IN_10HZ_INCREMENTS ) {
hist_position_base_x = _hist_position_estimate_x.peek(AP_INTERTIALNAV_GPS_LAG_IN_10HZ_INCREMENTS-1);
hist_position_base_y = _hist_position_estimate_y.peek(AP_INTERTIALNAV_GPS_LAG_IN_10HZ_INCREMENTS-1);
if( _hist_position_estimate_x.is_full()) {
hist_position_base_x = _hist_position_estimate_x.front();
hist_position_base_y = _hist_position_estimate_y.front();
}else{
hist_position_base_x = _position_base.x;
hist_position_base_y = _position_base.y;