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 "common/axis.h"
  29
  30 #include "drivers/system.h"
  31 #include "drivers/sensor.h"
  32 #include "drivers/accgyro.h"
  33 #include "drivers/compass.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/flight.h"
  44 #include "flight/imu.h"
  45
  46 #include "config/runtime_config.h"
  47
  48
  49 extern int16_t debug[4];
  50
  51 int16_t gyroADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT];
  52 int32_t accSum[XYZ_AXIS_COUNT];
  53
  54 uint32_t accTimeSum = 0;        // keep track for integration of acc
  55 int accSumCount = 0;
  56 float accVelScale;
  57
  58 int16_t smallAngle = 0;
  59
  60 float throttleAngleScale;
  61 float fc_acc;
  62
  63 float magneticDeclination = 0.0f;       // calculated at startup from config
  64 float gyroScaleRad;
  65
  66 // **************
  67 // gyro+acc IMU
  68 // **************
  69 int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
  70 int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
  71
  72 rollAndPitchInclination_t inclination = { { 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
  73 float anglerad[2] = { 0.0f, 0.0f };    // absolute angle inclination in radians
  74
  75 imuRuntimeConfig_t *imuRuntimeConfig;
  76 pidProfile_t *pidProfile;
  77 accDeadband_t *accDeadband;
  78
  79 void imuConfigure(
  80     imuRuntimeConfig_t *initialImuRuntimeConfig,
  81     pidProfile_t *initialPidProfile,
  82     accDeadband_t *initialAccDeadband,
  83     float accz_lpf_cutoff,
  84     uint16_t throttle_correction_angle
  85 )
  86 {
  87     imuRuntimeConfig = initialImuRuntimeConfig;
  88     pidProfile = initialPidProfile;
  89     accDeadband = initialAccDeadband;
  90     fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff);
  91     throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
  92 }
  93
  94 void imuInit()
  95 {
  96     smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle)));
  97     accVelScale = 9.80665f / acc_1G / 10000.0f;
  98     gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f;
  99 }
 100
 101 float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
 102 {
 103     return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
 104 }
 105
 106 /*
 107 * Calculate RC time constant used in the accZ lpf.
 108 */
 109 float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
 110 {
 111     return 0.5f / (M_PIf * accz_lpf_cutoff);
 112 }
 113
 114 // **************************************************
 115 // Simplified IMU based on "Complementary Filter"
 116 // Inspired by http://starlino.com/imu_guide.html
 117 //
 118 // adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198
 119 //
 120 // The following ideas was used in this project:
 121 // 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
 122 //
 123 // Currently Magnetometer uses separate CF which is used only
 124 // for heading approximation.
 125 //
 126 // **************************************************
 127
 128
 129 t_fp_vector EstG;
 130
 131 void imuResetAccelerationSum(void)
 132 {
 133     accSum[0] = 0;
 134     accSum[1] = 0;
 135     accSum[2] = 0;
 136     accSumCount = 0;
 137     accTimeSum = 0;
 138 }
 139
 140 // rotate acc into Earth frame and calculate acceleration in it
 141 void imuCalculateAcceleration(uint32_t deltaT)
 142 {
 143     static int32_t accZoffset = 0;
 144     static float accz_smooth = 0;
 145     float dT;
 146     fp_angles_t rpy;
 147     t_fp_vector accel_ned;
 148
 149     // deltaT is measured in us ticks
 150     dT = (float)deltaT * 1e-6f;
 151
 152     // the accel values have to be rotated into the earth frame
 153     rpy.angles.roll = -(float)anglerad[AI_ROLL];
 154     rpy.angles.pitch = -(float)anglerad[AI_PITCH];
 155     rpy.angles.yaw = -(float)heading * RAD;
 156
 157     accel_ned.V.X = accSmooth[0];
 158     accel_ned.V.Y = accSmooth[1];
 159     accel_ned.V.Z = accSmooth[2];
 160
 161     rotateV(&accel_ned.V, &rpy);
 162
 163     if (imuRuntimeConfig->acc_unarmedcal == 1) {
 164         if (!ARMING_FLAG(ARMED)) {
 165             accZoffset -= accZoffset / 64;
 166             accZoffset += accel_ned.V.Z;
 167         }
 168         accel_ned.V.Z -= accZoffset / 64;  // compensate for gravitation on z-axis
 169     } else
 170         accel_ned.V.Z -= acc_1G;
 171
 172     accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
 173
 174     // apply Deadband to reduce integration drift and vibration influence
 175     accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy);
 176     accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), accDeadband->xy);
 177     accSum[Z] += applyDeadband(lrintf(accz_smooth), accDeadband->z);
 178
 179     // sum up Values for later integration to get velocity and distance
 180     accTimeSum += deltaT;
 181     accSumCount++;
 182 }
 183
 184 /*
 185 * Baseflight calculation by Luggi09 originates from arducopter
 186 * ============================================================
 187 * This function rotates magnetic vector to cancel actual yaw and
 188 * pitch of craft. Then it computes it's direction in X/Y plane.
 189 * This value is returned as compass heading, value is 0-360 degrees.
 190 *
 191 * Note that Earth's magnetic field is not parallel with ground unless
 192 * you are near equator. Its inclination is considerable, >60 degrees
 193 * towards ground in most of Europe.
 194 *
 195 * First we consider it in 2D:
 196 *
 197 * An example, the vector <1, 1> would be turned into the heading
 198 * 45 degrees, representing it's angle clockwise from north.
 199 *
 200 *      ***************** *
 201 *      *       |   <1,1> *
 202 *      *       |  /      *
 203 *      *       | /       *
 204 *      *       |/        *
 205 *      *       *         *
 206 *      *                 *
 207 *      *                 *
 208 *      *                 *
 209 *      *                 *
 210 *      *******************
 211 *
 212 * //TODO: Add explanation for how it uses the Z dimension.
 213 */
 214 int16_t imuCalculateHeading(t_fp_vector *vec)
 215 {
 216     int16_t head;
 217
 218     float cosineRoll = cosf(anglerad[AI_ROLL]);
 219     float sineRoll = sinf(anglerad[AI_ROLL]);
 220     float cosinePitch = cosf(anglerad[AI_PITCH]);
 221     float sinePitch = sinf(anglerad[AI_PITCH]);
 222     float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll;
 223     float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;
 224     //TODO: Replace this comment with an explanation of why Yh and Xh can never simultanoeusly be zero,
 225     // or handle the case in which they are and (atan2f(0, 0) is undefined.
 226     float hd = (atan2f(Yh, Xh) * 1800.0f / M_PIf + magneticDeclination) / 10.0f;
 227     head = lrintf(hd);
 228
 229     // Arctan returns a value in the range -180 to 180 degrees. We 'normalize' negative angles to be positive.
 230     if (head < 0)
 231         head += 360;
 232
 233     return head;
 234 }
 235
 236 static void imuCalculateEstimatedAttitude(void)
 237 {
 238     int32_t axis;
 239     int32_t accMag = 0;
 240     static t_fp_vector EstM;
 241     static t_fp_vector EstN = { .A = { 1.0f, 0.0f, 0.0f } };
 242     static float accLPF[3];
 243     static uint32_t previousT;
 244     uint32_t currentT = micros();
 245     uint32_t deltaT;
 246     float scale;
 247     fp_angles_t deltaGyroAngle;
 248     deltaT = currentT - previousT;
 249     scale = deltaT * gyroScaleRad;
 250     previousT = currentT;
 251
 252     // Initialization
 253     for (axis = 0; axis < 3; axis++) {
 254         deltaGyroAngle.raw[axis] = gyroADC[axis] * scale;
 255         if (imuRuntimeConfig->acc_lpf_factor > 0) {
 256             accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / imuRuntimeConfig->acc_lpf_factor)) + accADC[axis] * (1.0f / imuRuntimeConfig->acc_lpf_factor);
 257             accSmooth[axis] = accLPF[axis];
 258         } else {
 259             accSmooth[axis] = accADC[axis];
 260         }
 261         accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
 262     }
 263     accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
 264
 265     rotateV(&EstG.V, &deltaGyroAngle);
 266
 267     // Apply complimentary filter (Gyro drift correction)
 268     // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
 269     // To do that, we just skip filter, as EstV already rotated by Gyro
 270
 271     float invGyroComplimentaryFilterFactor = (1.0f / (imuRuntimeConfig->gyro_cmpf_factor + 1.0f));
 272
 273     if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
 274         for (axis = 0; axis < 3; axis++)
 275             EstG.A[axis] = (EstG.A[axis] * imuRuntimeConfig->gyro_cmpf_factor + accSmooth[axis]) * invGyroComplimentaryFilterFactor;
 276     }
 277
 278     if (EstG.A[Z] > smallAngle) {
 279         ENABLE_STATE(SMALL_ANGLE);
 280     } else {
 281         DISABLE_STATE(SMALL_ANGLE);
 282     }
 283
 284     // Attitude of the estimated vector
 285     anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
 286     anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
 287     inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PIf));
 288     inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PIf));
 289
 290     if (sensors(SENSOR_MAG)) {
 291         rotateV(&EstM.V, &deltaGyroAngle);
 292         // FIXME what does the _M_ mean?
 293         float invGyroComplimentaryFilter_M_Factor = (1.0f / (imuRuntimeConfig->gyro_cmpfm_factor + 1.0f));
 294         for (axis = 0; axis < 3; axis++) {
 295             EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor;
 296         }
 297         heading = imuCalculateHeading(&EstM);
 298     } else {
 299         rotateV(&EstN.V, &deltaGyroAngle);
 300         normalizeV(&EstN.V, &EstN.V);
 301         heading = imuCalculateHeading(&EstN);
 302     }
 303
 304     imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
 305 }
 306
 307 void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
 308 {
 309     static int16_t gyroYawSmooth = 0;
 310
 311     gyroUpdate();
 312     if (sensors(SENSOR_ACC)) {
 313         updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer'
 314         imuCalculateEstimatedAttitude();
 315     } else {
 316         accADC[X] = 0;
 317         accADC[Y] = 0;
 318         accADC[Z] = 0;
 319     }
 320
 321     gyroData[FD_ROLL] = gyroADC[FD_ROLL];
 322     gyroData[FD_PITCH] = gyroADC[FD_PITCH];
 323
 324     if (mixerMode == MIXER_TRI) {
 325         gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3;
 326         gyroYawSmooth = gyroData[FD_YAW];
 327     } else {
 328         gyroData[FD_YAW] = gyroADC[FD_YAW];
 329     }
 330 }
 331
 332 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
 333 {
 334     float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
 335
 336     /*
 337     * Use 0 as the throttle angle correction if we are inverted, vertical or with a
 338     * small angle < 0.86 deg
 339     * TODO: Define this small angle in config.
 340     */
 341     if (cosZ <= 0.015f) {
 342         return 0;
 343     }
 344     int angle = lrintf(acosf(cosZ) * throttleAngleScale);
 345     if (angle > 900)
 346         angle = 900;
 347     return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PIf / 2.0f)));
 348 }