ArduCopter: added DMP_ENABLE #define and CH6 tuning value for AHRS_KP

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;
 	}
 }
 

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

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