From b97fa76596da6870c492efc1efd4aecea38bd579 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Thu, 18 Sep 2014 19:44:08 +0200 Subject: [PATCH] imu: clean MadgwickAHRSupdate - remove unused q0 q1 q2 q3 global variables - copy structs locally, work on local vars, then update struct at the end --- imuboard/MadgwickAHRS.c | 121 ++++++++++++++++++++++------------------ 1 file changed, 67 insertions(+), 54 deletions(-) diff --git a/imuboard/MadgwickAHRS.c b/imuboard/MadgwickAHRS.c index 5f03d6f..7043a7a 100644 --- a/imuboard/MadgwickAHRS.c +++ b/imuboard/MadgwickAHRS.c @@ -39,10 +39,6 @@ #define sampleFreq 85.0f // sample frequency in Hz #define betaDef 0.1f // 2 * proportional gain -volatile float beta = betaDef; // 2 * proportional gain (Kp) -volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame - - static float invSqrt(float x) { return 1.0f / sqrtf(x); @@ -58,11 +54,8 @@ void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat) float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz; float _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3; float q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; - float mx, my, mz, ax, ay, az; - float q0 = quat->q0; - float q1 = quat->q1; - float q2 = quat->q2; - float q3 = quat->q3; + float mx, my, mz, ax, ay, az, gx, gy, gz; + float q0, q1, q2, q3; /* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in * magnetometer normalisation) */ @@ -71,29 +64,35 @@ void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat) return; } + /* use local variables, it's more readable */ + q0 = quat->q0; q1 = quat->q1; q2 = quat->q2; q3 = quat->q3; + gx = imu->gx; gy = imu->gy; gz = imu->gz; + ax = imu->ax; ay = imu->ay; az = imu->az; + mx = imu->mx; my = imu->my; mz = imu->mz; + /* Rate of change of quaternion from gyroscope */ - qDot1 = 0.5f * (-q1 * imu->gx - q2 * imu->gy - q3 * imu->gz); - qDot2 = 0.5f * (q0 * imu->gx + q2 * imu->gz - q3 * imu->gy); - qDot3 = 0.5f * (q0 * imu->gy - q1 * imu->gz + q3 * imu->gx); - qDot4 = 0.5f * (q0 * imu->gz + q1 * imu->gy - q2 * imu->gx); + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); /* Compute feedback only if accelerometer measurement valid (avoids NaN * in accelerometer normalisation) */ - if (!((imu->ax == 0.0f) && (imu->ay == 0.0f) && (imu->az == 0.0f))) { + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { /* Normalise accelerometer measurement */ - recipNorm = invSqrt(imu->ax * imu->ax + imu->ay * imu->ay + - imu->az * imu->az); - ax = imu->ax * recipNorm; - ay = imu->ay * recipNorm; - az = imu->az * recipNorm; + recipNorm = invSqrt(ax * ax + ay * ay + + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; /* Normalise magnetometer measurement */ - recipNorm = invSqrt(imu->mx * imu->mx + imu->my * imu->my + - imu->mz * imu->mz); - mx = imu->mx * recipNorm; - my = imu->my * recipNorm; - mz = imu->mz * recipNorm; + recipNorm = invSqrt(mx * mx + my * my + + mz * mz); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; /* Auxiliary variables to avoid repeated arithmetic */ _2q0mx = 2.0f * q0 * mx; @@ -139,10 +138,10 @@ void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat) s3 *= recipNorm; /* Apply feedback step */ - qDot1 -= beta * s0; - qDot2 -= beta * s1; - qDot3 -= beta * s2; - qDot4 -= beta * s3; + qDot1 -= betaDef * s0; + qDot2 -= betaDef * s1; + qDot3 -= betaDef * s2; + qDot4 -= betaDef * s3; } /* Integrate rate of change of quaternion to yield quaternion */ @@ -153,10 +152,16 @@ void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat) /* Normalise quaternion */ recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - quat->q0 = q0 * recipNorm; - quat->q1 = q1 * recipNorm; - quat->q2 = q2 * recipNorm; - quat->q3 = q3 * recipNorm; + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + /* update quaternion in structure */ + quat->q0 = q0; + quat->q1 = q1; + quat->q2 = q2; + quat->q3 = q3; } /* IMU algorithm update (does not take magneto in account) */ @@ -166,27 +171,29 @@ void MadgwickAHRSupdateIMU(const struct imu_info *imu, struct quaternion *quat) float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; - float ax, ay, az; - float q0 = quat->q0; - float q1 = quat->q1; - float q2 = quat->q2; - float q3 = quat->q3; + float ax, ay, az, gx, gy, gz; + float q0, q1, q2, q3; + + /* use local variables, it's more readable */ + q0 = quat->q0; q1 = quat->q1; q2 = quat->q2; q3 = quat->q3; + gx = imu->gx; gy = imu->gy; gz = imu->gz; + ax = imu->ax; ay = imu->ay; az = imu->az; /* Rate of change of quaternion from gyroscope */ - qDot1 = 0.5f * (-q1 * imu->gx - q2 * imu->gy - q3 * imu->gz); - qDot2 = 0.5f * (q0 * imu->gx + q2 * imu->gz - q3 * imu->gy); - qDot3 = 0.5f * (q0 * imu->gy - q1 * imu->gz + q3 * imu->gx); - qDot4 = 0.5f * (q0 * imu->gz + q1 * imu->gy - q2 * imu->gx); + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); /* Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) */ - if(!((imu->ax == 0.0f) && (imu->ay == 0.0f) && (imu->az == 0.0f))) { + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { /* Normalise accelerometer measurement */ - recipNorm = invSqrt(imu->ax * imu->ax + imu->ay * imu->ay + imu->az * imu->az); - ax = imu->ax * recipNorm; - ay = imu->ay * recipNorm; - az = imu->az * recipNorm; + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; /* Auxiliary variables to avoid repeated arithmetic */ _2q0 = 2.0f * q0; @@ -216,10 +223,10 @@ void MadgwickAHRSupdateIMU(const struct imu_info *imu, struct quaternion *quat) s3 *= recipNorm; /* Apply feedback step */ - qDot1 -= beta * s0; - qDot2 -= beta * s1; - qDot3 -= beta * s2; - qDot4 -= beta * s3; + qDot1 -= betaDef * s0; + qDot2 -= betaDef * s1; + qDot3 -= betaDef * s2; + qDot4 -= betaDef * s3; } /* Integrate rate of change of quaternion to yield quaternion */ @@ -231,9 +238,15 @@ void MadgwickAHRSupdateIMU(const struct imu_info *imu, struct quaternion *quat) /* Normalise quaternion */ recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - quat->q0 = q0 * recipNorm; - quat->q1 = q1 * recipNorm; - quat->q2 = q2 * recipNorm; - quat->q3 = q3 * recipNorm; + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + /* update quaternion in structure */ + quat->q0 = q0; + quat->q1 = q1; + quat->q2 = q2; + quat->q3 = q3; } -- 2.39.5