| 265 | // Madgwick AHRS algorithm update |
| 266 | |
| 267 | void SF::MadgwickUpdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float deltat) { |
| 268 | float recipNorm; |
| 269 | float s0, s1, s2, s3; |
| 270 | float qDot1, qDot2, qDot3, qDot4; |
| 271 | float hx, hy; |
| 272 | float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; |
| 273 | |
| 274 | // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) |
| 275 | if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { |
| 276 | MadgwickUpdate(gx, gy, gz, ax, ay, az, deltat); |
| 277 | return; |
| 278 | } |
| 279 | |
| 280 | // Rate of change of quaternion from gyroscope |
| 281 | qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); |
| 282 | qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); |
| 283 | qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); |
| 284 | qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); |
| 285 | |
| 286 | // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) |
| 287 | if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { |
| 288 | |
| 289 | // Normalise accelerometer measurement |
| 290 | recipNorm = invSqrt(ax * ax + ay * ay + az * az); |
| 291 | ax *= recipNorm; |
| 292 | ay *= recipNorm; |
| 293 | az *= recipNorm; |
| 294 | |
| 295 | // Normalise magnetometer measurement |
| 296 | recipNorm = invSqrt(mx * mx + my * my + mz * mz); |
| 297 | mx *= recipNorm; |
| 298 | my *= recipNorm; |
| 299 | mz *= recipNorm; |
| 300 | |
| 301 | // Auxiliary variables to avoid repeated arithmetic |
| 302 | _2q0mx = 2.0f * q0 * mx; |
| 303 | _2q0my = 2.0f * q0 * my; |
| 304 | _2q0mz = 2.0f * q0 * mz; |
| 305 | _2q1mx = 2.0f * q1 * mx; |
| 306 | _2q0 = 2.0f * q0; |
| 307 | _2q1 = 2.0f * q1; |
| 308 | _2q2 = 2.0f * q2; |
| 309 | _2q3 = 2.0f * q3; |
| 310 | _2q0q2 = 2.0f * q0 * q2; |
| 311 | _2q2q3 = 2.0f * q2 * q3; |
| 312 | q0q0 = q0 * q0; |
| 313 | q0q1 = q0 * q1; |
| 314 | q0q2 = q0 * q2; |
| 315 | q0q3 = q0 * q3; |
| 316 | q1q1 = q1 * q1; |
| 317 | q1q2 = q1 * q2; |
| 318 | q1q3 = q1 * q3; |
| 319 | q2q2 = q2 * q2; |
| 320 | q2q3 = q2 * q3; |
| 321 | q3q3 = q3 * q3; |
| 322 | |
| 323 | // Reference direction of Earth's magnetic field |
| 324 | hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; |
nothing calls this directly
no outgoing calls
no test coverage detected