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