Skip to content

Commit

Permalink
AP_NavEKF: Prevent arming delays from failing GPS
Browse files Browse the repository at this point in the history
Due to the way that gyro calibration is done, the EKF could be effectively not run for up to 30 seconds in extreme cases, making it possible that the GPS would be failed on arming and the copter put into a non-GPS mode.

the longer term solution is to update the gyro calibration so that it does not hold up other processing. the short tyermfix in thsi patch is to look for evidence of a 3D lock in the last received GPS message.
  • Loading branch information
priseborough authored and jschall committed Jan 28, 2015
1 parent d665c78 commit 49d0646
Showing 1 changed file with 5 additions and 4 deletions.
9 changes: 5 additions & 4 deletions libraries/AP_NavEKF/AP_NavEKF.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -4006,11 +4006,12 @@ void NavEKF::readGpsData()
// calculate a position offset which is applied to NE position and velocity wherever it is used throughout code to allow GPS position jumps to be accommodated gradually
decayGpsOffset();
}
// If too long since last fix time or we are not allowed to use GPS, we declare no GPS available for use
if ((imuSampleTime_ms - lastFixTime_ms < 1000) || _fusionModeGPS == 3) {
gpsNotAvailable = false;
} else {

// If no previous GPS lock or told not to use it, we declare the GPS unavailable available for use
if ((_ahrs->get_gps().status() < AP_GPS::GPS_OK_FIX_3D) || _fusionModeGPS == 3) {
gpsNotAvailable = true;
} else {
gpsNotAvailable = false;
}
}

Expand Down

0 comments on commit 49d0646

Please sign in to comment.