From 6a24bdec0510ba127ca98341c0f9f60a67f4d698 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Nov 2012 20:06:44 +1100 Subject: [PATCH] AP_AHRS: require at least 6 satellites to use the GPS for velocity logs of a recent flight show the velocity estimate can be very poor if the GPS can see 5 satellites or less --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index dc6153e859..a931750099 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -390,10 +390,14 @@ AP_AHRS_DCM::drift_correction(float deltat) // we have integrated over _ra_deltat += deltat; - if (!have_gps()) { - // no GPS, or no lock. We assume zero velocity. This at - // least means we can cope with gyro drift while sitting - // on a bench with no GPS lock + if (!have_gps() || _gps->num_sats < 6) { + // no GPS, or not a good lock. From experience we need at + // least 6 satellites to get a really reliable velocity number + // from the GPS. + // + // As a fallback we use the fixed wing acceleration correction + // if we have an airspeed estimate (which we only have if + // _fly_forward is set), otherwise no correction if (_ra_deltat < 0.2) { // not enough time has accumulated return;