From 943a1d8c8db1732e59c56963e6397e6005cbcd8d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 5 May 2013 12:47:49 +1000
Subject: [PATCH] AP_AHRS: added AHRS_GPS_MINSATS option

if the number of visible satellites is below AHRS_GPS_MINSATS then
don't use the GPS for acceleration correction for attitude
---
 libraries/AP_AHRS/AP_AHRS.cpp     |  7 +++++++
 libraries/AP_AHRS/AP_AHRS.h       |  1 +
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 31 ++++++++++++++++++++-----------
 3 files changed, 28 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
index 2a88409b0b..f61283d28b 100644
--- a/libraries/AP_AHRS/AP_AHRS.cpp
+++ b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -84,6 +84,13 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
     // @Increment: .01
     AP_GROUPINFO("COMP_BETA",  10, AP_AHRS, beta, 0.1f),
 
+    // @Param: GPS_MINSATS
+    // @DisplayName: AHRS GPS Minimum satellites
+    // @Description: Minimum number of satellites visible to use GPS for velocity based corrections attitude correction. This defaults to 6, which is about the point at which the velocity numbers from a GPS become too unreliable for accurate correction of the accelerometers.
+    // @Range: 0 10
+    // @User: Advanced
+    AP_GROUPINFO("GPS_MINSATS", 11, AP_AHRS, _gps_minsats, 6),
+
     AP_GROUPEND
 };
 
diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 38b60d1067..f2bb0d0506 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -169,6 +169,7 @@ public:
     AP_Int8 _gps_use;
     AP_Int8 _wind_max;
     AP_Int8 _board_orientation;
+    AP_Int8 _gps_minsats;
 
     // for holding parameters
     static const struct AP_Param::GroupInfo var_info[];
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index b72cf38072..47027fc1d0 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -415,6 +415,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
     Matrix3f temp_dcm = _dcm_matrix;
     Vector3f velocity;
     uint32_t last_correction_time;
+    bool gps_based_velocity = false;
 
     // perform yaw drift correction if we have a new yaw reference
     // vector
@@ -433,7 +434,9 @@ AP_AHRS_DCM::drift_correction(float deltat)
     // we have integrated over
     _ra_deltat += deltat;
 
-    if (!have_gps()) {
+    if (!have_gps() || 
+        _gps->status() < GPS::GPS_OK_FIX_3D || 
+        _gps->num_sats < _gps_minsats) {
         // 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.
@@ -492,6 +495,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
         Vector3f airspeed = velocity - _wind;
         airspeed.z = 0;
         _last_airspeed = airspeed.length();
+
+        gps_based_velocity = true;
     }
 
     // see if this is our first time through - in which case we
@@ -505,16 +510,20 @@ AP_AHRS_DCM::drift_correction(float deltat)
     // equation 9: get the corrected acceleration vector in earth frame. Units
     // are m/s/s
     Vector3f GA_e;
-    float v_scale = gps_gain.get()/(_ra_deltat*GRAVITY_MSS);
-    Vector3f vdelta = (velocity - _last_velocity) * v_scale;
-    // limit vertical acceleration correction to 0.5 gravities. The
-    // barometer sometimes gives crazy acceleration changes. 
-    vdelta.z = constrain_float(vdelta.z, -0.5f, 0.5f);
-    GA_e = Vector3f(0, 0, -1.0f) + vdelta;
-    GA_e.normalize();
-    if (GA_e.is_inf()) {
-        // wait for some non-zero acceleration information
-        return;
+    GA_e = Vector3f(0, 0, -1.0f);
+    
+    if (gps_based_velocity || _fly_forward) {
+        float v_scale = gps_gain.get()/(_ra_deltat*GRAVITY_MSS);
+        Vector3f vdelta = (velocity - _last_velocity) * v_scale;
+        // limit vertical acceleration correction to 0.5 gravities. The
+        // barometer sometimes gives crazy acceleration changes. 
+        vdelta.z = constrain_float(vdelta.z, -0.5f, 0.5f);
+        GA_e += vdelta;
+        GA_e.normalize();
+        if (GA_e.is_inf()) {
+            // wait for some non-zero acceleration information
+            return;
+        }
     }
 
     // calculate the error term in earth frame.