From 014f5aae992291263c41d280c25a47e96a10b0cf Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Sat, 28 Jul 2012 14:21:07 +0900
Subject: [PATCH] ArduCopter: added DMP_ENABLE #define and CH6 tuning value for
 AHRS_KP

---
 ArduCopter/ArduCopter.pde | 11 ++++++++++-
 ArduCopter/config.h       |  4 ++++
 ArduCopter/defines.h      |  3 ++-
 3 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index e72c99cae5..691f7d3bb3 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -262,7 +262,11 @@ static GPS         *g_gps_null;
 #if QUATERNION_ENABLE == ENABLED
  AP_AHRS_Quaternion ahrs(&imu, g_gps_null);
 #else
- AP_AHRS_DCM ahrs(&imu, g_gps);
+ #if DMP_ENABLED == ENABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
+  AP_AHRS_MPU6000  ahrs(&imu, g_gps, &ins);		// only works with APM2
+ #else
+  AP_AHRS_DCM ahrs(&imu, g_gps);
+ #endif
 #endif
 
 AP_TimerProcess timer_scheduler;
@@ -2368,6 +2372,11 @@ static void tuning(){
 		case CH6_AHRS_YAW_KP:
 			ahrs._kp_yaw.set(tuning_value);
 			break;
+
+		case CH6_AHRS_KP:
+			ahrs._kp.set(tuning_value);
+			//ahrs.push_gains_to_dmp();
+			break;
 	}
 }
 
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 500ba1a20d..e8f7ba43ff 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -1025,6 +1025,10 @@
 # define QUATERNION_ENABLE DISABLED
 #endif
 
+// experimental mpu6000 DMP code
+#ifndef DMP_ENABLED
+# define DMP_ENABLED DISABLED
+#endif
 
 #ifndef ALTERNATIVE_YAW_MODE
 # define ALTERNATIVE_YAW_MODE DISABLED
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 30fbe21fb0..222b607b36 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -179,7 +179,8 @@
 #define CH6_LOITER_RATE_KI 28
 #define CH6_LOITER_RATE_KD 23
 
-#define CH6_AHRS_YAW_KP 30
+#define CH6_AHRS_YAW_KP	30
+#define CH6_AHRS_KP		31
 
 
 // nav byte mask