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/bus.h"
  28 #include "drivers/sensor.h"
  29
  30 #include "pg/pg.h"
  31
  32 #define FILTER_FREQUENCY_MAX 4000 // maximum frequency for filter cutoffs (nyquist limit of 8K max sampling)
  33
  34 typedef struct gyro_s {
  35     uint32_t targetLooptime;
  36     float gyroADCf[XYZ_AXIS_COUNT];
  37 } gyro_t;
  38
  39 extern gyro_t gyro;
  40
  41 enum {
  42     GYRO_OVERFLOW_CHECK_NONE = 0,
  43     GYRO_OVERFLOW_CHECK_YAW,
  44     GYRO_OVERFLOW_CHECK_ALL_AXES
  45 };
  46
  47 enum {
  48     DYN_NOTCH_RANGE_HIGH = 0,
  49     DYN_NOTCH_RANGE_MEDIUM,
  50     DYN_NOTCH_RANGE_LOW,
  51     DYN_NOTCH_RANGE_AUTO
  52 };
  53
  54 #define DYN_NOTCH_RANGE_HZ_HIGH 2000
  55 #define DYN_NOTCH_RANGE_HZ_MEDIUM 1333
  56 #define DYN_NOTCH_RANGE_HZ_LOW 1000
  57
  58 enum {
  59     DYN_LPF_NONE = 0,
  60     DYN_LPF_PT1,
  61     DYN_LPF_BIQUAD
  62 };
  63
  64 #define GYRO_CONFIG_USE_GYRO_1      0
  65 #define GYRO_CONFIG_USE_GYRO_2      1
  66 #define GYRO_CONFIG_USE_GYRO_BOTH   2
  67
  68 enum {
  69     FILTER_LOWPASS = 0,
  70     FILTER_LOWPASS2
  71 };
  72
  73 typedef enum gyroDetectionFlags_e {
  74     NO_GYROS_DETECTED = 0,
  75     DETECTED_GYRO_1 = (1 << 0),
  76 #if defined(USE_MULTI_GYRO)
  77     DETECTED_GYRO_2 = (1 << 1),
  78     DETECTED_BOTH_GYROS = (DETECTED_GYRO_1 | DETECTED_GYRO_2),
  79     DETECTED_DUAL_GYROS = (1 << 7), // All gyros are of the same hardware type
  80 #endif
  81 } gyroDetectionFlags_t;
  82
  83 typedef struct gyroConfig_s {
  84     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.
  85     uint8_t  gyro_sync_denom;                  // Gyro sample divider
  86     uint8_t  gyro_hardware_lpf;                // gyro DLPF setting
  87
  88     uint8_t  gyro_high_fsr;
  89     uint8_t  gyro_to_use;
  90
  91     uint16_t gyro_lowpass_hz;
  92     uint16_t gyro_lowpass2_hz;
  93
  94     uint16_t gyro_soft_notch_hz_1;
  95     uint16_t gyro_soft_notch_cutoff_1;
  96     uint16_t gyro_soft_notch_hz_2;
  97     uint16_t gyro_soft_notch_cutoff_2;
  98     int16_t  gyro_offset_yaw;
  99     uint8_t  checkOverflow;
 100
 101     // Lowpass primary/secondary
 102     uint8_t  gyro_lowpass_type;
 103     uint8_t  gyro_lowpass2_type;
 104
 105     uint8_t  yaw_spin_recovery;
 106     int16_t  yaw_spin_threshold;
 107
 108     uint16_t gyroCalibrationDuration;   // Gyro calibration duration in 1/100 second
 109
 110     uint16_t dyn_lpf_gyro_min_hz;
 111     uint16_t dyn_lpf_gyro_max_hz;
 112     uint8_t  dyn_notch_range;            // ignore any FFT bin below this threshold
 113     uint8_t  dyn_notch_width_percent;
 114     uint16_t dyn_notch_q;
 115     uint16_t dyn_notch_min_hz;
 116     uint8_t  gyro_filter_debug_axis;
 117 } gyroConfig_t;
 118
 119 PG_DECLARE(gyroConfig_t, gyroConfig);
 120
 121 void gyroPreInit(void);
 122 bool gyroInit(void);
 123
 124 void gyroInitFilters(void);
 125 void gyroUpdate(timeUs_t currentTimeUs);
 126 bool gyroGetAccumulationAverage(float *accumulation);
 127 const busDevice_t *gyroSensorBus(void);
 128 struct mpuDetectionResult_s;
 129 const struct mpuDetectionResult_s *gyroMpuDetectionResult(void);
 130 void gyroStartCalibration(bool isFirstArmingCalibration);
 131 bool isFirstArmingGyroCalibrationRunning(void);
 132 bool isGyroCalibrationComplete(void);
 133 void gyroReadTemperature(void);
 134 int16_t gyroGetTemperature(void);
 135 int16_t gyroRateDps(int axis);
 136 bool gyroOverflowDetected(void);
 137 bool gyroYawSpinDetected(void);
 138 uint16_t gyroAbsRateDps(int axis);
 139 uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg);
 140 gyroDetectionFlags_t getGyroDetectionFlags(void);
 141 #ifdef USE_DYN_LPF
 142 float dynThrottle(float throttle);
 143 void dynLpfGyroUpdate(float throttle);
 144 #endif