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