Erik van de Coevering / Madgwick

Library implementing Madgwick's IMU and AHRS algorithms

Dependents:   Hexi_GPSIMU_Hotshoe

MadgwickAHRS.cpp@1:d7c70d593694, 2018-07-12 (annotated)

Committer:
Anaesthetix
Date:
Thu Jul 12 14:00:30 2018 +0000
Revision:
1:d7c70d593694
Parent:
0:9b434b5e28d4 
Corrected an error in the algorithm and added a regular square root instead of the fast square root as it does not increase processing time by that much.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Anaesthetix 0:9b434b5e28d4 1 //=============================================================================================
Anaesthetix 0:9b434b5e28d4 2 // MadgwickAHRS.c
Anaesthetix 0:9b434b5e28d4 3 //=============================================================================================
Anaesthetix 0:9b434b5e28d4 4 //
Anaesthetix 0:9b434b5e28d4 5 // Implementation of Madgwick's IMU and AHRS algorithms.
Anaesthetix 0:9b434b5e28d4 6 // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
Anaesthetix 0:9b434b5e28d4 7 //
Anaesthetix 0:9b434b5e28d4 8 // From the x-io website "Open-source resources available on this website are
Anaesthetix 0:9b434b5e28d4 9 // provided under the GNU General Public Licence unless an alternative licence
Anaesthetix 0:9b434b5e28d4 10 // is provided in source."
Anaesthetix 0:9b434b5e28d4 11 //
Anaesthetix 0:9b434b5e28d4 12 // Date Author Notes
Anaesthetix 0:9b434b5e28d4 13 // 29/09/2011 SOH Madgwick Initial release
Anaesthetix 0:9b434b5e28d4 14 // 02/10/2011 SOH Madgwick Optimised for reduced CPU load
Anaesthetix 0:9b434b5e28d4 15 // 19/02/2012 SOH Madgwick Magnetometer measurement is normalised
Anaesthetix 0:9b434b5e28d4 16 //
Anaesthetix 0:9b434b5e28d4 17 //=============================================================================================
Anaesthetix 0:9b434b5e28d4 18
Anaesthetix 0:9b434b5e28d4 19 //-------------------------------------------------------------------------------------------
Anaesthetix 0:9b434b5e28d4 20 // Header files
Anaesthetix 0:9b434b5e28d4 21
Anaesthetix 0:9b434b5e28d4 22 #include "MadgwickAHRS.h"
Anaesthetix 0:9b434b5e28d4 23 #include <math.h>
Anaesthetix 0:9b434b5e28d4 24
Anaesthetix 0:9b434b5e28d4 25 //-------------------------------------------------------------------------------------------
Anaesthetix 0:9b434b5e28d4 26 // Definitions
Anaesthetix 0:9b434b5e28d4 27
Anaesthetix 1:d7c70d593694 28 #define sampleFreqDef 1500.0f // sample frequency in Hz
Anaesthetix 1:d7c70d593694 29 #define betaDef 0.1f // 2 * proportional gain 0.1 - 0.5 - 5 was 2.5
Anaesthetix 0:9b434b5e28d4 30
Anaesthetix 0:9b434b5e28d4 31
Anaesthetix 0:9b434b5e28d4 32 //============================================================================================
Anaesthetix 0:9b434b5e28d4 33 // Functions
Anaesthetix 0:9b434b5e28d4 34
Anaesthetix 0:9b434b5e28d4 35 //-------------------------------------------------------------------------------------------
Anaesthetix 0:9b434b5e28d4 36 // AHRS algorithm update
Anaesthetix 0:9b434b5e28d4 37
Anaesthetix 1:d7c70d593694 38 Madgwick::Madgwick()
Anaesthetix 1:d7c70d593694 39 {
Anaesthetix 0:9b434b5e28d4 40 beta = betaDef;
Anaesthetix 0:9b434b5e28d4 41 q0 = 1.0f;
Anaesthetix 0:9b434b5e28d4 42 q1 = 0.0f;
Anaesthetix 0:9b434b5e28d4 43 q2 = 0.0f;
Anaesthetix 0:9b434b5e28d4 44 q3 = 0.0f;
Anaesthetix 0:9b434b5e28d4 45 invSampleFreq = 1.0f / sampleFreqDef;
Anaesthetix 0:9b434b5e28d4 46 anglesComputed = 0;
Anaesthetix 0:9b434b5e28d4 47 }
Anaesthetix 0:9b434b5e28d4 48
Anaesthetix 1:d7c70d593694 49 void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
Anaesthetix 1:d7c70d593694 50 {
Anaesthetix 0:9b434b5e28d4 51 float recipNorm;
Anaesthetix 0:9b434b5e28d4 52 float s0, s1, s2, s3;
Anaesthetix 0:9b434b5e28d4 53 float qDot1, qDot2, qDot3, qDot4;
Anaesthetix 0:9b434b5e28d4 54 float hx, hy;
Anaesthetix 1:d7c70d593694 55 float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _8bx, _8bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
Anaesthetix 0:9b434b5e28d4 56
Anaesthetix 0:9b434b5e28d4 57 // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
Anaesthetix 0:9b434b5e28d4 58 if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
Anaesthetix 0:9b434b5e28d4 59 updateIMU(gx, gy, gz, ax, ay, az);
Anaesthetix 0:9b434b5e28d4 60 return;
Anaesthetix 0:9b434b5e28d4 61 }
Anaesthetix 0:9b434b5e28d4 62
Anaesthetix 0:9b434b5e28d4 63 // Convert gyroscope degrees/sec to radians/sec
Anaesthetix 0:9b434b5e28d4 64 gx *= 0.0174533f;
Anaesthetix 0:9b434b5e28d4 65 gy *= 0.0174533f;
Anaesthetix 0:9b434b5e28d4 66 gz *= 0.0174533f;
Anaesthetix 0:9b434b5e28d4 67
Anaesthetix 0:9b434b5e28d4 68 // Rate of change of quaternion from gyroscope
Anaesthetix 0:9b434b5e28d4 69 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
Anaesthetix 0:9b434b5e28d4 70 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
Anaesthetix 0:9b434b5e28d4 71 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
Anaesthetix 0:9b434b5e28d4 72 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
Anaesthetix 0:9b434b5e28d4 73
Anaesthetix 0:9b434b5e28d4 74 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
Anaesthetix 0:9b434b5e28d4 75 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
Anaesthetix 0:9b434b5e28d4 76
Anaesthetix 0:9b434b5e28d4 77 // Normalise accelerometer measurement
Anaesthetix 0:9b434b5e28d4 78 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
Anaesthetix 0:9b434b5e28d4 79 ax *= recipNorm;
Anaesthetix 0:9b434b5e28d4 80 ay *= recipNorm;
Anaesthetix 0:9b434b5e28d4 81 az *= recipNorm;
Anaesthetix 0:9b434b5e28d4 82
Anaesthetix 0:9b434b5e28d4 83 // Normalise magnetometer measurement
Anaesthetix 0:9b434b5e28d4 84 recipNorm = invSqrt(mx * mx + my * my + mz * mz);
Anaesthetix 0:9b434b5e28d4 85 mx *= recipNorm;
Anaesthetix 0:9b434b5e28d4 86 my *= recipNorm;
Anaesthetix 0:9b434b5e28d4 87 mz *= recipNorm;
Anaesthetix 0:9b434b5e28d4 88
Anaesthetix 0:9b434b5e28d4 89 // Auxiliary variables to avoid repeated arithmetic
Anaesthetix 0:9b434b5e28d4 90 _2q0mx = 2.0f * q0 * mx;
Anaesthetix 0:9b434b5e28d4 91 _2q0my = 2.0f * q0 * my;
Anaesthetix 0:9b434b5e28d4 92 _2q0mz = 2.0f * q0 * mz;
Anaesthetix 0:9b434b5e28d4 93 _2q1mx = 2.0f * q1 * mx;
Anaesthetix 0:9b434b5e28d4 94 _2q0 = 2.0f * q0;
Anaesthetix 0:9b434b5e28d4 95 _2q1 = 2.0f * q1;
Anaesthetix 0:9b434b5e28d4 96 _2q2 = 2.0f * q2;
Anaesthetix 0:9b434b5e28d4 97 _2q3 = 2.0f * q3;
Anaesthetix 0:9b434b5e28d4 98 _2q0q2 = 2.0f * q0 * q2;
Anaesthetix 0:9b434b5e28d4 99 _2q2q3 = 2.0f * q2 * q3;
Anaesthetix 0:9b434b5e28d4 100 q0q0 = q0 * q0;
Anaesthetix 0:9b434b5e28d4 101 q0q1 = q0 * q1;
Anaesthetix 0:9b434b5e28d4 102 q0q2 = q0 * q2;
Anaesthetix 0:9b434b5e28d4 103 q0q3 = q0 * q3;
Anaesthetix 0:9b434b5e28d4 104 q1q1 = q1 * q1;
Anaesthetix 0:9b434b5e28d4 105 q1q2 = q1 * q2;
Anaesthetix 0:9b434b5e28d4 106 q1q3 = q1 * q3;
Anaesthetix 0:9b434b5e28d4 107 q2q2 = q2 * q2;
Anaesthetix 0:9b434b5e28d4 108 q2q3 = q2 * q3;
Anaesthetix 0:9b434b5e28d4 109 q3q3 = q3 * q3;
Anaesthetix 1:d7c70d593694 110 /*
Anaesthetix 1:d7c70d593694 111 // Reference direction of Earth's magnetic field
Anaesthetix 1:d7c70d593694 112 hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
Anaesthetix 1:d7c70d593694 113 hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
Anaesthetix 1:d7c70d593694 114 _2bx = sqrtf(hx * hx + hy * hy);
Anaesthetix 1:d7c70d593694 115 _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
Anaesthetix 1:d7c70d593694 116 _4bx = 2.0f * _2bx;
Anaesthetix 1:d7c70d593694 117 _4bz = 2.0f * _2bz;
Anaesthetix 0:9b434b5e28d4 118
Anaesthetix 1:d7c70d593694 119 // Gradient decent algorithm corrective step
Anaesthetix 1:d7c70d593694 120 s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
Anaesthetix 1:d7c70d593694 121 s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
Anaesthetix 1:d7c70d593694 122 s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
Anaesthetix 1:d7c70d593694 123 s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
Anaesthetix 1:d7c70d593694 124 */
Anaesthetix 0:9b434b5e28d4 125 // Reference direction of Earth's magnetic field
Anaesthetix 0:9b434b5e28d4 126 hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
Anaesthetix 0:9b434b5e28d4 127 hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
Anaesthetix 1:d7c70d593694 128 _2bx = sqrt(hx * hx + hy * hy);
Anaesthetix 0:9b434b5e28d4 129 _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
Anaesthetix 0:9b434b5e28d4 130 _4bx = 2.0f * _2bx;
Anaesthetix 0:9b434b5e28d4 131 _4bz = 2.0f * _2bz;
Anaesthetix 1:d7c70d593694 132 _8bx = 2.0f * _4bx;
Anaesthetix 1:d7c70d593694 133 _8bz = 2.0f * _4bz;
Anaesthetix 0:9b434b5e28d4 134
Anaesthetix 0:9b434b5e28d4 135 // Gradient decent algorithm corrective step
Anaesthetix 1:d7c70d593694 136 s0= -_2q2*(2.0f*(q1q3 - q0q2) - ax) + _2q1*(2.0f*(q0q1 + q2q3) - ay) + -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx) + (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my) + _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
Anaesthetix 1:d7c70d593694 137 s1= _2q3*(2.0f*(q1q3 - q0q2) - ax) + _2q0*(2.0f*(q0q1 + q2q3) - ay) + -4.0f*q1*(2.0f*(0.5 - q1q1 - q2q2) - az) + _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx) + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my) + (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
Anaesthetix 1:d7c70d593694 138 s2= -_2q0*(2.0f*(q1q3 - q0q2) - ax) + _2q3*(2.0f*(q0q1 + q2q3) - ay) + (-4.0f*q2)*(2.0f*(0.5 - q1q1 - q2q2) - az) + (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
Anaesthetix 1:d7c70d593694 139 s3= _2q1*(2.0f*(q1q3 - q0q2) - ax) + _2q2*(2.0f*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
Anaesthetix 0:9b434b5e28d4 140 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
Anaesthetix 0:9b434b5e28d4 141 s0 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 142 s1 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 143 s2 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 144 s3 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 145
Anaesthetix 0:9b434b5e28d4 146 // Apply feedback step
Anaesthetix 0:9b434b5e28d4 147 qDot1 -= beta * s0;
Anaesthetix 0:9b434b5e28d4 148 qDot2 -= beta * s1;
Anaesthetix 0:9b434b5e28d4 149 qDot3 -= beta * s2;
Anaesthetix 0:9b434b5e28d4 150 qDot4 -= beta * s3;
Anaesthetix 0:9b434b5e28d4 151 }
Anaesthetix 0:9b434b5e28d4 152
Anaesthetix 0:9b434b5e28d4 153 // Integrate rate of change of quaternion to yield quaternion
Anaesthetix 0:9b434b5e28d4 154 q0 += qDot1 * invSampleFreq;
Anaesthetix 0:9b434b5e28d4 155 q1 += qDot2 * invSampleFreq;
Anaesthetix 0:9b434b5e28d4 156 q2 += qDot3 * invSampleFreq;
Anaesthetix 0:9b434b5e28d4 157 q3 += qDot4 * invSampleFreq;
Anaesthetix 0:9b434b5e28d4 158
Anaesthetix 0:9b434b5e28d4 159 // Normalise quaternion
Anaesthetix 0:9b434b5e28d4 160 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
Anaesthetix 0:9b434b5e28d4 161 q0 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 162 q1 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 163 q2 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 164 q3 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 165 anglesComputed = 0;
Anaesthetix 0:9b434b5e28d4 166 }
Anaesthetix 0:9b434b5e28d4 167
Anaesthetix 0:9b434b5e28d4 168 //-------------------------------------------------------------------------------------------
Anaesthetix 0:9b434b5e28d4 169 // IMU algorithm update
Anaesthetix 0:9b434b5e28d4 170
Anaesthetix 1:d7c70d593694 171 void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az)
Anaesthetix 1:d7c70d593694 172 {
Anaesthetix 0:9b434b5e28d4 173 float recipNorm;
Anaesthetix 0:9b434b5e28d4 174 float s0, s1, s2, s3;
Anaesthetix 0:9b434b5e28d4 175 float qDot1, qDot2, qDot3, qDot4;
Anaesthetix 0:9b434b5e28d4 176 float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
Anaesthetix 0:9b434b5e28d4 177
Anaesthetix 0:9b434b5e28d4 178 // Convert gyroscope degrees/sec to radians/sec
Anaesthetix 0:9b434b5e28d4 179 gx *= 0.0174533f;
Anaesthetix 0:9b434b5e28d4 180 gy *= 0.0174533f;
Anaesthetix 0:9b434b5e28d4 181 gz *= 0.0174533f;
Anaesthetix 0:9b434b5e28d4 182
Anaesthetix 0:9b434b5e28d4 183 // Rate of change of quaternion from gyroscope
Anaesthetix 0:9b434b5e28d4 184 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
Anaesthetix 0:9b434b5e28d4 185 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
Anaesthetix 0:9b434b5e28d4 186 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
Anaesthetix 0:9b434b5e28d4 187 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
Anaesthetix 0:9b434b5e28d4 188
Anaesthetix 0:9b434b5e28d4 189 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
Anaesthetix 0:9b434b5e28d4 190 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
Anaesthetix 0:9b434b5e28d4 191
Anaesthetix 0:9b434b5e28d4 192 // Normalise accelerometer measurement
Anaesthetix 0:9b434b5e28d4 193 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
Anaesthetix 0:9b434b5e28d4 194 ax *= recipNorm;
Anaesthetix 0:9b434b5e28d4 195 ay *= recipNorm;
Anaesthetix 0:9b434b5e28d4 196 az *= recipNorm;
Anaesthetix 0:9b434b5e28d4 197
Anaesthetix 0:9b434b5e28d4 198 // Auxiliary variables to avoid repeated arithmetic
Anaesthetix 0:9b434b5e28d4 199 _2q0 = 2.0f * q0;
Anaesthetix 0:9b434b5e28d4 200 _2q1 = 2.0f * q1;
Anaesthetix 0:9b434b5e28d4 201 _2q2 = 2.0f * q2;
Anaesthetix 0:9b434b5e28d4 202 _2q3 = 2.0f * q3;
Anaesthetix 0:9b434b5e28d4 203 _4q0 = 4.0f * q0;
Anaesthetix 0:9b434b5e28d4 204 _4q1 = 4.0f * q1;
Anaesthetix 0:9b434b5e28d4 205 _4q2 = 4.0f * q2;
Anaesthetix 0:9b434b5e28d4 206 _8q1 = 8.0f * q1;
Anaesthetix 0:9b434b5e28d4 207 _8q2 = 8.0f * q2;
Anaesthetix 0:9b434b5e28d4 208 q0q0 = q0 * q0;
Anaesthetix 0:9b434b5e28d4 209 q1q1 = q1 * q1;
Anaesthetix 0:9b434b5e28d4 210 q2q2 = q2 * q2;
Anaesthetix 0:9b434b5e28d4 211 q3q3 = q3 * q3;
Anaesthetix 0:9b434b5e28d4 212
Anaesthetix 0:9b434b5e28d4 213 // Gradient decent algorithm corrective step
Anaesthetix 0:9b434b5e28d4 214 s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
Anaesthetix 0:9b434b5e28d4 215 s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
Anaesthetix 0:9b434b5e28d4 216 s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
Anaesthetix 0:9b434b5e28d4 217 s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
Anaesthetix 0:9b434b5e28d4 218 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
Anaesthetix 0:9b434b5e28d4 219 s0 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 220 s1 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 221 s2 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 222 s3 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 223
Anaesthetix 0:9b434b5e28d4 224 // Apply feedback step
Anaesthetix 0:9b434b5e28d4 225 qDot1 -= beta * s0;
Anaesthetix 0:9b434b5e28d4 226 qDot2 -= beta * s1;
Anaesthetix 0:9b434b5e28d4 227 qDot3 -= beta * s2;
Anaesthetix 0:9b434b5e28d4 228 qDot4 -= beta * s3;
Anaesthetix 0:9b434b5e28d4 229 }
Anaesthetix 0:9b434b5e28d4 230
Anaesthetix 0:9b434b5e28d4 231 // Integrate rate of change of quaternion to yield quaternion
Anaesthetix 0:9b434b5e28d4 232 q0 += qDot1 * invSampleFreq;
Anaesthetix 0:9b434b5e28d4 233 q1 += qDot2 * invSampleFreq;
Anaesthetix 0:9b434b5e28d4 234 q2 += qDot3 * invSampleFreq;
Anaesthetix 0:9b434b5e28d4 235 q3 += qDot4 * invSampleFreq;
Anaesthetix 0:9b434b5e28d4 236
Anaesthetix 0:9b434b5e28d4 237 // Normalise quaternion
Anaesthetix 0:9b434b5e28d4 238 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
Anaesthetix 0:9b434b5e28d4 239 q0 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 240 q1 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 241 q2 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 242 q3 *= recipNorm;
Anaesthetix 0:9b434b5e28d4 243 anglesComputed = 0;
Anaesthetix 0:9b434b5e28d4 244 }
Anaesthetix 0:9b434b5e28d4 245
Anaesthetix 0:9b434b5e28d4 246 //-------------------------------------------------------------------------------------------
Anaesthetix 0:9b434b5e28d4 247 // Fast inverse square-root
Anaesthetix 0:9b434b5e28d4 248 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
Anaesthetix 0:9b434b5e28d4 249
Anaesthetix 0:9b434b5e28d4 250 /*float Madgwick::invSqrt(float x) {
Anaesthetix 0:9b434b5e28d4 251 float halfx = 0.5f * x;
Anaesthetix 0:9b434b5e28d4 252 float y = x;
Anaesthetix 0:9b434b5e28d4 253 long i = *(long*)&y;
Anaesthetix 0:9b434b5e28d4 254 i = 0x5f3759df - (i>>1);
Anaesthetix 0:9b434b5e28d4 255 y = *(float*)&i;
Anaesthetix 0:9b434b5e28d4 256 y = y * (1.5f - (halfx * y * y));
Anaesthetix 0:9b434b5e28d4 257 y = y * (1.5f - (halfx * y * y));
Anaesthetix 0:9b434b5e28d4 258 return y;
Anaesthetix 1:d7c70d593694 259 }
Anaesthetix 0:9b434b5e28d4 260
Anaesthetix 0:9b434b5e28d4 261 float Madgwick::invSqrt(float x){
Anaesthetix 0:9b434b5e28d4 262 unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1);
Anaesthetix 0:9b434b5e28d4 263 float tmp = *(float*)&i;
Anaesthetix 0:9b434b5e28d4 264 return tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
Anaesthetix 0:9b434b5e28d4 265 }
Anaesthetix 1:d7c70d593694 266 */
Anaesthetix 1:d7c70d593694 267 float Madgwick::invSqrt(float x)
Anaesthetix 1:d7c70d593694 268 {
Anaesthetix 1:d7c70d593694 269 float tmp = 1/(sqrt(x));
Anaesthetix 1:d7c70d593694 270 return tmp;
Anaesthetix 1:d7c70d593694 271 }
Anaesthetix 0:9b434b5e28d4 272
Anaesthetix 0:9b434b5e28d4 273 //-------------------------------------------------------------------------------------------
Anaesthetix 0:9b434b5e28d4 274
Anaesthetix 0:9b434b5e28d4 275 void Madgwick::computeAngles()
Anaesthetix 0:9b434b5e28d4 276 {
Anaesthetix 0:9b434b5e28d4 277 roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
Anaesthetix 0:9b434b5e28d4 278 pitch = asinf(-2.0f * (q1*q3 - q0*q2));
Anaesthetix 0:9b434b5e28d4 279 yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
Anaesthetix 0:9b434b5e28d4 280 anglesComputed = 1;
Anaesthetix 1:d7c70d593694 281 }
Anaesthetix 1:d7c70d593694 282