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