AP_Compass: tidy consistency calculations

AP_Compass: normalize vectors in-place to save a few bytes (thanks MdB)

libraries/AP_Compass/AP_Compass.cpp

@@ -1281,63 +1281,52 @@ void Compass::motor_compensation_type(const uint8_t comp_type)
 
 bool Compass::consistent() const
 {
-    Vector3f primary_mag_field = get_field();
-    Vector3f primary_mag_field_norm;
+    const Vector3f &primary_mag_field = get_field();
+    const Vector2f primary_mag_field_xy = Vector2f(primary_mag_field.x,primary_mag_field.y);
 
-    if (!primary_mag_field.is_zero()) {
-        primary_mag_field_norm = primary_mag_field.normalized();
-    } else {
+    if (primary_mag_field_xy.is_zero()) {
         return false;
     }
 
-    Vector2f primary_mag_field_xy = Vector2f(primary_mag_field.x,primary_mag_field.y);
-    Vector2f primary_mag_field_xy_norm;
+    const Vector3f primary_mag_field_norm = primary_mag_field.normalized();
+    const Vector2f primary_mag_field_xy_norm = primary_mag_field_xy.normalized();
 
-    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<get_count(); i++) {
+        if (!use_for_yaw(i)) {
+            // configured not-to-be-used
+            continue;
         }
 
-    for (uint8_t i=0; i<get_count(); i++) {
-        if (use_for_yaw(i)) {
         Vector3f mag_field = get_field(i);
-            Vector3f mag_field_norm;
+        Vector2f mag_field_xy = Vector2f(mag_field.x,mag_field.y);
 
-            if (!mag_field.is_zero()) {
-                mag_field_norm = mag_field.normalized();
-            } else {
+        if (mag_field_xy.is_zero()) {
             return false;
         }
 
-            Vector2f mag_field_xy = Vector2f(mag_field.x,mag_field.y);
-            Vector2f mag_field_xy_norm;
+        const float xy_len_diff  = (primary_mag_field_xy-mag_field_xy).length();
 
-            if (!mag_field_xy.is_zero()) {
-                mag_field_xy_norm = mag_field_xy.normalized();
-            } else {
-                return false;
-            }
+        mag_field.normalize();
+        mag_field_xy.normalize();
 
-            float xyz_ang_diff = acosf(constrain_float(mag_field_norm * primary_mag_field_norm,-1.0f,1.0f));
-            float xy_ang_diff  = acosf(constrain_float(mag_field_xy_norm*primary_mag_field_xy_norm,-1.0f,1.0f));
-            float xy_len_diff  = (primary_mag_field_xy-mag_field_xy).length();
+        const float xyz_ang_diff = acosf(constrain_float(mag_field*primary_mag_field_norm,-1.0f,1.0f));
+        const float xy_ang_diff  = acosf(constrain_float(mag_field_xy*primary_mag_field_xy_norm,-1.0f,1.0f));
 
         // check for gross misalignment on all axes
-            bool xyz_ang_diff_large = xyz_ang_diff > AP_COMPASS_MAX_XYZ_ANG_DIFF;
+        if (xyz_ang_diff > AP_COMPASS_MAX_XYZ_ANG_DIFF) {
+            return false;
+        }
 
         // check for an unacceptable angle difference on the xy plane
-            bool xy_ang_diff_large = xy_ang_diff > AP_COMPASS_MAX_XY_ANG_DIFF;
+        if (xy_ang_diff > AP_COMPASS_MAX_XY_ANG_DIFF) {
+            return false;
+        }
 
         // check for an unacceptable length difference on the xy plane
-            bool xy_length_diff_large = xy_len_diff > AP_COMPASS_MAX_XY_LENGTH_DIFF;
-
-            // check for inconsistency in the XY plane
-            if (xyz_ang_diff_large || xy_ang_diff_large || xy_length_diff_large) {
+        if (xy_len_diff > AP_COMPASS_MAX_XY_LENGTH_DIFF) {
             return false;
         }
     }
-    }
     return true;
 }