BE@R lab / Mbed 2 deprecated MPU9250AHRS_OOP

Class of MPU9250

Dependencies:   AHRS_fillter mbed

Fork of MPU9250AHRS by BE@R lab

AHRS.cpp@8:928673148b55, 2016-01-20 (annotated)

Committer:
icyzkungz
Date:
Wed Jan 20 02:42:22 2016 +0000
Revision:
8:928673148b55
Parent:
7:3e74239e3778 
add tx,rx in AHRS class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
icyzkungz 6:5665d427bceb 1 #include "AHRS.h"
icyzkungz 6:5665d427bceb 2
icyzkungz 6:5665d427bceb 3 #define Kp 5.1f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
icyzkungz 6:5665d427bceb 4 #define Ki 1.0f
icyzkungz 6:5665d427bceb 5
icyzkungz 6:5665d427bceb 6 /*AHRS::AHRS(PinName sda, PinName scl, PinName tx, PinName rx, int address):MPU9250(sda,scl,tx,rx,address)
icyzkungz 6:5665d427bceb 7 {
icyzkungz 6:5665d427bceb 8 for(int i=0; i<=3; i++) {
icyzkungz 6:5665d427bceb 9 eInt[i] = 0;
icyzkungz 6:5665d427bceb 10 q[i] = 0; // vector to hold quaternion
icyzkungz 6:5665d427bceb 11 }
icyzkungz 6:5665d427bceb 12
icyzkungz 6:5665d427bceb 13 q[0] = 1.0f;
icyzkungz 6:5665d427bceb 14
icyzkungz 6:5665d427bceb 15 PI = 3.14159265358979323846f;
icyzkungz 6:5665d427bceb 16 GyroMeasError = PI * (60.0f / 180.0f); // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
icyzkungz 6:5665d427bceb 17 beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
icyzkungz 6:5665d427bceb 18 GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
icyzkungz 6:5665d427bceb 19 zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
icyzkungz 6:5665d427bceb 20 }*/
icyzkungz 6:5665d427bceb 21
icyzkungz 6:5665d427bceb 22 /*void AHRS::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat)
icyzkungz 6:5665d427bceb 23 {
icyzkungz 6:5665d427bceb 24 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
icyzkungz 6:5665d427bceb 25 float norm;
icyzkungz 6:5665d427bceb 26 float hx, hy, _2bx, _2bz;
icyzkungz 6:5665d427bceb 27 float s1, s2, s3, s4;
icyzkungz 6:5665d427bceb 28 float qDot1, qDot2, qDot3, qDot4;
icyzkungz 6:5665d427bceb 29
icyzkungz 6:5665d427bceb 30 // Auxiliary variables to avoid repeated arithmetic
icyzkungz 6:5665d427bceb 31 float _2q1mx;
icyzkungz 6:5665d427bceb 32 float _2q1my;
icyzkungz 6:5665d427bceb 33 float _2q1mz;
icyzkungz 6:5665d427bceb 34 float _2q2mx;
icyzkungz 6:5665d427bceb 35 float _4bx;
icyzkungz 6:5665d427bceb 36 float _4bz;
icyzkungz 6:5665d427bceb 37 float _2q1 = 2.0f * q1;
icyzkungz 6:5665d427bceb 38 float _2q2 = 2.0f * q2;
icyzkungz 6:5665d427bceb 39 float _2q3 = 2.0f * q3;
icyzkungz 6:5665d427bceb 40 float _2q4 = 2.0f * q4;
icyzkungz 6:5665d427bceb 41 float _2q1q3 = 2.0f * q1 * q3;
icyzkungz 6:5665d427bceb 42 float _2q3q4 = 2.0f * q3 * q4;
icyzkungz 6:5665d427bceb 43 float q1q1 = q1 * q1;
icyzkungz 6:5665d427bceb 44 float q1q2 = q1 * q2;
icyzkungz 6:5665d427bceb 45 float q1q3 = q1 * q3;
icyzkungz 6:5665d427bceb 46 float q1q4 = q1 * q4;
icyzkungz 6:5665d427bceb 47 float q2q2 = q2 * q2;
icyzkungz 6:5665d427bceb 48 float q2q3 = q2 * q3;
icyzkungz 6:5665d427bceb 49 float q2q4 = q2 * q4;
icyzkungz 6:5665d427bceb 50 float q3q3 = q3 * q3;
icyzkungz 6:5665d427bceb 51 float q3q4 = q3 * q4;
icyzkungz 6:5665d427bceb 52 float q4q4 = q4 * q4;
icyzkungz 6:5665d427bceb 53
icyzkungz 6:5665d427bceb 54 // Normalise accelerometer measurement
icyzkungz 6:5665d427bceb 55 norm = sqrt(ax * ax + ay * ay + az * az);
icyzkungz 6:5665d427bceb 56 if (norm == 0.0f) return; // handle NaN
icyzkungz 6:5665d427bceb 57 norm = 1.0f/norm;
icyzkungz 6:5665d427bceb 58 ax *= norm;
icyzkungz 6:5665d427bceb 59 ay *= norm;
icyzkungz 6:5665d427bceb 60 az *= norm;
icyzkungz 6:5665d427bceb 61
icyzkungz 6:5665d427bceb 62 // Normalise magnetometer measurement
icyzkungz 6:5665d427bceb 63 norm = sqrt(mx * mx + my * my + mz * mz);
icyzkungz 6:5665d427bceb 64 if (norm == 0.0f) return; // handle NaN
icyzkungz 6:5665d427bceb 65 norm = 1.0f/norm;
icyzkungz 6:5665d427bceb 66 mx *= norm;
icyzkungz 6:5665d427bceb 67 my *= norm;
icyzkungz 6:5665d427bceb 68 mz *= norm;
icyzkungz 6:5665d427bceb 69
icyzkungz 6:5665d427bceb 70 // Reference direction of Earth's magnetic field
icyzkungz 6:5665d427bceb 71 _2q1mx = 2.0f * q1 * mx;
icyzkungz 6:5665d427bceb 72 _2q1my = 2.0f * q1 * my;
icyzkungz 6:5665d427bceb 73 _2q1mz = 2.0f * q1 * mz;
icyzkungz 6:5665d427bceb 74 _2q2mx = 2.0f * q2 * mx;
icyzkungz 6:5665d427bceb 75 hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
icyzkungz 6:5665d427bceb 76 hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
icyzkungz 6:5665d427bceb 77 _2bx = sqrt(hx * hx + hy * hy);
icyzkungz 6:5665d427bceb 78 _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
icyzkungz 6:5665d427bceb 79 _4bx = 2.0f * _2bx;
icyzkungz 6:5665d427bceb 80 _4bz = 2.0f * _2bz;
icyzkungz 6:5665d427bceb 81
icyzkungz 6:5665d427bceb 82 // Gradient decent algorithm corrective step
icyzkungz 6:5665d427bceb 83 s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
icyzkungz 6:5665d427bceb 84 s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
icyzkungz 6:5665d427bceb 85 s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
icyzkungz 6:5665d427bceb 86 s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
icyzkungz 6:5665d427bceb 87 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
icyzkungz 6:5665d427bceb 88 norm = 1.0f/norm;
icyzkungz 6:5665d427bceb 89 s1 *= norm;
icyzkungz 6:5665d427bceb 90 s2 *= norm;
icyzkungz 6:5665d427bceb 91 s3 *= norm;
icyzkungz 6:5665d427bceb 92 s4 *= norm;
icyzkungz 6:5665d427bceb 93
icyzkungz 6:5665d427bceb 94 // Compute rate of change of quaternion
icyzkungz 6:5665d427bceb 95 qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
icyzkungz 6:5665d427bceb 96 qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
icyzkungz 6:5665d427bceb 97 qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
icyzkungz 6:5665d427bceb 98 qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
icyzkungz 6:5665d427bceb 99
icyzkungz 6:5665d427bceb 100 // Integrate to yield quaternion
icyzkungz 6:5665d427bceb 101 q1 += qDot1 * deltat;
icyzkungz 6:5665d427bceb 102 q2 += qDot2 * deltat;
icyzkungz 6:5665d427bceb 103 q3 += qDot3 * deltat;
icyzkungz 6:5665d427bceb 104 q4 += qDot4 * deltat;
icyzkungz 6:5665d427bceb 105 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
icyzkungz 6:5665d427bceb 106 norm = 1.0f/norm;
icyzkungz 6:5665d427bceb 107 q[0] = q1 * norm;
icyzkungz 6:5665d427bceb 108 q[1] = q2 * norm;
icyzkungz 6:5665d427bceb 109 q[2] = q3 * norm;
icyzkungz 6:5665d427bceb 110 q[3] = q4 * norm;
icyzkungz 6:5665d427bceb 111
icyzkungz 6:5665d427bceb 112 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
icyzkungz 6:5665d427bceb 113 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
icyzkungz 6:5665d427bceb 114 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
icyzkungz 6:5665d427bceb 115
icyzkungz 6:5665d427bceb 116 float Xh = mx*cos(pitch)+my*sin(roll)*sin(pitch)-mz*cos(roll)*sin(pitch);
icyzkungz 6:5665d427bceb 117 float Yh = my*cos(roll)+mz*sin(roll);
icyzkungz 6:5665d427bceb 118
icyzkungz 6:5665d427bceb 119 float yawmag = atan2(Yh,Xh)+PI;
icyzkungz 6:5665d427bceb 120 //////test.printf("Xh= %f Yh= %f ",Xh,Yh);
icyzkungz 6:5665d427bceb 121 //////test.printf("Yaw[mag]= %f\n\r",yawmag*180.0f/PI);
icyzkungz 6:5665d427bceb 122 //test.printf(",%f",yawmag*180.0f/PI);
icyzkungz 6:5665d427bceb 123
icyzkungz 6:5665d427bceb 124
icyzkungz 6:5665d427bceb 125
icyzkungz 6:5665d427bceb 126 pitch *= 180.0f / PI;
icyzkungz 6:5665d427bceb 127 yaw *= 180.0f / PI;
icyzkungz 6:5665d427bceb 128 yaw += 180.0f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
icyzkungz 6:5665d427bceb 129 roll *= 180.0f / PI;
icyzkungz 6:5665d427bceb 130
icyzkungz 6:5665d427bceb 131 }
icyzkungz 6:5665d427bceb 132
icyzkungz 6:5665d427bceb 133
icyzkungz 6:5665d427bceb 134
icyzkungz 6:5665d427bceb 135 // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
icyzkungz 6:5665d427bceb 136 // measured ones.
icyzkungz 6:5665d427bceb 137 void AHRS::MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat)
icyzkungz 6:5665d427bceb 138 {
icyzkungz 6:5665d427bceb 139 q1 = q[0];
icyzkungz 6:5665d427bceb 140 q2 = q[1];
icyzkungz 6:5665d427bceb 141 q3 = q[2];
icyzkungz 6:5665d427bceb 142 q4 = q[3]; // short name local variable for readability
icyzkungz 6:5665d427bceb 143
icyzkungz 6:5665d427bceb 144 // Auxiliary variables to avoid repeated arithmetic
icyzkungz 6:5665d427bceb 145 q1q1 = q1 * q1;
icyzkungz 6:5665d427bceb 146 q1q2 = q1 * q2;
icyzkungz 6:5665d427bceb 147 q1q3 = q1 * q3;
icyzkungz 6:5665d427bceb 148 q1q4 = q1 * q4;
icyzkungz 6:5665d427bceb 149 q2q2 = q2 * q2;
icyzkungz 6:5665d427bceb 150 q2q3 = q2 * q3;
icyzkungz 6:5665d427bceb 151 q2q4 = q2 * q4;
icyzkungz 6:5665d427bceb 152 q3q3 = q3 * q3;
icyzkungz 6:5665d427bceb 153 q3q4 = q3 * q4;
icyzkungz 6:5665d427bceb 154 q4q4 = q4 * q4;
icyzkungz 6:5665d427bceb 155
icyzkungz 6:5665d427bceb 156 // Normalise accelerometer measurement
icyzkungz 6:5665d427bceb 157 norm = sqrt(ax * ax + ay * ay + az * az);
icyzkungz 6:5665d427bceb 158 if (norm == 0.0f) return; // handle NaN
icyzkungz 6:5665d427bceb 159 norm = 1.0f / norm; // use reciprocal for division
icyzkungz 6:5665d427bceb 160 ax *= norm;
icyzkungz 6:5665d427bceb 161 ay *= norm;
icyzkungz 6:5665d427bceb 162 az *= norm;
icyzkungz 6:5665d427bceb 163
icyzkungz 6:5665d427bceb 164 // Normalise magnetometer measurement
icyzkungz 6:5665d427bceb 165 norm = sqrt(mx * mx + my * my + mz * mz);
icyzkungz 6:5665d427bceb 166 if (norm == 0.0f) return; // handle NaN
icyzkungz 6:5665d427bceb 167 norm = 1.0f / norm; // use reciprocal for division
icyzkungz 6:5665d427bceb 168 mx *= norm;
icyzkungz 6:5665d427bceb 169 my *= norm;
icyzkungz 6:5665d427bceb 170 mz *= norm;
icyzkungz 6:5665d427bceb 171
icyzkungz 6:5665d427bceb 172 // Reference direction of Earth's magnetic field
icyzkungz 6:5665d427bceb 173 hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
icyzkungz 6:5665d427bceb 174 hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
icyzkungz 6:5665d427bceb 175 bx = sqrt((hx * hx) + (hy * hy));
icyzkungz 6:5665d427bceb 176 bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
icyzkungz 6:5665d427bceb 177
icyzkungz 6:5665d427bceb 178 // Estimated direction of gravity and magnetic field
icyzkungz 6:5665d427bceb 179 vx = 2.0f * (q2q4 - q1q3);
icyzkungz 6:5665d427bceb 180 vy = 2.0f * (q1q2 + q3q4);
icyzkungz 6:5665d427bceb 181 vz = q1q1 - q2q2 - q3q3 + q4q4;
icyzkungz 6:5665d427bceb 182 wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
icyzkungz 6:5665d427bceb 183 wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
icyzkungz 6:5665d427bceb 184 wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
icyzkungz 6:5665d427bceb 185
icyzkungz 6:5665d427bceb 186 // Error is cross product between estimated direction and measured direction of gravity
icyzkungz 6:5665d427bceb 187 ex = (ay * vz - az * vy) + (my * wz - mz * wy);
icyzkungz 6:5665d427bceb 188 ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
icyzkungz 6:5665d427bceb 189 ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
icyzkungz 6:5665d427bceb 190 if (Ki > 0.0f) {
icyzkungz 6:5665d427bceb 191 eInt[0] += ex; // accumulate integral error
icyzkungz 6:5665d427bceb 192 eInt[1] += ey;
icyzkungz 6:5665d427bceb 193 eInt[2] += ez;
icyzkungz 6:5665d427bceb 194 } else {
icyzkungz 6:5665d427bceb 195 eInt[0] = 0.0f; // prevent integral wind up
icyzkungz 6:5665d427bceb 196 eInt[1] = 0.0f;
icyzkungz 6:5665d427bceb 197 eInt[2] = 0.0f;
icyzkungz 6:5665d427bceb 198 }
icyzkungz 6:5665d427bceb 199
icyzkungz 6:5665d427bceb 200 // Apply feedback terms
icyzkungz 6:5665d427bceb 201 gx = gx + Kp * ex + Ki * eInt[0];
icyzkungz 6:5665d427bceb 202 gy = gy + Kp * ey + Ki * eInt[1];
icyzkungz 6:5665d427bceb 203 gz = gz + Kp * ez + Ki * eInt[2];
icyzkungz 6:5665d427bceb 204
icyzkungz 6:5665d427bceb 205 // Integrate rate of change of quaternion
icyzkungz 6:5665d427bceb 206 pa = q2;
icyzkungz 6:5665d427bceb 207 pb = q3;
icyzkungz 6:5665d427bceb 208 pc = q4;
icyzkungz 6:5665d427bceb 209 q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
icyzkungz 6:5665d427bceb 210 q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
icyzkungz 6:5665d427bceb 211 q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
icyzkungz 6:5665d427bceb 212 q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
icyzkungz 6:5665d427bceb 213
icyzkungz 6:5665d427bceb 214 // Normalise quaternion
icyzkungz 6:5665d427bceb 215 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
icyzkungz 6:5665d427bceb 216 norm = 1.0f / norm;
icyzkungz 6:5665d427bceb 217 q[0] = q1 * norm;
icyzkungz 6:5665d427bceb 218 q[1] = q2 * norm;
icyzkungz 6:5665d427bceb 219 q[2] = q3 * norm;
icyzkungz 6:5665d427bceb 220 q[3] = q4 * norm;
icyzkungz 6:5665d427bceb 221
icyzkungz 6:5665d427bceb 222 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
icyzkungz 6:5665d427bceb 223 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
icyzkungz 6:5665d427bceb 224 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
icyzkungz 6:5665d427bceb 225
icyzkungz 6:5665d427bceb 226 Xh = mx*cos(pitch)+my*sin(roll)*sin(pitch)-mz*cos(roll)*sin(pitch);
icyzkungz 6:5665d427bceb 227 Yh = my*cos(roll)+mz*sin(roll);
icyzkungz 6:5665d427bceb 228
icyzkungz 6:5665d427bceb 229 yawmag = atan2(Yh,Xh)+PI;
icyzkungz 6:5665d427bceb 230 ////////////test.printf("Xh= %f Yh= %f ",Xh,Yh);
icyzkungz 6:5665d427bceb 231 //////test.printf("Yaw[mag]= %f\n\r",yawmag*180.0f/PI);
icyzkungz 6:5665d427bceb 232 //test.printf(",%f",yawmag*180.0f/PI);
icyzkungz 6:5665d427bceb 233
icyzkungz 6:5665d427bceb 234
icyzkungz 6:5665d427bceb 235
icyzkungz 6:5665d427bceb 236 pitch *= 180.0f / PI;
icyzkungz 6:5665d427bceb 237 yaw *= 180.0f / PI;
icyzkungz 6:5665d427bceb 238 yaw += 180.0f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
icyzkungz 6:5665d427bceb 239 roll *= 180.0f / PI;
icyzkungz 6:5665d427bceb 240 }*/
icyzkungz 6:5665d427bceb 241
icyzkungz 6:5665d427bceb 242 void AHRS::MadgwickQuaternionUpdate()
icyzkungz 6:5665d427bceb 243 {
icyzkungz 6:5665d427bceb 244 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
icyzkungz 6:5665d427bceb 245 float norm;
icyzkungz 6:5665d427bceb 246 float hx, hy, _2bx, _2bz;
icyzkungz 6:5665d427bceb 247 float s1, s2, s3, s4;
icyzkungz 6:5665d427bceb 248 float qDot1, qDot2, qDot3, qDot4;
icyzkungz 6:5665d427bceb 249
icyzkungz 6:5665d427bceb 250 // Auxiliary variables to avoid repeated arithmetic
icyzkungz 6:5665d427bceb 251 float _2q1mx;
icyzkungz 6:5665d427bceb 252 float _2q1my;
icyzkungz 6:5665d427bceb 253 float _2q1mz;
icyzkungz 6:5665d427bceb 254 float _2q2mx;
icyzkungz 6:5665d427bceb 255 float _4bx;
icyzkungz 6:5665d427bceb 256 float _4bz;
icyzkungz 6:5665d427bceb 257 float _2q1 = 2.0f * q1;
icyzkungz 6:5665d427bceb 258 float _2q2 = 2.0f * q2;
icyzkungz 6:5665d427bceb 259 float _2q3 = 2.0f * q3;
icyzkungz 6:5665d427bceb 260 float _2q4 = 2.0f * q4;
icyzkungz 6:5665d427bceb 261 float _2q1q3 = 2.0f * q1 * q3;
icyzkungz 6:5665d427bceb 262 float _2q3q4 = 2.0f * q3 * q4;
icyzkungz 6:5665d427bceb 263 float q1q1 = q1 * q1;
icyzkungz 6:5665d427bceb 264 float q1q2 = q1 * q2;
icyzkungz 6:5665d427bceb 265 float q1q3 = q1 * q3;
icyzkungz 6:5665d427bceb 266 float q1q4 = q1 * q4;
icyzkungz 6:5665d427bceb 267 float q2q2 = q2 * q2;
icyzkungz 6:5665d427bceb 268 float q2q3 = q2 * q3;
icyzkungz 6:5665d427bceb 269 float q2q4 = q2 * q4;
icyzkungz 6:5665d427bceb 270 float q3q3 = q3 * q3;
icyzkungz 6:5665d427bceb 271 float q3q4 = q3 * q4;
icyzkungz 6:5665d427bceb 272 float q4q4 = q4 * q4;
icyzkungz 6:5665d427bceb 273
icyzkungz 6:5665d427bceb 274 // Normalise accelerometer measurement
icyzkungz 6:5665d427bceb 275 norm = sqrt(ax * ax + ay * ay + az * az);
icyzkungz 6:5665d427bceb 276 if (norm == 0.0f) return; // handle NaN
icyzkungz 6:5665d427bceb 277 norm = 1.0f/norm;
icyzkungz 6:5665d427bceb 278 ax *= norm;
icyzkungz 6:5665d427bceb 279 ay *= norm;
icyzkungz 6:5665d427bceb 280 az *= norm;
icyzkungz 6:5665d427bceb 281
icyzkungz 6:5665d427bceb 282 // Normalise magnetometer measurement
icyzkungz 6:5665d427bceb 283 norm = sqrt(mx * mx + my * my + mz * mz);
icyzkungz 6:5665d427bceb 284 if (norm == 0.0f) return; // handle NaN
icyzkungz 6:5665d427bceb 285 norm = 1.0f/norm;
icyzkungz 6:5665d427bceb 286 mx *= norm;
icyzkungz 6:5665d427bceb 287 my *= norm;
icyzkungz 6:5665d427bceb 288 mz *= norm;
icyzkungz 6:5665d427bceb 289
icyzkungz 6:5665d427bceb 290 // Reference direction of Earth's magnetic field
icyzkungz 6:5665d427bceb 291 _2q1mx = 2.0f * q1 * mx;
icyzkungz 6:5665d427bceb 292 _2q1my = 2.0f * q1 * my;
icyzkungz 6:5665d427bceb 293 _2q1mz = 2.0f * q1 * mz;
icyzkungz 6:5665d427bceb 294 _2q2mx = 2.0f * q2 * mx;
icyzkungz 6:5665d427bceb 295 hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
icyzkungz 6:5665d427bceb 296 hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
icyzkungz 6:5665d427bceb 297 _2bx = sqrt(hx * hx + hy * hy);
icyzkungz 6:5665d427bceb 298 _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
icyzkungz 6:5665d427bceb 299 _4bx = 2.0f * _2bx;
icyzkungz 6:5665d427bceb 300 _4bz = 2.0f * _2bz;
icyzkungz 6:5665d427bceb 301
icyzkungz 6:5665d427bceb 302 // Gradient decent algorithm corrective step
icyzkungz 6:5665d427bceb 303 s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
icyzkungz 6:5665d427bceb 304 s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
icyzkungz 6:5665d427bceb 305 s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
icyzkungz 6:5665d427bceb 306 s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
icyzkungz 6:5665d427bceb 307 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
icyzkungz 6:5665d427bceb 308 norm = 1.0f/norm;
icyzkungz 6:5665d427bceb 309 s1 *= norm;
icyzkungz 6:5665d427bceb 310 s2 *= norm;
icyzkungz 6:5665d427bceb 311 s3 *= norm;
icyzkungz 6:5665d427bceb 312 s4 *= norm;
icyzkungz 6:5665d427bceb 313
icyzkungz 6:5665d427bceb 314 // Compute rate of change of quaternion
icyzkungz 6:5665d427bceb 315 qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
icyzkungz 6:5665d427bceb 316 qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
icyzkungz 6:5665d427bceb 317 qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
icyzkungz 6:5665d427bceb 318 qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
icyzkungz 6:5665d427bceb 319
icyzkungz 6:5665d427bceb 320 // Integrate to yield quaternion
icyzkungz 6:5665d427bceb 321 q1 += qDot1 * deltat;
icyzkungz 6:5665d427bceb 322 q2 += qDot2 * deltat;
icyzkungz 6:5665d427bceb 323 q3 += qDot3 * deltat;
icyzkungz 6:5665d427bceb 324 q4 += qDot4 * deltat;
icyzkungz 6:5665d427bceb 325 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
icyzkungz 6:5665d427bceb 326 norm = 1.0f/norm;
icyzkungz 6:5665d427bceb 327 q[0] = q1 * norm;
icyzkungz 6:5665d427bceb 328 q[1] = q2 * norm;
icyzkungz 6:5665d427bceb 329 q[2] = q3 * norm;
icyzkungz 6:5665d427bceb 330 q[3] = q4 * norm;
icyzkungz 6:5665d427bceb 331
icyzkungz 6:5665d427bceb 332 }
icyzkungz 6:5665d427bceb 333
icyzkungz 6:5665d427bceb 334
icyzkungz 6:5665d427bceb 335
icyzkungz 6:5665d427bceb 336 // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
icyzkungz 6:5665d427bceb 337 // measured ones.
icyzkungz 6:5665d427bceb 338 void AHRS::MahonyQuaternionUpdate()
icyzkungz 6:5665d427bceb 339 {
icyzkungz 6:5665d427bceb 340 q1 = q[0];
icyzkungz 6:5665d427bceb 341 q2 = q[1];
icyzkungz 6:5665d427bceb 342 q3 = q[2];
icyzkungz 6:5665d427bceb 343 q4 = q[3]; // short name local variable for readability
icyzkungz 6:5665d427bceb 344
icyzkungz 6:5665d427bceb 345 // Auxiliary variables to avoid repeated arithmetic
icyzkungz 6:5665d427bceb 346 q1q1 = q1 * q1;
icyzkungz 6:5665d427bceb 347 q1q2 = q1 * q2;
icyzkungz 6:5665d427bceb 348 q1q3 = q1 * q3;
icyzkungz 6:5665d427bceb 349 q1q4 = q1 * q4;
icyzkungz 6:5665d427bceb 350 q2q2 = q2 * q2;
icyzkungz 6:5665d427bceb 351 q2q3 = q2 * q3;
icyzkungz 6:5665d427bceb 352 q2q4 = q2 * q4;
icyzkungz 6:5665d427bceb 353 q3q3 = q3 * q3;
icyzkungz 6:5665d427bceb 354 q3q4 = q3 * q4;
icyzkungz 6:5665d427bceb 355 q4q4 = q4 * q4;
icyzkungz 6:5665d427bceb 356
icyzkungz 6:5665d427bceb 357 // Normalise accelerometer measurement
icyzkungz 6:5665d427bceb 358 norm = sqrt(ax * ax + ay * ay + az * az);
icyzkungz 6:5665d427bceb 359 if (norm == 0.0f) return; // handle NaN
icyzkungz 6:5665d427bceb 360 norm = 1.0f / norm; // use reciprocal for division
icyzkungz 6:5665d427bceb 361 ax *= norm;
icyzkungz 6:5665d427bceb 362 ay *= norm;
icyzkungz 6:5665d427bceb 363 az *= norm;
icyzkungz 6:5665d427bceb 364
icyzkungz 6:5665d427bceb 365 // Normalise magnetometer measurement
icyzkungz 6:5665d427bceb 366 norm = sqrt(mx * mx + my * my + mz * mz);
icyzkungz 6:5665d427bceb 367 if (norm == 0.0f) return; // handle NaN
icyzkungz 6:5665d427bceb 368 norm = 1.0f / norm; // use reciprocal for division
icyzkungz 6:5665d427bceb 369 mx *= norm;
icyzkungz 6:5665d427bceb 370 my *= norm;
icyzkungz 6:5665d427bceb 371 mz *= norm;
icyzkungz 6:5665d427bceb 372
icyzkungz 6:5665d427bceb 373 // Reference direction of Earth's magnetic field
icyzkungz 6:5665d427bceb 374 hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
icyzkungz 6:5665d427bceb 375 hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
icyzkungz 6:5665d427bceb 376 bx = sqrt((hx * hx) + (hy * hy));
icyzkungz 6:5665d427bceb 377 bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
icyzkungz 6:5665d427bceb 378
icyzkungz 6:5665d427bceb 379 // Estimated direction of gravity and magnetic field
icyzkungz 6:5665d427bceb 380 vx = 2.0f * (q2q4 - q1q3);
icyzkungz 6:5665d427bceb 381 vy = 2.0f * (q1q2 + q3q4);
icyzkungz 6:5665d427bceb 382 vz = q1q1 - q2q2 - q3q3 + q4q4;
icyzkungz 6:5665d427bceb 383 wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
icyzkungz 6:5665d427bceb 384 wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
icyzkungz 6:5665d427bceb 385 wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
icyzkungz 6:5665d427bceb 386
icyzkungz 6:5665d427bceb 387 // Error is cross product between estimated direction and measured direction of gravity
icyzkungz 6:5665d427bceb 388 ex = (ay * vz - az * vy) + (my * wz - mz * wy);
icyzkungz 6:5665d427bceb 389 ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
icyzkungz 6:5665d427bceb 390 ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
icyzkungz 6:5665d427bceb 391 if (Ki > 0.0f) {
icyzkungz 6:5665d427bceb 392 eInt[0] += ex; // accumulate integral error
icyzkungz 6:5665d427bceb 393 eInt[1] += ey;
icyzkungz 6:5665d427bceb 394 eInt[2] += ez;
icyzkungz 6:5665d427bceb 395 } else {
icyzkungz 6:5665d427bceb 396 eInt[0] = 0.0f; // prevent integral wind up
icyzkungz 6:5665d427bceb 397 eInt[1] = 0.0f;
icyzkungz 6:5665d427bceb 398 eInt[2] = 0.0f;
icyzkungz 6:5665d427bceb 399 }
icyzkungz 6:5665d427bceb 400
icyzkungz 6:5665d427bceb 401 // Apply feedback terms
icyzkungz 6:5665d427bceb 402 gx = gx + Kp * ex + Ki * eInt[0];
icyzkungz 6:5665d427bceb 403 gy = gy + Kp * ey + Ki * eInt[1];
icyzkungz 6:5665d427bceb 404 gz = gz + Kp * ez + Ki * eInt[2];
icyzkungz 6:5665d427bceb 405
icyzkungz 6:5665d427bceb 406 // Integrate rate of change of quaternion
icyzkungz 6:5665d427bceb 407 pa = q2;
icyzkungz 6:5665d427bceb 408 pb = q3;
icyzkungz 6:5665d427bceb 409 pc = q4;
icyzkungz 6:5665d427bceb 410 q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
icyzkungz 6:5665d427bceb 411 q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
icyzkungz 6:5665d427bceb 412 q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
icyzkungz 6:5665d427bceb 413 q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
icyzkungz 6:5665d427bceb 414
icyzkungz 6:5665d427bceb 415 // Normalise quaternion
icyzkungz 6:5665d427bceb 416 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
icyzkungz 6:5665d427bceb 417 norm = 1.0f / norm;
icyzkungz 6:5665d427bceb 418 q[0] = q1 * norm;
icyzkungz 6:5665d427bceb 419 q[1] = q2 * norm;
icyzkungz 6:5665d427bceb 420 q[2] = q3 * norm;
icyzkungz 6:5665d427bceb 421 q[3] = q4 * norm;
icyzkungz 6:5665d427bceb 422
icyzkungz 6:5665d427bceb 423 }
icyzkungz 6:5665d427bceb 424
icyzkungz 6:5665d427bceb 425 void AHRS::TimeStart()
icyzkungz 6:5665d427bceb 426 {
icyzkungz 6:5665d427bceb 427 t.start();
icyzkungz 6:5665d427bceb 428 }
icyzkungz 6:5665d427bceb 429
icyzkungz 6:5665d427bceb 430 void AHRS::TimeCal()
icyzkungz 6:5665d427bceb 431 {
icyzkungz 6:5665d427bceb 432 Now = t.read_us();
icyzkungz 6:5665d427bceb 433 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
icyzkungz 6:5665d427bceb 434 lastUpdate = Now;
icyzkungz 6:5665d427bceb 435
icyzkungz 6:5665d427bceb 436 sum += deltat;
icyzkungz 6:5665d427bceb 437 sumCount++;
icyzkungz 6:5665d427bceb 438 }
icyzkungz 6:5665d427bceb 439
icyzkungz 6:5665d427bceb 440 void AHRS::Run()
icyzkungz 6:5665d427bceb 441 {
icyzkungz 6:5665d427bceb 442 ReadRawAccGyroMag();
icyzkungz 6:5665d427bceb 443 TimeCal();
icyzkungz 6:5665d427bceb 444 MadgwickQuaternionUpdate();
icyzkungz 6:5665d427bceb 445
icyzkungz 6:5665d427bceb 446 delt_t = t.read_ms() - count;
icyzkungz 6:5665d427bceb 447 if (delt_t > 500) { // update LCD once per half-second independent of read rate
icyzkungz 6:5665d427bceb 448
icyzkungz 8:928673148b55 449 /*pc2.printf("ax = %f", 1000*ax);
icyzkungz 8:928673148b55 450 pc2.printf(" ay = %f", 1000*ay);
icyzkungz 8:928673148b55 451 pc2.printf(" az = %f mg\n\r", 1000*az);
icyzkungz 6:5665d427bceb 452
icyzkungz 8:928673148b55 453 pc2.printf("gx = %f", gx);
icyzkungz 8:928673148b55 454 pc2.printf(" gy = %f", gy);
icyzkungz 8:928673148b55 455 pc2.printf(" gz = %f deg/s\n\r", gz);
icyzkungz 6:5665d427bceb 456
icyzkungz 8:928673148b55 457 pc2.printf("mx = %f", mx);
icyzkungz 8:928673148b55 458 pc2.printf(" my = %f", my);
icyzkungz 8:928673148b55 459 pc2.printf(" mz = %f mG\n\r", mz);*/
icyzkungz 6:5665d427bceb 460
icyzkungz 6:5665d427bceb 461
icyzkungz 8:928673148b55 462 //pc2.printf("%f,%f,%f",mx,my,mz);
icyzkungz 6:5665d427bceb 463
icyzkungz 6:5665d427bceb 464 whoami = readByte(AK8963_ADDRESS, AK8963_ST2); // Read WHO_AM_I register for MPU-9250
icyzkungz 8:928673148b55 465 // pc2.printf("I AM 0x%x\n\r", whoami); pc2.printf("I SHOULD BE 0x10\n\r");
icyzkungz 6:5665d427bceb 466 if(whoami == 0x14) {
icyzkungz 6:5665d427bceb 467 printf("I AM 0x%x\n\r", whoami);
icyzkungz 6:5665d427bceb 468 while(1);
icyzkungz 6:5665d427bceb 469 }
icyzkungz 6:5665d427bceb 470
icyzkungz 6:5665d427bceb 471
icyzkungz 6:5665d427bceb 472 readTempData();
icyzkungz 6:5665d427bceb 473 temperature = ((float) temperature) / 333.87f + 21.0f; // Temperature in degrees Centigrade
icyzkungz 8:928673148b55 474 //pc2.printf(" temperature = %f C\n\r", temperature);
icyzkungz 6:5665d427bceb 475
icyzkungz 8:928673148b55 476 // pc2.printf("q0 = %f\n\r", q[0]);
icyzkungz 8:928673148b55 477 // pc2.printf("q1 = %f\n\r", q[1]);
icyzkungz 8:928673148b55 478 // pc2.printf("q2 = %f\n\r", q[2]);
icyzkungz 8:928673148b55 479 // pc2.printf("q3 = %f\n\r", q[3]);
icyzkungz 6:5665d427bceb 480
icyzkungz 6:5665d427bceb 481 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
icyzkungz 6:5665d427bceb 482 // In this coordinate system, the positive z-axis is down toward Earth.
icyzkungz 6:5665d427bceb 483 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
icyzkungz 6:5665d427bceb 484 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
icyzkungz 6:5665d427bceb 485 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
icyzkungz 6:5665d427bceb 486 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
icyzkungz 6:5665d427bceb 487 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
icyzkungz 6:5665d427bceb 488 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
icyzkungz 6:5665d427bceb 489 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
icyzkungz 6:5665d427bceb 490 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
icyzkungz 6:5665d427bceb 491 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
icyzkungz 6:5665d427bceb 492 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
icyzkungz 6:5665d427bceb 493
icyzkungz 6:5665d427bceb 494 Xh = mx*cos(pitch)+my*sin(roll)*sin(pitch)-mz*cos(roll)*sin(pitch);
icyzkungz 6:5665d427bceb 495 Yh = my*cos(roll)+mz*sin(roll);
icyzkungz 6:5665d427bceb 496
icyzkungz 6:5665d427bceb 497 yawmag = atan2(Yh,Xh)+PI;
icyzkungz 8:928673148b55 498 //pc2.printf("Xh= %f Yh= %f ",Xh,Yh);
icyzkungz 8:928673148b55 499 //pc2.printf("Yaw[mag]= %f\n\r",yawmag*180.0f/PI);
icyzkungz 8:928673148b55 500 //pc2.printf(",%f",yawmag*180.0f/PI);
icyzkungz 6:5665d427bceb 501
icyzkungz 6:5665d427bceb 502
icyzkungz 6:5665d427bceb 503
icyzkungz 6:5665d427bceb 504 pitch *= 180.0f / PI;
icyzkungz 6:5665d427bceb 505 yaw *= 180.0f / PI;
icyzkungz 6:5665d427bceb 506 yaw += 180.0f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
icyzkungz 6:5665d427bceb 507 roll *= 180.0f / PI;
icyzkungz 6:5665d427bceb 508
icyzkungz 8:928673148b55 509 //pc2.printf(",%f,%f,%f\n",roll,pitch,yaw);
icyzkungz 8:928673148b55 510 //pc2.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
icyzkungz 8:928673148b55 511 //pc2.printf("average rate = %f\n\r", (float) sumCount/sum);
icyzkungz 6:5665d427bceb 512
icyzkungz 6:5665d427bceb 513 myled= !myled;
icyzkungz 6:5665d427bceb 514 count = t.read_ms();
icyzkungz 6:5665d427bceb 515
icyzkungz 6:5665d427bceb 516 if(count > 1<<21) {
icyzkungz 6:5665d427bceb 517 t.start(); // start the timer over again if ~30 minutes has passed
icyzkungz 6:5665d427bceb 518 count = 0;
icyzkungz 6:5665d427bceb 519 deltat= 0;
icyzkungz 6:5665d427bceb 520 lastUpdate = t.read_us();
icyzkungz 6:5665d427bceb 521 }
icyzkungz 6:5665d427bceb 522 sum = 0;
icyzkungz 6:5665d427bceb 523 sumCount = 0;
icyzkungz 6:5665d427bceb 524 }
icyzkungz 6:5665d427bceb 525 }
icyzkungz 6:5665d427bceb 526
icyzkungz 8:928673148b55 527 void AHRS::PrintRollPitchYaw()
icyzkungz 6:5665d427bceb 528 {
icyzkungz 8:928673148b55 529 pc2.printf("roll : %f, pitch : %f, yaw : %f\n",roll,pitch,yaw);
icyzkungz 6:5665d427bceb 530 }