|
|
@ -21,6 +21,7 @@ |
|
|
|
|
|
|
|
|
|
|
|
#include "MadgwickAHRS.h" |
|
|
|
#include "MadgwickAHRS.h" |
|
|
|
#include <math.h> |
|
|
|
#include <math.h> |
|
|
|
|
|
|
|
#include <string.h> |
|
|
|
|
|
|
|
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
//-------------------------------------------------------------------------------------------
|
|
|
|
// Definitions
|
|
|
|
// Definitions
|
|
|
@ -269,9 +270,12 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, |
|
|
|
float Madgwick::invSqrt(float x) { |
|
|
|
float Madgwick::invSqrt(float x) { |
|
|
|
float halfx = 0.5f * x; |
|
|
|
float halfx = 0.5f * x; |
|
|
|
float y = x; |
|
|
|
float y = x; |
|
|
|
long i = *(long *)&y; |
|
|
|
// long i = *(long *)&y;
|
|
|
|
|
|
|
|
long i; |
|
|
|
|
|
|
|
memcpy(&i, &y, sizeof(float)); |
|
|
|
i = 0x5f3759df - (i >> 1); |
|
|
|
i = 0x5f3759df - (i >> 1); |
|
|
|
y = *(float *)&i; |
|
|
|
// y = *(float *)&i;
|
|
|
|
|
|
|
|
memcpy(&y, &i, sizeof(float)); |
|
|
|
y = y * (1.5f - (halfx * y * y)); |
|
|
|
y = y * (1.5f - (halfx * y * y)); |
|
|
|
y = y * (1.5f - (halfx * y * y)); |
|
|
|
y = y * (1.5f - (halfx * y * y)); |
|
|
|
return y; |
|
|
|
return y; |
|
|
|