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 #include <math.h>
  21
  22 #include "platform.h"
  23 #include "debug.h"
  24
  25 #include "common/axis.h"
  26 #include "common/maths.h"
  27 #include "common/filter.h"
  28
  29 #include "drivers/sensor.h"
  30 #include "drivers/accgyro.h"
  31 #include "drivers/gyro_sync.h"
  32 #include "sensors/sensors.h"
  33 #include "io/beeper.h"
  34 #include "io/statusindicator.h"
  35 #include "sensors/boardalignment.h"
  36
  37 #include "sensors/gyro.h"
  38
  39 uint16_t calibratingG = 0;
  40 int32_t gyroADC[XYZ_AXIS_COUNT];
  41 int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
  42
  43 static gyroConfig_t *gyroConfig;
  44 static biquad_t gyroFilterState[3];
  45 static bool gyroFilterStateIsSet;
  46 static float gyroLpfCutFreq;
  47
  48 gyro_t gyro;                      // gyro access functions
  49 sensor_align_e gyroAlign = 0;
  50
  51 void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz)
  52 {
  53     gyroConfig = gyroConfigToUse;
  54     gyroLpfCutFreq = gyro_lpf_hz;
  55 }
  56
  57 void initGyroFilterCoefficients(void) {
  58         int axis;
  59     if (gyroLpfCutFreq && targetLooptime) {  /* Initialisation needs to happen once samplingrate is known */
  60         for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], targetLooptime);
  61         gyroFilterStateIsSet = true;
  62     }
  63 }
  64
  65 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
  66 {
  67     calibratingG = calibrationCyclesRequired;
  68 }
  69
  70 bool isGyroCalibrationComplete(void)
  71 {
  72     return calibratingG == 0;
  73 }
  74
  75 bool isOnFinalGyroCalibrationCycle(void)
  76 {
  77     return calibratingG == 1;
  78 }
  79
  80 bool isOnFirstGyroCalibrationCycle(void)
  81 {
  82     return calibratingG == (CALIBRATING_GYRO_CYCLES / targetLooptime) * CALIBRATING_GYRO_CYCLES;
  83 }
  84
  85 static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
  86 {
  87     int8_t axis;
  88     static int32_t g[3];
  89     static stdev_t var[3];
  90
  91     for (axis = 0; axis < 3; axis++) {
  92
  93         // Reset g[axis] at start of calibration
  94         if (isOnFirstGyroCalibrationCycle()) {
  95             g[axis] = 0;
  96             devClear(&var[axis]);
  97         }
  98
  99         // Sum up CALIBRATING_GYRO_CYCLES readings
 100         g[axis] += gyroADC[axis];
 101         devPush(&var[axis], gyroADC[axis]);
 102
 103         // Reset global variables to prevent other code from using un-calibrated data
 104         gyroADC[axis] = 0;
 105         gyroZero[axis] = 0;
 106
 107         if (isOnFinalGyroCalibrationCycle()) {
 108             float dev = devStandardDeviation(&var[axis]);
 109             // check deviation and startover in case the model was moved
 110             if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
 111                 gyroSetCalibrationCycles((CALIBRATING_GYRO_CYCLES / targetLooptime) * CALIBRATING_GYRO_CYCLES);
 112                 return;
 113             }
 114             gyroZero[axis] = (g[axis] + ((CALIBRATING_GYRO_CYCLES / targetLooptime) * CALIBRATING_GYRO_CYCLES / 2)) / (CALIBRATING_GYRO_CYCLES / targetLooptime) * CALIBRATING_GYRO_CYCLES;
 115         }
 116     }
 117
 118     if (isOnFinalGyroCalibrationCycle()) {
 119         beeper(BEEPER_GYRO_CALIBRATED);
 120     }
 121     calibratingG--;
 122
 123 }
 124
 125 static void applyGyroZero(void)
 126 {
 127     int8_t axis;
 128     for (axis = 0; axis < 3; axis++) {
 129         gyroADC[axis] -= gyroZero[axis];
 130     }
 131 }
 132
 133 void gyroUpdate(void)
 134 {
 135         int16_t gyroADCRaw[XYZ_AXIS_COUNT];
 136         int axis;
 137
 138     // range: +/- 8192; +/- 2000 deg/sec
 139     if (!gyro.read(gyroADCRaw)) {
 140         return;
 141     }
 142
 143     for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 144         if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis];
 145         gyroADC[axis] = gyroADCRaw[axis];
 146     }
 147
 148     alignSensors(gyroADC, gyroADC, gyroAlign);
 149
 150     if (gyroLpfCutFreq) {
 151         if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */
 152
 153         if (gyroFilterStateIsSet) {
 154             for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]));
 155         }
 156     }
 157
 158     if (!isGyroCalibrationComplete()) {
 159         performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
 160     }
 161
 162     applyGyroZero();
 163 }