From 8e90aeea4efb3067ebf31b62e4c3bca71f526d1b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 16 Apr 2012 20:51:46 +1000
Subject: [PATCH] AHRS: added AHRS_YAW_P parameter

this allows users to change the yaw gain in DCM
---
 libraries/AP_AHRS/AP_AHRS.h       | 2 ++
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 8 +++++++-
 libraries/AP_AHRS/AP_AHRS_DCM.h   | 6 ++++--
 3 files changed, 13 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 883c158a53..10e2b90381 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -82,6 +82,8 @@ public:
 	// attitude
 	virtual Matrix3f get_dcm_matrix(void) = 0;
 
+	static const struct AP_Param::GroupInfo var_info[];
+
 protected:
 	// pointer to compass object, if enabled
 	Compass 	* _compass;
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index b6fa2e1f45..2cf29b3249 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -23,6 +23,12 @@
 // from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
 #define GPS_SPEED_RESET 100
 
+// table of user settable parameters
+const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
+    AP_GROUPINFO("YAW_P", 0, AP_AHRS_DCM, _kp_yaw),
+    AP_GROUPEND
+};
+
 // run a full DCM update round
 void
 AP_AHRS_DCM::update(void)
@@ -453,7 +459,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
 	// allow the yaw reference source to affect all 3 components
 	// of _omega_yaw_P as we need to be able to correctly hold a
 	// heading when roll and pitch are non-zero
-	_omega_yaw_P = error * _kp_yaw;
+	_omega_yaw_P = error * _kp_yaw.get();
 
 	// add yaw correction to integrator correction vector, but
 	// only for the z gyro. We rely on the accelerometers for x
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index 5952380c7e..600592d8e5 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -17,7 +17,7 @@ public:
 	AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
 	{
 		_kp_roll_pitch = 0.13;
-		_kp_yaw        = 0.4;
+		_kp_yaw.set(0.4);
 		_dcm_matrix(Vector3f(1, 0, 0),
 			    Vector3f(0, 1, 0),
 			    Vector3f(0, 0, 1));
@@ -42,10 +42,12 @@ public:
 	float		get_error_rp(void);
 	float		get_error_yaw(void);
 
+	// settable parameters
+	AP_Float	_kp_yaw;
+
 private:
 	float		_kp_roll_pitch;
 	float		_ki_roll_pitch;
-	float		_kp_yaw;
 	float		_ki_yaw;
 	bool		_have_initial_yaw;