Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: fixed position estimate with 2D fix or low satellite count 

we should still use the GPS for position fixes when we have a low
number of satellites, but we should stop using it for velocity
estimates and attitude correction.
This commit is contained in:
Andrew Tridgell 2013-05-07 10:52:14 +10:00
parent 055da3c4b6
commit 6f1cee6406
1 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -461,10 +461,6 @@ AP_AHRS_DCM::drift_correction(float deltat)
last_correction_time = hal.scheduler->millis(); last_correction_time = hal.scheduler->millis();
_have_gps_lock = false; _have_gps_lock = false;
// update position delta for get_position()
_position_offset_north += velocity.x * _ra_deltat;
_position_offset_east += velocity.y * _ra_deltat;
} else { } else {
if (_gps->last_fix_time == _ra_sum_start) { if (_gps->last_fix_time == _ra_sum_start) {
// we don't have a new GPS fix - nothing more to do // we don't have a new GPS fix - nothing more to do
@ -479,20 +475,26 @@ AP_AHRS_DCM::drift_correction(float deltat)
} }
_have_gps_lock = true; _have_gps_lock = true;
// remember position for get_position() // keep last airspeed estimate for dead-reckoning purposes
Vector3f airspeed = velocity - _wind;
airspeed.z = 0;
_last_airspeed = airspeed.length();
}
if (have_gps()) {
// use GPS for positioning with any fix, even a 2D fix
_last_lat = _gps->latitude; _last_lat = _gps->latitude;
_last_lng = _gps->longitude; _last_lng = _gps->longitude;
_position_offset_north = 0; _position_offset_north = 0;
_position_offset_east = 0; _position_offset_east = 0;
// once we have a single GPS lock, we update using // once we have a single GPS lock, we can update using
// dead-reckoning from then on // dead-reckoning from then on
_have_position = true; _have_position = true;
} else {
// keep last airspeed estimate for dead-reckoning purposes // update dead-reckoning position estimate
Vector3f airspeed = velocity - _wind; _position_offset_north += velocity.x * _ra_deltat;
airspeed.z = 0; _position_offset_east += velocity.y * _ra_deltat;
_last_airspeed = airspeed.length();
} }
// see if this is our first time through - in which case we // see if this is our first time through - in which case we