diff --git a/src/Adafruit_AHRS_Madgwick.cpp b/src/Adafruit_AHRS_Madgwick.cpp index d8234ff..14aef48 100644 --- a/src/Adafruit_AHRS_Madgwick.cpp +++ b/src/Adafruit_AHRS_Madgwick.cpp @@ -280,15 +280,17 @@ void Adafruit_Madgwick::updateIMU(float gx, float gy, float gz, float ax, // Fast inverse square-root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root -float Adafruit_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; +float Adafruit_Madgwick::invSqrt(float x) +{ + float halfx = 0.5f * x; + union { + float f; + uint32_t i; + } conv = { .f = x }; + conv.i = 0x5f3759df - (conv.i >> 1); + conv.f *= 1.5f - (halfx * conv.f * conv.f); + conv.f *= 1.5f - (halfx * conv.f * conv.f); + return y; } //------------------------------------------------------------------------------------------- diff --git a/src/Adafruit_AHRS_Mahony.cpp b/src/Adafruit_AHRS_Mahony.cpp index c01b510..c0e0693 100644 --- a/src/Adafruit_AHRS_Mahony.cpp +++ b/src/Adafruit_AHRS_Mahony.cpp @@ -259,15 +259,17 @@ void Adafruit_Mahony::updateIMU(float gx, float gy, float gz, float ax, // Fast inverse square-root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root -float Adafruit_Mahony::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; +float Adafruit_Mahony::invSqrt(float x) +{ + float halfx = 0.5f * x; + union { + float f; + uint32_t i; + } conv = { .f = x }; + conv.i = 0x5f3759df - (conv.i >> 1); + conv.f *= 1.5f - (halfx * conv.f * conv.f); + conv.f *= 1.5f - (halfx * conv.f * conv.f); + return y; } //-------------------------------------------------------------------------------------------