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