Copter: remove support for dmp ahrs

ArduCopter/APM_Config.h

@@ -22,8 +22,6 @@
 //#define LOGGING_ENABLED       DISABLED            // disable dataflash logging to save 11K of flash space
 //#define MOUNT                 DISABLED            // disable the camera gimbal to save 8K of flash space
 //#define CLI_ENABLED           DISABLED            // disable the CLI (command-line-interface) to save 21K of flash space
-//#define DMP_ENABLED           ENABLED             // use MPU6000's DMP instead of DCM for attitude estimation
-//#define SECONDARY_DMP_ENABLED ENABLED             // allows running DMP in parallel with DCM for testing purposes
 
 //#define HIL_MODE              HIL_MODE_SENSORS    // build for hardware-in-the-loop simulation
 

ArduCopter/ArduCopter.pde

@@ -283,16 +283,7 @@ AP_GPS_None     g_gps_driver;
   #error Unrecognised GPS_PROTOCOL setting.
  #endif // GPS PROTOCOL
 
- #if DMP_ENABLED == ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_APM2
-static AP_AHRS_MPU6000  ahrs(&ins, g_gps);               // only works with APM2
- #else
 static AP_AHRS_DCM ahrs(&ins, g_gps);
- #endif
-
-// ahrs2 object is the secondary ahrs to allow running DMP in parallel with DCM
-  #if SECONDARY_DMP_ENABLED == ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_APM2
-static AP_AHRS_MPU6000  ahrs2(&ins, g_gps);               // only works with APM2
-  #endif
 
 #elif HIL_MODE == HIL_MODE_SENSORS
 // sensor emulators
@@ -1113,9 +1104,6 @@ static void fifty_hz_loop()
 # if HIL_MODE == HIL_MODE_DISABLED
     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_IMU && motors.armed())
@@ -1195,9 +1183,6 @@ static void medium_loop()
         if(motors.armed()) {
             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();
@@ -2112,10 +2097,6 @@ static void read_AHRS(void)
 
     ahrs.update();
     omega = ins.get_gyro();
-
-#if SECONDARY_DMP_ENABLED == ENABLED
-    ahrs2.update();
-#endif
 }
 
 static void update_trig(void){

ArduCopter/Log.pde

@@ -666,33 +666,6 @@ static void Log_Write_PID(uint8_t pid_id, int32_t error, int32_t p, int32_t i, i
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
-struct PACKED log_DMP {
-    LOG_PACKET_HEADER;
-    int16_t  dcm_roll;
-    int16_t  dmp_roll;
-    int16_t  dcm_pitch;
-    int16_t  dmp_pitch;
-    uint16_t dcm_yaw;
-    uint16_t dmp_yaw;
-};
-
-#if SECONDARY_DMP_ENABLED == ENABLED
-// Write a DMP attitude packet
-static void Log_Write_DMP()
-{
-    struct log_DMP pkt = {
-        LOG_PACKET_HEADER_INIT(LOG_DMP_MSG),
-        dcm_roll    : (int16_t)ahrs.roll_sensor,
-        dmp_roll    : (int16_t)ahrs2.roll_sensor,
-        dcm_pitch   : (int16_t)ahrs.pitch_sensor,
-        dmp_pitch   : (int16_t)ahrs2.pitch_sensor,
-        dcm_yaw     : (uint16_t)ahrs.yaw_sensor,
-        dmp_yaw     : (uint16_t)ahrs2.yaw_sensor
-    };
-    DataFlash.WriteBlock(&pkt, sizeof(pkt));
-}
-#endif
-
 struct PACKED log_Camera {
     LOG_PACKET_HEADER;
     uint32_t gps_time;
@@ -792,8 +765,6 @@ static const struct LogStructure log_structure[] PROGMEM = {
       "DFLT",  "Bf",         "Id,Value" },
     { LOG_PID_MSG, sizeof(log_PID),         
       "PID",   "Biiiiif",    "Id,Error,P,I,D,Out,Gain" },
-    { LOG_DMP_MSG, sizeof(log_DMP),         
-      "DMP",   "ccccCC",     "DCMRoll,DMPRoll,DCMPtch,DMPPtch,DCMYaw,DMPYaw" },
     { LOG_CAMERA_MSG, sizeof(log_Camera),                 
       "CAM",   "ILLeccC",    "GPSTime,Lat,Lng,Alt,Roll,Pitch,Yaw" },
     { LOG_ERROR_MSG, sizeof(log_Error),         
@@ -852,9 +823,6 @@ static void Log_Write_Control_Tuning() {}
 static void Log_Write_Motors() {}
 static void Log_Write_Performance() {}
 static void Log_Write_PID(uint8_t pid_id, int32_t error, int32_t p, int32_t i, int32_t d, int32_t output, float gain) {}
-#if SECONDARY_DMP_ENABLED == ENABLED
-static void Log_Write_DMP() {}
-#endif
 static void Log_Write_Camera() {}
 static void Log_Write_Error(uint8_t sub_system, uint8_t error_code) {}
 static int8_t process_logs(uint8_t argc, const Menu::arg *argv) {

ArduCopter/Parameters.pde

@@ -1080,15 +1080,6 @@ 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
-
     // setup different Compass learn setting for ArduCopter than the default
     // but allow users to override in their config
     if (!compass._learn.load()) {

ArduCopter/config.h

@@ -1065,14 +1065,4 @@
   #  define CLI_ENABLED           ENABLED
 #endif
 
-// experimental mpu6000 DMP code
-#ifndef DMP_ENABLED
- # define DMP_ENABLED DISABLED
-#endif
-
-// experimental mpu6000 DMP code
-#ifndef SECONDARY_DMP_ENABLED
- # define SECONDARY_DMP_ENABLED DISABLED
-#endif
-
 #endif // __ARDUCOPTER_CONFIG_H__

ArduCopter/defines.h

@@ -282,7 +282,6 @@ enum ap_message {
 #define LOG_EVENT_MSG                   0x0D
 #define LOG_PID_MSG                     0x0E
 #define LOG_COMPASS_MSG                 0x0F
-#define LOG_DMP_MSG                     0x10
 #define LOG_INAV_MSG                    0x11
 #define LOG_CAMERA_MSG                  0x12
 #define LOG_ERROR_MSG                   0x13

ArduCopter/motors.pde

@@ -172,9 +172,6 @@ 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
 
     // enable gps velocity based centrefugal force compensation
     ahrs.set_correct_centrifugal(true);
@@ -410,9 +407,6 @@ 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
 
     // log disarm to the dataflash
     Log_Write_Event(DATA_DISARMED);

ArduCopter/sensors.pde

@@ -73,9 +73,6 @@ 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

@@ -276,12 +276,6 @@ 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
-
     // set landed flag
     set_land_complete(true);
 }