Erik van de Coevering / Mbed 2 deprecated Multicopter_2018

Latest version of my quadcopter controller with an LPC1768 and MPU9250.

Dependencies:   mbed

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?

UserRevisionLine numberNew 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