imu: clean MadgwickAHRSupdate

- remove unused q0 q1 q2 q3 global variables
- copy structs locally, work on local vars, then update struct at the
  end

diff --git a/imuboard/MadgwickAHRS.c b/imuboard/MadgwickAHRS.c
index 5f03d6f4b1dae4912e05aeff95cbfecdb401d4de..7043a7a2f2c9fa9766d2efda79f7e93d20745bad 100644 (file)
--- a/imuboard/MadgwickAHRS.c
+++ b/imuboard/MadgwickAHRS.c
@@ -39,10 +39,6 @@
 #define sampleFreq     85.0f           // sample frequency in Hz\r
 #define betaDef                0.1f            // 2 * proportional gain\r
 \r
-volatile float beta = betaDef;                                                         // 2 * proportional gain (Kp)\r
-volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;     // quaternion of sensor frame relative to auxiliary frame\r
-\r
-\r
 static float invSqrt(float x)\r
 {\r
        return 1.0f / sqrtf(x);\r
@@ -58,11 +54,8 @@ void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat)
        float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz;\r
        float _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3;\r
        float q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;\r
-       float mx, my, mz, ax, ay, az;\r
-       float q0 = quat->q0;\r
-       float q1 = quat->q1;\r
-       float q2 = quat->q2;\r
-       float q3 = quat->q3;\r
+       float mx, my, mz, ax, ay, az, gx, gy, gz;\r
+       float q0, q1, q2, q3;\r
 \r
        /* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in\r
         * magnetometer normalisation) */\r
@@ -71,29 +64,35 @@ void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat)
                return;\r
        }\r
 \r
+       /* use local variables, it's more readable */\r
+       q0 = quat->q0; q1 = quat->q1; q2 = quat->q2; q3 = quat->q3;\r
+       gx = imu->gx; gy = imu->gy; gz = imu->gz;\r
+       ax = imu->ax; ay = imu->ay; az = imu->az;\r
+       mx = imu->mx; my = imu->my; mz = imu->mz;\r
+\r
        /* Rate of change of quaternion from gyroscope */\r
-       qDot1 = 0.5f * (-q1 * imu->gx - q2 * imu->gy - q3 * imu->gz);\r
-       qDot2 = 0.5f * (q0 * imu->gx + q2 * imu->gz - q3 * imu->gy);\r
-       qDot3 = 0.5f * (q0 * imu->gy - q1 * imu->gz + q3 * imu->gx);\r
-       qDot4 = 0.5f * (q0 * imu->gz + q1 * imu->gy - q2 * imu->gx);\r
+       qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);\r
+       qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);\r
+       qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);\r
+       qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);\r
 \r
        /* Compute feedback only if accelerometer measurement valid (avoids NaN\r
         * in accelerometer normalisation) */\r
-       if (!((imu->ax == 0.0f) && (imu->ay == 0.0f) && (imu->az == 0.0f))) {\r
+       if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {\r
 \r
                /* Normalise accelerometer measurement */\r
-               recipNorm = invSqrt(imu->ax * imu->ax + imu->ay * imu->ay +\r
-                       imu->az * imu->az);\r
-               ax = imu->ax * recipNorm;\r
-               ay = imu->ay * recipNorm;\r
-               az = imu->az * recipNorm;\r
+               recipNorm = invSqrt(ax * ax + ay * ay +\r
+                       az * az);\r
+               ax *= recipNorm;\r
+               ay *= recipNorm;\r
+               az *= recipNorm;\r
 \r
                /* Normalise magnetometer measurement */\r
-               recipNorm = invSqrt(imu->mx * imu->mx + imu->my * imu->my +\r
-                       imu->mz * imu->mz);\r
-               mx = imu->mx * recipNorm;\r
-               my = imu->my * recipNorm;\r
-               mz = imu->mz * recipNorm;\r
+               recipNorm = invSqrt(mx * mx + my * my +\r
+                       mz * mz);\r
+               mx *= recipNorm;\r
+               my *= recipNorm;\r
+               mz *= recipNorm;\r
 \r
                /* Auxiliary variables to avoid repeated arithmetic */\r
                _2q0mx = 2.0f * q0 * mx;\r
@@ -139,10 +138,10 @@ void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat)
                s3 *= recipNorm;\r
 \r
                /* Apply feedback step */\r
-               qDot1 -= beta * s0;\r
-               qDot2 -= beta * s1;\r
-               qDot3 -= beta * s2;\r
-               qDot4 -= beta * s3;\r
+               qDot1 -= betaDef * s0;\r
+               qDot2 -= betaDef * s1;\r
+               qDot3 -= betaDef * s2;\r
+               qDot4 -= betaDef * s3;\r
        }\r
 \r
        /* Integrate rate of change of quaternion to yield quaternion */\r
@@ -153,10 +152,16 @@ void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat)
 \r
        /* Normalise quaternion */\r
        recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);\r
-       quat->q0 = q0 * recipNorm;\r
-       quat->q1 = q1 * recipNorm;\r
-       quat->q2 = q2 * recipNorm;\r
-       quat->q3 = q3 * recipNorm;\r
+       q0 *= recipNorm;\r
+       q1 *= recipNorm;\r
+       q2 *= recipNorm;\r
+       q3 *= recipNorm;\r
+\r
+       /* update quaternion in structure */\r
+       quat->q0 = q0;\r
+       quat->q1 = q1;\r
+       quat->q2 = q2;\r
+       quat->q3 = q3;\r
 }\r
 \r
 /* IMU algorithm update (does not take magneto in account) */\r
@@ -166,27 +171,29 @@ void MadgwickAHRSupdateIMU(const struct imu_info *imu, struct quaternion *quat)
        float s0, s1, s2, s3;\r
        float qDot1, qDot2, qDot3, qDot4;\r
        float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;\r
-       float ax, ay, az;\r
-       float q0 = quat->q0;\r
-       float q1 = quat->q1;\r
-       float q2 = quat->q2;\r
-       float q3 = quat->q3;\r
+       float ax, ay, az, gx, gy, gz;\r
+       float q0, q1, q2, q3;\r
+\r
+       /* use local variables, it's more readable */\r
+       q0 = quat->q0; q1 = quat->q1; q2 = quat->q2; q3 = quat->q3;\r
+       gx = imu->gx; gy = imu->gy; gz = imu->gz;\r
+       ax = imu->ax; ay = imu->ay; az = imu->az;\r
 \r
        /* Rate of change of quaternion from gyroscope */\r
-       qDot1 = 0.5f * (-q1 * imu->gx - q2 * imu->gy - q3 * imu->gz);\r
-       qDot2 = 0.5f * (q0 * imu->gx + q2 * imu->gz - q3 * imu->gy);\r
-       qDot3 = 0.5f * (q0 * imu->gy - q1 * imu->gz + q3 * imu->gx);\r
-       qDot4 = 0.5f * (q0 * imu->gz + q1 * imu->gy - q2 * imu->gx);\r
+       qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);\r
+       qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);\r
+       qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);\r
+       qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);\r
 \r
 \r
        /* Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) */\r
-       if(!((imu->ax == 0.0f) && (imu->ay == 0.0f) && (imu->az == 0.0f))) {\r
+       if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {\r
 \r
                /* Normalise accelerometer measurement */\r
-               recipNorm = invSqrt(imu->ax * imu->ax + imu->ay * imu->ay + imu->az * imu->az);\r
-               ax = imu->ax * recipNorm;\r
-               ay = imu->ay * recipNorm;\r
-               az = imu->az * recipNorm;\r
+               recipNorm = invSqrt(ax * ax + ay * ay + az * az);\r
+               ax *= recipNorm;\r
+               ay *= recipNorm;\r
+               az *= recipNorm;\r
 \r
                /* Auxiliary variables to avoid repeated arithmetic */\r
                _2q0 = 2.0f * q0;\r
@@ -216,10 +223,10 @@ void MadgwickAHRSupdateIMU(const struct imu_info *imu, struct quaternion *quat)
                s3 *= recipNorm;\r
 \r
                /* Apply feedback step */\r
-               qDot1 -= beta * s0;\r
-               qDot2 -= beta * s1;\r
-               qDot3 -= beta * s2;\r
-               qDot4 -= beta * s3;\r
+               qDot1 -= betaDef * s0;\r
+               qDot2 -= betaDef * s1;\r
+               qDot3 -= betaDef * s2;\r
+               qDot4 -= betaDef * s3;\r
        }\r
 \r
        /* Integrate rate of change of quaternion to yield quaternion */\r
@@ -231,9 +238,15 @@ void MadgwickAHRSupdateIMU(const struct imu_info *imu, struct quaternion *quat)
        /* Normalise quaternion */\r
        recipNorm = invSqrt(q0 * q0 + q1 * q1 +\r
                q2 * q2 + q3 * q3);\r
-       quat->q0 = q0 * recipNorm;\r
-       quat->q1 = q1 * recipNorm;\r
-       quat->q2 = q2 * recipNorm;\r
-       quat->q3 = q3 * recipNorm;\r
+       q0 *= recipNorm;\r
+       q1 *= recipNorm;\r
+       q2 *= recipNorm;\r
+       q3 *= recipNorm;\r
+\r
+       /* update quaternion in structure */\r
+       quat->q0 = q0;\r
+       quat->q1 = q1;\r
+       quat->q2 = q2;\r
+       quat->q3 = q3;\r
 }\r
 \r