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