src/main/flight/imu.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 // Inertial Measurement Unit (IMU)
  22
  23 #include <stdbool.h>
  24 #include <stdint.h>
  25 #include <stdlib.h>
  26 #include <math.h>
  27
  28 #include "platform.h"
  29
  30 #include "build/build_config.h"
  31 #include "build/debug.h"
  32
  33 #include "common/axis.h"
  34 #include "common/vector.h"
  35
  36 #include "pg/pg.h"
  37 #include "pg/pg_ids.h"
  38
  39 #include "drivers/time.h"
  40
  41 #include "fc/runtime_config.h"
  42
  43 #include "flight/gps_rescue.h"
  44 #include "flight/imu.h"
  45 #include "flight/mixer.h"
  46 #include "flight/pid.h"
  47
  48 #include "io/gps.h"
  49
  50 #include "scheduler/scheduler.h"
  51
  52 #include "sensors/acceleration.h"
  53 #include "sensors/barometer.h"
  54 #include "sensors/compass.h"
  55 #include "sensors/gyro.h"
  56 #include "sensors/sensors.h"
  57
  58 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
  59 #include <stdio.h>
  60 #include <pthread.h>
  61
  62 static pthread_mutex_t imuUpdateLock;
  63
  64 #if defined(SIMULATOR_IMU_SYNC)
  65 static uint32_t imuDeltaT = 0;
  66 static bool imuUpdated = false;
  67 #endif
  68
  69 #define IMU_LOCK pthread_mutex_lock(&imuUpdateLock)
  70 #define IMU_UNLOCK pthread_mutex_unlock(&imuUpdateLock)
  71
  72 #else
  73
  74 #define IMU_LOCK
  75 #define IMU_UNLOCK
  76
  77 #endif
  78
  79 // the limit (in degrees/second) beyond which we stop integrating
  80 // omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
  81 // which results in false gyro drift. See
  82 // https://drive.google.com/file/d/0ByvTkVQo3tqXQUVCVUNyZEgtRGs/view?usp=sharing&resourcekey=0-Mo4254cxdWWx2Y4mGN78Zw
  83
  84 #define SPIN_RATE_LIMIT 20
  85
  86 #define ATTITUDE_RESET_QUIET_TIME 250000   // 250ms - gyro quiet period after disarm before attitude reset
  87 #define ATTITUDE_RESET_GYRO_LIMIT 15       // 15 deg/sec - gyro limit for quiet period
  88 #define ATTITUDE_RESET_KP_GAIN    25.0     // dcmKpGain value to use during attitude reset
  89 #define ATTITUDE_RESET_ACTIVE_TIME 500000  // 500ms - Time to wait for attitude to converge at high gain
  90 #define GPS_COG_MIN_GROUNDSPEED 200        // 200cm/s minimum groundspeed for a gps based IMU heading to be considered valid
  91                                            // Better to have some update than none for GPS Rescue at slow return speeds
  92
  93 bool canUseGPSHeading = true;
  94
  95 static float throttleAngleScale;
  96 static int throttleAngleValue;
  97 static float smallAngleCosZ = 0;
  98
  99 static imuRuntimeConfig_t imuRuntimeConfig;
 100
 101 float rMat[3][3];
 102
 103 #if defined(USE_ACC)
 104 STATIC_UNIT_TESTED bool attitudeIsEstablished = false;
 105 #endif
 106
 107 // quaternion of sensor frame relative to earth frame
 108 STATIC_UNIT_TESTED quaternion q = QUATERNION_INITIALIZE;
 109 STATIC_UNIT_TESTED quaternionProducts qP = QUATERNION_PRODUCTS_INITIALIZE;
 110 // headfree quaternions
 111 quaternion headfree = QUATERNION_INITIALIZE;
 112 quaternion offset = QUATERNION_INITIALIZE;
 113
 114 // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 115 attitudeEulerAngles_t attitude = EULER_INITIALIZE;
 116
 117 PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2);
 118
 119 PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
 120     .dcm_kp = 2500,                // 1.0 * 10000
 121     .dcm_ki = 0,                   // 0.003 * 10000
 122     .small_angle = 25,
 123     .imu_process_denom = 2
 124 );
 125
 126 static void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd)
 127 {
 128     quatProd->ww = quat->w * quat->w;
 129     quatProd->wx = quat->w * quat->x;
 130     quatProd->wy = quat->w * quat->y;
 131     quatProd->wz = quat->w * quat->z;
 132     quatProd->xx = quat->x * quat->x;
 133     quatProd->xy = quat->x * quat->y;
 134     quatProd->xz = quat->x * quat->z;
 135     quatProd->yy = quat->y * quat->y;
 136     quatProd->yz = quat->y * quat->z;
 137     quatProd->zz = quat->z * quat->z;
 138 }
 139
 140 STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
 141 {
 142     imuQuaternionComputeProducts(&q, &qP);
 143
 144     rMat[0][0] = 1.0f - 2.0f * qP.yy - 2.0f * qP.zz;
 145     rMat[0][1] = 2.0f * (qP.xy + -qP.wz);
 146     rMat[0][2] = 2.0f * (qP.xz - -qP.wy);
 147
 148     rMat[1][0] = 2.0f * (qP.xy - -qP.wz);
 149     rMat[1][1] = 1.0f - 2.0f * qP.xx - 2.0f * qP.zz;
 150     rMat[1][2] = 2.0f * (qP.yz + -qP.wx);
 151
 152     rMat[2][0] = 2.0f * (qP.xz + -qP.wy);
 153     rMat[2][1] = 2.0f * (qP.yz - -qP.wx);
 154     rMat[2][2] = 1.0f - 2.0f * qP.xx - 2.0f * qP.yy;
 155
 156 #if defined(SIMULATOR_BUILD) && !defined(USE_IMU_CALC) && !defined(SET_IMU_FROM_EULER)
 157     rMat[1][0] = -2.0f * (qP.xy - -qP.wz);
 158     rMat[2][0] = -2.0f * (qP.xz + -qP.wy);
 159 #endif
 160 }
 161
 162 static float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
 163 {
 164     return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
 165 }
 166
 167 void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value)
 168 {
 169     imuRuntimeConfig.dcm_kp = imuConfig()->dcm_kp / 10000.0f;
 170     imuRuntimeConfig.dcm_ki = imuConfig()->dcm_ki / 10000.0f;
 171
 172     smallAngleCosZ = cos_approx(degreesToRadians(imuConfig()->small_angle));
 173
 174     throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
 175
 176     throttleAngleValue = throttle_correction_value;
 177 }
 178
 179 void imuInit(void)
 180 {
 181 #ifdef USE_GPS
 182     canUseGPSHeading = true;
 183 #else
 184     canUseGPSHeading = false;
 185 #endif
 186
 187     imuComputeRotationMatrix();
 188
 189 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
 190     if (pthread_mutex_init(&imuUpdateLock, NULL) != 0) {
 191         printf("Create imuUpdateLock error!\n");
 192     }
 193 #endif
 194 }
 195
 196 #if defined(USE_ACC)
 197 static float invSqrt(float x)
 198 {
 199     return 1.0f / sqrtf(x);
 200 }
 201
 202 STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
 203                                 bool useAcc, float ax, float ay, float az,
 204                                 bool useMag,
 205                                 float cogYawGain, float courseOverGround, const float dcmKpGain)
 206 {
 207     static float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;    // integral error terms scaled by Ki
 208
 209     // Calculate general spin rate (rad/s)
 210     const float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
 211
 212     // Use raw heading error (from GPS or whatever else)
 213     float ex = 0, ey = 0, ez = 0;
 214     if (cogYawGain != 0.0f) {
 215         // Used in a GPS Rescue to boost IMU yaw gain when course over ground and velocity to home differ significantly
 216
 217         // Compute heading vector in EF from scalar CoG. CoG is clockwise from North
 218         // Note that Earth frame X is pointing north and sin/cos argument is anticlockwise
 219         const fpVector2_t cog_ef = {.x = cos_approx(-courseOverGround), .y = sin_approx(-courseOverGround)};
 220 #define THRUST_COG 1
 221 #if THRUST_COG
 222         const fpVector2_t heading_ef = {.x = rMat[X][Z], .y = rMat[Y][Z]};  // body Z axis (up) - direction of thrust vector
 223 #else
 224         const fpVector2_t heading_ef = {.x = rMat[0][0], .y = rMat[1][0]};  // body X axis. Projected vector magnitude is reduced as pitch increases
 225 #endif
 226         // cross product = 1 * |heading| * sin(angle) (magnitude of Z vector in 3D)
 227         // order operands so that rotation is in direction of zero error
 228         const float cross = vector2Cross(&heading_ef, &cog_ef);
 229         // dot product, 1 * |heading| * cos(angle)
 230         const float dot = vector2Dot(&heading_ef, &cog_ef);
 231         // use cross product / sin(angle) when error < 90deg (cos > 0),
 232         //   |heading| if error is larger (cos < 0)
 233         const float heading_mag = vector2Mag(&heading_ef);
 234         float ez_ef = (dot > 0) ? cross : (cross < 0 ? -1.0f : 1.0f) * heading_mag;
 235 #if THRUST_COG
 236         // increase gain for small tilt (just heuristic; sqrt is cheap on F4+)
 237         ez_ef /= sqrtf(heading_mag);
 238 #endif
 239         ez_ef *= cogYawGain;          // apply gain parameter
 240         // covert to body frame
 241         ex += rMat[2][0] * ez_ef;
 242         ey += rMat[2][1] * ez_ef;
 243         ez += rMat[2][2] * ez_ef;
 244     }
 245
 246 #ifdef USE_MAG
 247     // Use measured magnetic field vector
 248     fpVector3_t mag_bf = {{mag.magADC[X], mag.magADC[Y], mag.magADC[Z]}};
 249     float recipMagNorm = vectorNormSquared(&mag_bf);
 250     if (useMag && recipMagNorm > 0.01f) {
 251         // Normalise magnetometer measurement
 252         vectorNormalize(&mag_bf, &mag_bf);
 253
 254         // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
 255         // This way magnetic field will only affect heading and wont mess roll/pitch angles
 256
 257         // (hx; hy; 0) - measured mag field vector in EF (forcing Z-component to zero)
 258         // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
 259         fpVector3_t mag_ef;
 260         matrixVectorMul(&mag_ef, (const fpMat33_t*)&rMat, &mag_bf);  // BF->EF
 261         mag_ef.z = 0.0f;                // project to XY plane (optimized away)
 262
 263         fpVector2_t north_ef = {{ 1.0f, 0.0f }};
 264         // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
 265         // increase gain on large misalignment
 266         const float dot = vector2Dot((fpVector2_t*)&mag_ef, &north_ef);
 267         const float cross = vector2Cross((fpVector2_t*)&mag_ef, &north_ef);
 268         const float ez_ef = (dot > 0) ? cross : (cross < 0 ? -1.0f : 1.0f) * vector2Mag((fpVector2_t*)&mag_ef);
 269         // Rotate mag error vector back to BF and accumulate
 270         ex += rMat[2][0] * ez_ef;
 271         ey += rMat[2][1] * ez_ef;
 272         ez += rMat[2][2] * ez_ef;
 273     }
 274 #else
 275     UNUSED(useMag);
 276 #endif
 277
 278     // Use measured acceleration vector
 279     float recipAccNorm = sq(ax) + sq(ay) + sq(az);
 280     if (useAcc && recipAccNorm > 0.01f) {
 281         // Normalise accelerometer measurement; useAcc is true when all smoothed acc axes are within 20% of 1G
 282         recipAccNorm = invSqrt(recipAccNorm);
 283         ax *= recipAccNorm;
 284         ay *= recipAccNorm;
 285         az *= recipAccNorm;
 286
 287         // Error is sum of cross product between estimated direction and measured direction of gravity
 288         ex += (ay * rMat[2][2] - az * rMat[2][1]);
 289         ey += (az * rMat[2][0] - ax * rMat[2][2]);
 290         ez += (ax * rMat[2][1] - ay * rMat[2][0]);
 291     }
 292
 293     // Compute and apply integral feedback if enabled
 294     if (imuRuntimeConfig.dcm_ki > 0.0f) {
 295         // Stop integrating if spinning beyond the certain limit
 296         if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
 297             const float dcmKiGain = imuRuntimeConfig.dcm_ki;
 298             integralFBx += dcmKiGain * ex * dt;    // integral error scaled by Ki
 299             integralFBy += dcmKiGain * ey * dt;
 300             integralFBz += dcmKiGain * ez * dt;
 301         }
 302     } else {
 303         integralFBx = 0.0f;    // prevent integral windup
 304         integralFBy = 0.0f;
 305         integralFBz = 0.0f;
 306     }
 307
 308     // Apply proportional and integral feedback
 309     gx += dcmKpGain * ex + integralFBx;
 310     gy += dcmKpGain * ey + integralFBy;
 311     gz += dcmKpGain * ez + integralFBz;
 312
 313     // Integrate rate of change of quaternion
 314     gx *= (0.5f * dt);
 315     gy *= (0.5f * dt);
 316     gz *= (0.5f * dt);
 317
 318     quaternion buffer;
 319     buffer.w = q.w;
 320     buffer.x = q.x;
 321     buffer.y = q.y;
 322     buffer.z = q.z;
 323
 324     q.w += (-buffer.x * gx - buffer.y * gy - buffer.z * gz);
 325     q.x += (+buffer.w * gx + buffer.y * gz - buffer.z * gy);
 326     q.y += (+buffer.w * gy - buffer.x * gz + buffer.z * gx);
 327     q.z += (+buffer.w * gz + buffer.x * gy - buffer.y * gx);
 328
 329     // Normalise quaternion
 330     float recipNorm = invSqrt(sq(q.w) + sq(q.x) + sq(q.y) + sq(q.z));
 331     q.w *= recipNorm;
 332     q.x *= recipNorm;
 333     q.y *= recipNorm;
 334     q.z *= recipNorm;
 335
 336     // Pre-compute rotation matrix from quaternion
 337     imuComputeRotationMatrix();
 338
 339     attitudeIsEstablished = true;
 340 }
 341
 342 STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
 343 {
 344     quaternionProducts buffer;
 345
 346     if (FLIGHT_MODE(HEADFREE_MODE)) {
 347        imuQuaternionComputeProducts(&headfree, &buffer);
 348
 349        attitude.values.roll = lrintf(atan2_approx((+2.0f * (buffer.wx + buffer.yz)), (+1.0f - 2.0f * (buffer.xx + buffer.yy))) * (1800.0f / M_PIf));
 350        attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(+2.0f * (buffer.wy - buffer.xz))) * (1800.0f / M_PIf));
 351        attitude.values.yaw = lrintf((-atan2_approx((+2.0f * (buffer.wz + buffer.xy)), (+1.0f - 2.0f * (buffer.yy + buffer.zz))) * (1800.0f / M_PIf)));
 352     } else {
 353        attitude.values.roll = lrintf(atan2_approx(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf));
 354        attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(-rMat[2][0])) * (1800.0f / M_PIf));
 355        attitude.values.yaw = lrintf((-atan2_approx(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf)));
 356     }
 357
 358     if (attitude.values.yaw < 0) {
 359         attitude.values.yaw += 3600;
 360     }
 361 }
 362
 363 static bool imuIsAccelerometerHealthy(float *accAverage)
 364 {
 365     float accMagnitudeSq = 0;
 366     for (int axis = 0; axis < 3; axis++) {
 367         const float a = accAverage[axis];
 368         accMagnitudeSq += a * a;
 369     }
 370
 371     accMagnitudeSq = accMagnitudeSq * sq(acc.dev.acc_1G_rec);
 372
 373     // Accept accel readings only in range 0.9g - 1.1g
 374     return (0.81f < accMagnitudeSq) && (accMagnitudeSq < 1.21f);
 375 }
 376
 377 // Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.dcm_kp * 1.0 scaling.
 378 // When disarmed after initial boot, the scaling is set to 10.0 for the first 20 seconds to speed up initial convergence.
 379 // After disarming we want to quickly reestablish convergence to deal with the attitude estimation being incorrect due to a crash.
 380 //   - wait for a 250ms period of low gyro activity to ensure the craft is not moving
 381 //   - use a large dcmKpGain value for 500ms to allow the attitude estimate to quickly converge
 382 //   - reset the gain back to the standard setting
 383 static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
 384 {
 385     static bool lastArmState = false;
 386     static timeUs_t gyroQuietPeriodTimeEnd = 0;
 387     static timeUs_t attitudeResetTimeEnd = 0;
 388     static bool attitudeResetCompleted = false;
 389     float ret;
 390     bool attitudeResetActive = false;
 391
 392     const bool armState = ARMING_FLAG(ARMED);
 393
 394     if (!armState) {
 395         if (lastArmState) {   // Just disarmed; start the gyro quiet period
 396             gyroQuietPeriodTimeEnd = currentTimeUs + ATTITUDE_RESET_QUIET_TIME;
 397             attitudeResetTimeEnd = 0;
 398             attitudeResetCompleted = false;
 399         }
 400
 401         // If gyro activity exceeds the threshold then restart the quiet period.
 402         // Also, if the attitude reset has been complete and there is subsequent gyro activity then
 403         // start the reset cycle again. This addresses the case where the pilot rights the craft after a crash.
 404         if ((attitudeResetTimeEnd > 0) || (gyroQuietPeriodTimeEnd > 0) || attitudeResetCompleted) {
 405             if ((fabsf(gyroAverage[X]) > ATTITUDE_RESET_GYRO_LIMIT)
 406                 || (fabsf(gyroAverage[Y]) > ATTITUDE_RESET_GYRO_LIMIT)
 407                 || (fabsf(gyroAverage[Z]) > ATTITUDE_RESET_GYRO_LIMIT)
 408                 || (!useAcc)) {
 409
 410                 gyroQuietPeriodTimeEnd = currentTimeUs + ATTITUDE_RESET_QUIET_TIME;
 411                 attitudeResetTimeEnd = 0;
 412             }
 413         }
 414         if (attitudeResetTimeEnd > 0) {        // Resetting the attitude estimation
 415             if (currentTimeUs >= attitudeResetTimeEnd) {
 416                 gyroQuietPeriodTimeEnd = 0;
 417                 attitudeResetTimeEnd = 0;
 418                 attitudeResetCompleted = true;
 419             } else {
 420                 attitudeResetActive = true;
 421             }
 422         } else if ((gyroQuietPeriodTimeEnd > 0) && (currentTimeUs >= gyroQuietPeriodTimeEnd)) {
 423             // Start the high gain period to bring the estimation into convergence
 424             attitudeResetTimeEnd = currentTimeUs + ATTITUDE_RESET_ACTIVE_TIME;
 425             gyroQuietPeriodTimeEnd = 0;
 426         }
 427     }
 428     lastArmState = armState;
 429
 430     if (attitudeResetActive) {
 431         ret = ATTITUDE_RESET_KP_GAIN;
 432     } else {
 433         ret = imuRuntimeConfig.dcm_kp;
 434         if (!armState) {
 435             ret *= 10.0f; // Scale the kP to generally converge faster when disarmed.
 436         }
 437     }
 438
 439     return ret;
 440 }
 441
 442 #if defined(USE_GPS)
 443 static void imuComputeQuaternionFromRPY(quaternionProducts *quatProd, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
 444 {
 445     if (initialRoll > 1800) {
 446         initialRoll -= 3600;
 447     }
 448
 449     if (initialPitch > 1800) {
 450         initialPitch -= 3600;
 451     }
 452
 453     if (initialYaw > 1800) {
 454         initialYaw -= 3600;
 455     }
 456
 457     const float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
 458     const float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
 459
 460     const float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
 461     const float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
 462
 463     const float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
 464     const float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
 465
 466     const float q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
 467     const float q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
 468     const float q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
 469     const float q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
 470
 471     quatProd->xx = sq(q1);
 472     quatProd->yy = sq(q2);
 473     quatProd->zz = sq(q3);
 474
 475     quatProd->xy = q1 * q2;
 476     quatProd->xz = q1 * q3;
 477     quatProd->yz = q2 * q3;
 478
 479     quatProd->wx = q0 * q1;
 480     quatProd->wy = q0 * q2;
 481     quatProd->wz = q0 * q3;
 482
 483     imuComputeRotationMatrix();
 484
 485     attitudeIsEstablished = true;
 486 }
 487 #endif
 488
 489 static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
 490 {
 491     static timeUs_t previousIMUUpdateTime;
 492     bool useAcc = false;
 493     bool useMag = false;
 494     float cogYawGain = 0.0f; // IMU yaw gain to be applied in imuMahonyAHRSupdate from ground course, default to no correction from CoG
 495     float courseOverGround = 0; // To be used when cogYawGain is non-zero, in radians
 496
 497     const timeDelta_t deltaT = currentTimeUs - previousIMUUpdateTime;
 498     previousIMUUpdateTime = currentTimeUs;
 499
 500 #ifdef USE_MAG
 501     if (sensors(SENSOR_MAG) && compassIsHealthy()
 502 #ifdef USE_GPS_RESCUE
 503         && !gpsRescueDisableMag()
 504 #endif
 505         ) {
 506         useMag = true;
 507     }
 508 #endif
 509 #if defined(USE_GPS)
 510     if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat > GPS_MIN_SAT_COUNT && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) {
 511         // Use GPS course over ground to correct attitude.values.yaw
 512         courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
 513         cogYawGain = (FLIGHT_MODE(GPS_RESCUE_MODE)) ? gpsRescueGetImuYawCogGain() : 1.0f;
 514         // normally update yaw heading with GPS data, but when in a Rescue, modify the IMU yaw gain dynamically
 515         if (shouldInitializeGPSHeading()) {
 516             // Reset our reference and reinitialize quaternion.
 517             // shouldInitializeGPSHeading() returns true only once.
 518             imuComputeQuaternionFromRPY(&qP, attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
 519             cogYawGain = 0.0f; // Don't use the COG when we first initialize
 520         }
 521     }
 522 #endif
 523
 524 #if defined(SIMULATOR_BUILD) && !defined(USE_IMU_CALC)
 525     UNUSED(imuMahonyAHRSupdate);
 526     UNUSED(imuIsAccelerometerHealthy);
 527     UNUSED(useAcc);
 528     UNUSED(useMag);
 529     UNUSED(cogYawGain);
 530     UNUSED(canUseGPSHeading);
 531     UNUSED(courseOverGround);
 532     UNUSED(deltaT);
 533     UNUSED(imuCalcKpGain);
 534 #else
 535
 536 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
 537 //  printf("[imu]deltaT = %u, imuDeltaT = %u, currentTimeUs = %u, micros64_real = %lu\n", deltaT, imuDeltaT, currentTimeUs, micros64_real());
 538     deltaT = imuDeltaT;
 539 #endif
 540     float gyroAverage[XYZ_AXIS_COUNT];
 541     for (int axis = 0; axis < XYZ_AXIS_COUNT; ++axis) {
 542         gyroAverage[axis] = gyroGetFilteredDownsampled(axis);
 543     }
 544
 545     useAcc = imuIsAccelerometerHealthy(acc.accADC); // all smoothed accADC values are within 20% of 1G
 546
 547     imuMahonyAHRSupdate(deltaT * 1e-6f,
 548                         DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
 549                         useAcc, acc.accADC[X], acc.accADC[Y], acc.accADC[Z],
 550                         useMag,
 551                         cogYawGain, courseOverGround,  imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
 552
 553     imuUpdateEulerAngles();
 554 #endif
 555 }
 556
 557 static int calculateThrottleAngleCorrection(void)
 558 {
 559     /*
 560     * Use 0 as the throttle angle correction if we are inverted, vertical or with a
 561     * small angle < 0.86 deg
 562     * TODO: Define this small angle in config.
 563     */
 564     if (getCosTiltAngle() <= 0.015f) {
 565         return 0;
 566     }
 567     int angle = lrintf(acos_approx(getCosTiltAngle()) * throttleAngleScale);
 568     if (angle > 900)
 569         angle = 900;
 570     return lrintf(throttleAngleValue * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
 571 }
 572
 573 void imuUpdateAttitude(timeUs_t currentTimeUs)
 574 {
 575     if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) {
 576         IMU_LOCK;
 577 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
 578         if (imuUpdated == false) {
 579             IMU_UNLOCK;
 580             return;
 581         }
 582         imuUpdated = false;
 583 #endif
 584         imuCalculateEstimatedAttitude(currentTimeUs);
 585         IMU_UNLOCK;
 586
 587         // Update the throttle correction for angle and supply it to the mixer
 588         int throttleAngleCorrection = 0;
 589         if (throttleAngleValue && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && ARMING_FLAG(ARMED)) {
 590             throttleAngleCorrection = calculateThrottleAngleCorrection();
 591         }
 592         mixerSetThrottleAngleCorrection(throttleAngleCorrection);
 593
 594     } else {
 595         acc.accADC[X] = 0;
 596         acc.accADC[Y] = 0;
 597         acc.accADC[Z] = 0;
 598         schedulerIgnoreTaskStateTime();
 599     }
 600
 601     DEBUG_SET(DEBUG_ATTITUDE, X, acc.accADC[X]);
 602     DEBUG_SET(DEBUG_ATTITUDE, Y, acc.accADC[Y]);
 603 }
 604 #endif // USE_ACC
 605
 606 bool shouldInitializeGPSHeading(void)
 607 {
 608     static bool initialized = false;
 609
 610     if (!initialized) {
 611         initialized = true;
 612
 613         return true;
 614     }
 615
 616     return false;
 617 }
 618
 619 float getCosTiltAngle(void)
 620 {
 621     return rMat[2][2];
 622 }
 623
 624 void getQuaternion(quaternion *quat)
 625 {
 626    quat->w = q.w;
 627    quat->x = q.x;
 628    quat->y = q.y;
 629    quat->z = q.z;
 630 }
 631
 632 #ifdef SIMULATOR_BUILD
 633 void imuSetAttitudeRPY(float roll, float pitch, float yaw)
 634 {
 635     IMU_LOCK;
 636
 637     attitude.values.roll = roll * 10;
 638     attitude.values.pitch = pitch * 10;
 639     attitude.values.yaw = yaw * 10;
 640
 641     IMU_UNLOCK;
 642 }
 643
 644 void imuSetAttitudeQuat(float w, float x, float y, float z)
 645 {
 646     IMU_LOCK;
 647
 648     q.w = w;
 649     q.x = x;
 650     q.y = y;
 651     q.z = z;
 652
 653     imuComputeRotationMatrix();
 654
 655     attitudeIsEstablished = true;
 656
 657     imuUpdateEulerAngles();
 658
 659     IMU_UNLOCK;
 660 }
 661 #endif
 662 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
 663 void imuSetHasNewData(uint32_t dt)
 664 {
 665     IMU_LOCK;
 666
 667     imuUpdated = true;
 668     imuDeltaT = dt;
 669
 670     IMU_UNLOCK;
 671 }
 672 #endif
 673
 674 bool imuQuaternionHeadfreeOffsetSet(void)
 675 {
 676     if ((abs(attitude.values.roll) < 450)  && (abs(attitude.values.pitch) < 450)) {
 677         const float yaw = -atan2_approx((+2.0f * (qP.wz + qP.xy)), (+1.0f - 2.0f * (qP.yy + qP.zz)));
 678
 679         offset.w = cos_approx(yaw/2);
 680         offset.x = 0;
 681         offset.y = 0;
 682         offset.z = sin_approx(yaw/2);
 683
 684         return true;
 685     } else {
 686         return false;
 687     }
 688 }
 689
 690 void imuQuaternionMultiplication(quaternion *q1, quaternion *q2, quaternion *result)
 691 {
 692     const float A = (q1->w + q1->x) * (q2->w + q2->x);
 693     const float B = (q1->z - q1->y) * (q2->y - q2->z);
 694     const float C = (q1->w - q1->x) * (q2->y + q2->z);
 695     const float D = (q1->y + q1->z) * (q2->w - q2->x);
 696     const float E = (q1->x + q1->z) * (q2->x + q2->y);
 697     const float F = (q1->x - q1->z) * (q2->x - q2->y);
 698     const float G = (q1->w + q1->y) * (q2->w - q2->z);
 699     const float H = (q1->w - q1->y) * (q2->w + q2->z);
 700
 701     result->w = B + (- E - F + G + H) / 2.0f;
 702     result->x = A - (+ E + F + G + H) / 2.0f;
 703     result->y = C + (+ E - F + G - H) / 2.0f;
 704     result->z = D + (+ E - F - G + H) / 2.0f;
 705 }
 706
 707 void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def *v)
 708 {
 709     quaternionProducts buffer;
 710
 711     imuQuaternionMultiplication(&offset, &q, &headfree);
 712     imuQuaternionComputeProducts(&headfree, &buffer);
 713
 714     const float x = (buffer.ww + buffer.xx - buffer.yy - buffer.zz) * v->X + 2.0f * (buffer.xy + buffer.wz) * v->Y + 2.0f * (buffer.xz - buffer.wy) * v->Z;
 715     const float y = 2.0f * (buffer.xy - buffer.wz) * v->X + (buffer.ww - buffer.xx + buffer.yy - buffer.zz) * v->Y + 2.0f * (buffer.yz + buffer.wx) * v->Z;
 716     const float z = 2.0f * (buffer.xz + buffer.wy) * v->X + 2.0f * (buffer.yz - buffer.wx) * v->Y + (buffer.ww - buffer.xx - buffer.yy + buffer.zz) * v->Z;
 717
 718     v->X = x;
 719     v->Y = y;
 720     v->Z = z;
 721 }
 722
 723 bool isUpright(void)
 724 {
 725 #ifdef USE_ACC
 726     return !sensors(SENSOR_ACC) || (attitudeIsEstablished && getCosTiltAngle() > smallAngleCosZ);
 727 #else
 728     return true;
 729 #endif
 730 }