Madgwick
Dependents: IMU_ethernet IMU_serial_test IMU_serial
Diff: MadgwickAHRS.cpp
- Revision:
- 2:11ffe983c3f8
- Parent:
- 1:189289643d44
- Child:
- 3:689b64501b7d
--- a/MadgwickAHRS.cpp Thu Oct 06 17:00:09 2016 +0000 +++ b/MadgwickAHRS.cpp Thu Oct 06 17:34:56 2016 +0000 @@ -41,7 +41,8 @@ //--------------------------------------------------------------------------------------------------- // AHRS algorithm update -void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { +void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) +{ float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; @@ -67,7 +68,7 @@ recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; - az *= recipNorm; + az *= recipNorm; // Normalise magnetometer measurement recipNorm = invSqrt(mx * mx + my * my + mz * mz); @@ -140,7 +141,8 @@ //--------------------------------------------------------------------------------------------------- // IMU algorithm update -void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) { +void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) +{ float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; @@ -159,7 +161,7 @@ recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; - az *= recipNorm; + az *= recipNorm; // Auxiliary variables to avoid repeated arithmetic _2q0 = 2.0f * q0; @@ -212,7 +214,8 @@ // Fast inverse square-root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root -float invSqrt(float x) { +float invSqrt(float x) +{ float halfx = 0.5f * x; float y = x; long i = *(long*)&y;