diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp
index cfd2e68a12..f71932ef6d 100644
--- a/libraries/AP_Compass/Compass.cpp
+++ b/libraries/AP_Compass/Compass.cpp
@@ -637,57 +637,42 @@ void Compass::motor_compensation_type(const uint8_t comp_type)
     }
 }
 
-uint8_t Compass::get_use_mask() const
-{
-    uint8_t ret = 0;
-    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
-        if (use_for_yaw(i)) {
-            ret |= 1 << i;
-        }
-    }
-    return ret;
-}
-
-bool Compass::consistent(uint8_t mask) const
+bool Compass::consistent() const
 {
     Vector3f primary_mag_field = get_field();
-    float primary_mag_field_mag = primary_mag_field.length();
     Vector3f primary_mag_field_norm;
 
-    if (!is_zero(primary_mag_field_mag)) {
-        primary_mag_field_norm = primary_mag_field / primary_mag_field_mag;
+    if (!primary_mag_field.is_zero()) {
+        primary_mag_field_norm = primary_mag_field.normalized();
     } else {
         return false;
     }
 
     Vector2f primary_mag_field_xy = Vector2f(primary_mag_field.x,primary_mag_field.y);
-    float primary_mag_field_xy_mag = primary_mag_field_xy.length();
     Vector2f primary_mag_field_xy_norm;
 
-    if (!is_zero(primary_mag_field_xy_mag)) {
-        primary_mag_field_xy_norm = primary_mag_field_xy / primary_mag_field_xy_mag;
+    if (!primary_mag_field_xy.is_zero()) {
+        primary_mag_field_xy_norm = primary_mag_field_xy.normalized();
     } else {
         return false;
     }
 
-    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
-        if ((1<<i) & mask) {
+    for (uint8_t i=0; i<get_count(); i++) {
+        if (use_for_yaw(i)) {
             Vector3f mag_field = get_field(i);
-            float mag_field_mag = mag_field.length();
             Vector3f mag_field_norm;
 
-            if (!is_zero(mag_field_mag)) {
-                mag_field_norm = mag_field / mag_field_mag;
+            if (!mag_field.is_zero()) {
+                mag_field_norm = mag_field.normalized();
             } else {
                 return false;
             }
 
             Vector2f mag_field_xy = Vector2f(mag_field.x,mag_field.y);
-            float mag_field_xy_mag = mag_field_xy.length();
             Vector2f mag_field_xy_norm;
 
-            if (!is_zero(mag_field_xy_mag)) {
-                mag_field_xy_norm = mag_field_xy / mag_field_xy_mag;
+            if (!mag_field_xy.is_zero()) {
+                mag_field_xy_norm = mag_field_xy.normalized();
             } else {
                 return false;
             }
@@ -713,3 +698,4 @@ bool Compass::consistent(uint8_t mask) const
     }
     return true;
 }
+
diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h
index 1b5b7aea79..5e84b6f438 100644
--- a/libraries/AP_Compass/Compass.h
+++ b/libraries/AP_Compass/Compass.h
@@ -110,9 +110,8 @@ public:
     const Vector3f &get_field(uint8_t i) const { return _state[i].field; }
     const Vector3f &get_field(void) const { return get_field(get_primary()); }
 
-    uint8_t get_use_mask() const;
-    bool consistent(uint8_t mask) const;
-    bool consistent() const { return consistent(get_use_mask()); }
+    // check if the compasses are pointing in the same direction
+    bool consistent() const;
 
     /// Return the health of a compass
     bool healthy(uint8_t i) const { return _state[i].healthy; }