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