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