Files
quadrotor/fisqrt.c
Gavan Fantom e8adff93c1 Bye bye Wii Motion Plus, hello MPU6050. Also, increase control loop
to 200Hz, add SD card logging, and a number of other changes.
2014-05-06 21:03:48 +00:00

27 lines
382 B
C

/* Implementation of fast inverse square root.
* See http://en.wikipedia.org/wiki/Fast_inverse_square_root
*/
float fisqrt(float n)
{
long i;
float x2, y;
union {
float f;
long l;
} u;
x2 = n * 0.5f;
y = n;
/* i = *(long *)&y; */
u.f = y;
i = u.l;
i = 0x5f3759df - (i>>1);
/* y = *(float *)&i; */
u.l = i;
y = u.f;
y = y * (1.5f - (x2*y*y));
return y;
}