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