2 * This file is part of Cleanflight and Betaflight.
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)
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.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
29 #include "build/debug.h"
31 #include "common/axis.h"
32 #include "common/maths.h"
33 #include "common/filter.h"
35 #include "config/feature.h"
38 #include "pg/pg_ids.h"
39 #include "pg/gyrodev.h"
41 #include "drivers/bus_spi.h"
42 #include "drivers/io.h"
44 #include "config/config.h"
45 #include "fc/runtime_config.h"
47 #ifdef USE_GYRO_DATA_ANALYSE
48 #include "flight/gyroanalyse.h"
50 #include "flight/rpm_filter.h"
52 #include "io/beeper.h"
53 #include "io/statusindicator.h"
55 #include "scheduler/scheduler.h"
57 #include "sensors/boardalignment.h"
58 #include "sensors/gyro.h"
59 #include "sensors/gyro_init.h"
61 #if ((TARGET_FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6500)))
62 #define USE_GYRO_SLEW_LIMITER
65 FAST_RAM_ZERO_INIT gyro_t gyro
;
67 static FAST_RAM_ZERO_INIT
bool overflowDetected
;
68 #ifdef USE_GYRO_OVERFLOW_CHECK
69 static FAST_RAM_ZERO_INIT timeUs_t overflowTimeUs
;
72 #ifdef USE_YAW_SPIN_RECOVERY
73 static FAST_RAM_ZERO_INIT
bool yawSpinRecoveryEnabled
;
74 static FAST_RAM_ZERO_INIT
int yawSpinRecoveryThreshold
;
75 static FAST_RAM_ZERO_INIT
bool yawSpinDetected
;
76 static FAST_RAM_ZERO_INIT timeUs_t yawSpinTimeUs
;
79 static FAST_RAM_ZERO_INIT
float accumulatedMeasurements
[XYZ_AXIS_COUNT
];
80 static FAST_RAM_ZERO_INIT
float gyroPrevious
[XYZ_AXIS_COUNT
];
81 static FAST_RAM_ZERO_INIT
int accumulatedMeasurementCount
;
83 static FAST_RAM_ZERO_INIT
int16_t gyroSensorTemperature
;
85 FAST_RAM
uint8_t activePidLoopDenom
= 1;
87 static bool firstArmingCalibrationWasStarted
= false;
90 STATIC_UNIT_TESTED gyroSensor_t
* const gyroSensorPtr
= &gyro
.gyroSensor1
;
91 STATIC_UNIT_TESTED gyroDev_t
* const gyroDevPtr
= &gyro
.gyroSensor1
.gyroDev
;
95 #define DEBUG_GYRO_CALIBRATION 3
97 #define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro)
98 #define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro)
100 PG_REGISTER_WITH_RESET_FN(gyroConfig_t
, gyroConfig
, PG_GYRO_CONFIG
, 8);
102 #ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
103 #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
106 void pgResetFn_gyroConfig(gyroConfig_t
*gyroConfig
)
108 gyroConfig
->gyroCalibrationDuration
= 125; // 1.25 seconds
109 gyroConfig
->gyroMovementCalibrationThreshold
= 48;
110 gyroConfig
->gyro_hardware_lpf
= GYRO_HARDWARE_LPF_NORMAL
;
111 gyroConfig
->gyro_lowpass_type
= FILTER_PT1
;
112 gyroConfig
->gyro_lowpass_hz
= 200; // NOTE: dynamic lpf is enabled by default so this setting is actually
113 // overridden and the static lowpass 1 is disabled. We can't set this
114 // value to 0 otherwise Configurator versions 10.4 and earlier will also
115 // reset the lowpass filter type to PT1 overriding the desired BIQUAD setting.
116 gyroConfig
->gyro_lowpass2_type
= FILTER_PT1
;
117 gyroConfig
->gyro_lowpass2_hz
= 250;
118 gyroConfig
->gyro_high_fsr
= false;
119 gyroConfig
->gyro_to_use
= GYRO_CONFIG_USE_GYRO_DEFAULT
;
120 gyroConfig
->gyro_soft_notch_hz_1
= 0;
121 gyroConfig
->gyro_soft_notch_cutoff_1
= 0;
122 gyroConfig
->gyro_soft_notch_hz_2
= 0;
123 gyroConfig
->gyro_soft_notch_cutoff_2
= 0;
124 gyroConfig
->checkOverflow
= GYRO_OVERFLOW_CHECK_ALL_AXES
;
125 gyroConfig
->gyro_offset_yaw
= 0;
126 gyroConfig
->yaw_spin_recovery
= YAW_SPIN_RECOVERY_AUTO
;
127 gyroConfig
->yaw_spin_threshold
= 1950;
128 gyroConfig
->dyn_lpf_gyro_min_hz
= 200;
129 gyroConfig
->dyn_lpf_gyro_max_hz
= 500;
130 gyroConfig
->dyn_notch_max_hz
= 600;
131 gyroConfig
->dyn_notch_width_percent
= 8;
132 gyroConfig
->dyn_notch_q
= 120;
133 gyroConfig
->dyn_notch_min_hz
= 150;
134 gyroConfig
->gyro_filter_debug_axis
= FD_ROLL
;
137 #ifdef USE_GYRO_DATA_ANALYSE
138 bool isDynamicFilterActive(void)
140 return featureIsEnabled(FEATURE_DYNAMIC_FILTER
);
144 FAST_CODE
bool isGyroSensorCalibrationComplete(const gyroSensor_t
*gyroSensor
)
146 return gyroSensor
->calibration
.cyclesRemaining
== 0;
149 FAST_CODE
bool gyroIsCalibrationComplete(void)
151 switch (gyro
.gyroToUse
) {
153 case GYRO_CONFIG_USE_GYRO_1
: {
154 return isGyroSensorCalibrationComplete(&gyro
.gyroSensor1
);
156 #ifdef USE_MULTI_GYRO
157 case GYRO_CONFIG_USE_GYRO_2
: {
158 return isGyroSensorCalibrationComplete(&gyro
.gyroSensor2
);
160 case GYRO_CONFIG_USE_GYRO_BOTH
: {
161 return isGyroSensorCalibrationComplete(&gyro
.gyroSensor1
) && isGyroSensorCalibrationComplete(&gyro
.gyroSensor2
);
167 static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t
*gyroCalibration
)
169 return gyroCalibration
->cyclesRemaining
== 1;
172 static int32_t gyroCalculateCalibratingCycles(void)
174 return (gyroConfig()->gyroCalibrationDuration
* 10000) / gyro
.sampleLooptime
;
177 static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t
*gyroCalibration
)
179 return gyroCalibration
->cyclesRemaining
== gyroCalculateCalibratingCycles();
182 static void gyroSetCalibrationCycles(gyroSensor_t
*gyroSensor
)
184 #if defined(USE_FAKE_GYRO) && !defined(UNIT_TEST)
185 if (gyroSensor
->gyroDev
.gyroHardware
== GYRO_FAKE
) {
186 gyroSensor
->calibration
.cyclesRemaining
= 0;
190 gyroSensor
->calibration
.cyclesRemaining
= gyroCalculateCalibratingCycles();
193 void gyroStartCalibration(bool isFirstArmingCalibration
)
195 if (isFirstArmingCalibration
&& firstArmingCalibrationWasStarted
) {
199 gyroSetCalibrationCycles(&gyro
.gyroSensor1
);
200 #ifdef USE_MULTI_GYRO
201 gyroSetCalibrationCycles(&gyro
.gyroSensor2
);
204 if (isFirstArmingCalibration
) {
205 firstArmingCalibrationWasStarted
= true;
209 bool isFirstArmingGyroCalibrationRunning(void)
211 return firstArmingCalibrationWasStarted
&& !gyroIsCalibrationComplete();
214 STATIC_UNIT_TESTED
void performGyroCalibration(gyroSensor_t
*gyroSensor
, uint8_t gyroMovementCalibrationThreshold
)
216 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
217 // Reset g[axis] at start of calibration
218 if (isOnFirstGyroCalibrationCycle(&gyroSensor
->calibration
)) {
219 gyroSensor
->calibration
.sum
[axis
] = 0.0f
;
220 devClear(&gyroSensor
->calibration
.var
[axis
]);
221 // gyroZero is set to zero until calibration complete
222 gyroSensor
->gyroDev
.gyroZero
[axis
] = 0.0f
;
225 // Sum up CALIBRATING_GYRO_TIME_US readings
226 gyroSensor
->calibration
.sum
[axis
] += gyroSensor
->gyroDev
.gyroADCRaw
[axis
];
227 devPush(&gyroSensor
->calibration
.var
[axis
], gyroSensor
->gyroDev
.gyroADCRaw
[axis
]);
229 if (isOnFinalGyroCalibrationCycle(&gyroSensor
->calibration
)) {
230 const float stddev
= devStandardDeviation(&gyroSensor
->calibration
.var
[axis
]);
231 // DEBUG_GYRO_CALIBRATION records the standard deviation of roll
232 // into the spare field - debug[3], in DEBUG_GYRO_RAW
234 DEBUG_SET(DEBUG_GYRO_RAW
, DEBUG_GYRO_CALIBRATION
, lrintf(stddev
));
237 // check deviation and startover in case the model was moved
238 if (gyroMovementCalibrationThreshold
&& stddev
> gyroMovementCalibrationThreshold
) {
239 gyroSetCalibrationCycles(gyroSensor
);
243 // please take care with exotic boardalignment !!
244 gyroSensor
->gyroDev
.gyroZero
[axis
] = gyroSensor
->calibration
.sum
[axis
] / gyroCalculateCalibratingCycles();
246 gyroSensor
->gyroDev
.gyroZero
[axis
] -= ((float)gyroConfig()->gyro_offset_yaw
/ 100);
251 if (isOnFinalGyroCalibrationCycle(&gyroSensor
->calibration
)) {
252 schedulerResetTaskStatistics(TASK_SELF
); // so calibration cycles do not pollute tasks statistics
253 if (!firstArmingCalibrationWasStarted
|| (getArmingDisableFlags() & ~ARMING_DISABLED_CALIBRATING
) == 0) {
254 beeper(BEEPER_GYRO_CALIBRATED
);
258 --gyroSensor
->calibration
.cyclesRemaining
;
261 #if defined(USE_GYRO_SLEW_LIMITER)
262 FAST_CODE
int32_t gyroSlewLimiter(gyroSensor_t
*gyroSensor
, int axis
)
264 int32_t ret
= (int32_t)gyroSensor
->gyroDev
.gyroADCRaw
[axis
];
265 if (gyroConfig()->checkOverflow
|| gyro
.gyroHasOverflowProtection
) {
266 // don't use the slew limiter if overflow checking is on or gyro is not subject to overflow bug
269 if (abs(ret
- gyroSensor
->gyroDev
.gyroADCRawPrevious
[axis
]) > (1<<14)) {
270 // there has been a large change in value, so assume overflow has occurred and return the previous value
271 ret
= gyroSensor
->gyroDev
.gyroADCRawPrevious
[axis
];
273 gyroSensor
->gyroDev
.gyroADCRawPrevious
[axis
] = ret
;
279 #ifdef USE_GYRO_OVERFLOW_CHECK
280 static FAST_CODE_NOINLINE
void handleOverflow(timeUs_t currentTimeUs
)
282 // This will need to be revised if we ever allow different sensor types to be
283 // used simultaneously. In that case the scale might be different between sensors.
284 // It's complicated by the fact that we're using filtered gyro data here which is
285 // after both sensors are scaled and averaged.
286 const float gyroOverflowResetRate
= GYRO_OVERFLOW_RESET_THRESHOLD
* gyro
.scale
;
288 if ((fabsf(gyro
.gyroADCf
[X
]) < gyroOverflowResetRate
)
289 && (fabsf(gyro
.gyroADCf
[Y
]) < gyroOverflowResetRate
)
290 && (fabsf(gyro
.gyroADCf
[Z
]) < gyroOverflowResetRate
)) {
291 // if we have 50ms of consecutive OK gyro vales, then assume yaw readings are OK again and reset overflowDetected
292 // reset requires good OK values on all axes
293 if (cmpTimeUs(currentTimeUs
, overflowTimeUs
) > 50000) {
294 overflowDetected
= false;
297 // not a consecutive OK value, so reset the overflow time
298 overflowTimeUs
= currentTimeUs
;
302 static FAST_CODE
void checkForOverflow(timeUs_t currentTimeUs
)
304 // check for overflow to handle Yaw Spin To The Moon (YSTTM)
305 // ICM gyros are specified to +/- 2000 deg/sec, in a crash they can go out of spec.
306 // This can cause an overflow and sign reversal in the output.
307 // Overflow and sign reversal seems to result in a gyro value of +1996 or -1996.
308 if (overflowDetected
) {
309 handleOverflow(currentTimeUs
);
311 #ifndef SIMULATOR_BUILD
312 // check for overflow in the axes set in overflowAxisMask
313 gyroOverflow_e overflowCheck
= GYRO_OVERFLOW_NONE
;
315 // This will need to be revised if we ever allow different sensor types to be
316 // used simultaneously. In that case the scale might be different between sensors.
317 // It's complicated by the fact that we're using filtered gyro data here which is
318 // after both sensors are scaled and averaged.
319 const float gyroOverflowTriggerRate
= GYRO_OVERFLOW_TRIGGER_THRESHOLD
* gyro
.scale
;
321 if (fabsf(gyro
.gyroADCf
[X
]) > gyroOverflowTriggerRate
) {
322 overflowCheck
|= GYRO_OVERFLOW_X
;
324 if (fabsf(gyro
.gyroADCf
[Y
]) > gyroOverflowTriggerRate
) {
325 overflowCheck
|= GYRO_OVERFLOW_Y
;
327 if (fabsf(gyro
.gyroADCf
[Z
]) > gyroOverflowTriggerRate
) {
328 overflowCheck
|= GYRO_OVERFLOW_Z
;
330 if (overflowCheck
& gyro
.overflowAxisMask
) {
331 overflowDetected
= true;
332 overflowTimeUs
= currentTimeUs
;
333 #ifdef USE_YAW_SPIN_RECOVERY
334 yawSpinDetected
= false;
335 #endif // USE_YAW_SPIN_RECOVERY
337 #endif // SIMULATOR_BUILD
340 #endif // USE_GYRO_OVERFLOW_CHECK
342 #ifdef USE_YAW_SPIN_RECOVERY
343 static FAST_CODE_NOINLINE
void handleYawSpin(timeUs_t currentTimeUs
)
345 const float yawSpinResetRate
= yawSpinRecoveryThreshold
- 100.0f
;
346 if (fabsf(gyro
.gyroADCf
[Z
]) < yawSpinResetRate
) {
347 // testing whether 20ms of consecutive OK gyro yaw values is enough
348 if (cmpTimeUs(currentTimeUs
, yawSpinTimeUs
) > 20000) {
349 yawSpinDetected
= false;
352 // reset the yaw spin time
353 yawSpinTimeUs
= currentTimeUs
;
357 static FAST_CODE
void checkForYawSpin(timeUs_t currentTimeUs
)
359 // if not in overflow mode, handle yaw spins above threshold
360 #ifdef USE_GYRO_OVERFLOW_CHECK
361 if (overflowDetected
) {
362 yawSpinDetected
= false;
365 #endif // USE_GYRO_OVERFLOW_CHECK
367 if (yawSpinDetected
) {
368 handleYawSpin(currentTimeUs
);
370 #ifndef SIMULATOR_BUILD
371 // check for spin on yaw axis only
372 if (abs((int)gyro
.gyroADCf
[Z
]) > yawSpinRecoveryThreshold
) {
373 yawSpinDetected
= true;
374 yawSpinTimeUs
= currentTimeUs
;
376 #endif // SIMULATOR_BUILD
379 #endif // USE_YAW_SPIN_RECOVERY
381 static FAST_CODE FAST_CODE_NOINLINE
void gyroUpdateSensor(gyroSensor_t
*gyroSensor
)
383 if (!gyroSensor
->gyroDev
.readFn(&gyroSensor
->gyroDev
)) {
386 gyroSensor
->gyroDev
.dataReady
= false;
388 if (isGyroSensorCalibrationComplete(gyroSensor
)) {
389 // move 16-bit gyro data into 32-bit variables to avoid overflows in calculations
391 #if defined(USE_GYRO_SLEW_LIMITER)
392 gyroSensor
->gyroDev
.gyroADC
[X
] = gyroSlewLimiter(gyroSensor
, X
) - gyroSensor
->gyroDev
.gyroZero
[X
];
393 gyroSensor
->gyroDev
.gyroADC
[Y
] = gyroSlewLimiter(gyroSensor
, Y
) - gyroSensor
->gyroDev
.gyroZero
[Y
];
394 gyroSensor
->gyroDev
.gyroADC
[Z
] = gyroSlewLimiter(gyroSensor
, Z
) - gyroSensor
->gyroDev
.gyroZero
[Z
];
396 gyroSensor
->gyroDev
.gyroADC
[X
] = gyroSensor
->gyroDev
.gyroADCRaw
[X
] - gyroSensor
->gyroDev
.gyroZero
[X
];
397 gyroSensor
->gyroDev
.gyroADC
[Y
] = gyroSensor
->gyroDev
.gyroADCRaw
[Y
] - gyroSensor
->gyroDev
.gyroZero
[Y
];
398 gyroSensor
->gyroDev
.gyroADC
[Z
] = gyroSensor
->gyroDev
.gyroADCRaw
[Z
] - gyroSensor
->gyroDev
.gyroZero
[Z
];
401 if (gyroSensor
->gyroDev
.gyroAlign
== ALIGN_CUSTOM
) {
402 alignSensorViaMatrix(gyroSensor
->gyroDev
.gyroADC
, &gyroSensor
->gyroDev
.rotationMatrix
);
404 alignSensorViaRotation(gyroSensor
->gyroDev
.gyroADC
, gyroSensor
->gyroDev
.gyroAlign
);
407 performGyroCalibration(gyroSensor
, gyroConfig()->gyroMovementCalibrationThreshold
);
411 FAST_CODE
void gyroUpdate(void)
413 switch (gyro
.gyroToUse
) {
414 case GYRO_CONFIG_USE_GYRO_1
:
415 gyroUpdateSensor(&gyro
.gyroSensor1
);
416 if (isGyroSensorCalibrationComplete(&gyro
.gyroSensor1
)) {
417 gyro
.gyroADC
[X
] = gyro
.gyroSensor1
.gyroDev
.gyroADC
[X
] * gyro
.gyroSensor1
.gyroDev
.scale
;
418 gyro
.gyroADC
[Y
] = gyro
.gyroSensor1
.gyroDev
.gyroADC
[Y
] * gyro
.gyroSensor1
.gyroDev
.scale
;
419 gyro
.gyroADC
[Z
] = gyro
.gyroSensor1
.gyroDev
.gyroADC
[Z
] * gyro
.gyroSensor1
.gyroDev
.scale
;
422 #ifdef USE_MULTI_GYRO
423 case GYRO_CONFIG_USE_GYRO_2
:
424 gyroUpdateSensor(&gyro
.gyroSensor2
);
425 if (isGyroSensorCalibrationComplete(&gyro
.gyroSensor2
)) {
426 gyro
.gyroADC
[X
] = gyro
.gyroSensor2
.gyroDev
.gyroADC
[X
] * gyro
.gyroSensor2
.gyroDev
.scale
;
427 gyro
.gyroADC
[Y
] = gyro
.gyroSensor2
.gyroDev
.gyroADC
[Y
] * gyro
.gyroSensor2
.gyroDev
.scale
;
428 gyro
.gyroADC
[Z
] = gyro
.gyroSensor2
.gyroDev
.gyroADC
[Z
] * gyro
.gyroSensor2
.gyroDev
.scale
;
431 case GYRO_CONFIG_USE_GYRO_BOTH
:
432 gyroUpdateSensor(&gyro
.gyroSensor1
);
433 gyroUpdateSensor(&gyro
.gyroSensor2
);
434 if (isGyroSensorCalibrationComplete(&gyro
.gyroSensor1
) && isGyroSensorCalibrationComplete(&gyro
.gyroSensor2
)) {
435 gyro
.gyroADC
[X
] = ((gyro
.gyroSensor1
.gyroDev
.gyroADC
[X
] * gyro
.gyroSensor1
.gyroDev
.scale
) + (gyro
.gyroSensor2
.gyroDev
.gyroADC
[X
] * gyro
.gyroSensor2
.gyroDev
.scale
)) / 2.0f
;
436 gyro
.gyroADC
[Y
] = ((gyro
.gyroSensor1
.gyroDev
.gyroADC
[Y
] * gyro
.gyroSensor1
.gyroDev
.scale
) + (gyro
.gyroSensor2
.gyroDev
.gyroADC
[Y
] * gyro
.gyroSensor2
.gyroDev
.scale
)) / 2.0f
;
437 gyro
.gyroADC
[Z
] = ((gyro
.gyroSensor1
.gyroDev
.gyroADC
[Z
] * gyro
.gyroSensor1
.gyroDev
.scale
) + (gyro
.gyroSensor2
.gyroDev
.gyroADC
[Z
] * gyro
.gyroSensor2
.gyroDev
.scale
)) / 2.0f
;
443 if (gyro
.downsampleFilterEnabled
) {
444 // using gyro lowpass 2 filter for downsampling
445 gyro
.sampleSum
[X
] = gyro
.lowpass2FilterApplyFn((filter_t
*)&gyro
.lowpass2Filter
[X
], gyro
.gyroADC
[X
]);
446 gyro
.sampleSum
[Y
] = gyro
.lowpass2FilterApplyFn((filter_t
*)&gyro
.lowpass2Filter
[Y
], gyro
.gyroADC
[Y
]);
447 gyro
.sampleSum
[Z
] = gyro
.lowpass2FilterApplyFn((filter_t
*)&gyro
.lowpass2Filter
[Z
], gyro
.gyroADC
[Z
]);
449 // using simple averaging for downsampling
450 gyro
.sampleSum
[X
] += gyro
.gyroADC
[X
];
451 gyro
.sampleSum
[Y
] += gyro
.gyroADC
[Y
];
452 gyro
.sampleSum
[Z
] += gyro
.gyroADC
[Z
];
457 #define GYRO_FILTER_FUNCTION_NAME filterGyro
458 #define GYRO_FILTER_DEBUG_SET(mode, index, value) do { UNUSED(mode); UNUSED(index); UNUSED(value); } while (0)
459 #define GYRO_FILTER_AXIS_DEBUG_SET(axis, mode, index, value) do { UNUSED(axis); UNUSED(mode); UNUSED(index); UNUSED(value); } while (0)
460 #include "gyro_filter_impl.c"
461 #undef GYRO_FILTER_FUNCTION_NAME
462 #undef GYRO_FILTER_DEBUG_SET
463 #undef GYRO_FILTER_AXIS_DEBUG_SET
465 #define GYRO_FILTER_FUNCTION_NAME filterGyroDebug
466 #define GYRO_FILTER_DEBUG_SET DEBUG_SET
467 #define GYRO_FILTER_AXIS_DEBUG_SET(axis, mode, index, value) if (axis == (int)gyro.gyroDebugAxis) DEBUG_SET(mode, index, value)
468 #include "gyro_filter_impl.c"
469 #undef GYRO_FILTER_FUNCTION_NAME
470 #undef GYRO_FILTER_DEBUG_SET
471 #undef GYRO_FILTER_AXIS_DEBUG_SET
473 FAST_CODE
void gyroFiltering(timeUs_t currentTimeUs
)
475 if (gyro
.gyroDebugMode
== DEBUG_NONE
) {
481 #ifdef USE_GYRO_DATA_ANALYSE
482 if (isDynamicFilterActive()) {
483 gyroDataAnalyse(&gyro
.gyroAnalyseState
, gyro
.notchFilterDyn
, gyro
.notchFilterDyn2
);
487 if (gyro
.useDualGyroDebugging
) {
488 switch (gyro
.gyroToUse
) {
489 case GYRO_CONFIG_USE_GYRO_1
:
490 DEBUG_SET(DEBUG_DUAL_GYRO_RAW
, 0, gyro
.gyroSensor1
.gyroDev
.gyroADCRaw
[X
]);
491 DEBUG_SET(DEBUG_DUAL_GYRO_RAW
, 1, gyro
.gyroSensor1
.gyroDev
.gyroADCRaw
[Y
]);
492 DEBUG_SET(DEBUG_DUAL_GYRO_SCALED
, 0, lrintf(gyro
.gyroSensor1
.gyroDev
.gyroADC
[X
] * gyro
.gyroSensor1
.gyroDev
.scale
));
493 DEBUG_SET(DEBUG_DUAL_GYRO_SCALED
, 1, lrintf(gyro
.gyroSensor1
.gyroDev
.gyroADC
[Y
] * gyro
.gyroSensor1
.gyroDev
.scale
));
496 #ifdef USE_MULTI_GYRO
497 case GYRO_CONFIG_USE_GYRO_2
:
498 DEBUG_SET(DEBUG_DUAL_GYRO_RAW
, 2, gyro
.gyroSensor2
.gyroDev
.gyroADCRaw
[X
]);
499 DEBUG_SET(DEBUG_DUAL_GYRO_RAW
, 3, gyro
.gyroSensor2
.gyroDev
.gyroADCRaw
[Y
]);
500 DEBUG_SET(DEBUG_DUAL_GYRO_SCALED
, 2, lrintf(gyro
.gyroSensor2
.gyroDev
.gyroADC
[X
] * gyro
.gyroSensor2
.gyroDev
.scale
));
501 DEBUG_SET(DEBUG_DUAL_GYRO_SCALED
, 3, lrintf(gyro
.gyroSensor2
.gyroDev
.gyroADC
[Y
] * gyro
.gyroSensor2
.gyroDev
.scale
));
504 case GYRO_CONFIG_USE_GYRO_BOTH
:
505 DEBUG_SET(DEBUG_DUAL_GYRO_RAW
, 0, gyro
.gyroSensor1
.gyroDev
.gyroADCRaw
[X
]);
506 DEBUG_SET(DEBUG_DUAL_GYRO_RAW
, 1, gyro
.gyroSensor1
.gyroDev
.gyroADCRaw
[Y
]);
507 DEBUG_SET(DEBUG_DUAL_GYRO_RAW
, 2, gyro
.gyroSensor2
.gyroDev
.gyroADCRaw
[X
]);
508 DEBUG_SET(DEBUG_DUAL_GYRO_RAW
, 3, gyro
.gyroSensor2
.gyroDev
.gyroADCRaw
[Y
]);
509 DEBUG_SET(DEBUG_DUAL_GYRO_SCALED
, 0, lrintf(gyro
.gyroSensor1
.gyroDev
.gyroADC
[X
] * gyro
.gyroSensor1
.gyroDev
.scale
));
510 DEBUG_SET(DEBUG_DUAL_GYRO_SCALED
, 1, lrintf(gyro
.gyroSensor1
.gyroDev
.gyroADC
[Y
] * gyro
.gyroSensor1
.gyroDev
.scale
));
511 DEBUG_SET(DEBUG_DUAL_GYRO_SCALED
, 2, lrintf(gyro
.gyroSensor2
.gyroDev
.gyroADC
[X
] * gyro
.gyroSensor2
.gyroDev
.scale
));
512 DEBUG_SET(DEBUG_DUAL_GYRO_SCALED
, 3, lrintf(gyro
.gyroSensor2
.gyroDev
.gyroADC
[Y
] * gyro
.gyroSensor2
.gyroDev
.scale
));
513 DEBUG_SET(DEBUG_DUAL_GYRO_DIFF
, 0, lrintf((gyro
.gyroSensor1
.gyroDev
.gyroADC
[X
] * gyro
.gyroSensor1
.gyroDev
.scale
) - (gyro
.gyroSensor2
.gyroDev
.gyroADC
[X
] * gyro
.gyroSensor2
.gyroDev
.scale
)));
514 DEBUG_SET(DEBUG_DUAL_GYRO_DIFF
, 1, lrintf((gyro
.gyroSensor1
.gyroDev
.gyroADC
[Y
] * gyro
.gyroSensor1
.gyroDev
.scale
) - (gyro
.gyroSensor2
.gyroDev
.gyroADC
[Y
] * gyro
.gyroSensor2
.gyroDev
.scale
)));
515 DEBUG_SET(DEBUG_DUAL_GYRO_DIFF
, 2, lrintf((gyro
.gyroSensor1
.gyroDev
.gyroADC
[Z
] * gyro
.gyroSensor1
.gyroDev
.scale
) - (gyro
.gyroSensor2
.gyroDev
.gyroADC
[Z
] * gyro
.gyroSensor2
.gyroDev
.scale
)));
521 #ifdef USE_GYRO_OVERFLOW_CHECK
522 if (gyroConfig()->checkOverflow
&& !gyro
.gyroHasOverflowProtection
) {
523 checkForOverflow(currentTimeUs
);
527 #ifdef USE_YAW_SPIN_RECOVERY
528 if (yawSpinRecoveryEnabled
) {
529 checkForYawSpin(currentTimeUs
);
533 if (!overflowDetected
) {
534 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
535 // integrate using trapezium rule to avoid bias
536 accumulatedMeasurements
[axis
] += 0.5f
* (gyroPrevious
[axis
] + gyro
.gyroADCf
[axis
]) * gyro
.targetLooptime
;
537 gyroPrevious
[axis
] = gyro
.gyroADCf
[axis
];
539 accumulatedMeasurementCount
++;
542 #if !defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY)
543 UNUSED(currentTimeUs
);
547 bool gyroGetAccumulationAverage(float *accumulationAverage
)
549 if (accumulatedMeasurementCount
) {
550 // If we have gyro data accumulated, calculate average rate that will yield the same rotation
551 const timeUs_t accumulatedMeasurementTimeUs
= accumulatedMeasurementCount
* gyro
.targetLooptime
;
552 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
553 accumulationAverage
[axis
] = accumulatedMeasurements
[axis
] / accumulatedMeasurementTimeUs
;
554 accumulatedMeasurements
[axis
] = 0.0f
;
556 accumulatedMeasurementCount
= 0;
559 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
560 accumulationAverage
[axis
] = 0.0f
;
566 int16_t gyroReadSensorTemperature(gyroSensor_t gyroSensor
)
568 if (gyroSensor
.gyroDev
.temperatureFn
) {
569 gyroSensor
.gyroDev
.temperatureFn(&gyroSensor
.gyroDev
, &gyroSensor
.gyroDev
.temperature
);
571 return gyroSensor
.gyroDev
.temperature
;
574 void gyroReadTemperature(void)
576 switch (gyro
.gyroToUse
) {
577 case GYRO_CONFIG_USE_GYRO_1
:
578 gyroSensorTemperature
= gyroReadSensorTemperature(gyro
.gyroSensor1
);
581 #ifdef USE_MULTI_GYRO
582 case GYRO_CONFIG_USE_GYRO_2
:
583 gyroSensorTemperature
= gyroReadSensorTemperature(gyro
.gyroSensor2
);
586 case GYRO_CONFIG_USE_GYRO_BOTH
:
587 gyroSensorTemperature
= MAX(gyroReadSensorTemperature(gyro
.gyroSensor1
), gyroReadSensorTemperature(gyro
.gyroSensor2
));
589 #endif // USE_MULTI_GYRO
593 int16_t gyroGetTemperature(void)
595 return gyroSensorTemperature
;
598 bool gyroOverflowDetected(void)
600 #ifdef USE_GYRO_OVERFLOW_CHECK
601 return overflowDetected
;
604 #endif // USE_GYRO_OVERFLOW_CHECK
607 #ifdef USE_YAW_SPIN_RECOVERY
608 bool gyroYawSpinDetected(void)
610 return yawSpinDetected
;
612 #endif // USE_YAW_SPIN_RECOVERY
614 uint16_t gyroAbsRateDps(int axis
)
616 return fabsf(gyro
.gyroADCf
[axis
]);
621 float dynThrottle(float throttle
) {
622 return throttle
* (1 - (throttle
* throttle
) / 3.0f
) * 1.5f
;
625 void dynLpfGyroUpdate(float throttle
)
627 if (gyro
.dynLpfFilter
!= DYN_LPF_NONE
) {
628 const unsigned int cutoffFreq
= fmax(dynThrottle(throttle
) * gyro
.dynLpfMax
, gyro
.dynLpfMin
);
630 if (gyro
.dynLpfFilter
== DYN_LPF_PT1
) {
631 DEBUG_SET(DEBUG_DYN_LPF
, 2, cutoffFreq
);
632 const float gyroDt
= gyro
.targetLooptime
* 1e-6f
;
633 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
634 pt1FilterUpdateCutoff(&gyro
.lowpassFilter
[axis
].pt1FilterState
, pt1FilterGain(cutoffFreq
, gyroDt
));
636 } else if (gyro
.dynLpfFilter
== DYN_LPF_BIQUAD
) {
637 DEBUG_SET(DEBUG_DYN_LPF
, 2, cutoffFreq
);
638 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
639 biquadFilterUpdateLPF(&gyro
.lowpassFilter
[axis
].biquadFilterState
, cutoffFreq
, gyro
.targetLooptime
);
646 #ifdef USE_YAW_SPIN_RECOVERY
647 void initYawSpinRecovery(int maxYawRate
)
652 switch (gyroConfig()->yaw_spin_recovery
) {
653 case YAW_SPIN_RECOVERY_ON
:
655 threshold
= gyroConfig()->yaw_spin_threshold
;
657 case YAW_SPIN_RECOVERY_AUTO
:
659 const int overshootAllowance
= MAX(maxYawRate
/ 4, 200); // Allow a 25% or minimum 200dps overshoot tolerance
660 threshold
= constrain(maxYawRate
+ overshootAllowance
, YAW_SPIN_RECOVERY_THRESHOLD_MIN
, YAW_SPIN_RECOVERY_THRESHOLD_MAX
);
662 case YAW_SPIN_RECOVERY_OFF
:
665 threshold
= YAW_SPIN_RECOVERY_THRESHOLD_MAX
;
669 yawSpinRecoveryEnabled
= enabledFlag
;
670 yawSpinRecoveryThreshold
= threshold
;