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.

Mahony/MahonyAHRS.cpp@8:981f7e2365b6, 2018-07-31 (annotated)

Committer:
Anaesthetix
Date:
Tue Jul 31 20:36:57 2018 +0000
Revision:
8:981f7e2365b6
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 8:981f7e2365b6 1 //=============================================================================================
Anaesthetix 8:981f7e2365b6 2 // MahonyAHRS.c
Anaesthetix 8:981f7e2365b6 3 //=============================================================================================
Anaesthetix 8:981f7e2365b6 4 //
Anaesthetix 8:981f7e2365b6 5 // Madgwick's implementation of Mayhony's AHRS algorithm.
Anaesthetix 8:981f7e2365b6 6 // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
Anaesthetix 8:981f7e2365b6 7 //
Anaesthetix 8:981f7e2365b6 8 // From the x-io website "Open-source resources available on this website are
Anaesthetix 8:981f7e2365b6 9 // provided under the GNU General Public Licence unless an alternative licence
Anaesthetix 8:981f7e2365b6 10 // is provided in source."
Anaesthetix 8:981f7e2365b6 11 //
Anaesthetix 8:981f7e2365b6 12 // Date Author Notes
Anaesthetix 8:981f7e2365b6 13 // 29/09/2011 SOH Madgwick Initial release
Anaesthetix 8:981f7e2365b6 14 // 02/10/2011 SOH Madgwick Optimised for reduced CPU load
Anaesthetix 8:981f7e2365b6 15 //
Anaesthetix 8:981f7e2365b6 16 // Algorithm paper:
Anaesthetix 8:981f7e2365b6 17 // http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4608934&url=http%3A%2F%2Fieeexplore.ieee.org%2Fstamp%2Fstamp.jsp%3Ftp%3D%26arnumber%3D4608934
Anaesthetix 8:981f7e2365b6 18 //
Anaesthetix 8:981f7e2365b6 19 //=============================================================================================
Anaesthetix 8:981f7e2365b6 20
Anaesthetix 8:981f7e2365b6 21 //-------------------------------------------------------------------------------------------
Anaesthetix 8:981f7e2365b6 22 // Header files
Anaesthetix 8:981f7e2365b6 23
Anaesthetix 8:981f7e2365b6 24 #include "MahonyAHRS.h"
Anaesthetix 8:981f7e2365b6 25 #include <math.h>
Anaesthetix 8:981f7e2365b6 26
Anaesthetix 8:981f7e2365b6 27 //-------------------------------------------------------------------------------------------
Anaesthetix 8:981f7e2365b6 28 // Definitions
Anaesthetix 8:981f7e2365b6 29
Anaesthetix 8:981f7e2365b6 30 #define DEFAULT_SAMPLE_FREQ 1500.0f // sample frequency in Hz
Anaesthetix 8:981f7e2365b6 31 #define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
Anaesthetix 8:981f7e2365b6 32 #define twoKiDef (2.0f * 0.1f) // 2 * integral gain
Anaesthetix 8:981f7e2365b6 33
Anaesthetix 8:981f7e2365b6 34
Anaesthetix 8:981f7e2365b6 35 //============================================================================================
Anaesthetix 8:981f7e2365b6 36 // Functions
Anaesthetix 8:981f7e2365b6 37
Anaesthetix 8:981f7e2365b6 38 //-------------------------------------------------------------------------------------------
Anaesthetix 8:981f7e2365b6 39 // AHRS algorithm update
Anaesthetix 8:981f7e2365b6 40
Anaesthetix 8:981f7e2365b6 41 Mahony::Mahony()
Anaesthetix 8:981f7e2365b6 42 {
Anaesthetix 8:981f7e2365b6 43 twoKp = twoKpDef; // 2 * proportional gain (Kp)
Anaesthetix 8:981f7e2365b6 44 twoKi = twoKiDef; // 2 * integral gain (Ki)
Anaesthetix 8:981f7e2365b6 45 q0 = 1.0f;
Anaesthetix 8:981f7e2365b6 46 q1 = 0.0f;
Anaesthetix 8:981f7e2365b6 47 q2 = 0.0f;
Anaesthetix 8:981f7e2365b6 48 q3 = 0.0f;
Anaesthetix 8:981f7e2365b6 49 integralFBx = 0.0f;
Anaesthetix 8:981f7e2365b6 50 integralFBy = 0.0f;
Anaesthetix 8:981f7e2365b6 51 integralFBz = 0.0f;
Anaesthetix 8:981f7e2365b6 52 anglesComputed = 0;
Anaesthetix 8:981f7e2365b6 53 invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ;
Anaesthetix 8:981f7e2365b6 54 }
Anaesthetix 8:981f7e2365b6 55
Anaesthetix 8:981f7e2365b6 56 void Mahony::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
Anaesthetix 8:981f7e2365b6 57 {
Anaesthetix 8:981f7e2365b6 58 float recipNorm;
Anaesthetix 8:981f7e2365b6 59 float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
Anaesthetix 8:981f7e2365b6 60 float hx, hy, bx, bz;
Anaesthetix 8:981f7e2365b6 61 float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
Anaesthetix 8:981f7e2365b6 62 float halfex, halfey, halfez;
Anaesthetix 8:981f7e2365b6 63 float qa, qb, qc;
Anaesthetix 8:981f7e2365b6 64
Anaesthetix 8:981f7e2365b6 65 // Use IMU algorithm if magnetometer measurement invalid
Anaesthetix 8:981f7e2365b6 66 // (avoids NaN in magnetometer normalisation)
Anaesthetix 8:981f7e2365b6 67 if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
Anaesthetix 8:981f7e2365b6 68 updateIMU(gx, gy, gz, ax, ay, az);
Anaesthetix 8:981f7e2365b6 69 return;
Anaesthetix 8:981f7e2365b6 70 }
Anaesthetix 8:981f7e2365b6 71
Anaesthetix 8:981f7e2365b6 72 // Convert gyroscope degrees/sec to radians/sec
Anaesthetix 8:981f7e2365b6 73 gx *= 0.0174533f;
Anaesthetix 8:981f7e2365b6 74 gy *= 0.0174533f;
Anaesthetix 8:981f7e2365b6 75 gz *= 0.0174533f;
Anaesthetix 8:981f7e2365b6 76
Anaesthetix 8:981f7e2365b6 77 // Compute feedback only if accelerometer measurement valid
Anaesthetix 8:981f7e2365b6 78 // (avoids NaN in accelerometer normalisation)
Anaesthetix 8:981f7e2365b6 79 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
Anaesthetix 8:981f7e2365b6 80
Anaesthetix 8:981f7e2365b6 81 // Normalise accelerometer measurement
Anaesthetix 8:981f7e2365b6 82 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
Anaesthetix 8:981f7e2365b6 83 ax *= recipNorm;
Anaesthetix 8:981f7e2365b6 84 ay *= recipNorm;
Anaesthetix 8:981f7e2365b6 85 az *= recipNorm;
Anaesthetix 8:981f7e2365b6 86
Anaesthetix 8:981f7e2365b6 87 // Normalise magnetometer measurement
Anaesthetix 8:981f7e2365b6 88 recipNorm = invSqrt(mx * mx + my * my + mz * mz);
Anaesthetix 8:981f7e2365b6 89 mx *= recipNorm;
Anaesthetix 8:981f7e2365b6 90 my *= recipNorm;
Anaesthetix 8:981f7e2365b6 91 mz *= recipNorm;
Anaesthetix 8:981f7e2365b6 92
Anaesthetix 8:981f7e2365b6 93 // Auxiliary variables to avoid repeated arithmetic
Anaesthetix 8:981f7e2365b6 94 q0q0 = q0 * q0;
Anaesthetix 8:981f7e2365b6 95 q0q1 = q0 * q1;
Anaesthetix 8:981f7e2365b6 96 q0q2 = q0 * q2;
Anaesthetix 8:981f7e2365b6 97 q0q3 = q0 * q3;
Anaesthetix 8:981f7e2365b6 98 q1q1 = q1 * q1;
Anaesthetix 8:981f7e2365b6 99 q1q2 = q1 * q2;
Anaesthetix 8:981f7e2365b6 100 q1q3 = q1 * q3;
Anaesthetix 8:981f7e2365b6 101 q2q2 = q2 * q2;
Anaesthetix 8:981f7e2365b6 102 q2q3 = q2 * q3;
Anaesthetix 8:981f7e2365b6 103 q3q3 = q3 * q3;
Anaesthetix 8:981f7e2365b6 104
Anaesthetix 8:981f7e2365b6 105 // Reference direction of Earth's magnetic field
Anaesthetix 8:981f7e2365b6 106 hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
Anaesthetix 8:981f7e2365b6 107 hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
Anaesthetix 8:981f7e2365b6 108 bx = sqrtf(hx * hx + hy * hy);
Anaesthetix 8:981f7e2365b6 109 bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
Anaesthetix 8:981f7e2365b6 110
Anaesthetix 8:981f7e2365b6 111 // Estimated direction of gravity and magnetic field
Anaesthetix 8:981f7e2365b6 112 halfvx = q1q3 - q0q2;
Anaesthetix 8:981f7e2365b6 113 halfvy = q0q1 + q2q3;
Anaesthetix 8:981f7e2365b6 114 halfvz = q0q0 - 0.5f + q3q3;
Anaesthetix 8:981f7e2365b6 115 halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
Anaesthetix 8:981f7e2365b6 116 halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
Anaesthetix 8:981f7e2365b6 117 halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
Anaesthetix 8:981f7e2365b6 118
Anaesthetix 8:981f7e2365b6 119 // Error is sum of cross product between estimated direction
Anaesthetix 8:981f7e2365b6 120 // and measured direction of field vectors
Anaesthetix 8:981f7e2365b6 121 halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
Anaesthetix 8:981f7e2365b6 122 halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
Anaesthetix 8:981f7e2365b6 123 halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
Anaesthetix 8:981f7e2365b6 124
Anaesthetix 8:981f7e2365b6 125 // Compute and apply integral feedback if enabled
Anaesthetix 8:981f7e2365b6 126 if(twoKi > 0.0f) {
Anaesthetix 8:981f7e2365b6 127 // integral error scaled by Ki
Anaesthetix 8:981f7e2365b6 128 integralFBx += twoKi * halfex * invSampleFreq;
Anaesthetix 8:981f7e2365b6 129 integralFBy += twoKi * halfey * invSampleFreq;
Anaesthetix 8:981f7e2365b6 130 integralFBz += twoKi * halfez * invSampleFreq;
Anaesthetix 8:981f7e2365b6 131 gx += integralFBx; // apply integral feedback
Anaesthetix 8:981f7e2365b6 132 gy += integralFBy;
Anaesthetix 8:981f7e2365b6 133 gz += integralFBz;
Anaesthetix 8:981f7e2365b6 134 } else {
Anaesthetix 8:981f7e2365b6 135 integralFBx = 0.0f; // prevent integral windup
Anaesthetix 8:981f7e2365b6 136 integralFBy = 0.0f;
Anaesthetix 8:981f7e2365b6 137 integralFBz = 0.0f;
Anaesthetix 8:981f7e2365b6 138 }
Anaesthetix 8:981f7e2365b6 139
Anaesthetix 8:981f7e2365b6 140 // Apply proportional feedback
Anaesthetix 8:981f7e2365b6 141 gx += twoKp * halfex;
Anaesthetix 8:981f7e2365b6 142 gy += twoKp * halfey;
Anaesthetix 8:981f7e2365b6 143 gz += twoKp * halfez;
Anaesthetix 8:981f7e2365b6 144 }
Anaesthetix 8:981f7e2365b6 145
Anaesthetix 8:981f7e2365b6 146 // Integrate rate of change of quaternion
Anaesthetix 8:981f7e2365b6 147 gx *= (0.5f * invSampleFreq); // pre-multiply common factors
Anaesthetix 8:981f7e2365b6 148 gy *= (0.5f * invSampleFreq);
Anaesthetix 8:981f7e2365b6 149 gz *= (0.5f * invSampleFreq);
Anaesthetix 8:981f7e2365b6 150 qa = q0;
Anaesthetix 8:981f7e2365b6 151 qb = q1;
Anaesthetix 8:981f7e2365b6 152 qc = q2;
Anaesthetix 8:981f7e2365b6 153 q0 += (-qb * gx - qc * gy - q3 * gz);
Anaesthetix 8:981f7e2365b6 154 q1 += (qa * gx + qc * gz - q3 * gy);
Anaesthetix 8:981f7e2365b6 155 q2 += (qa * gy - qb * gz + q3 * gx);
Anaesthetix 8:981f7e2365b6 156 q3 += (qa * gz + qb * gy - qc * gx);
Anaesthetix 8:981f7e2365b6 157
Anaesthetix 8:981f7e2365b6 158 // Normalise quaternion
Anaesthetix 8:981f7e2365b6 159 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
Anaesthetix 8:981f7e2365b6 160 q0 *= recipNorm;
Anaesthetix 8:981f7e2365b6 161 q1 *= recipNorm;
Anaesthetix 8:981f7e2365b6 162 q2 *= recipNorm;
Anaesthetix 8:981f7e2365b6 163 q3 *= recipNorm;
Anaesthetix 8:981f7e2365b6 164 anglesComputed = 0;
Anaesthetix 8:981f7e2365b6 165 }
Anaesthetix 8:981f7e2365b6 166
Anaesthetix 8:981f7e2365b6 167 //-------------------------------------------------------------------------------------------
Anaesthetix 8:981f7e2365b6 168 // IMU algorithm update
Anaesthetix 8:981f7e2365b6 169
Anaesthetix 8:981f7e2365b6 170 void Mahony::updateIMU(float gx, float gy, float gz, float ax, float ay, float az)
Anaesthetix 8:981f7e2365b6 171 {
Anaesthetix 8:981f7e2365b6 172 float recipNorm;
Anaesthetix 8:981f7e2365b6 173 float halfvx, halfvy, halfvz;
Anaesthetix 8:981f7e2365b6 174 float halfex, halfey, halfez;
Anaesthetix 8:981f7e2365b6 175 float qa, qb, qc;
Anaesthetix 8:981f7e2365b6 176
Anaesthetix 8:981f7e2365b6 177 // Convert gyroscope degrees/sec to radians/sec
Anaesthetix 8:981f7e2365b6 178 gx *= 0.0174533f;
Anaesthetix 8:981f7e2365b6 179 gy *= 0.0174533f;
Anaesthetix 8:981f7e2365b6 180 gz *= 0.0174533f;
Anaesthetix 8:981f7e2365b6 181
Anaesthetix 8:981f7e2365b6 182 // Compute feedback only if accelerometer measurement valid
Anaesthetix 8:981f7e2365b6 183 // (avoids NaN in accelerometer normalisation)
Anaesthetix 8:981f7e2365b6 184 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
Anaesthetix 8:981f7e2365b6 185
Anaesthetix 8:981f7e2365b6 186 // Normalise accelerometer measurement
Anaesthetix 8:981f7e2365b6 187 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
Anaesthetix 8:981f7e2365b6 188 ax *= recipNorm;
Anaesthetix 8:981f7e2365b6 189 ay *= recipNorm;
Anaesthetix 8:981f7e2365b6 190 az *= recipNorm;
Anaesthetix 8:981f7e2365b6 191
Anaesthetix 8:981f7e2365b6 192 // Estimated direction of gravity
Anaesthetix 8:981f7e2365b6 193 halfvx = q1 * q3 - q0 * q2;
Anaesthetix 8:981f7e2365b6 194 halfvy = q0 * q1 + q2 * q3;
Anaesthetix 8:981f7e2365b6 195 halfvz = q0 * q0 - 0.5f + q3 * q3;
Anaesthetix 8:981f7e2365b6 196
Anaesthetix 8:981f7e2365b6 197 // Error is sum of cross product between estimated
Anaesthetix 8:981f7e2365b6 198 // and measured direction of gravity
Anaesthetix 8:981f7e2365b6 199 halfex = (ay * halfvz - az * halfvy);
Anaesthetix 8:981f7e2365b6 200 halfey = (az * halfvx - ax * halfvz);
Anaesthetix 8:981f7e2365b6 201 halfez = (ax * halfvy - ay * halfvx);
Anaesthetix 8:981f7e2365b6 202
Anaesthetix 8:981f7e2365b6 203 // Compute and apply integral feedback if enabled
Anaesthetix 8:981f7e2365b6 204 if(twoKi > 0.0f) {
Anaesthetix 8:981f7e2365b6 205 // integral error scaled by Ki
Anaesthetix 8:981f7e2365b6 206 integralFBx += twoKi * halfex * invSampleFreq;
Anaesthetix 8:981f7e2365b6 207 integralFBy += twoKi * halfey * invSampleFreq;
Anaesthetix 8:981f7e2365b6 208 integralFBz += twoKi * halfez * invSampleFreq;
Anaesthetix 8:981f7e2365b6 209 gx += integralFBx; // apply integral feedback
Anaesthetix 8:981f7e2365b6 210 gy += integralFBy;
Anaesthetix 8:981f7e2365b6 211 gz += integralFBz;
Anaesthetix 8:981f7e2365b6 212 } else {
Anaesthetix 8:981f7e2365b6 213 integralFBx = 0.0f; // prevent integral windup
Anaesthetix 8:981f7e2365b6 214 integralFBy = 0.0f;
Anaesthetix 8:981f7e2365b6 215 integralFBz = 0.0f;
Anaesthetix 8:981f7e2365b6 216 }
Anaesthetix 8:981f7e2365b6 217
Anaesthetix 8:981f7e2365b6 218 // Apply proportional feedback
Anaesthetix 8:981f7e2365b6 219 gx += twoKp * halfex;
Anaesthetix 8:981f7e2365b6 220 gy += twoKp * halfey;
Anaesthetix 8:981f7e2365b6 221 gz += twoKp * halfez;
Anaesthetix 8:981f7e2365b6 222 }
Anaesthetix 8:981f7e2365b6 223
Anaesthetix 8:981f7e2365b6 224 // Integrate rate of change of quaternion
Anaesthetix 8:981f7e2365b6 225 gx *= (0.5f * invSampleFreq); // pre-multiply common factors
Anaesthetix 8:981f7e2365b6 226 gy *= (0.5f * invSampleFreq);
Anaesthetix 8:981f7e2365b6 227 gz *= (0.5f * invSampleFreq);
Anaesthetix 8:981f7e2365b6 228 qa = q0;
Anaesthetix 8:981f7e2365b6 229 qb = q1;
Anaesthetix 8:981f7e2365b6 230 qc = q2;
Anaesthetix 8:981f7e2365b6 231 q0 += (-qb * gx - qc * gy - q3 * gz);
Anaesthetix 8:981f7e2365b6 232 q1 += (qa * gx + qc * gz - q3 * gy);
Anaesthetix 8:981f7e2365b6 233 q2 += (qa * gy - qb * gz + q3 * gx);
Anaesthetix 8:981f7e2365b6 234 q3 += (qa * gz + qb * gy - qc * gx);
Anaesthetix 8:981f7e2365b6 235
Anaesthetix 8:981f7e2365b6 236 // Normalise quaternion
Anaesthetix 8:981f7e2365b6 237 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
Anaesthetix 8:981f7e2365b6 238 q0 *= recipNorm;
Anaesthetix 8:981f7e2365b6 239 q1 *= recipNorm;
Anaesthetix 8:981f7e2365b6 240 q2 *= recipNorm;
Anaesthetix 8:981f7e2365b6 241 q3 *= recipNorm;
Anaesthetix 8:981f7e2365b6 242 anglesComputed = 0;
Anaesthetix 8:981f7e2365b6 243 }
Anaesthetix 8:981f7e2365b6 244
Anaesthetix 8:981f7e2365b6 245 //-------------------------------------------------------------------------------------------
Anaesthetix 8:981f7e2365b6 246 // Fast inverse square-root
Anaesthetix 8:981f7e2365b6 247 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
Anaesthetix 8:981f7e2365b6 248 /*
Anaesthetix 8:981f7e2365b6 249 float Mahony::invSqrt(float x)
Anaesthetix 8:981f7e2365b6 250 {
Anaesthetix 8:981f7e2365b6 251 float halfx = 0.5f * x;
Anaesthetix 8:981f7e2365b6 252 float y = x;
Anaesthetix 8:981f7e2365b6 253 long i = *(long*)&y;
Anaesthetix 8:981f7e2365b6 254 i = 0x5f3759df - (i>>1);
Anaesthetix 8:981f7e2365b6 255 y = *(float*)&i;
Anaesthetix 8:981f7e2365b6 256 y = y * (1.5f - (halfx * y * y));
Anaesthetix 8:981f7e2365b6 257 y = y * (1.5f - (halfx * y * y));
Anaesthetix 8:981f7e2365b6 258 return y;
Anaesthetix 8:981f7e2365b6 259 }
Anaesthetix 8:981f7e2365b6 260 */
Anaesthetix 8:981f7e2365b6 261 /*
Anaesthetix 8:981f7e2365b6 262 float Mahony::invSqrt(float x){
Anaesthetix 8:981f7e2365b6 263 unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1);
Anaesthetix 8:981f7e2365b6 264 float tmp = *(float*)&i;
Anaesthetix 8:981f7e2365b6 265 return tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
Anaesthetix 8:981f7e2365b6 266 }
Anaesthetix 8:981f7e2365b6 267 */
Anaesthetix 8:981f7e2365b6 268
Anaesthetix 8:981f7e2365b6 269 float Mahony::invSqrt(float x)
Anaesthetix 8:981f7e2365b6 270 {
Anaesthetix 8:981f7e2365b6 271 float temp = 1/(sqrt(x));
Anaesthetix 8:981f7e2365b6 272 return temp;
Anaesthetix 8:981f7e2365b6 273 }
Anaesthetix 8:981f7e2365b6 274
Anaesthetix 8:981f7e2365b6 275 //-------------------------------------------------------------------------------------------
Anaesthetix 8:981f7e2365b6 276
Anaesthetix 8:981f7e2365b6 277 void Mahony::computeAngles()
Anaesthetix 8:981f7e2365b6 278 {
Anaesthetix 8:981f7e2365b6 279 roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
Anaesthetix 8:981f7e2365b6 280 pitch = asinf(-2.0f * (q1*q3 - q0*q2));
Anaesthetix 8:981f7e2365b6 281 yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
Anaesthetix 8:981f7e2365b6 282 anglesComputed = 1;
Anaesthetix 8:981f7e2365b6 283 }
Anaesthetix 8:981f7e2365b6 284
Anaesthetix 8:981f7e2365b6 285
Anaesthetix 8:981f7e2365b6 286 //============================================================================================
Anaesthetix 8:981f7e2365b6 287 // END OF CODE
Anaesthetix 8:981f7e2365b6 288 //============================================================================================