jlpoole/MotionCal
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Fix compiler warning in invSqrt

Browse source
This commit is contained in:
PaulStoffregen 2016-03-25 18:05:42 -07:00
commit 108a3ce457
1 changed files with 19 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

31
mahony.c
View file

@ -161,14 +161,16 @@ static void mahony_update(float gx, float gy, float gz, float ax, float ay, floa
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// Error is sum of cross product between estimated direction and measured direction of field vectors // Error is sum of cross product between estimated direction
// and measured direction of field vectors
halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy); halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz); halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx); halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
// Compute and apply integral feedback if enabled // Compute and apply integral feedback if enabled
if(twoKi > 0.0f) { if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / SENSORFS); // integral error scaled by Ki // integral error scaled by Ki
integralFBx += twoKi * halfex * (1.0f / SENSORFS);
integralFBy += twoKi * halfey * (1.0f / SENSORFS); integralFBy += twoKi * halfey * (1.0f / SENSORFS);
integralFBz += twoKi * halfez * (1.0f / SENSORFS); integralFBz += twoKi * halfez * (1.0f / SENSORFS);
gx += integralFBx; // apply integral feedback gx += integralFBx; // apply integral feedback
@ -217,8 +219,9 @@ void mahony_updateIMU(float gx, float gy, float gz, float ax, float ay, float az
float halfex, halfey, halfez; float halfex, halfey, halfez;
float qa, qb, qc; float qa, qb, qc;
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) // Compute feedback only if accelerometer measurement valid
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // (avoids NaN in accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement // Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az); recipNorm = invSqrt(ax * ax + ay * ay + az * az);
@ -238,7 +241,8 @@ void mahony_updateIMU(float gx, float gy, float gz, float ax, float ay, float az
// Compute and apply integral feedback if enabled // Compute and apply integral feedback if enabled
if(twoKi > 0.0f) { if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / SENSORFS); // integral error scaled by Ki // integral error scaled by Ki
integralFBx += twoKi * halfex * (1.0f / SENSORFS);
integralFBy += twoKi * halfey * (1.0f / SENSORFS); integralFBy += twoKi * halfey * (1.0f / SENSORFS);
integralFBz += twoKi * halfez * (1.0f / SENSORFS); integralFBz += twoKi * halfez * (1.0f / SENSORFS);
gx += integralFBx; // apply integral feedback gx += integralFBx; // apply integral feedback
@ -282,14 +286,17 @@ void mahony_updateIMU(float gx, float gy, float gz, float ax, float ay, float az
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
static float invSqrt(float x) { static float invSqrt(float x) {
union {
float f;
int32_t i;
} y;
float halfx = 0.5f * x; float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y; y.f = x;
i = 0x5f3759df - (i>>1); y.i = 0x5f375a86 - (y.i >> 1);
y = *(float*)&i; y.f = y.f * (1.5f - (halfx * y.f * y.f));
y = y * (1.5f - (halfx * y * y)); y.f = y.f * (1.5f - (halfx * y.f * y.f));
y = y * (1.5f - (halfx * y * y)); return y.f;
return y;
} }
//============================================================================================== //==============================================================================================