add better inverse square matrix

Author: utamadonny

src/MadgwickAHRS.cpp

@@ -13,6 +13,7 @@
 // 29/09/2011	SOH Madgwick    Initial release
 // 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
 // 19/02/2012	SOH Madgwick	Magnetometer measurement is normalised
+// 08/07/2021 			Added Better Fast Inverse Square Root Option
 //
 //=============================================================================================
 
@@ -227,16 +228,29 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float
 //-------------------------------------------------------------------------------------------
 // Fast inverse square-root
 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
-
+int instability_fix = 0; // 0 is original code
 float Madgwick::invSqrt(float x) {
-	float halfx = 0.5f * x;
-	float y = x;
-	long i = *(long*)&y;
-	i = 0x5f3759df - (i>>1);
-	y = *(float*)&i;
-	y = y * (1.5f - (halfx * y * y));
-	y = y * (1.5f - (halfx * y * y));
-	return y;
+if (instability_fix == 0){
+        /* original code */
+        float halfx = 0.5f * x;
+        float y = x;
+        long i = *(long*)&y;
+        i = 0x5f3759df - (i>>1);
+        y = *(float*)&i;
+        y = y * (1.5f - (halfx * y * y));
+        y = y * (1.5f - (halfx * y * y));
+        return y;
+	}
+else if (instability_fix == 1){
+        /* close-to-optimal  method with low cost from http://pizer.wordpress.com/2008/10/12/fast-inverse-square-root */
+        unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1);
+        float tmp = *(float*)&i;
+        return tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
+	}
+else{
+        /* optimal but expensive method: */
+        return 1.0f / sqrtf(x);
+	}
 }
 
 //-------------------------------------------------------------------------------------------

src/MadgwickAHRS.h

@@ -34,6 +34,7 @@ class Madgwick{
     float yaw;
     char anglesComputed;
     void computeAngles();
+    // int instability_fix;
 
 //-------------------------------------------------------------------------------------------
 // Function declarations