LP-204 Replace fast_invsqrtf() with invsqrtf() : 1.0f / sqrtf()

flight/libraries/insgps13state.c

@@ -280,11 +280,11 @@ void INSSetBaroVar(float baro_var)
 
 void INSSetMagNorth(float B[3])
 {
-    float mag = sqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);
+    float invmag = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);
 
-    ekf.Be[0] = B[0] / mag;
+    ekf.Be[0] = B[0] * invmag;
-    ekf.Be[1] = B[1] / mag;
+    ekf.Be[1] = B[1] * invmag;
-    ekf.Be[2] = B[2] / mag;
+    ekf.Be[2] = B[2] * invmag;
 }
 
 void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
@@ -305,7 +305,7 @@ void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
     // EKF prediction step
     LinearizeFG(ekf.X, U, ekf.F, ekf.G);
     RungeKutta(ekf.X, U, dT);
-    invqmag   = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
+    invqmag   = invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
     ekf.X[6] *= invqmag;
     ekf.X[7] *= invqmag;
     ekf.X[8] *= invqmag;
@@ -390,7 +390,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
 
 
     if (SensorsUsed & MAG_SENSORS) {
-        float invBmag = fast_invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]);
+        float invBmag = invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]);
         Z[6] = mag_data[0] * invBmag;
         Z[7] = mag_data[1] * invBmag;
         Z[8] = mag_data[2] * invBmag;
@@ -404,7 +404,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
     MeasurementEq(ekf.X, ekf.Be, Y);
     SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed);
 
-    float invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
+    float invqmag = invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
     ekf.X[6]  *= invqmag;
     ekf.X[7]  *= invqmag;
     ekf.X[8]  *= invqmag;

flight/libraries/math/mathmisc.h

@@ -119,28 +119,12 @@ static inline float y_on_curve(float x, const pointf points[], int num_points)
     // Find the y value on the selected line.
     return y_on_line(x, &points[end_point - 1], &points[end_point]);
 }
-// Fast inverse square root implementation from "quake3-1.32b/code/game/q_math.c"
-// http://en.wikipedia.org/wiki/Fast_inverse_square_root
 
-static inline float fast_invsqrtf(float number)
+static inline float invsqrtf(float number)
 {
-    float x2, y;
+    float y;
-    const float threehalfs = 1.5F;
-
-    union {
-        float    f;
-        uint32_t u;
-    } i;
-
-    x2  = number * 0.5F;
-    y   = number;
-
-    i.f = y; // evil floating point bit level hacking
-    i.u = 0x5f3759df - (i.u >> 1); // what the fxck?
-    y   = i.f;
-    y   = y * (threehalfs - (x2 * y * y));   // 1st iteration
-// y  = y * ( threehalfs - ( x2 * y * y ) );   // 2nd iteration, this can be removed
 
+    y = 1.0f / sqrtf(number);
     return y;
 }
 

flight/modules/Attitude/attitude.c

@@ -628,7 +628,7 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
     CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
 
     // Account for accel magnitude
-    float inv_accel_mag = fast_invsqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]);
+    float inv_accel_mag = invsqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]);
     if (inv_accel_mag > 1e3f) {
         return;
     }
@@ -637,7 +637,7 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
     float inv_grot_mag;
 
     if (accel_filter_enabled) {
-        inv_grot_mag = fast_invsqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
+        inv_grot_mag = invsqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
     } else {
         inv_grot_mag = 1.0f;
     }
@@ -685,8 +685,8 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
         }
     }
 
-    // Renomalize
+    // Renormalize
-    float inv_qmag = fast_invsqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+    float inv_qmag = invsqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
 
     // If quaternion has become inappropriately short or is nan reinit.
     // THIS SHOULD NEVER ACTUALLY HAPPEN

flight/modules/StateEstimation/filtercf.c

@@ -438,16 +438,16 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
         attitude[3] = -attitude[3];
     }
 
-    // Renomalize
+    // Renormalize
-    float qmag = sqrtf(attitude[0] * attitude[0] + attitude[1] * attitude[1] + attitude[2] * attitude[2] + attitude[3] * attitude[3]);
+    float inv_qmag = invsqrtf(attitude[0] * attitude[0] + attitude[1] * attitude[1] + attitude[2] * attitude[2] + attitude[3] * attitude[3]);
-    attitude[0] = attitude[0] / qmag;
+    attitude[0] = attitude[0] * inv_qmag;
-    attitude[1] = attitude[1] / qmag;
+    attitude[1] = attitude[1] * inv_qmag;
-    attitude[2] = attitude[2] / qmag;
+    attitude[2] = attitude[2] * inv_qmag;
-    attitude[3] = attitude[3] / qmag;
+    attitude[3] = attitude[3] * inv_qmag;
 
     // If quaternion has become inappropriately short or is nan reinit.
     // THIS SHOULD NEVER ACTUALLY HAPPEN
-    if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
+    if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) {
         this->first_run = 1;
         return FILTERRESULT_WARNING;
     }