| 82 | // Mahony AHRS algorithm update |
| 83 | |
| 84 | void SF::MahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float deltat) |
| 85 | { |
| 86 | float recipNorm; |
| 87 | float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; |
| 88 | float hx, hy, bx, bz; |
| 89 | float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz; |
| 90 | float halfex, halfey, halfez; |
| 91 | float qa, qb, qc; |
| 92 | |
| 93 | // Use IMU algorithm if magnetometer measurement invalid |
| 94 | // (avoids NaN in magnetometer normalisation) |
| 95 | if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { |
| 96 | MahonyUpdate(gx, gy, gz, ax, ay, az, deltat); |
| 97 | return; |
| 98 | } |
| 99 | |
| 100 | // Compute feedback only if accelerometer measurement valid |
| 101 | // (avoids NaN in accelerometer normalisation) |
| 102 | if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { |
| 103 | |
| 104 | // Normalise accelerometer measurement |
| 105 | recipNorm = invSqrt(ax * ax + ay * ay + az * az); |
| 106 | ax *= recipNorm; |
| 107 | ay *= recipNorm; |
| 108 | az *= recipNorm; |
| 109 | |
| 110 | // Normalise magnetometer measurement |
| 111 | recipNorm = invSqrt(mx * mx + my * my + mz * mz); |
| 112 | mx *= recipNorm; |
| 113 | my *= recipNorm; |
| 114 | mz *= recipNorm; |
| 115 | |
| 116 | // Auxiliary variables to avoid repeated arithmetic |
| 117 | q0q0 = q0 * q0; |
| 118 | q0q1 = q0 * q1; |
| 119 | q0q2 = q0 * q2; |
| 120 | q0q3 = q0 * q3; |
| 121 | q1q1 = q1 * q1; |
| 122 | q1q2 = q1 * q2; |
| 123 | q1q3 = q1 * q3; |
| 124 | q2q2 = q2 * q2; |
| 125 | q2q3 = q2 * q3; |
| 126 | q3q3 = q3 * q3; |
| 127 | |
| 128 | // Reference direction of Earth's magnetic field |
| 129 | hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); |
| 130 | hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); |
| 131 | bx = sqrtf(hx * hx + hy * hy); |
| 132 | bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2)); |
| 133 | |
| 134 | // Estimated direction of gravity and magnetic field |
| 135 | halfvx = q1q3 - q0q2; |
| 136 | halfvy = q0q1 + q2q3; |
| 137 | halfvz = q0q0 - 0.5f + q3q3; |
| 138 | halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); |
| 139 | halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); |
| 140 | halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); |
| 141 |
nothing calls this directly
no outgoing calls
no test coverage detected