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