maedalab / Mbed 2 deprecated MPU9250_AHRS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MPU9250_SPI mbed

MahonyAHRS.h@28:76e2ba7a1ecd, 2016-07-06 (annotated)

Committer:
uribotail
Date:
Wed Jul 06 10:02:49 2016 +0000
Revision:
28:76e2ba7a1ecd
Parent:
27:7dd32c696d17
Child:
29:6075f35f472f
Mahony work slow

Who changed what in which revision?

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