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 100        // 1.0m/s - the 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 #define GPS_COG_MAX_GROUNDSPEED 500        // 5.0m/s - Value for 'normal' 1.0 yaw IMU CogGain
  93
  94 bool canUseGPSHeading = true;
  95
  96 static float throttleAngleScale;
  97 static int throttleAngleValue;
  98 static float smallAngleCosZ = 0;
  99
 100 static imuRuntimeConfig_t imuRuntimeConfig;
 101
 102 float rMat[3][3];
 103 static fpVector2_t north_ef;
 104
 105 #if defined(USE_ACC)
 106 STATIC_UNIT_TESTED bool attitudeIsEstablished = false;
 107 #endif
 108
 109 // quaternion of sensor frame relative to earth frame
 110 STATIC_UNIT_TESTED quaternion q = QUATERNION_INITIALIZE;
 111 STATIC_UNIT_TESTED quaternionProducts qP = QUATERNION_PRODUCTS_INITIALIZE;
 112 // headfree quaternions
 113 quaternion headfree = QUATERNION_INITIALIZE;
 114 quaternion offset = QUATERNION_INITIALIZE;
 115
 116 // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 117 attitudeEulerAngles_t attitude = EULER_INITIALIZE;
 118
 119 PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 3);
 120
 121 PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
 122     .imu_dcm_kp = 2500,      // 1.0 * 10000
 123     .imu_dcm_ki = 0,         // 0.003 * 10000
 124     .small_angle = 25,
 125     .imu_process_denom = 2,
 126     .mag_declination = 0
 127 );
 128
 129 static void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd)
 130 {
 131     quatProd->ww = quat->w * quat->w;
 132     quatProd->wx = quat->w * quat->x;
 133     quatProd->wy = quat->w * quat->y;
 134     quatProd->wz = quat->w * quat->z;
 135     quatProd->xx = quat->x * quat->x;
 136     quatProd->xy = quat->x * quat->y;
 137     quatProd->xz = quat->x * quat->z;
 138     quatProd->yy = quat->y * quat->y;
 139     quatProd->yz = quat->y * quat->z;
 140     quatProd->zz = quat->z * quat->z;
 141 }
 142
 143 STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
 144 {
 145     imuQuaternionComputeProducts(&q, &qP);
 146
 147     rMat[0][0] = 1.0f - 2.0f * qP.yy - 2.0f * qP.zz;
 148     rMat[0][1] = 2.0f * (qP.xy + -qP.wz);
 149     rMat[0][2] = 2.0f * (qP.xz - -qP.wy);
 150
 151     rMat[1][0] = 2.0f * (qP.xy - -qP.wz);
 152     rMat[1][1] = 1.0f - 2.0f * qP.xx - 2.0f * qP.zz;
 153     rMat[1][2] = 2.0f * (qP.yz + -qP.wx);
 154
 155     rMat[2][0] = 2.0f * (qP.xz + -qP.wy);
 156     rMat[2][1] = 2.0f * (qP.yz - -qP.wx);
 157     rMat[2][2] = 1.0f - 2.0f * qP.xx - 2.0f * qP.yy;
 158
 159 #if defined(SIMULATOR_BUILD) && !defined(USE_IMU_CALC) && !defined(SET_IMU_FROM_EULER)
 160     rMat[1][0] = -2.0f * (qP.xy - -qP.wz);
 161     rMat[2][0] = -2.0f * (qP.xz + -qP.wy);
 162 #endif
 163 }
 164
 165 static float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
 166 {
 167     return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
 168 }
 169
 170 void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value)
 171 {
 172     imuRuntimeConfig.imuDcmKp = imuConfig()->imu_dcm_kp / 10000.0f;
 173     imuRuntimeConfig.imuDcmKi = imuConfig()->imu_dcm_ki / 10000.0f;
 174     // magnetic declination has negative sign (positive clockwise when seen from top)
 175     const float imuMagneticDeclinationRad = DEGREES_TO_RADIANS(imuConfig()->mag_declination / 10.0f);
 176     north_ef.x = cos_approx(imuMagneticDeclinationRad);
 177     north_ef.y = -sin_approx(imuMagneticDeclinationRad);
 178
 179     smallAngleCosZ = cos_approx(degreesToRadians(imuConfig()->small_angle));
 180
 181     throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
 182
 183     throttleAngleValue = throttle_correction_value;
 184 }
 185
 186 void imuInit(void)
 187 {
 188 #ifdef USE_GPS
 189     canUseGPSHeading = true;
 190 #else
 191     canUseGPSHeading = false;
 192 #endif
 193
 194     imuComputeRotationMatrix();
 195
 196 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
 197     if (pthread_mutex_init(&imuUpdateLock, NULL) != 0) {
 198         printf("Create imuUpdateLock error!\n");
 199     }
 200 #endif
 201 }
 202
 203 #if defined(USE_ACC)
 204 static float invSqrt(float x)
 205 {
 206     return 1.0f / sqrtf(x);
 207 }
 208
 209 STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
 210                                 bool useAcc, float ax, float ay, float az,
 211                                 bool useMag,
 212                                 float cogYawGain, float courseOverGround, const float dcmKpGain)
 213 {
 214     static float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;    // integral error terms scaled by Ki
 215
 216     // Calculate general spin rate (rad/s)
 217     const float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
 218
 219     // Use raw heading error (from GPS or whatever else)
 220     float ex = 0, ey = 0, ez = 0;
 221     if (cogYawGain != 0.0f) {
 222         // Used in a GPS Rescue to boost IMU yaw gain when course over ground and velocity to home differ significantly
 223
 224         // Compute heading vector in EF from scalar CoG. CoG is clockwise from North
 225         // Note that Earth frame X is pointing north and sin/cos argument is anticlockwise
 226         const fpVector2_t cog_ef = {.x = cos_approx(-courseOverGround), .y = sin_approx(-courseOverGround)};
 227 #define THRUST_COG 1
 228 #if THRUST_COG
 229         const fpVector2_t heading_ef = {.x = rMat[X][Z], .y = rMat[Y][Z]};  // body Z axis (up) - direction of thrust vector
 230 #else
 231         const fpVector2_t heading_ef = {.x = rMat[0][0], .y = rMat[1][0]};  // body X axis. Projected vector magnitude is reduced as pitch increases
 232 #endif
 233         // cross product = 1 * |heading| * sin(angle) (magnitude of Z vector in 3D)
 234         // order operands so that rotation is in direction of zero error
 235         const float cross = vector2Cross(&heading_ef, &cog_ef);
 236         // dot product, 1 * |heading| * cos(angle)
 237         const float dot = vector2Dot(&heading_ef, &cog_ef);
 238         // use cross product / sin(angle) when error < 90deg (cos > 0),
 239         //   |heading| if error is larger (cos < 0)
 240         const float heading_mag = vector2Mag(&heading_ef);
 241         float ez_ef = (dot > 0) ? cross : (cross < 0 ? -1.0f : 1.0f) * heading_mag;
 242 #if THRUST_COG
 243         // increase gain for small tilt (just heuristic; sqrt is cheap on F4+)
 244         ez_ef /= sqrtf(heading_mag);
 245 #endif
 246         ez_ef *= cogYawGain;          // apply gain parameter
 247         // convert to body frame
 248         ex += rMat[2][0] * ez_ef;
 249         ey += rMat[2][1] * ez_ef;
 250         ez += rMat[2][2] * ez_ef;
 251
 252         DEBUG_SET(DEBUG_ATTITUDE, 3, (ez_ef * 100));
 253     }
 254
 255     DEBUG_SET(DEBUG_ATTITUDE, 2, cogYawGain * 100.0f);
 256     DEBUG_SET(DEBUG_ATTITUDE, 7, (dcmKpGain * 100));
 257
 258 #ifdef USE_MAG
 259     // Use measured magnetic field vector
 260     fpVector3_t mag_bf = {{mag.magADC[X], mag.magADC[Y], mag.magADC[Z]}};
 261     float magNormSquared = vectorNormSquared(&mag_bf);
 262     fpVector3_t mag_ef;
 263     matrixVectorMul(&mag_ef, (const fpMat33_t*)&rMat, &mag_bf); // BF->EF true north
 264
 265 #ifdef USE_GPS_RESCUE
 266     // Encapsulate additional operations in a block so that it is only executed when the according debug mode is used
 267     // Only re-calculate magYaw when there is a new Mag data reading, to avoid spikes
 268     if (debugMode == DEBUG_GPS_RESCUE_HEADING && mag.isNewMagADCFlag) {
 269         fpMat33_t rMatZTrans;
 270         yawToRotationMatrixZ(&rMatZTrans, -atan2_approx(rMat[1][0], rMat[0][0]));
 271         fpVector3_t mag_ef_yawed;
 272         matrixVectorMul(&mag_ef_yawed, &rMatZTrans, &mag_ef); // EF->EF yawed
 273         // Magnetic yaw is the angle between true north and the X axis of the body frame
 274         int16_t magYaw = lrintf((atan2_approx(mag_ef_yawed.y, mag_ef_yawed.x) * (1800.0f / M_PIf)));
 275         if (magYaw < 0) {
 276             magYaw += 3600;
 277         }
 278         DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 4, magYaw); // mag heading in degrees * 10
 279         // reset new mag data flag to false to initiate monitoring for new Mag data.
 280         // note that if the debug doesn't run, this reset will not occur, and we won't waste cycles on the comparison
 281         mag.isNewMagADCFlag = false;
 282     }
 283 #endif
 284
 285     if (useMag && magNormSquared > 0.01f) {
 286         // Normalise magnetometer measurement
 287         vectorNormalize(&mag_ef, &mag_ef);
 288
 289         // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
 290         // This way magnetic field will only affect heading and wont mess roll/pitch angles
 291
 292         // (hx; hy; 0) - measured mag field vector in EF (forcing Z-component to zero)
 293         // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
 294         mag_ef.z = 0.0f;                // project to XY plane (optimized away)
 295
 296         // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
 297         // increase gain on large misalignment
 298         const float dot = vector2Dot((fpVector2_t*)&mag_ef, &north_ef);
 299         const float cross = vector2Cross((fpVector2_t*)&mag_ef, &north_ef);
 300         const float ez_ef = (dot > 0) ? cross : (cross < 0 ? -1.0f : 1.0f) * vector2Mag((fpVector2_t*)&mag_ef);
 301         // Rotate mag error vector back to BF and accumulate
 302         ex += rMat[2][0] * ez_ef;
 303         ey += rMat[2][1] * ez_ef;
 304         ez += rMat[2][2] * ez_ef;
 305     }
 306 #else
 307     UNUSED(useMag);
 308 #endif
 309
 310     // Use measured acceleration vector
 311     float recipAccNorm = sq(ax) + sq(ay) + sq(az);
 312     if (useAcc && recipAccNorm > 0.01f) {
 313         // Normalise accelerometer measurement; useAcc is true when all smoothed acc axes are within 20% of 1G
 314         recipAccNorm = invSqrt(recipAccNorm);
 315         ax *= recipAccNorm;
 316         ay *= recipAccNorm;
 317         az *= recipAccNorm;
 318
 319         // Error is sum of cross product between estimated direction and measured direction of gravity
 320         ex += (ay * rMat[2][2] - az * rMat[2][1]);
 321         ey += (az * rMat[2][0] - ax * rMat[2][2]);
 322         ez += (ax * rMat[2][1] - ay * rMat[2][0]);
 323     }
 324
 325     // Compute and apply integral feedback if enabled
 326     if (imuRuntimeConfig.imuDcmKi > 0.0f) {
 327         // Stop integrating if spinning beyond the certain limit
 328         if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
 329             const float dcmKiGain = imuRuntimeConfig.imuDcmKi;
 330             integralFBx += dcmKiGain * ex * dt;    // integral error scaled by Ki
 331             integralFBy += dcmKiGain * ey * dt;
 332             integralFBz += dcmKiGain * ez * dt;
 333         }
 334     } else {
 335         integralFBx = 0.0f;    // prevent integral windup
 336         integralFBy = 0.0f;
 337         integralFBz = 0.0f;
 338     }
 339
 340     // Apply proportional and integral feedback
 341     gx += dcmKpGain * ex + integralFBx;
 342     gy += dcmKpGain * ey + integralFBy;
 343     gz += dcmKpGain * ez + integralFBz;
 344
 345     // Integrate rate of change of quaternion
 346     gx *= (0.5f * dt);
 347     gy *= (0.5f * dt);
 348     gz *= (0.5f * dt);
 349
 350     quaternion buffer;
 351     buffer.w = q.w;
 352     buffer.x = q.x;
 353     buffer.y = q.y;
 354     buffer.z = q.z;
 355
 356     q.w += (-buffer.x * gx - buffer.y * gy - buffer.z * gz);
 357     q.x += (+buffer.w * gx + buffer.y * gz - buffer.z * gy);
 358     q.y += (+buffer.w * gy - buffer.x * gz + buffer.z * gx);
 359     q.z += (+buffer.w * gz + buffer.x * gy - buffer.y * gx);
 360
 361     // Normalise quaternion
 362     float recipNorm = invSqrt(sq(q.w) + sq(q.x) + sq(q.y) + sq(q.z));
 363     q.w *= recipNorm;
 364     q.x *= recipNorm;
 365     q.y *= recipNorm;
 366     q.z *= recipNorm;
 367
 368     // Pre-compute rotation matrix from quaternion
 369     imuComputeRotationMatrix();
 370
 371     attitudeIsEstablished = true;
 372 }
 373
 374 STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
 375 {
 376     quaternionProducts buffer;
 377
 378     if (FLIGHT_MODE(HEADFREE_MODE)) {
 379        imuQuaternionComputeProducts(&headfree, &buffer);
 380
 381        attitude.values.roll = lrintf(atan2_approx((+2.0f * (buffer.wx + buffer.yz)), (+1.0f - 2.0f * (buffer.xx + buffer.yy))) * (1800.0f / M_PIf));
 382        attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(+2.0f * (buffer.wy - buffer.xz))) * (1800.0f / M_PIf));
 383        attitude.values.yaw = lrintf((-atan2_approx((+2.0f * (buffer.wz + buffer.xy)), (+1.0f - 2.0f * (buffer.yy + buffer.zz))) * (1800.0f / M_PIf)));
 384     } else {
 385        attitude.values.roll = lrintf(atan2_approx(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf));
 386        attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(-rMat[2][0])) * (1800.0f / M_PIf));
 387        attitude.values.yaw = lrintf((-atan2_approx(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf)));
 388     }
 389
 390     if (attitude.values.yaw < 0) {
 391         attitude.values.yaw += 3600;
 392     }
 393 }
 394
 395 static bool imuIsAccelerometerHealthy(float *accAverage)
 396 {
 397     float accMagnitudeSq = 0;
 398     for (int axis = 0; axis < 3; axis++) {
 399         const float a = accAverage[axis];
 400         accMagnitudeSq += a * a;
 401     }
 402
 403     accMagnitudeSq = accMagnitudeSq * sq(acc.dev.acc_1G_rec);
 404
 405     // Accept accel readings only in range 0.9g - 1.1g
 406     return (0.81f < accMagnitudeSq) && (accMagnitudeSq < 1.21f);
 407 }
 408
 409 // Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.imuDcmKp * 1.0 scaling.
 410 // When disarmed after initial boot, the scaling is set to 10.0 for the first 20 seconds to speed up initial convergence.
 411 // After disarming we want to quickly reestablish convergence to deal with the attitude estimation being incorrect due to a crash.
 412 //   - wait for a 250ms period of low gyro activity to ensure the craft is not moving
 413 //   - use a large dcmKpGain value for 500ms to allow the attitude estimate to quickly converge
 414 //   - reset the gain back to the standard setting
 415 static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
 416 {
 417     static enum {
 418         stArmed,
 419         stRestart,
 420         stQuiet,
 421         stReset,
 422         stDisarmed
 423     } arState = stDisarmed;
 424
 425     static timeUs_t stateTimeout;
 426
 427     const bool armState = ARMING_FLAG(ARMED);
 428
 429     if (!armState) {
 430         // If gyro activity exceeds the threshold then restart the quiet period.
 431         // Also, if the attitude reset has been complete and there is subsequent gyro activity then
 432         //  start the reset cycle again. This addresses the case where the pilot rights the craft after a crash.
 433         if (   (fabsf(gyroAverage[X]) > ATTITUDE_RESET_GYRO_LIMIT)  // gyro axis limit exceeded
 434             || (fabsf(gyroAverage[Y]) > ATTITUDE_RESET_GYRO_LIMIT)
 435             || (fabsf(gyroAverage[Z]) > ATTITUDE_RESET_GYRO_LIMIT)
 436             || !useAcc                                              // acc reading out of range
 437             ) {
 438             arState = stRestart;
 439         }
 440
 441         switch (arState) {
 442         default: // should not happen, safeguard only
 443         case stArmed:
 444         case stRestart:
 445             stateTimeout = currentTimeUs + ATTITUDE_RESET_QUIET_TIME;
 446             arState = stQuiet;
 447             // fallthrough
 448         case stQuiet:
 449             if (cmpTimeUs(currentTimeUs, stateTimeout) >= 0) {
 450                 stateTimeout = currentTimeUs + ATTITUDE_RESET_ACTIVE_TIME;
 451                 arState = stReset;
 452             }
 453             // low gain during quiet phase
 454             return imuRuntimeConfig.imuDcmKp;
 455         case stReset:
 456             if (cmpTimeUs(currentTimeUs, stateTimeout) >= 0) {
 457                 arState = stDisarmed;
 458             }
 459             // high gain after quiet period
 460             return ATTITUDE_RESET_KP_GAIN;
 461         case stDisarmed:
 462             // Scale the kP to generally converge faster when disarmed.
 463             return imuRuntimeConfig.imuDcmKp * 10.0f;
 464         }
 465     } else {
 466         arState = stArmed;
 467         return imuRuntimeConfig.imuDcmKp;
 468     }
 469 }
 470
 471 #if defined(USE_GPS)
 472 static void imuComputeQuaternionFromRPY(quaternionProducts *quatProd, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
 473 {
 474     if (initialRoll > 1800) {
 475         initialRoll -= 3600;
 476     }
 477
 478     if (initialPitch > 1800) {
 479         initialPitch -= 3600;
 480     }
 481
 482     if (initialYaw > 1800) {
 483         initialYaw -= 3600;
 484     }
 485
 486     const float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
 487     const float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
 488
 489     const float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
 490     const float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
 491
 492     const float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
 493     const float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
 494
 495     const float q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
 496     const float q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
 497     const float q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
 498     const float q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
 499
 500     quatProd->xx = sq(q1);
 501     quatProd->yy = sq(q2);
 502     quatProd->zz = sq(q3);
 503
 504     quatProd->xy = q1 * q2;
 505     quatProd->xz = q1 * q3;
 506     quatProd->yz = q2 * q3;
 507
 508     quatProd->wx = q0 * q1;
 509     quatProd->wy = q0 * q2;
 510     quatProd->wz = q0 * q3;
 511
 512     imuComputeRotationMatrix();
 513
 514     attitudeIsEstablished = true;
 515 }
 516 #endif
 517
 518 static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
 519 {
 520     static timeUs_t previousIMUUpdateTime;
 521     bool useAcc = false;
 522     bool useMag = false;
 523     float cogYawGain = 0.0f; // IMU yaw gain to be applied in imuMahonyAHRSupdate from ground course, default to no correction from CoG
 524     float courseOverGround = 0; // To be used when cogYawGain is non-zero, in radians
 525
 526     const timeDelta_t deltaT = currentTimeUs - previousIMUUpdateTime;
 527     previousIMUUpdateTime = currentTimeUs;
 528
 529 #ifdef USE_MAG
 530     if (sensors(SENSOR_MAG) && compassIsHealthy()
 531 #ifdef USE_GPS_RESCUE
 532         && !gpsRescueDisableMag()
 533 #endif
 534         ) {
 535         useMag = true;
 536     }
 537 #endif
 538 #if defined(USE_GPS)
 539     if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat > GPS_MIN_SAT_COUNT && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) {
 540         // Use GPS course over ground to correct attitude.values.yaw
 541         const float imuYawGroundspeed = fminf (gpsSol.groundSpeed / GPS_COG_MAX_GROUNDSPEED, 2.0f);
 542         courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
 543         cogYawGain = (FLIGHT_MODE(GPS_RESCUE_MODE)) ? gpsRescueGetImuYawCogGain() : imuYawGroundspeed;
 544         // normally update yaw heading with GPS data, but when in a Rescue, modify the IMU yaw gain dynamically
 545         if (shouldInitializeGPSHeading()) {
 546             // Reset our reference and reinitialize quaternion.
 547             // shouldInitializeGPSHeading() returns true only once.
 548             imuComputeQuaternionFromRPY(&qP, attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
 549             cogYawGain = 0.0f; // Don't use the COG when we first initialize
 550         }
 551     }
 552 #endif
 553
 554 #if defined(SIMULATOR_BUILD) && !defined(USE_IMU_CALC)
 555     UNUSED(imuMahonyAHRSupdate);
 556     UNUSED(imuIsAccelerometerHealthy);
 557     UNUSED(useAcc);
 558     UNUSED(useMag);
 559     UNUSED(cogYawGain);
 560     UNUSED(canUseGPSHeading);
 561     UNUSED(courseOverGround);
 562     UNUSED(deltaT);
 563     UNUSED(imuCalcKpGain);
 564 #else
 565
 566 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
 567 //  printf("[imu]deltaT = %u, imuDeltaT = %u, currentTimeUs = %u, micros64_real = %lu\n", deltaT, imuDeltaT, currentTimeUs, micros64_real());
 568     deltaT = imuDeltaT;
 569 #endif
 570     float gyroAverage[XYZ_AXIS_COUNT];
 571     for (int axis = 0; axis < XYZ_AXIS_COUNT; ++axis) {
 572         gyroAverage[axis] = gyroGetFilteredDownsampled(axis);
 573     }
 574
 575     useAcc = imuIsAccelerometerHealthy(acc.accADC); // all smoothed accADC values are within 20% of 1G
 576
 577     imuMahonyAHRSupdate(deltaT * 1e-6f,
 578                         DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
 579                         useAcc, acc.accADC[X], acc.accADC[Y], acc.accADC[Z],
 580                         useMag,
 581                         cogYawGain, courseOverGround,  imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
 582
 583     imuUpdateEulerAngles();
 584 #endif
 585 }
 586
 587 static int calculateThrottleAngleCorrection(void)
 588 {
 589     /*
 590     * Use 0 as the throttle angle correction if we are inverted, vertical or with a
 591     * small angle < 0.86 deg
 592     * TODO: Define this small angle in config.
 593     */
 594     if (getCosTiltAngle() <= 0.015f) {
 595         return 0;
 596     }
 597     int angle = lrintf(acos_approx(getCosTiltAngle()) * throttleAngleScale);
 598     if (angle > 900)
 599         angle = 900;
 600     return lrintf(throttleAngleValue * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
 601 }
 602
 603 void imuUpdateAttitude(timeUs_t currentTimeUs)
 604 {
 605     if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) {
 606         IMU_LOCK;
 607 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
 608         if (imuUpdated == false) {
 609             IMU_UNLOCK;
 610             return;
 611         }
 612         imuUpdated = false;
 613 #endif
 614         imuCalculateEstimatedAttitude(currentTimeUs);
 615         IMU_UNLOCK;
 616
 617         // Update the throttle correction for angle and supply it to the mixer
 618         int throttleAngleCorrection = 0;
 619         if (throttleAngleValue && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && ARMING_FLAG(ARMED)) {
 620             throttleAngleCorrection = calculateThrottleAngleCorrection();
 621         }
 622         mixerSetThrottleAngleCorrection(throttleAngleCorrection);
 623
 624     } else {
 625         acc.accADC[X] = 0;
 626         acc.accADC[Y] = 0;
 627         acc.accADC[Z] = 0;
 628         schedulerIgnoreTaskStateTime();
 629     }
 630
 631     DEBUG_SET(DEBUG_ATTITUDE, X, acc.accADC[X]); // roll
 632     DEBUG_SET(DEBUG_ATTITUDE, Y, acc.accADC[Y]); // pitch
 633 }
 634 #endif // USE_ACC
 635
 636 bool shouldInitializeGPSHeading(void)
 637 {
 638     static bool initialized = false;
 639
 640     if (!initialized) {
 641         initialized = true;
 642
 643         return true;
 644     }
 645
 646     return false;
 647 }
 648
 649 float getCosTiltAngle(void)
 650 {
 651     return rMat[2][2];
 652 }
 653
 654 void getQuaternion(quaternion *quat)
 655 {
 656    quat->w = q.w;
 657    quat->x = q.x;
 658    quat->y = q.y;
 659    quat->z = q.z;
 660 }
 661
 662 #ifdef SIMULATOR_BUILD
 663 void imuSetAttitudeRPY(float roll, float pitch, float yaw)
 664 {
 665     IMU_LOCK;
 666
 667     attitude.values.roll = roll * 10;
 668     attitude.values.pitch = pitch * 10;
 669     attitude.values.yaw = yaw * 10;
 670
 671     IMU_UNLOCK;
 672 }
 673
 674 void imuSetAttitudeQuat(float w, float x, float y, float z)
 675 {
 676     IMU_LOCK;
 677
 678     q.w = w;
 679     q.x = x;
 680     q.y = y;
 681     q.z = z;
 682
 683     imuComputeRotationMatrix();
 684
 685     attitudeIsEstablished = true;
 686
 687     imuUpdateEulerAngles();
 688
 689     IMU_UNLOCK;
 690 }
 691 #endif
 692 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
 693 void imuSetHasNewData(uint32_t dt)
 694 {
 695     IMU_LOCK;
 696
 697     imuUpdated = true;
 698     imuDeltaT = dt;
 699
 700     IMU_UNLOCK;
 701 }
 702 #endif
 703
 704 bool imuQuaternionHeadfreeOffsetSet(void)
 705 {
 706     if ((abs(attitude.values.roll) < 450)  && (abs(attitude.values.pitch) < 450)) {
 707         const float yaw = -atan2_approx((+2.0f * (qP.wz + qP.xy)), (+1.0f - 2.0f * (qP.yy + qP.zz)));
 708
 709         offset.w = cos_approx(yaw/2);
 710         offset.x = 0;
 711         offset.y = 0;
 712         offset.z = sin_approx(yaw/2);
 713
 714         return true;
 715     } else {
 716         return false;
 717     }
 718 }
 719
 720 void imuQuaternionMultiplication(quaternion *q1, quaternion *q2, quaternion *result)
 721 {
 722     const float A = (q1->w + q1->x) * (q2->w + q2->x);
 723     const float B = (q1->z - q1->y) * (q2->y - q2->z);
 724     const float C = (q1->w - q1->x) * (q2->y + q2->z);
 725     const float D = (q1->y + q1->z) * (q2->w - q2->x);
 726     const float E = (q1->x + q1->z) * (q2->x + q2->y);
 727     const float F = (q1->x - q1->z) * (q2->x - q2->y);
 728     const float G = (q1->w + q1->y) * (q2->w - q2->z);
 729     const float H = (q1->w - q1->y) * (q2->w + q2->z);
 730
 731     result->w = B + (- E - F + G + H) / 2.0f;
 732     result->x = A - (+ E + F + G + H) / 2.0f;
 733     result->y = C + (+ E - F + G - H) / 2.0f;
 734     result->z = D + (+ E - F - G + H) / 2.0f;
 735 }
 736
 737 void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def *v)
 738 {
 739     quaternionProducts buffer;
 740
 741     imuQuaternionMultiplication(&offset, &q, &headfree);
 742     imuQuaternionComputeProducts(&headfree, &buffer);
 743
 744     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;
 745     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;
 746     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;
 747
 748     v->X = x;
 749     v->Y = y;
 750     v->Z = z;
 751 }
 752
 753 bool isUpright(void)
 754 {
 755 #ifdef USE_ACC
 756     return !sensors(SENSOR_ACC) || (attitudeIsEstablished && getCosTiltAngle() > smallAngleCosZ);
 757 #else
 758     return true;
 759 #endif
 760 }