altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

Mahony.cpp@3:6811c0ce95f6, 2018-12-04 (annotated)

Committer:
altb
Date:
Tue Dec 04 15:49:48 2018 +0000
Revision:
3:6811c0ce95f6
Parent:
1:36bbe04e1f6f 
AHRS Klasse mit Mahony filter etc

Who changed what in which revision?

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