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 #include "debug.h"
  28
  29 #include "common/axis.h"
  30
  31 #include "drivers/system.h"
  32 #include "drivers/sensor.h"
  33 #include "drivers/accgyro.h"
  34 #include "drivers/compass.h"
  35
  36 #include "sensors/sensors.h"
  37 #include "sensors/gyro.h"
  38 #include "sensors/compass.h"
  39 #include "sensors/acceleration.h"
  40 #include "sensors/barometer.h"
  41 #include "sensors/sonar.h"
  42
  43 #include "flight/mixer.h"
  44 #include "flight/pid.h"
  45 #include "flight/imu.h"
  46
  47 #include "config/runtime_config.h"
  48
  49 #include "tracker/config.h"
  50 #include "tracker/defines.h"
  51
  52 int16_t accSmooth[XYZ_AXIS_COUNT];
  53 int32_t accSum[XYZ_AXIS_COUNT];
  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[ANGLE_INDEX_COUNT] = { 0.0f, 0.0f };    // absolute angle inclination in radians
  70
  71 static imuRuntimeConfig_t *imuRuntimeConfig;
  72 static pidProfile_t *pidProfile;
  73 static accDeadband_t *accDeadband;
  74
  75 extern int16_t OFFSET;
  76 extern float DECLINATION;
  77
  78 void imuConfigure(
  79     imuRuntimeConfig_t *initialImuRuntimeConfig,
  80     pidProfile_t *initialPidProfile,
  81     accDeadband_t *initialAccDeadband,
  82     float accz_lpf_cutoff,
  83     uint16_t throttle_correction_angle
  84 )
  85 {
  86     imuRuntimeConfig = initialImuRuntimeConfig;
  87     pidProfile = initialPidProfile;
  88     accDeadband = initialAccDeadband;
  89     //fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff);
  90     //throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
  91 }
  92
  93 void imuInit(void)
  94 {
  95     smallAngle = lrintf(acc_1G * cos_approx(degreesToRadians(imuRuntimeConfig->small_angle)));
  96     accVelScale = 9.80665f / acc_1G / 10000.0f;
  97     gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f;
  98 }
  99
 100
 101
 102 /*
 103 * Baseflight calculation by Luggi09 originates from arducopter
 104 * ============================================================
 105 * This function rotates magnetic vector to cancel actual yaw and
 106 * pitch of craft. Then it computes it's direction in X/Y plane.
 107 * This value is returned as compass heading, value is 0-360 degrees.
 108 *
 109 * Note that Earth's magnetic field is not parallel with ground unless
 110 * you are near equator. Its inclination is considerable, >60 degrees
 111 * towards ground in most of Europe.
 112 *
 113 * First we consider it in 2D:
 114 *
 115 * An example, the vector <1, 1> would be turned into the heading
 116 * 45 degrees, representing it's angle clockwise from north.
 117 *
 118 *      ***************** *
 119 *      *       |   <1,1> *
 120 *      *       |  /      *
 121 *      *       | /       *
 122 *      *       |/        *
 123 *      *       *         *
 124 *      *                 *
 125 *      *                 *
 126 *      *                 *
 127 *      *                 *
 128 *      *******************
 129 *
 130 * //TODO: Add explanation for how it uses the Z dimension.
 131 */
 132
 133
 134 int16_t imuCalculateHeading(t_fp_vector *vec)
 135 {
 136     int16_t head;
 137
 138     float cosineRoll = cos_approx(anglerad[AI_ROLL]);
 139     float sineRoll = sin_approx(anglerad[AI_ROLL]);
 140     float cosinePitch = cos_approx(anglerad[AI_PITCH]);
 141     float sinePitch = sin_approx(anglerad[AI_PITCH]);
 142     float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll;
 143     float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;
 144     //TODO: Replace this comment with an explanation of why Yh and Xh can never simultanoeusly be zero,
 145     // or handle the case in which they are and (atan2f(0, 0) is undefined.
 146     //float hd = (atan2f(Yh, Xh) * 1800.0f / M_PIf + magneticDeclination) / 1.0f; //10.0f;
 147     float hd = atan2f(Yh, Xh);
 148     //head = lrintf(hd);
 149
 150     if (hd < 0)
 151         hd += 2 * M_PIf;
 152
 153     if (hd > 2 * M_PIf)
 154         hd -= 2 * M_PIf;
 155
 156     return (int16_t) ((hd * 1800.0 / M_PIf) + magneticDeclination + OFFSET * 10.0f)%3600;
 157 }