ArduPlane: allow MPU6000's DMP to be used for attitude estimation.

Enable by adding this line to APM_Config.h:
#define DMP_ENABLED ENABLED

ArduPlane/APM_Config.h

@@ -17,6 +17,7 @@
 // simulation. Remove the leading "/* and trailing "*/" to enable:
 
 //#define HIL_MODE            HIL_MODE_DISABLED
+//#define DMP_ENABLED ENABLED
 
 /*
 // HIL_MODE SELECTION

ArduPlane/ArduPlane.pde

@@ -204,7 +204,11 @@ AP_IMU_INS imu( &ins );
 #if QUATERNION_ENABLE == ENABLED
   AP_AHRS_Quaternion ahrs(&imu, g_gps);
 #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
 
 #elif HIL_MODE == HIL_MODE_SENSORS

ArduPlane/Parameters.pde

@@ -530,9 +530,13 @@ static const AP_Param::Info var_info[] PROGMEM = {
 	// @Path: ../libraries/AP_IMU/IMU.cpp
 	GOBJECT(imu,					"IMU_",     IMU),
 
-    // @Group: AHRS_
-	// @Path: ../libraries/AP_AHRS/AP_AHRS_DCM.cpp, ../libraries/AP_AHRS/AP_AHRS_Quaternion.cpp
-	GOBJECT(ahrs,					"AHRS_",    AP_AHRS_DCM),
+	// @Group: AHRS_
+	// @Path: ../libraries/AP_AHRS/AP_AHRS_DCM.cpp, ../libraries/AP_AHRS/AP_AHRS_Quaternion.cpp, ../libraries/AP_AHRS/AP_AHRS_MPU6000.cpp
+	#if DMP_ENABLED == ENABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
+	GOBJECT(ahrs,			"AHRS_",    AP_AHRS_DCM),
+	#else
+	GOBJECT(ahrs,			"AHRS_",    AP_AHRS_MPU6000),
+	#endif
 
     // @Group: ARSPD_
 	// @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp

ArduPlane/config.h

@@ -869,3 +869,8 @@
 #ifndef QUATERNION_ENABLE
 # define QUATERNION_ENABLE DISABLED
 #endif
+
+// experimental mpu6000 DMP code
+#ifndef DMP_ENABLED
+# define DMP_ENABLED DISABLED
+#endif

ArduPlane/system.pde

@@ -258,6 +258,9 @@ static void init_ardupilot()
 		//read_EEPROM_airstart_critical();
 #if HIL_MODE != HIL_MODE_ATTITUDE
 		imu.init(IMU::WARM_START, mavlink_delay, flash_leds, &timer_scheduler);
+
+		// initialise ahrs (may push imu calibration into the mpu6000 if using that device).
+		ahrs.init();
 		ahrs.set_fly_forward(true);
 #endif