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