src/main/sensors/gyro.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 #include <stdbool.h>
  19 #include <stdint.h>
  20
  21 #include "platform.h"
  22
  23 #include "common/axis.h"
  24 #include "common/maths.h"
  25
  26 #include "drivers/sensor.h"
  27 #include "drivers/accgyro.h"
  28 #include "sensors/sensors.h"
  29 #include "io/statusindicator.h"
  30 #include "sensors/boardalignment.h"
  31
  32 #include "sensors/gyro.h"
  33
  34 uint16_t calibratingG = 0;
  35 int16_t gyroADC[XYZ_AXIS_COUNT];
  36 int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
  37
  38 static gyroConfig_t *gyroConfig;
  39
  40 gyro_t gyro;                      // gyro access functions
  41 sensor_align_e gyroAlign = 0;
  42
  43 void useGyroConfig(gyroConfig_t *gyroConfigToUse)
  44 {
  45     gyroConfig = gyroConfigToUse;
  46 }
  47
  48 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
  49 {
  50     calibratingG = calibrationCyclesRequired;
  51 }
  52
  53 bool isGyroCalibrationComplete(void)
  54 {
  55     return calibratingG == 0;
  56 }
  57
  58 bool isOnFinalGyroCalibrationCycle(void)
  59 {
  60     return calibratingG == 1;
  61 }
  62
  63 bool isOnFirstGyroCalibrationCycle(void)
  64 {
  65     return calibratingG == CALIBRATING_GYRO_CYCLES;
  66 }
  67
  68 static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
  69 {
  70     int8_t axis;
  71     static int32_t g[3];
  72     static stdev_t var[3];
  73
  74     for (axis = 0; axis < 3; axis++) {
  75
  76         // Reset g[axis] at start of calibration
  77         if (isOnFirstGyroCalibrationCycle()) {
  78             g[axis] = 0;
  79             devClear(&var[axis]);
  80         }
  81
  82         // Sum up CALIBRATING_GYRO_CYCLES readings
  83         g[axis] += gyroADC[axis];
  84         devPush(&var[axis], gyroADC[axis]);
  85
  86         // Reset global variables to prevent other code from using un-calibrated data
  87         gyroADC[axis] = 0;
  88         gyroZero[axis] = 0;
  89
  90         if (isOnFinalGyroCalibrationCycle()) {
  91             float dev = devStandardDeviation(&var[axis]);
  92             // check deviation and startover in case the model was moved
  93             if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
  94                 gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
  95                 return;
  96             }
  97             gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
  98             blinkLedAndSoundBeeper(10, 15, 1);
  99         }
 100     }
 101     calibratingG--;
 102 }
 103
 104 static void applyGyroZero(void)
 105 {
 106     int8_t axis;
 107     for (axis = 0; axis < 3; axis++) {
 108         gyroADC[axis] -= gyroZero[axis];
 109     }
 110 }
 111
 112 void gyroUpdate(void)
 113 {
 114     // FIXME When gyro.read() fails due to i2c or other error gyroZero is continually re-applied to gyroADC resulting in a old reading that gets worse over time.
 115
 116     // range: +/- 8192; +/- 2000 deg/sec
 117     gyro.read(gyroADC);
 118     alignSensors(gyroADC, gyroADC, gyroAlign);
 119
 120     if (!isGyroCalibrationComplete()) {
 121         performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
 122     }
 123
 124     applyGyroZero();
 125 }