src/main/flight/imu.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 // Inertial Measurement Unit (IMU)
  19
  20 #include <stdbool.h>
  21 #include <stdint.h>
  22 #include <string.h>
  23 #include <math.h>
  24
  25 #include "common/maths.h"
  26
  27 #include "build/build_config.h"
  28 #include <platform.h>
  29 #include "build/debug.h"
  30
  31 #include "common/axis.h"
  32 #include "common/filter.h"
  33
  34 #include "config/parameter_group_ids.h"
  35 #include "config/parameter_group.h"
  36 #include "config/config_reset.h"
  37 #include "config/profile.h"
  38
  39 #include "drivers/system.h"
  40 #include "drivers/sensor.h"
  41 #include "drivers/accgyro.h"
  42 #include "drivers/compass.h"
  43
  44 #include "sensors/sensors.h"
  45 #include "sensors/gyro.h"
  46 #include "sensors/compass.h"
  47 #include "sensors/acceleration.h"
  48 #include "sensors/barometer.h"
  49 #include "sensors/sonar.h"
  50
  51 #include "flight/mixer.h"
  52 #include "flight/pid.h"
  53 #include "flight/imu.h"
  54
  55 #include "io/gps.h"
  56
  57 #include "fc/runtime_config.h"
  58
  59 // the limit (in degrees/second) beyond which we stop integrating
  60 // omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
  61 // which results in false gyro drift. See
  62 // http://gentlenav.googlecode.com/files/fastRotations.pdf
  63 #define SPIN_RATE_LIMIT 20
  64
  65 int16_t accSmooth[XYZ_AXIS_COUNT];
  66 int32_t accSum[XYZ_AXIS_COUNT];
  67
  68 uint32_t accTimeSum = 0;        // keep track for integration of acc
  69 int accSumCount = 0;
  70 float accVelScale;
  71
  72 float throttleAngleScale;
  73 float fc_acc;
  74 float smallAngleCosZ = 0;
  75
  76 static bool isAccelUpdatedAtLeastOnce = false;
  77
  78 static imuRuntimeConfig_t *imuRuntimeConfig;
  79 static accDeadband_t *accDeadband;
  80
  81 PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 1);
  82 PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, PG_THROTTLE_CORRECTION_CONFIG, 0);
  83
  84 PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
  85 #ifdef STM32F10X
  86     .gyro_sync_denom = 8,
  87     .pid_process_denom = 1,
  88 #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500)
  89     .gyro_sync_denom = 1,
  90     .pid_process_denom = 4,
  91 #else
  92     .gyro_sync_denom = 4,
  93     .pid_process_denom = 2,
  94 #endif
  95     .dcm_kp = 2500,                // 1.0 * 10000
  96     .small_angle = 25,
  97     .max_angle_inclination = 500,    // 50 degrees
  98 );
  99
 100 PG_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig,
 101     .throttle_correction_value = 0,      // could 10 with althold or 40 for fpv
 102     .throttle_correction_angle = 800,    // could be 80.0 deg with atlhold or 45.0 for fpv
 103 );
 104
 105 STATIC_UNIT_TESTED float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;    // quaternion of sensor frame relative to earth frame
 106 static float rMat[3][3];
 107
 108 attitudeEulerAngles_t attitude = { { 0, 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 109
 110 static float gyroScale;
 111
 112 STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
 113 {
 114     float q1q1 = sq(q1);
 115     float q2q2 = sq(q2);
 116     float q3q3 = sq(q3);
 117
 118     float q0q1 = q0 * q1;
 119     float q0q2 = q0 * q2;
 120     float q0q3 = q0 * q3;
 121     float q1q2 = q1 * q2;
 122     float q1q3 = q1 * q3;
 123     float q2q3 = q2 * q3;
 124
 125     rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
 126     rMat[0][1] = 2.0f * (q1q2 + -q0q3);
 127     rMat[0][2] = 2.0f * (q1q3 - -q0q2);
 128
 129     rMat[1][0] = 2.0f * (q1q2 - -q0q3);
 130     rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
 131     rMat[1][2] = 2.0f * (q2q3 + -q0q1);
 132
 133     rMat[2][0] = 2.0f * (q1q3 + -q0q2);
 134     rMat[2][1] = 2.0f * (q2q3 - -q0q1);
 135     rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
 136 }
 137
 138 void imuConfigure(
 139     imuRuntimeConfig_t *initialImuRuntimeConfig,
 140     accDeadband_t *initialAccDeadband,
 141     float accz_lpf_cutoff,
 142     uint16_t throttle_correction_angle
 143 )
 144 {
 145     imuRuntimeConfig = initialImuRuntimeConfig;
 146     accDeadband = initialAccDeadband;
 147     fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff);
 148     throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
 149 }
 150
 151 void imuInit(void)
 152 {
 153     smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig->small_angle));
 154     gyroScale = gyro.scale * (M_PIf / 180.0f);  // gyro output scaled to rad per second
 155     accVelScale = 9.80665f / acc.acc_1G / 10000.0f;
 156
 157     imuComputeRotationMatrix();
 158 }
 159
 160 float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
 161 {
 162     return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
 163 }
 164
 165 /*
 166 * Calculate RC time constant used in the accZ lpf.
 167 */
 168 float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
 169 {
 170     return 0.5f / (M_PIf * accz_lpf_cutoff);
 171 }
 172
 173 void imuResetAccelerationSum(void)
 174 {
 175     accSum[0] = 0;
 176     accSum[1] = 0;
 177     accSum[2] = 0;
 178     accSumCount = 0;
 179     accTimeSum = 0;
 180 }
 181
 182 void imuTransformVectorBodyToEarth(t_fp_vector * v)
 183 {
 184     float x,y,z;
 185
 186     /* From body frame to earth frame */
 187     x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
 188     y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
 189     z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
 190
 191     v->V.X = x;
 192     v->V.Y = -y;
 193     v->V.Z = z;
 194 }
 195
 196 // rotate acc into Earth frame and calculate acceleration in it
 197 void imuCalculateAcceleration(uint32_t deltaT)
 198 {
 199     static int32_t accZoffset = 0;
 200     static float accz_smooth = 0;
 201     float dT;
 202     t_fp_vector accel_ned;
 203
 204     // deltaT is measured in us ticks
 205     dT = (float)deltaT * 1e-6f;
 206
 207     accel_ned.V.X = accSmooth[0];
 208     accel_ned.V.Y = accSmooth[1];
 209     accel_ned.V.Z = accSmooth[2];
 210
 211     imuTransformVectorBodyToEarth(&accel_ned);
 212
 213     if (imuRuntimeConfig->acc_unarmedcal == 1) {
 214         if (!ARMING_FLAG(ARMED)) {
 215             accZoffset -= accZoffset / 64;
 216             accZoffset += accel_ned.V.Z;
 217         }
 218         accel_ned.V.Z -= accZoffset / 64;  // compensate for gravitation on z-axis
 219     } else
 220         accel_ned.V.Z -= acc.acc_1G;
 221
 222     accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
 223
 224     // apply Deadband to reduce integration drift and vibration influence
 225     accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy);
 226     accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), accDeadband->xy);
 227     accSum[Z] += applyDeadband(lrintf(accz_smooth), accDeadband->z);
 228
 229     // sum up Values for later integration to get velocity and distance
 230     accTimeSum += deltaT;
 231     accSumCount++;
 232 }
 233
 234 static float invSqrt(float x)
 235 {
 236     return 1.0f / sqrtf(x);
 237 }
 238
 239 static bool imuUseFastGains(void)
 240 {
 241     return !ARMING_FLAG(ARMED) && millis() < 20000;
 242 }
 243
 244 static float imuGetPGainScaleFactor(void)
 245 {
 246     if (imuUseFastGains()) {
 247         return 10.0f;
 248     }
 249     else {
 250         return 1.0f;
 251     }
 252 }
 253
 254 static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
 255                                 bool useAcc, float ax, float ay, float az,
 256                                 bool useMag, float mx, float my, float mz,
 257                                 bool useYaw, float yawError)
 258 {
 259     static float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;    // integral error terms scaled by Ki
 260     float recipNorm;
 261     float hx, hy, bx;
 262     float ex = 0, ey = 0, ez = 0;
 263     float qa, qb, qc;
 264
 265     // Calculate general spin rate (rad/s)
 266     float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
 267
 268     // Use raw heading error (from GPS or whatever else)
 269     if (useYaw) {
 270         while (yawError >  M_PIf) yawError -= (2.0f * M_PIf);
 271         while (yawError < -M_PIf) yawError += (2.0f * M_PIf);
 272
 273         ez += sin_approx(yawError / 2.0f);
 274     }
 275
 276     // Use measured magnetic field vector
 277     recipNorm = sq(mx) + sq(my) + sq(mz);
 278     if (useMag && recipNorm > 0.01f) {
 279         // Normalise magnetometer measurement
 280         recipNorm = invSqrt(recipNorm);
 281         mx *= recipNorm;
 282         my *= recipNorm;
 283         mz *= recipNorm;
 284
 285         // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
 286         // This way magnetic field will only affect heading and wont mess roll/pitch angles
 287
 288         // (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
 289         // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
 290         hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
 291         hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
 292         bx = sqrtf(hx * hx + hy * hy);
 293
 294         // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
 295         float ez_ef = -(hy * bx);
 296
 297         // Rotate mag error vector back to BF and accumulate
 298         ex += rMat[2][0] * ez_ef;
 299         ey += rMat[2][1] * ez_ef;
 300         ez += rMat[2][2] * ez_ef;
 301     }
 302
 303     // Use measured acceleration vector
 304     recipNorm = sq(ax) + sq(ay) + sq(az);
 305     if (useAcc && recipNorm > 0.01f) {
 306         // Normalise accelerometer measurement
 307         recipNorm = invSqrt(recipNorm);
 308         ax *= recipNorm;
 309         ay *= recipNorm;
 310         az *= recipNorm;
 311
 312         // Error is sum of cross product between estimated direction and measured direction of gravity
 313         ex += (ay * rMat[2][2] - az * rMat[2][1]);
 314         ey += (az * rMat[2][0] - ax * rMat[2][2]);
 315         ez += (ax * rMat[2][1] - ay * rMat[2][0]);
 316     }
 317
 318     // Compute and apply integral feedback if enabled
 319     if(imuRuntimeConfig->dcm_ki > 0.0f) {
 320         // Stop integrating if spinning beyond the certain limit
 321         if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
 322             float dcmKiGain = imuRuntimeConfig->dcm_ki;
 323             integralFBx += dcmKiGain * ex * dt;    // integral error scaled by Ki
 324             integralFBy += dcmKiGain * ey * dt;
 325             integralFBz += dcmKiGain * ez * dt;
 326         }
 327     }
 328     else {
 329         integralFBx = 0.0f;    // prevent integral windup
 330         integralFBy = 0.0f;
 331         integralFBz = 0.0f;
 332     }
 333
 334     // Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster
 335     float dcmKpGain = imuRuntimeConfig->dcm_kp * imuGetPGainScaleFactor();
 336
 337     // Apply proportional and integral feedback
 338     gx += dcmKpGain * ex + integralFBx;
 339     gy += dcmKpGain * ey + integralFBy;
 340     gz += dcmKpGain * ez + integralFBz;
 341
 342     // Integrate rate of change of quaternion
 343     gx *= (0.5f * dt);
 344     gy *= (0.5f * dt);
 345     gz *= (0.5f * dt);
 346
 347     qa = q0;
 348     qb = q1;
 349     qc = q2;
 350     q0 += (-qb * gx - qc * gy - q3 * gz);
 351     q1 += (qa * gx + qc * gz - q3 * gy);
 352     q2 += (qa * gy - qb * gz + q3 * gx);
 353     q3 += (qa * gz + qb * gy - qc * gx);
 354
 355     // Normalise quaternion
 356     recipNorm = invSqrt(sq(q0) + sq(q1) + sq(q2) + sq(q3));
 357     q0 *= recipNorm;
 358     q1 *= recipNorm;
 359     q2 *= recipNorm;
 360     q3 *= recipNorm;
 361
 362     // Pre-compute rotation matrix from quaternion
 363     imuComputeRotationMatrix();
 364 }
 365
 366 STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
 367 {
 368 #ifndef GPS
 369     // this local variable should be optimized out when GPS is not used.
 370     float magneticDeclination = 0.0f;
 371 #endif
 372     /* Compute pitch/roll angles */
 373     attitude.values.roll = lrintf(atan2_approx(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf));
 374     attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(-rMat[2][0])) * (1800.0f / M_PIf));
 375     attitude.values.yaw = lrintf((-atan2_approx(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf) + magneticDeclination));
 376
 377     if (attitude.values.yaw < 0)
 378         attitude.values.yaw += 3600;
 379
 380     /* Update small angle state */
 381     if (rMat[2][2] > smallAngleCosZ) {
 382         ENABLE_STATE(SMALL_ANGLE);
 383     } else {
 384         DISABLE_STATE(SMALL_ANGLE);
 385     }
 386 }
 387
 388 bool imuIsAircraftArmable(uint8_t arming_angle)
 389 {
 390     /* Update small angle state */
 391
 392     float armingAngleCosZ = cos_approx(degreesToRadians(arming_angle));
 393
 394     return (rMat[2][2] > armingAngleCosZ);
 395 }
 396
 397 static bool imuIsAccelerometerHealthy(void)
 398 {
 399     int32_t axis;
 400     int32_t accMagnitude = 0;
 401
 402     for (axis = 0; axis < 3; axis++) {
 403         accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis];
 404     }
 405
 406     accMagnitude = accMagnitude * 100 / (sq((int32_t)acc.acc_1G));
 407
 408     // Accept accel readings only in range 0.90g - 1.10g
 409     return (81 < accMagnitude) && (accMagnitude < 121);
 410 }
 411
 412 #ifdef MAG
 413 static bool isMagnetometerHealthy(void)
 414 {
 415     return (magADC[X] != 0) && (magADC[Y] != 0) && (magADC[Z] != 0);
 416 }
 417 #endif
 418
 419 static void imuCalculateEstimatedAttitude(void)
 420 {
 421     static pt1Filter_t accLPFState[3];
 422     static uint32_t previousIMUUpdateTime;
 423     float rawYawError = 0;
 424     int32_t axis;
 425     bool useAcc = false;
 426     bool useMag = false;
 427     bool useYaw = false;
 428
 429     uint32_t currentTime = micros();
 430     uint32_t deltaT = currentTime - previousIMUUpdateTime;
 431     previousIMUUpdateTime = currentTime;
 432
 433     // Smooth and use only valid accelerometer readings
 434     for (axis = 0; axis < 3; axis++) {
 435         if (imuRuntimeConfig->acc_cut_hz > 0) {
 436             accSmooth[axis] = pt1FilterApply4(&accLPFState[axis], accADC[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6f);
 437         } else {
 438             accSmooth[axis] = accADC[axis];
 439         }
 440     }
 441
 442     if (imuIsAccelerometerHealthy()) {
 443         useAcc = true;
 444     }
 445
 446 #ifdef MAG
 447     if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) {
 448         useMag = true;
 449     }
 450 #endif
 451 #if defined(GPS)
 452     else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5 && GPS_speed >= 300) {
 453         // In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
 454         rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - GPS_ground_course);
 455         useYaw = true;
 456     }
 457 #endif
 458
 459     imuMahonyAHRSupdate(deltaT * 1e-6f,
 460                         gyroADC[X] * gyroScale, gyroADC[Y] * gyroScale, gyroADC[Z] * gyroScale,
 461                         useAcc, accSmooth[X], accSmooth[Y], accSmooth[Z],
 462                         useMag, magADC[X], magADC[Y], magADC[Z],
 463                         useYaw, rawYawError);
 464
 465     imuUpdateEulerAngles();
 466
 467     imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
 468 }
 469
 470 void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims)
 471 {
 472     if (sensors(SENSOR_ACC)) {
 473         updateAccelerationReadings(accelerometerTrims);
 474         isAccelUpdatedAtLeastOnce = true;
 475     }
 476 }
 477
 478 void imuUpdateAttitude(void)
 479 {
 480     if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
 481         imuCalculateEstimatedAttitude();
 482     } else {
 483         accADC[X] = 0;
 484         accADC[Y] = 0;
 485         accADC[Z] = 0;
 486     }
 487 }
 488
 489 float getCosTiltAngle(void)
 490 {
 491     return rMat[2][2];
 492 }
 493
 494 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
 495 {
 496     /*
 497     * Use 0 as the throttle angle correction if we are inverted, vertical or with a
 498     * small angle < 0.86 deg
 499     * TODO: Define this small angle in config.
 500     */
 501     if (rMat[2][2] <= 0.015f) {
 502         return 0;
 503     }
 504     int angle = lrintf(acos_approx(rMat[2][2]) * throttleAngleScale);
 505     if (angle > 900)
 506         angle = 900;
 507     return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
 508 }