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