src/main/sensors/gyro.h

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #pragma once
  22
  23 #include "common/axis.h"
  24 #include "common/filter.h"
  25 #include "common/time.h"
  26 #include "common/utils.h"
  27
  28 #include "drivers/accgyro/accgyro.h"
  29 #include "drivers/bus.h"
  30 #include "drivers/sensor.h"
  31
  32 #ifdef USE_DYN_NOTCH_FILTER
  33 #include "flight/dyn_notch_filter.h"
  34 #endif
  35
  36 #include "flight/pid.h"
  37
  38 #include "pg/pg.h"
  39
  40 #define LPF_MAX_HZ 1000 // so little filtering above 1000hz that if the user wants less delay, they must disable the filter
  41 #define DYN_LPF_MAX_HZ 1000
  42
  43 #define GYRO_LPF1_DYN_MIN_HZ_DEFAULT 250
  44 #define GYRO_LPF1_DYN_MAX_HZ_DEFAULT 500
  45 #define GYRO_LPF2_HZ_DEFAULT 500
  46
  47 #ifdef USE_YAW_SPIN_RECOVERY
  48 #define YAW_SPIN_RECOVERY_THRESHOLD_MIN 500
  49 #define YAW_SPIN_RECOVERY_THRESHOLD_MAX 1950
  50 #endif
  51
  52 typedef union gyroLowpassFilter_u {
  53     pt1Filter_t pt1FilterState;
  54     biquadFilter_t biquadFilterState;
  55     pt2Filter_t pt2FilterState;
  56     pt3Filter_t pt3FilterState;
  57 } gyroLowpassFilter_t;
  58
  59 typedef enum gyroDetectionFlags_e {
  60     GYRO_NONE_MASK = 0,
  61     GYRO_1_MASK = BIT(0),
  62 #if defined(USE_MULTI_GYRO)
  63     GYRO_2_MASK = BIT(1),
  64     GYRO_ALL_MASK = (GYRO_1_MASK | GYRO_2_MASK),
  65     GYRO_IDENTICAL_MASK = BIT(7), // All gyros are of the same hardware type
  66 #endif
  67 } gyroDetectionFlags_t;
  68
  69 typedef struct gyroCalibration_s {
  70     float sum[XYZ_AXIS_COUNT];
  71     stdev_t var[XYZ_AXIS_COUNT];
  72     int32_t cyclesRemaining;
  73 } gyroCalibration_t;
  74
  75 typedef struct gyroSensor_s {
  76     gyroDev_t gyroDev;
  77     gyroCalibration_t calibration;
  78 } gyroSensor_t;
  79
  80 typedef struct gyro_s {
  81     uint16_t sampleRateHz;
  82     uint32_t targetLooptime;
  83     uint32_t sampleLooptime;
  84     float scale;
  85     float gyroADC[XYZ_AXIS_COUNT];     // aligned, calibrated, scaled, but unfiltered data from the sensor(s)
  86     float gyroADCf[XYZ_AXIS_COUNT];    // filtered gyro data
  87     uint8_t sampleCount;               // gyro sensor sample counter
  88     float sampleSum[XYZ_AXIS_COUNT];   // summed samples used for downsampling
  89     bool downsampleFilterEnabled;      // if true then downsample using gyro lowpass 2, otherwise use averaging
  90
  91     gyroSensor_t gyroSensor1;
  92 #ifdef USE_MULTI_GYRO
  93     gyroSensor_t gyroSensor2;
  94 #endif
  95
  96     gyroDev_t *rawSensorDev;           // pointer to the sensor providing the raw data for DEBUG_GYRO_RAW
  97
  98     // lowpass gyro soft filter
  99     filterApplyFnPtr lowpassFilterApplyFn;
 100     gyroLowpassFilter_t lowpassFilter[XYZ_AXIS_COUNT];
 101
 102     // lowpass2 gyro soft filter
 103     filterApplyFnPtr lowpass2FilterApplyFn;
 104     gyroLowpassFilter_t lowpass2Filter[XYZ_AXIS_COUNT];
 105
 106     // notch filters
 107     filterApplyFnPtr notchFilter1ApplyFn;
 108     biquadFilter_t notchFilter1[XYZ_AXIS_COUNT];
 109
 110     filterApplyFnPtr notchFilter2ApplyFn;
 111     biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
 112
 113     uint16_t accSampleRateHz;
 114     uint8_t gyroToUse;
 115     uint8_t gyroDebugMode;
 116     bool gyroHasOverflowProtection;
 117     bool useDualGyroDebugging;
 118     flight_dynamics_index_t gyroDebugAxis;
 119
 120 #ifdef USE_DYN_LPF
 121     uint8_t dynLpfFilter;
 122     uint16_t dynLpfMin;
 123     uint16_t dynLpfMax;
 124     uint8_t dynLpfCurveExpo;
 125 #endif
 126
 127 #ifdef USE_GYRO_OVERFLOW_CHECK
 128     uint8_t overflowAxisMask;
 129 #endif
 130
 131 } gyro_t;
 132
 133 extern gyro_t gyro;
 134 extern uint8_t activePidLoopDenom;
 135
 136 enum {
 137     GYRO_OVERFLOW_CHECK_NONE = 0,
 138     GYRO_OVERFLOW_CHECK_YAW,
 139     GYRO_OVERFLOW_CHECK_ALL_AXES
 140 };
 141
 142 enum {
 143     DYN_LPF_NONE = 0,
 144     DYN_LPF_PT1,
 145     DYN_LPF_BIQUAD,
 146     DYN_LPF_PT2,
 147     DYN_LPF_PT3,
 148 };
 149
 150 typedef enum {
 151     YAW_SPIN_RECOVERY_OFF,
 152     YAW_SPIN_RECOVERY_ON,
 153     YAW_SPIN_RECOVERY_AUTO
 154 } yawSpinRecoveryMode_e;
 155
 156 #define GYRO_CONFIG_USE_GYRO_1      0
 157 #define GYRO_CONFIG_USE_GYRO_2      1
 158 #define GYRO_CONFIG_USE_GYRO_BOTH   2
 159
 160 enum {
 161     FILTER_LPF1 = 0,
 162     FILTER_LPF2
 163 };
 164
 165 typedef struct gyroConfig_s {
 166     uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
 167     uint8_t gyro_hardware_lpf;                // gyro DLPF setting
 168
 169     uint8_t gyro_high_fsr;
 170     uint8_t gyro_to_use;
 171
 172     uint16_t gyro_lpf1_static_hz;
 173     uint16_t gyro_lpf2_static_hz;
 174
 175     uint16_t gyro_soft_notch_hz_1;
 176     uint16_t gyro_soft_notch_cutoff_1;
 177     uint16_t gyro_soft_notch_hz_2;
 178     uint16_t gyro_soft_notch_cutoff_2;
 179     int16_t gyro_offset_yaw;
 180     uint8_t checkOverflow;
 181
 182     // Lowpass primary/secondary
 183     uint8_t gyro_lpf1_type;
 184     uint8_t gyro_lpf2_type;
 185
 186     uint8_t yaw_spin_recovery;
 187     int16_t yaw_spin_threshold;
 188
 189     uint16_t gyroCalibrationDuration;   // Gyro calibration duration in 1/100 second
 190
 191     uint16_t gyro_lpf1_dyn_min_hz;
 192     uint16_t gyro_lpf1_dyn_max_hz;
 193
 194     uint8_t gyro_filter_debug_axis;
 195
 196     uint8_t gyrosDetected; // What gyros should detection be attempted for on startup. Automatically set on first startup.
 197     uint8_t gyro_lpf1_dyn_expo; // set the curve for dynamic gyro lowpass filter
 198     uint8_t simplified_gyro_filter;
 199     uint8_t simplified_gyro_filter_multiplier;
 200 } gyroConfig_t;
 201
 202 PG_DECLARE(gyroConfig_t, gyroConfig);
 203
 204 void gyroUpdate(void);
 205 void gyroFiltering(timeUs_t currentTimeUs);
 206 bool gyroGetAccumulationAverage(float *accumulation);
 207 void gyroStartCalibration(bool isFirstArmingCalibration);
 208 bool isFirstArmingGyroCalibrationRunning(void);
 209 bool gyroIsCalibrationComplete(void);
 210 void gyroReadTemperature(void);
 211 int16_t gyroGetTemperature(void);
 212 bool gyroOverflowDetected(void);
 213 bool gyroYawSpinDetected(void);
 214 uint16_t gyroAbsRateDps(int axis);
 215 #ifdef USE_DYN_LPF
 216 float dynThrottle(float throttle);
 217 void dynLpfGyroUpdate(float throttle);
 218 #endif
 219 #ifdef USE_YAW_SPIN_RECOVERY
 220 void initYawSpinRecovery(int maxYawRate);
 221 #endif