diff --git a/ArduCopter/compassmot.pde b/ArduCopter/compassmot.pde
index 3d9a089893..61cb11473a 100644
--- a/ArduCopter/compassmot.pde
+++ b/ArduCopter/compassmot.pde
@@ -8,14 +8,14 @@
 static uint8_t mavlink_compassmot(mavlink_channel_t chan)
 {
     int8_t   comp_type;                 // throttle or current based compensation
-    Vector3f compass_base;              // compass vector when throttle is zero
-    Vector3f motor_impact;              // impact of motors on compass vector
-    Vector3f motor_impact_scaled;       // impact of motors on compass vector scaled with throttle
-    Vector3f motor_compensation;        // final compensation to be stored to eeprom
+    Vector3f compass_base[COMPASS_MAX_INSTANCES];           // compass vector when throttle is zero
+    Vector3f motor_impact[COMPASS_MAX_INSTANCES];           // impact of motors on compass vector
+    Vector3f motor_impact_scaled[COMPASS_MAX_INSTANCES];    // impact of motors on compass vector scaled with throttle
+    Vector3f motor_compensation[COMPASS_MAX_INSTANCES];     // final compensation to be stored to eeprom
     float    throttle_pct;              // throttle as a percentage 0.0 ~ 1.0
     float    throttle_pct_max = 0.0f;   // maximum throttle reached (as a percentage 0~1.0)
     float    current_amps_max = 0.0f;   // maximum current reached
-    float    interference_pct = 0.0f;   // interference as a percentage of total mag field (for reporting purposes only)
+    float    interference_pct[COMPASS_MAX_INSTANCES];       // interference as a percentage of total mag field (for reporting purposes only)
     uint32_t last_run_time;
     uint32_t last_send_time;
     bool     updated = false;           // have we updated the compensation vector at least once
@@ -38,10 +38,12 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
 
     // check compass health
     compass.read();
-    if (!compass.healthy(0)) {
-        gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("check compass"));
-        ap.compass_mot = false;
-        return 1;
+    for (uint8_t i=0; i<compass.get_count(); i++) {
+        if (!compass.healthy(i)) {
+            gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("check compass"));
+            ap.compass_mot = false;
+            return 1;
+        }
     }
 
     // check if radio is calibrated
@@ -103,7 +105,9 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
 
     // disable motor compensation
     compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
-    compass.set_motor_compensation(Vector3f(0,0,0));
+    for (uint8_t i=0; i<compass.get_count(); i++) {
+        compass.set_motor_compensation(i, Vector3f(0,0,0));
+    }
 
     // get initial compass readings
     last_run_time = millis();
@@ -113,10 +117,11 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
     compass.read();
 
     // store initial x,y,z compass values
-    compass_base = compass.get_field();
-
-    // initialise motor compensation
-    motor_compensation = Vector3f(0,0,0);
+    // initialise interference percentage
+    for (uint8_t i=0; i<compass.get_count(); i++) {
+        compass_base[i] = compass.get_field(i);
+        interference_pct[i] = 0.0f;
+    }
 
     // enable motors and pass through throttle
     init_rc_out();
@@ -128,7 +133,7 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
     last_send_time = millis();
 
     // main run while there is no user input and the compass is healthy
-    while (command_ack_start == command_ack_counter && compass.healthy(0) && motors.armed()) {
+    while (command_ack_start == command_ack_counter && compass.healthy(compass.get_primary()) && motors.armed()) {
         // 50hz loop
         if (millis() - last_run_time < 20) {
             // grab some compass values
@@ -153,43 +158,57 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
         // calculate scaling for throttle
         throttle_pct = (float)g.rc_3.control_in / 1000.0f;
         throttle_pct = constrain_float(throttle_pct,0.0f,1.0f);
-        
+
         // if throttle is near zero, update base x,y,z values
         if (throttle_pct <= 0.0f) {
-            compass_base = compass_base * 0.99f + compass.get_field() * 0.01f;
-            
+            for (uint8_t i=0; i<compass.get_count(); i++) {
+                compass_base[i] = compass_base[i] * 0.99f + compass.get_field(i) * 0.01f;
+            }
+
             // causing printing to happen as soon as throttle is lifted
         } else {
 
             // calculate diff from compass base and scale with throttle
-            motor_impact = compass.get_field() - compass_base;
-            
+            for (uint8_t i=0; i<compass.get_count(); i++) {
+                motor_impact[i] = compass.get_field(i) - compass_base[i];
+            }
+
             // throttle based compensation
             if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
-                // scale by throttle
-                motor_impact_scaled = motor_impact / throttle_pct;
-                
-                // adjust the motor compensation to negate the impact
-                motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
+                // for each compass
+                for (uint8_t i=0; i<compass.get_count(); i++) {
+                    // scale by throttle
+                    motor_impact_scaled[i] = motor_impact[i] / throttle_pct;
+                    // adjust the motor compensation to negate the impact
+                    motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
+                }
+
                 updated = true;
             } else {
-                // current based compensation if more than 3amps being drawn
-                motor_impact_scaled = motor_impact / battery.current_amps();
+                // for each compass
+                for (uint8_t i=0; i<compass.get_count(); i++) {
+                    // current based compensation if more than 3amps being drawn
+                    motor_impact_scaled[i] = motor_impact[i] / battery.current_amps();
                 
-                // adjust the motor compensation to negate the impact if drawing over 3amps
-                if( battery.current_amps() >= 3.0f ) {
-                    motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
-                    updated = true;
+                    // adjust the motor compensation to negate the impact if drawing over 3amps
+                    if (battery.current_amps() >= 3.0f) {
+                        motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
+                        updated = true;
+                    }
                 }
             }
 
             // calculate interference percentage at full throttle as % of total mag field
             if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
-                // interference is impact@fullthrottle / mag field * 100
-                interference_pct = motor_compensation.length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
+                for (uint8_t i=0; i<compass.get_count(); i++) {
+                    // interference is impact@fullthrottle / mag field * 100
+                    interference_pct[i] = motor_compensation[i].length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
+                }
             }else{
-                // interference is impact/amp * (max current seen / max throttle seen) / mag field * 100
-                interference_pct = motor_compensation.length() * (current_amps_max/throttle_pct_max) / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
+                for (uint8_t i=0; i<compass.get_count(); i++) {
+                    // interference is impact/amp * (max current seen / max throttle seen) / mag field * 100
+                    interference_pct[i] = motor_compensation[i].length() * (current_amps_max/throttle_pct_max) / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
+                }
             }
 
             // record maximum throttle and current
@@ -202,10 +221,10 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
             mavlink_msg_compassmot_status_send(chan, 
                                                g.rc_3.control_in,
                                                battery.current_amps(),
-                                               interference_pct,
-                                               motor_compensation.x,
-                                               motor_compensation.y,
-                                               motor_compensation.z);
+                                               interference_pct[compass.get_primary()],
+                                               motor_compensation[compass.get_primary()].x,
+                                               motor_compensation[compass.get_primary()].y,
+                                               motor_compensation[compass.get_primary()].z);
         }
     }
 
@@ -216,7 +235,9 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
     // set and save motor compensation
     if (updated) {
         compass.motor_compensation_type(comp_type);
-        compass.set_motor_compensation(motor_compensation);
+        for (uint8_t i=0; i<compass.get_count(); i++) {
+            compass.set_motor_compensation(i, motor_compensation[i]);
+        }
         compass.save_motor_compensation();
         // display success message
         gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("Calibration Successful!"));