ArduCopter: allow DMP to run in parallel with DCM

Parallel DMP can be enabled by #define SECONDARY_DMP_ENABLED in APM_Config.h
New DMP dataflash log type added to allow easy comparison with DCM

ArduCopter/APM_Config.h

@@ -15,6 +15,7 @@
 //#define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
 //#define HIL_MODE				HIL_MODE_ATTITUDE
 //#define DMP_ENABLED ENABLED
+//#define SECONDARY_DMP_ENABLED ENABLED       // allows running DMP in parallel with DCM for testing purposes
 
 //#define FRAME_CONFIG QUAD_FRAME
 /*

ArduCopter/ArduCopter.pde

@@ -272,6 +272,11 @@ AP_AHRS_MPU6000  ahrs(&imu, g_gps, &ins);               // only works with APM2
 AP_AHRS_DCM ahrs(&imu, g_gps);
  #endif
 
+// ahrs2 object is the secondary ahrs to allow running DMP in parallel with DCM
+  #if SECONDARY_DMP_ENABLED == ENABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
+AP_AHRS_MPU6000  ahrs2(&imu, g_gps, &ins);               // only works with APM2
+  #endif
+
 #elif HIL_MODE == HIL_MODE_SENSORS
 // sensor emulators
 AP_ADC_HIL adc;
@@ -1187,8 +1192,12 @@ static void medium_loop()
         }
 
         if(motors.armed()) {
-            if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
+            if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) {
                 Log_Write_Attitude();
+#if SECONDARY_DMP_ENABLED == ENABLED
+                Log_Write_DMP();
+#endif
+            }
 
             if (g.log_bitmask & MASK_LOG_MOTORS)
                 Log_Write_Motors();
@@ -1288,8 +1297,12 @@ static void fifty_hz_loop()
 #endif
 
 # if HIL_MODE == HIL_MODE_DISABLED
-    if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motors.armed())
+    if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motors.armed()) {
         Log_Write_Attitude();
+#if SECONDARY_DMP_ENABLED == ENABLED
+        Log_Write_DMP();
+#endif
+    }
 
     if (g.log_bitmask & MASK_LOG_RAW && motors.armed())
         Log_Write_Raw();
@@ -2089,6 +2102,10 @@ static void read_AHRS(void)
 
     ahrs.update();
     omega = imu.get_gyro();
+
+#if SECONDARY_DMP_ENABLED == ENABLED
+    ahrs2.update();
+#endif
 }
 
 static void update_trig(void){

ArduCopter/Log.pde

@@ -942,6 +942,44 @@ static void Log_Read_PID()
                     temp7);             // gain
 }
 
+// Write a DMP attitude packet. Total length : 16 bytes
+static void Log_Write_DMP()
+{
+#if SECONDARY_DMP_ENABLED == ENABLED
+    DataFlash.WriteByte(HEAD_BYTE1);
+    DataFlash.WriteByte(HEAD_BYTE2);
+    DataFlash.WriteByte(LOG_DMP_MSG);
+
+    DataFlash.WriteInt((int16_t)ahrs.roll_sensor);                          // 1
+    DataFlash.WriteInt((int16_t)ahrs2.roll_sensor);                         // 2
+    DataFlash.WriteInt((int16_t)ahrs.pitch_sensor);                         // 3
+    DataFlash.WriteInt((int16_t)ahrs2.pitch_sensor);                        // 4
+    DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor);                          // 5
+    DataFlash.WriteInt((uint16_t)ahrs2.yaw_sensor);                         // 6
+    DataFlash.WriteByte(END_BYTE);
+#endif
+}
+
+// Read an attitude packet
+static void Log_Read_DMP()
+{
+    int16_t temp1   = DataFlash.ReadInt();
+    int16_t temp2   = DataFlash.ReadInt();
+    int16_t temp3   = DataFlash.ReadInt();
+    int16_t temp4   = DataFlash.ReadInt();
+    uint16_t temp5   = DataFlash.ReadInt();
+    uint16_t temp6  = DataFlash.ReadInt();
+
+                             // 1   2   3   4   5   6
+    Serial.printf_P(PSTR("DMP, %d, %d, %d, %d, %u, %u\n"),
+                    (int)temp1,
+                    (int)temp2,
+                    (int)temp3,
+                    (int)temp4,
+                    (unsigned)temp5,
+                    (unsigned)temp6);
+}
+
 // Read the DataFlash log memory
 static void Log_Read(int16_t start_page, int16_t end_page)
 {
@@ -1069,6 +1107,10 @@ static int16_t Log_Read_Process(int16_t start_page, int16_t end_page)
 					case LOG_ITERM_MSG:
 						Log_Read_Iterm();
 						break;
+
+					case LOG_DMP_MSG:
+						Log_Read_DMP();
+						break;
 				}
 				break;
 		case 3:
@@ -1122,7 +1164,9 @@ static void Log_Write_Motors() {
 }
 static void Log_Write_Performance() {
 }
-static void Log_Write_PID() {
+static void Log_Write_PID(int8_t pid_id, int32_t error, int32_t p, int32_t i, int32_t d, int32_t output, float gain) {
+}
+static void Log_Write_DMP() {
 }
 static int8_t process_logs(uint8_t argc, const Menu::arg *argv) {
     return 0;

ArduCopter/Parameters.pde

@@ -373,6 +373,15 @@ static void load_parameters(void)
         ahrs._kp_yaw.set_and_save(0.1);
     }
 
+#if SECONDARY_DMP_ENABLED == ENABLED
+    if (!ahrs2._kp.load()) {
+        ahrs2._kp.set(0.1);
+    }
+    if (!ahrs2._kp_yaw.load()) {
+        ahrs2._kp_yaw.set(0.1);
+    }
+#endif
+
 
     if (!g.format_version.load() ||
         g.format_version != Parameters::k_format_version) {

ArduCopter/config.h

@@ -1061,6 +1061,11 @@
  # define DMP_ENABLED DISABLED
 #endif
 
+// experimental mpu6000 DMP code
+#ifndef SECONDARY_DMP_ENABLED
+ # define SECONDARY_DMP_ENABLED DISABLED
+#endif
+
 #ifndef ALTERNATIVE_YAW_MODE
  # define ALTERNATIVE_YAW_MODE DISABLED
 #endif

ArduCopter/control_modes.pde

@@ -165,6 +165,9 @@ static void auto_trim()
         if( motors.armed() ) {
             // set high AHRS gains to force accelerometers to impact attitude estimate
             ahrs.set_fast_gains(true);
+#if SECONDARY_DMP_ENABLED == ENABLED
+            ahrs2.set_fast_gains(true);
+#endif
         }
 
         trim_accel();
@@ -183,6 +186,9 @@ static void auto_trim()
             // go back to normal AHRS gains
             if( motors.armed() ) {
                 ahrs.set_fast_gains(false);
+#if SECONDARY_DMP_ENABLED == ENABLED
+                ahrs2.set_fast_gains(false);
+#endif
             }
 
             //Serial.println("Done");

ArduCopter/defines.h

@@ -296,6 +296,7 @@ enum gcs_severity {
 #define LOG_DATA_MSG                    0x0D
 #define LOG_PID_MSG                     0x0E
 #define LOG_ITERM_MSG                   0x0F
+#define LOG_DMP_MSG                     0x10
 #define LOG_INDEX_MSG                   0xF0
 #define MAX_NUM_LOGS                    50
 

ArduCopter/motors.pde

@@ -161,6 +161,9 @@ static void init_arm_motors()
 
     // go back to normal AHRS gains
     ahrs.set_fast_gains(false);
+#if SECONDARY_DMP_ENABLED == ENABLED
+    ahrs2.set_fast_gains(false);
+#endif
 }
 
 
@@ -187,6 +190,9 @@ static void init_disarm_motors()
 
     // setup fast AHRS gains to get right attitude
     ahrs.set_fast_gains(true);
+#if SECONDARY_DMP_ENABLED == ENABLED
+    ahrs2.set_fast_gains(true);
+#endif
 }
 
 /*****************************************

ArduCopter/sensors.pde

@@ -43,6 +43,9 @@ static void init_compass()
         return;
     }
     ahrs.set_compass(&compass);
+#if SECONDARY_DMP_ENABLED == ENABLED
+    ahrs2.set_compass(&compass);
+#endif
 }
 
 static void init_optflow()

ArduCopter/system.pde

@@ -367,6 +367,12 @@ static void startup_ground(void)
     // setup fast AHRS gains to get right attitude
     ahrs.set_fast_gains(true);
 
+#if SECONDARY_DMP_ENABLED == ENABLED
+    ahrs2.init(&timer_scheduler);
+    ahrs2.set_as_secondary(true);
+    ahrs2.set_fast_gains(true);
+#endif
+
     // reset the leds
     // ---------------------------
     clear_leds();