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
  27 #include "drivers/accgyro/accgyro.h"
  28 #include "drivers/bus.h"
  29 #include "drivers/sensor.h"
  30
  31 #ifdef USE_GYRO_DATA_ANALYSE
  32 #include "flight/gyroanalyse.h"
  33 #endif
  34
  35 #include "flight/pid.h"
  36
  37 #include "pg/pg.h"
  38
  39 #define FILTER_FREQUENCY_MAX 1000 // so little filtering above 1000hz that if the user wants less delay, they must disable the filter
  40 #define DYN_LPF_FILTER_FREQUENCY_MAX 1000
  41
  42 #define DYN_LPF_GYRO_MIN_HZ_DEFAULT 200
  43 #define DYN_LPF_GYRO_MAX_HZ_DEFAULT 500
  44 #define GYRO_LOWPASS_2_HZ_DEFAULT 250
  45
  46 #ifdef USE_YAW_SPIN_RECOVERY
  47 #define YAW_SPIN_RECOVERY_THRESHOLD_MIN 500
  48 #define YAW_SPIN_RECOVERY_THRESHOLD_MAX 1950
  49 #endif
  50
  51 typedef union gyroLowpassFilter_u {
  52     pt1Filter_t pt1FilterState;
  53     biquadFilter_t biquadFilterState;
  54     pt2Filter_t pt2FilterState;
  55     pt3Filter_t pt3FilterState;
  56 } gyroLowpassFilter_t;
  57
  58 typedef enum gyroDetectionFlags_e {
  59     GYRO_NONE_MASK = 0,
  60     GYRO_1_MASK = BIT(0),
  61 #if defined(USE_MULTI_GYRO)
  62     GYRO_2_MASK = BIT(1),
  63     GYRO_ALL_MASK = (GYRO_1_MASK | GYRO_2_MASK),
  64     GYRO_IDENTICAL_MASK = BIT(7), // All gyros are of the same hardware type
  65 #endif
  66 } gyroDetectionFlags_t;
  67
  68 typedef struct gyroCalibration_s {
  69     float sum[XYZ_AXIS_COUNT];
  70     stdev_t var[XYZ_AXIS_COUNT];
  71     int32_t cyclesRemaining;
  72 } gyroCalibration_t;
  73
  74 typedef struct gyroSensor_s {
  75     gyroDev_t gyroDev;
  76     gyroCalibration_t calibration;
  77 } gyroSensor_t;
  78
  79 typedef struct gyro_s {
  80     uint16_t sampleRateHz;
  81     uint32_t targetLooptime;
  82     uint32_t sampleLooptime;
  83     float scale;
  84     float gyroADC[XYZ_AXIS_COUNT];     // aligned, calibrated, scaled, but unfiltered data from the sensor(s)
  85     float gyroADCf[XYZ_AXIS_COUNT];    // filtered gyro data
  86     uint8_t sampleCount;               // gyro sensor sample counter
  87     float sampleSum[XYZ_AXIS_COUNT];   // summed samples used for downsampling
  88     bool downsampleFilterEnabled;      // if true then downsample using gyro lowpass 2, otherwise use averaging
  89
  90     gyroSensor_t gyroSensor1;
  91 #ifdef USE_MULTI_GYRO
  92     gyroSensor_t gyroSensor2;
  93 #endif
  94
  95     gyroDev_t *rawSensorDev;           // pointer to the sensor providing the raw data for DEBUG_GYRO_RAW
  96
  97     // lowpass gyro soft filter
  98     filterApplyFnPtr lowpassFilterApplyFn;
  99     gyroLowpassFilter_t lowpassFilter[XYZ_AXIS_COUNT];
 100
 101     // lowpass2 gyro soft filter
 102     filterApplyFnPtr lowpass2FilterApplyFn;
 103     gyroLowpassFilter_t lowpass2Filter[XYZ_AXIS_COUNT];
 104
 105     // notch filters
 106     filterApplyFnPtr notchFilter1ApplyFn;
 107     biquadFilter_t notchFilter1[XYZ_AXIS_COUNT];
 108
 109     filterApplyFnPtr notchFilter2ApplyFn;
 110     biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
 111
 112 #ifdef USE_GYRO_DATA_ANALYSE
 113     filterApplyFnPtr notchFilterDynApplyFn;
 114     biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
 115     uint8_t notchFilterDynCount;
 116
 117     gyroAnalyseState_t gyroAnalyseState;
 118 #endif
 119
 120     uint16_t accSampleRateHz;
 121     uint8_t gyroToUse;
 122     uint8_t gyroDebugMode;
 123     bool gyroHasOverflowProtection;
 124     bool useDualGyroDebugging;
 125     flight_dynamics_index_t gyroDebugAxis;
 126
 127 #ifdef USE_DYN_LPF
 128     uint8_t dynLpfFilter;
 129     uint16_t dynLpfMin;
 130     uint16_t dynLpfMax;
 131     uint8_t dynLpfCurveExpo;
 132 #endif
 133
 134 #ifdef USE_GYRO_OVERFLOW_CHECK
 135     uint8_t overflowAxisMask;
 136 #endif
 137
 138 } gyro_t;
 139
 140 extern gyro_t gyro;
 141 extern uint8_t activePidLoopDenom;
 142
 143 enum {
 144     GYRO_OVERFLOW_CHECK_NONE = 0,
 145     GYRO_OVERFLOW_CHECK_YAW,
 146     GYRO_OVERFLOW_CHECK_ALL_AXES
 147 };
 148
 149 enum {
 150     DYN_LPF_NONE = 0,
 151     DYN_LPF_PT1,
 152     DYN_LPF_BIQUAD,
 153     DYN_LPF_PT2,
 154     DYN_LPF_PT3,
 155 };
 156
 157 typedef enum {
 158     YAW_SPIN_RECOVERY_OFF,
 159     YAW_SPIN_RECOVERY_ON,
 160     YAW_SPIN_RECOVERY_AUTO
 161 } yawSpinRecoveryMode_e;
 162
 163 #define GYRO_CONFIG_USE_GYRO_1      0
 164 #define GYRO_CONFIG_USE_GYRO_2      1
 165 #define GYRO_CONFIG_USE_GYRO_BOTH   2
 166
 167 enum {
 168     FILTER_LOWPASS = 0,
 169     FILTER_LOWPASS2
 170 };
 171
 172 typedef struct gyroConfig_s {
 173     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.
 174     uint8_t  gyro_hardware_lpf;                // gyro DLPF setting
 175
 176     uint8_t  gyro_high_fsr;
 177     uint8_t  gyro_to_use;
 178
 179     uint16_t gyro_lowpass_hz;
 180     uint16_t gyro_lowpass2_hz;
 181
 182     uint16_t gyro_soft_notch_hz_1;
 183     uint16_t gyro_soft_notch_cutoff_1;
 184     uint16_t gyro_soft_notch_hz_2;
 185     uint16_t gyro_soft_notch_cutoff_2;
 186     int16_t  gyro_offset_yaw;
 187     uint8_t  checkOverflow;
 188
 189     // Lowpass primary/secondary
 190     uint8_t  gyro_lowpass_type;
 191     uint8_t  gyro_lowpass2_type;
 192
 193     uint8_t  yaw_spin_recovery;
 194     int16_t  yaw_spin_threshold;
 195
 196     uint16_t gyroCalibrationDuration;   // Gyro calibration duration in 1/100 second
 197
 198     uint16_t dyn_lpf_gyro_min_hz;
 199     uint16_t dyn_lpf_gyro_max_hz;
 200
 201     uint16_t dyn_notch_max_hz;
 202     uint8_t  dyn_notch_count;
 203     uint16_t dyn_notch_bandwidth_hz;
 204     uint16_t dyn_notch_min_hz;
 205
 206     uint8_t  gyro_filter_debug_axis;
 207
 208     uint8_t gyrosDetected; // What gyros should detection be attempted for on startup. Automatically set on first startup.
 209     uint8_t dyn_lpf_curve_expo; // set the curve for dynamic gyro lowpass filter
 210     uint8_t  simplified_gyro_filter;
 211     uint8_t  simplified_gyro_filter_multiplier;
 212 } gyroConfig_t;
 213
 214 PG_DECLARE(gyroConfig_t, gyroConfig);
 215
 216 void gyroUpdate(void);
 217 void gyroFiltering(timeUs_t currentTimeUs);
 218 bool gyroGetAccumulationAverage(float *accumulation);
 219 void gyroStartCalibration(bool isFirstArmingCalibration);
 220 bool isFirstArmingGyroCalibrationRunning(void);
 221 bool gyroIsCalibrationComplete(void);
 222 void gyroReadTemperature(void);
 223 int16_t gyroGetTemperature(void);
 224 bool gyroOverflowDetected(void);
 225 bool gyroYawSpinDetected(void);
 226 uint16_t gyroAbsRateDps(int axis);
 227 #ifdef USE_DYN_LPF
 228 float dynThrottle(float throttle);
 229 void dynLpfGyroUpdate(float throttle);
 230 #endif
 231 #ifdef USE_YAW_SPIN_RECOVERY
 232 void initYawSpinRecovery(int maxYawRate);
 233 #endif
 234 #ifdef USE_GYRO_DATA_ANALYSE
 235 bool isDynamicFilterActive(void);
 236 #endif