BE@R lab / AHRS_fillter

filter for imu

Dependencies:   MPU9250_Interface

Dependents:   MPU9250AHRS_OOP

AHRS.cpp@1:054bfc57f0f9, 2016-01-20 (annotated)

Committer:
soulx
Date:
Wed Jan 20 15:13:41 2016 +0000
Revision:
1:054bfc57f0f9
Parent:
0:bf9febe45e1d 
ahrs filter

Who changed what in which revision?

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