From 9b6acf168eb1f1b390e662d2713ffb3f69f69ea8 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 10 Jul 2012 16:22:48 +1000
Subject: [PATCH] AHRS: added AHRS_GPS_GAIN parameter

this allows control of how much GPS velocity information is used in
doing centripetal correction in DCM
---
 ArduCopter/Parameters.pde         |  3 +++
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 24 +++++++++++++++++++-----
 libraries/AP_AHRS/AP_AHRS_DCM.h   |  2 ++
 3 files changed, 24 insertions(+), 5 deletions(-)

diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde
index 52febe84f9..e8f370b422 100644
--- a/ArduCopter/Parameters.pde
+++ b/ArduCopter/Parameters.pde
@@ -314,6 +314,9 @@ static void load_parameters(void)
 		}
 	}
 
+	// change the default for the AHRS_GPS_GAIN for ArduCopter
+	ahrs.gps_gain.set(0.0);
+
 	if (!g.format_version.load() ||
 	    g.format_version != Parameters::k_format_version) {
 
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index df0122d797..7a920fbfcb 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -31,13 +31,26 @@
 
 // table of user settable parameters
 const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
-	// @Param: YAW_P
-	// @DisplayName: Yaw P
-	// @Description: This controls the weight the compass has on the overall heading
-	// @Range: 0 .4
-	// @Increment: .01
+    // @Param: AHRS_YAW_P
+    // @DisplayName: Yaw P
+    // @Description: This controls the weight the compass has on the overall heading
+    // @Range: 0 .4
+    // @Increment: .01
     AP_GROUPINFO("YAW_P", 0, AP_AHRS_DCM, _kp_yaw),
+
+    // @Param: AHRS_RP_P
+    // @DisplayName: AHRS RP_P
+    // @Description: This controls how fast the accelerometers correct the attitude
+    // @Range: 0 .4
+    // @Increment: .01
     AP_GROUPINFO("RP_P",  1, AP_AHRS_DCM, _kp),
+
+    // @Param: AHRS_GPS_GAIN
+    // @DisplayName: AHRS GPS gain
+    // @Description: This controls how how much to use the GPS to correct the attitude
+    // @Range: 0.0 1.0
+    // @Increment: .01
+    AP_GROUPINFO("GPS_GAIN",  2, AP_AHRS_DCM, gps_gain),
     AP_GROUPEND
 };
 
@@ -437,6 +450,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
     // are m/s/s
     Vector3f GA_e;
     float v_scale = 1.0/(_ra_deltat*_gravity);
+    v_scale *= gps_gain;
     GA_e = Vector3f(0, 0, -1.0) + ((velocity - _last_velocity) * v_scale);
     GA_e.normalize();
     if (GA_e.is_inf()) {
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index 4443634ccc..136fa4a8f0 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -25,6 +25,7 @@ public:
 
                 _kp.set(0.4);
 		_kp_yaw.set(0.4);
+		gps_gain.set(1.0);
 	}
 
 	// return the smoothed gyro vector corrected for drift
@@ -45,6 +46,7 @@ public:
 	// settable parameters
 	AP_Float	_kp_yaw;
 	AP_Float	_kp;
+	AP_Float	gps_gain;
 
 private:
 	float		_ki;