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