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