Skip to content
Permalink
Browse files

added support for selecting different inverse square root implementat…

…ions
  • Loading branch information...
TobiasSimon
TobiasSimon committed Nov 2, 2012
1 parent fff1203 commit 4f76ef1475219bedbdba7afab297e3468d9e7c44
Showing with 27 additions and 8 deletions.
  1. +26 −7 MadgwickAHRS.c
  2. +1 −1 MadgwickAHRS.h
@@ -208,18 +208,37 @@ void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, flo
q3 *= recipNorm;
}


int instability_fix = 1;

//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

float 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));
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));
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);
}
}

//====================================================================================================
@@ -15,7 +15,7 @@

//----------------------------------------------------------------------------------------------------
// Variable declaration

extern int instability_fix;
extern volatile float beta; // algorithm gain
extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame

0 comments on commit 4f76ef1

Please sign in to comment.
You can’t perform that action at this time.