Masahiro Furukawa / Mbed 2 deprecated MPU-9250-Ch2_AHRS_test

fork

Dependencies:   MPU9250_SPI mbed

Fork of MPU9250_AHRS by maedalab

MahonyAHRS.h@29:6075f35f472f, 2016-07-06 (annotated)

Committer:
uribotail
Date:
Wed Jul 06 11:44:22 2016 +0000
Revision:
29:6075f35f472f
Parent:
28:76e2ba7a1ecd 
20:44wada debag now

Who changed what in which revision?

UserRevisionLine numberNew contents of line
uribotail 29:6075f35f472f 1 //=====================================================================================================
uribotail 29:6075f35f472f 2 // MahonyAHRS.h
uribotail 29:6075f35f472f 3 //=====================================================================================================
uribotail 29:6075f35f472f 4 //
uribotail 29:6075f35f472f 5 // Madgwick's implementation of Mayhony's AHRS algorithm.
uribotail 29:6075f35f472f 6 // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
uribotail 29:6075f35f472f 7 //
uribotail 29:6075f35f472f 8 // Date Author Notes
uribotail 29:6075f35f472f 9 // 29/09/2011 SOH Madgwick Initial release
uribotail 29:6075f35f472f 10 // 02/10/2011 SOH Madgwick Optimised for reduced CPU load
uribotail 29:6075f35f472f 11 //
uribotail 29:6075f35f472f 12 //=====================================================================================================
uribotail 27:7dd32c696d17 13
uribotail 27:7dd32c696d17 14 //----------------------------------------------------------------------------------------------------
uribotail 29:6075f35f472f 15 // Variable declaration
uribotail 29:6075f35f472f 16
uribotail 29:6075f35f472f 17 extern volatile float twoKp; // 2 * proportional gain (Kp)
uribotail 29:6075f35f472f 18 extern volatile float twoKi; // 2 * integral gain (Ki)
uribotail 29:6075f35f472f 19 extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
uribotail 27:7dd32c696d17 20
uribotail 27:7dd32c696d17 21 //---------------------------------------------------------------------------------------------------
uribotail 27:7dd32c696d17 22 // Function declarations
uribotail 27:7dd32c696d17 23
uribotail 29:6075f35f472f 24 void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
uribotail 29:6075f35f472f 25 void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
uribotail 29:6075f35f472f 26
uribotail 29:6075f35f472f 27
uribotail 29:6075f35f472f 28 #include <math.h>
uribotail 29:6075f35f472f 29
uribotail 29:6075f35f472f 30 //---------------------------------------------------------------------------------------------------
uribotail 29:6075f35f472f 31 // Definitions
uribotail 29:6075f35f472f 32
uribotail 29:6075f35f472f 33 #define sampleFreq 512.0f // sample frequency in Hz
uribotail 29:6075f35f472f 34 #define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
uribotail 29:6075f35f472f 35 #define twoKiDef (2.0f * 0.0f) // 2 * integral gain
uribotail 27:7dd32c696d17 36
uribotail 29:6075f35f472f 37 //---------------------------------------------------------------------------------------------------
uribotail 29:6075f35f472f 38 // Variable definitions
uribotail 29:6075f35f472f 39
uribotail 29:6075f35f472f 40 volatile float twoKp = twoKpDef; // 2 * proportional gain (Kp)
uribotail 29:6075f35f472f 41 volatile float twoKi = twoKiDef; // 2 * integral gain (Ki)
uribotail 29:6075f35f472f 42 volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
uribotail 29:6075f35f472f 43 volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
uribotail 29:6075f35f472f 44
uribotail 29:6075f35f472f 45 //---------------------------------------------------------------------------------------------------
uribotail 29:6075f35f472f 46 // Function declarations
uribotail 29:6075f35f472f 47
uribotail 29:6075f35f472f 48 float invSqrt(float x);
uribotail 27:7dd32c696d17 49
uribotail 27:7dd32c696d17 50 //====================================================================================================
uribotail 27:7dd32c696d17 51 // Functions
uribotail 27:7dd32c696d17 52
uribotail 27:7dd32c696d17 53 //---------------------------------------------------------------------------------------------------
uribotail 27:7dd32c696d17 54 // AHRS algorithm update
uribotail 27:7dd32c696d17 55
uribotail 29:6075f35f472f 56 void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
uribotail 27:7dd32c696d17 57 float recipNorm;
uribotail 27:7dd32c696d17 58 float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
uribotail 27:7dd32c696d17 59 float hx, hy, bx, bz;
uribotail 27:7dd32c696d17 60 float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
uribotail 27:7dd32c696d17 61 float halfex, halfey, halfez;
uribotail 27:7dd32c696d17 62 float qa, qb, qc;
uribotail 27:7dd32c696d17 63
uribotail 27:7dd32c696d17 64 // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
uribotail 27:7dd32c696d17 65 if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
uribotail 27:7dd32c696d17 66 MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
uribotail 27:7dd32c696d17 67 return;
uribotail 27:7dd32c696d17 68 }
uribotail 27:7dd32c696d17 69
uribotail 27:7dd32c696d17 70 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
uribotail 27:7dd32c696d17 71 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
uribotail 27:7dd32c696d17 72
uribotail 27:7dd32c696d17 73 // Normalise accelerometer measurement
uribotail 27:7dd32c696d17 74 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
uribotail 27:7dd32c696d17 75 ax *= recipNorm;
uribotail 27:7dd32c696d17 76 ay *= recipNorm;
uribotail 27:7dd32c696d17 77 az *= recipNorm;
uribotail 27:7dd32c696d17 78
uribotail 27:7dd32c696d17 79 // Normalise magnetometer measurement
uribotail 27:7dd32c696d17 80 recipNorm = invSqrt(mx * mx + my * my + mz * mz);
uribotail 27:7dd32c696d17 81 mx *= recipNorm;
uribotail 27:7dd32c696d17 82 my *= recipNorm;
uribotail 27:7dd32c696d17 83 mz *= recipNorm;
uribotail 27:7dd32c696d17 84
uribotail 27:7dd32c696d17 85 // Auxiliary variables to avoid repeated arithmetic
uribotail 27:7dd32c696d17 86 q0q0 = q0 * q0;
uribotail 27:7dd32c696d17 87 q0q1 = q0 * q1;
uribotail 27:7dd32c696d17 88 q0q2 = q0 * q2;
uribotail 27:7dd32c696d17 89 q0q3 = q0 * q3;
uribotail 27:7dd32c696d17 90 q1q1 = q1 * q1;
uribotail 27:7dd32c696d17 91 q1q2 = q1 * q2;
uribotail 27:7dd32c696d17 92 q1q3 = q1 * q3;
uribotail 27:7dd32c696d17 93 q2q2 = q2 * q2;
uribotail 27:7dd32c696d17 94 q2q3 = q2 * q3;
uribotail 27:7dd32c696d17 95 q3q3 = q3 * q3;
uribotail 27:7dd32c696d17 96
uribotail 27:7dd32c696d17 97 // Reference direction of Earth's magnetic field
uribotail 27:7dd32c696d17 98 hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
uribotail 27:7dd32c696d17 99 hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
uribotail 27:7dd32c696d17 100 bx = sqrt(hx * hx + hy * hy);
uribotail 27:7dd32c696d17 101 bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
uribotail 27:7dd32c696d17 102
uribotail 27:7dd32c696d17 103 // Estimated direction of gravity and magnetic field
uribotail 27:7dd32c696d17 104 halfvx = q1q3 - q0q2;
uribotail 27:7dd32c696d17 105 halfvy = q0q1 + q2q3;
uribotail 27:7dd32c696d17 106 halfvz = q0q0 - 0.5f + q3q3;
uribotail 27:7dd32c696d17 107 halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
uribotail 27:7dd32c696d17 108 halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
uribotail 27:7dd32c696d17 109 halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
uribotail 27:7dd32c696d17 110
uribotail 27:7dd32c696d17 111 // Error is sum of cross product between estimated direction and measured direction of field vectors
uribotail 27:7dd32c696d17 112 halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
uribotail 27:7dd32c696d17 113 halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
uribotail 27:7dd32c696d17 114 halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
uribotail 27:7dd32c696d17 115
uribotail 27:7dd32c696d17 116 // Compute and apply integral feedback if enabled
uribotail 27:7dd32c696d17 117 if(twoKi > 0.0f) {
uribotail 27:7dd32c696d17 118 integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
uribotail 27:7dd32c696d17 119 integralFBy += twoKi * halfey * (1.0f / sampleFreq);
uribotail 27:7dd32c696d17 120 integralFBz += twoKi * halfez * (1.0f / sampleFreq);
uribotail 27:7dd32c696d17 121 gx += integralFBx; // apply integral feedback
uribotail 27:7dd32c696d17 122 gy += integralFBy;
uribotail 27:7dd32c696d17 123 gz += integralFBz;
uribotail 27:7dd32c696d17 124 }
uribotail 27:7dd32c696d17 125 else {
uribotail 27:7dd32c696d17 126 integralFBx = 0.0f; // prevent integral windup
uribotail 27:7dd32c696d17 127 integralFBy = 0.0f;
uribotail 27:7dd32c696d17 128 integralFBz = 0.0f;
uribotail 27:7dd32c696d17 129 }
uribotail 27:7dd32c696d17 130
uribotail 27:7dd32c696d17 131 // Apply proportional feedback
uribotail 27:7dd32c696d17 132 gx += twoKp * halfex;
uribotail 27:7dd32c696d17 133 gy += twoKp * halfey;
uribotail 27:7dd32c696d17 134 gz += twoKp * halfez;
uribotail 27:7dd32c696d17 135 }
uribotail 27:7dd32c696d17 136
uribotail 27:7dd32c696d17 137 // Integrate rate of change of quaternion
uribotail 27:7dd32c696d17 138 gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
uribotail 27:7dd32c696d17 139 gy *= (0.5f * (1.0f / sampleFreq));
uribotail 27:7dd32c696d17 140 gz *= (0.5f * (1.0f / sampleFreq));
uribotail 27:7dd32c696d17 141 qa = q0;
uribotail 27:7dd32c696d17 142 qb = q1;
uribotail 27:7dd32c696d17 143 qc = q2;
uribotail 27:7dd32c696d17 144 q0 += (-qb * gx - qc * gy - q3 * gz);
uribotail 27:7dd32c696d17 145 q1 += (qa * gx + qc * gz - q3 * gy);
uribotail 27:7dd32c696d17 146 q2 += (qa * gy - qb * gz + q3 * gx);
uribotail 27:7dd32c696d17 147 q3 += (qa * gz + qb * gy - qc * gx);
uribotail 27:7dd32c696d17 148
uribotail 27:7dd32c696d17 149 // Normalise quaternion
uribotail 27:7dd32c696d17 150 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
uribotail 27:7dd32c696d17 151 q0 *= recipNorm;
uribotail 27:7dd32c696d17 152 q1 *= recipNorm;
uribotail 27:7dd32c696d17 153 q2 *= recipNorm;
uribotail 27:7dd32c696d17 154 q3 *= recipNorm;
uribotail 27:7dd32c696d17 155 }
uribotail 27:7dd32c696d17 156
uribotail 27:7dd32c696d17 157 //---------------------------------------------------------------------------------------------------
uribotail 27:7dd32c696d17 158 // IMU algorithm update
uribotail 27:7dd32c696d17 159
uribotail 29:6075f35f472f 160 void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
uribotail 27:7dd32c696d17 161 float recipNorm;
uribotail 27:7dd32c696d17 162 float halfvx, halfvy, halfvz;
uribotail 27:7dd32c696d17 163 float halfex, halfey, halfez;
uribotail 27:7dd32c696d17 164 float qa, qb, qc;
uribotail 27:7dd32c696d17 165
uribotail 27:7dd32c696d17 166 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
uribotail 27:7dd32c696d17 167 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
uribotail 27:7dd32c696d17 168
uribotail 27:7dd32c696d17 169 // Normalise accelerometer measurement
uribotail 27:7dd32c696d17 170 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
uribotail 27:7dd32c696d17 171 ax *= recipNorm;
uribotail 27:7dd32c696d17 172 ay *= recipNorm;
uribotail 27:7dd32c696d17 173 az *= recipNorm;
uribotail 27:7dd32c696d17 174
uribotail 27:7dd32c696d17 175 // Estimated direction of gravity and vector perpendicular to magnetic flux
uribotail 27:7dd32c696d17 176 halfvx = q1 * q3 - q0 * q2;
uribotail 27:7dd32c696d17 177 halfvy = q0 * q1 + q2 * q3;
uribotail 27:7dd32c696d17 178 halfvz = q0 * q0 - 0.5f + q3 * q3;
uribotail 27:7dd32c696d17 179
uribotail 27:7dd32c696d17 180 // Error is sum of cross product between estimated and measured direction of gravity
uribotail 27:7dd32c696d17 181 halfex = (ay * halfvz - az * halfvy);
uribotail 27:7dd32c696d17 182 halfey = (az * halfvx - ax * halfvz);
uribotail 27:7dd32c696d17 183 halfez = (ax * halfvy - ay * halfvx);
uribotail 27:7dd32c696d17 184
uribotail 27:7dd32c696d17 185 // Compute and apply integral feedback if enabled
uribotail 27:7dd32c696d17 186 if(twoKi > 0.0f) {
uribotail 27:7dd32c696d17 187 integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
uribotail 27:7dd32c696d17 188 integralFBy += twoKi * halfey * (1.0f / sampleFreq);
uribotail 27:7dd32c696d17 189 integralFBz += twoKi * halfez * (1.0f / sampleFreq);
uribotail 27:7dd32c696d17 190 gx += integralFBx; // apply integral feedback
uribotail 27:7dd32c696d17 191 gy += integralFBy;
uribotail 27:7dd32c696d17 192 gz += integralFBz;
uribotail 27:7dd32c696d17 193 }
uribotail 27:7dd32c696d17 194 else {
uribotail 27:7dd32c696d17 195 integralFBx = 0.0f; // prevent integral windup
uribotail 27:7dd32c696d17 196 integralFBy = 0.0f;
uribotail 27:7dd32c696d17 197 integralFBz = 0.0f;
uribotail 27:7dd32c696d17 198 }
uribotail 27:7dd32c696d17 199
uribotail 27:7dd32c696d17 200 // Apply proportional feedback
uribotail 27:7dd32c696d17 201 gx += twoKp * halfex;
uribotail 27:7dd32c696d17 202 gy += twoKp * halfey;
uribotail 27:7dd32c696d17 203 gz += twoKp * halfez;
uribotail 27:7dd32c696d17 204 }
uribotail 27:7dd32c696d17 205
uribotail 27:7dd32c696d17 206 // Integrate rate of change of quaternion
uribotail 27:7dd32c696d17 207 gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
uribotail 27:7dd32c696d17 208 gy *= (0.5f * (1.0f / sampleFreq));
uribotail 27:7dd32c696d17 209 gz *= (0.5f * (1.0f / sampleFreq));
uribotail 27:7dd32c696d17 210 qa = q0;
uribotail 27:7dd32c696d17 211 qb = q1;
uribotail 27:7dd32c696d17 212 qc = q2;
uribotail 27:7dd32c696d17 213 q0 += (-qb * gx - qc * gy - q3 * gz);
uribotail 27:7dd32c696d17 214 q1 += (qa * gx + qc * gz - q3 * gy);
uribotail 27:7dd32c696d17 215 q2 += (qa * gy - qb * gz + q3 * gx);
uribotail 27:7dd32c696d17 216 q3 += (qa * gz + qb * gy - qc * gx);
uribotail 27:7dd32c696d17 217
uribotail 27:7dd32c696d17 218 // Normalise quaternion
uribotail 27:7dd32c696d17 219 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
uribotail 27:7dd32c696d17 220 q0 *= recipNorm;
uribotail 27:7dd32c696d17 221 q1 *= recipNorm;
uribotail 27:7dd32c696d17 222 q2 *= recipNorm;
uribotail 27:7dd32c696d17 223 q3 *= recipNorm;
uribotail 27:7dd32c696d17 224 }
uribotail 27:7dd32c696d17 225
uribotail 27:7dd32c696d17 226 //---------------------------------------------------------------------------------------------------
uribotail 27:7dd32c696d17 227 // Fast inverse square-root
uribotail 27:7dd32c696d17 228 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
uribotail 27:7dd32c696d17 229
uribotail 29:6075f35f472f 230 float invSqrt(float x) {
uribotail 27:7dd32c696d17 231 float halfx = 0.5f * x;
uribotail 27:7dd32c696d17 232 float y = x;
uribotail 27:7dd32c696d17 233 long i = *(long*)&y;
uribotail 27:7dd32c696d17 234 i = 0x5f3759df - (i>>1);
uribotail 27:7dd32c696d17 235 y = *(float*)&i;
uribotail 27:7dd32c696d17 236 y = y * (1.5f - (halfx * y * y));
uribotail 27:7dd32c696d17 237 return y;
uribotail 27:7dd32c696d17 238 }
uribotail 27:7dd32c696d17 239
uribotail 27:7dd32c696d17 240 //====================================================================================================
uribotail 27:7dd32c696d17 241 // END OF CODE
uribotail 27:7dd32c696d17 242 //====================================================================================================
uribotail 27:7dd32c696d17 243
uribotail 27:7dd32c696d17 244 //=====================================================================================================
uribotail 27:7dd32c696d17 245 // End of file
uribotail 27:7dd32c696d17 246 //=====================================================================================================
uribotail 27:7dd32c696d17 247