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

AP_NavEKF: only call calcGpsGoodToAlign if we need to

Browse Source 
avoid calling it once we have an origin. This avoids some calculations
and string operations
This commit is contained in:
Andrew Tridgell 2015-09-08 15:56:28 +10:00
parent dff9fe9cb2
commit 0ca534bfab
1 changed files with 1 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4223,7 +4223,6 @@ void NavEKF::readIMUData()
// check for new valid GPS data and update stored measurement if available
void NavEKF::readGpsData()
{
bool goodToAlign = false;
// check for new GPS data
if ((_ahrs->get_gps().last_message_time_ms() != lastFixTime_ms) &&
(_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D))
@ -4272,15 +4271,12 @@ void NavEKF::readGpsData()
useGpsVertVel = false;
}
// Monitor quality of the GPS velocity data for alignment
goodToAlign = calcGpsGoodToAlign();
// read latitutde and longitude from GPS and convert to local NE position relative to the stored origin
// If we don't have an origin, then set it to the current GPS coordinates
const struct Location &gpsloc = _ahrs->get_gps().location();
if (validOrigin) {
gpsPosNE = location_diff(EKF_origin, gpsloc);
} else if (goodToAlign){
} else if (calcGpsGoodToAlign()){
// Set the NE origin to the current GPS position
setOrigin();
// Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly