AP_Compass: more efficient calculate_heading() implementation

Thanks to piersh for this improvement. See

b5f0635455 (commitcomment-3171806)

libraries/AP_Compass/Compass.cpp

@@ -216,29 +216,14 @@ Compass::calculate_heading(float roll, float pitch)
 float
 Compass::calculate_heading(const Matrix3f &dcm_matrix)
 {
-    float headX;
-    float headY;
-    float cos_pitch = safe_sqrt(1-(dcm_matrix.c.x*dcm_matrix.c.x));
-    float heading;
-
-    // sinf(pitch) = - dcm_matrix(3,1)
-    // cosf(pitch)*sinf(roll) = - dcm_matrix(3,2)
-    // cosf(pitch)*cosf(roll) = - dcm_matrix(3,3)
-
-    if (cos_pitch == 0.0f) {
-        // we are pointing straight up or down so don't update our
-        // heading using the compass. Wait for the next iteration when
-        // we hopefully will have valid values again.
-        return 0;
-    }
+    float headY = mag_y * dcm_matrix.c.z - mag_z * dcm_matrix.c.y;
 
     // Tilt compensated magnetic field X component:
-    headX = mag_x*cos_pitch - mag_y*dcm_matrix.c.y*dcm_matrix.c.x/cos_pitch - mag_z*dcm_matrix.c.z*dcm_matrix.c.x/cos_pitch;
-    // Tilt compensated magnetic field Y component:
-    headY = mag_y*dcm_matrix.c.z/cos_pitch - mag_z*dcm_matrix.c.y/cos_pitch;
+    float headX = mag_x + dcm_matrix.c.x * (headY - mag_x * dcm_matrix.c.x);
+
     // magnetic heading
     // 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
-    heading = constrain_float(atan2f(-headY,headX), -3.15f, 3.15f);
+    float heading = constrain_float(atan2f(-headY,headX), -3.15f, 3.15f);
 
     // Declination correction (if supplied)
     if( fabsf(_declination) > 0.0f )