Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 17:55:30 +03:00
Code Issues Wiki Activity

Merge pull request #8778 from etracer65/gyro_filter_restructure

Browse source 
Gyro filtering restructure
This commit is contained in:
Michael Keller 2019-09-01 15:41:07 +12:00 • committed by GitHub
commit e069e8e0e6
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
8 changed files with 242 additions and 277 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/build/debug.c
View file

@ -54,9 +54,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"RX_FRSKY_SPI", "RX_FRSKY_SPI",
"RX_SFHSS_SPI", "RX_SFHSS_SPI",
"GYRO_RAW", "GYRO_RAW",
"DUAL_GYRO",
"DUAL_GYRO_RAW", "DUAL_GYRO_RAW",
"DUAL_GYRO_COMBINE",
"DUAL_GYRO_DIFF", "DUAL_GYRO_DIFF",
"MAX7456_SIGNAL", "MAX7456_SIGNAL",
"MAX7456_SPICLOCK", "MAX7456_SPICLOCK",

2
src/main/build/debug.h
View file

@ -70,9 +70,7 @@ typedef enum {
DEBUG_RX_FRSKY_SPI, DEBUG_RX_FRSKY_SPI,
DEBUG_RX_SFHSS_SPI, DEBUG_RX_SFHSS_SPI,
DEBUG_GYRO_RAW, DEBUG_GYRO_RAW,
DEBUG_DUAL_GYRO,
DEBUG_DUAL_GYRO_RAW, DEBUG_DUAL_GYRO_RAW,
DEBUG_DUAL_GYRO_COMBINE,
DEBUG_DUAL_GYRO_DIFF, DEBUG_DUAL_GYRO_DIFF,
DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SIGNAL,
DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK,

3
src/main/drivers/accgyro/accgyro.h
View file

@ -90,9 +90,8 @@ typedef struct gyroDev_s {
float scale; // scalefactor float scale; // scalefactor
float gyroZero[XYZ_AXIS_COUNT]; float gyroZero[XYZ_AXIS_COUNT];
float gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment float gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment
float gyroADCf[XYZ_AXIS_COUNT];
int32_t gyroADCRawPrevious[XYZ_AXIS_COUNT]; int32_t gyroADCRawPrevious[XYZ_AXIS_COUNT];
int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int16_t gyroADCRaw[XYZ_AXIS_COUNT]; // raw data from sensor
int16_t temperature; int16_t temperature;
mpuDetectionResult_t mpuDetectionResult; mpuDetectionResult_t mpuDetectionResult;
sensor_align_e gyroAlign; sensor_align_e gyroAlign;

8
src/main/fc/config.c
View file

@ -509,14 +509,6 @@ static void validateAndFixConfig(void)
#endif // USE_DSHOT_TELEMETRY #endif // USE_DSHOT_TELEMETRY
#endif // USE_DSHOT #endif // USE_DSHOT
// Temporary workaround until RPM Filter supports dual-gyro using both sensors
// Once support is added remove this block
#if defined(USE_MULTI_GYRO) && defined(USE_RPM_FILTER)
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH && isRpmFilterEnabled()) {
gyroConfigMutable()->gyro_to_use = GYRO_CONFIG_USE_GYRO_1;
}
#endif
#if defined(USE_OSD) #if defined(USE_OSD)
for (int i = 0; i < OSD_TIMER_COUNT; i++) { for (int i = 0; i < OSD_TIMER_COUNT; i++) {
const uint16_t t = osdConfig()->timers[i]; const uint16_t t = osdConfig()->timers[i];

1
src/main/flight/gyroanalyse.h
View file

@ -24,7 +24,6 @@
#include "common/filter.h" #include "common/filter.h"
#include "sensors/gyro.h"
// max for F3 targets // max for F3 targets
#define FFT_WINDOW_SIZE 32 #define FFT_WINDOW_SIZE 32

411
src/main/sensors/gyro.c
View file

@ -89,6 +89,9 @@ static FAST_RAM_ZERO_INIT uint8_t gyroDebugMode;
static FAST_RAM_ZERO_INIT uint8_t gyroToUse; static FAST_RAM_ZERO_INIT uint8_t gyroToUse;
static FAST_RAM_ZERO_INIT bool overflowDetected; static FAST_RAM_ZERO_INIT bool overflowDetected;
#ifdef USE_GYRO_OVERFLOW_CHECK
static FAST_RAM_ZERO_INIT timeUs_t overflowTimeUs;
#endif
#ifdef USE_GYRO_OVERFLOW_CHECK #ifdef USE_GYRO_OVERFLOW_CHECK
static FAST_RAM_ZERO_INIT uint8_t overflowAxisMask; static FAST_RAM_ZERO_INIT uint8_t overflowAxisMask;
@ -96,6 +99,7 @@ static FAST_RAM_ZERO_INIT uint8_t overflowAxisMask;
#ifdef USE_YAW_SPIN_RECOVERY #ifdef USE_YAW_SPIN_RECOVERY
static FAST_RAM_ZERO_INIT bool yawSpinDetected; static FAST_RAM_ZERO_INIT bool yawSpinDetected;
static FAST_RAM_ZERO_INIT timeUs_t yawSpinTimeUs;
#endif #endif
static FAST_RAM_ZERO_INIT float accumulatedMeasurements[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float accumulatedMeasurements[XYZ_AXIS_COUNT];
@ -107,6 +111,7 @@ static FAST_RAM_ZERO_INIT int16_t gyroSensorTemperature;
static bool gyroHasOverflowProtection = true; static bool gyroHasOverflowProtection = true;
static FAST_RAM_ZERO_INIT bool useDualGyroDebugging; static FAST_RAM_ZERO_INIT bool useDualGyroDebugging;
static FAST_RAM_ZERO_INIT flight_dynamics_index_t gyroDebugAxis;
typedef struct gyroCalibration_s { typedef struct gyroCalibration_s {
float sum[XYZ_AXIS_COUNT]; float sum[XYZ_AXIS_COUNT];
@ -116,50 +121,9 @@ typedef struct gyroCalibration_s {
bool firstArmingCalibrationWasStarted = false; bool firstArmingCalibrationWasStarted = false;
typedef union gyroLowpassFilter_u {
pt1Filter_t pt1FilterState;
biquadFilter_t biquadFilterState;
} gyroLowpassFilter_t;
typedef struct gyroSensor_s { typedef struct gyroSensor_s {
gyroDev_t gyroDev; gyroDev_t gyroDev;
gyroCalibration_t calibration; gyroCalibration_t calibration;
// lowpass gyro soft filter
filterApplyFnPtr lowpassFilterApplyFn;
gyroLowpassFilter_t lowpassFilter[XYZ_AXIS_COUNT];
// lowpass2 gyro soft filter
filterApplyFnPtr lowpass2FilterApplyFn;
gyroLowpassFilter_t lowpass2Filter[XYZ_AXIS_COUNT];
// notch filters
filterApplyFnPtr notchFilter1ApplyFn;
biquadFilter_t notchFilter1[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilter2ApplyFn;
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilterDynApplyFn;
filterApplyFnPtr notchFilterDynApplyFn2;
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT];
// overflow and recovery
timeUs_t overflowTimeUs;
bool overflowDetected;
#ifdef USE_YAW_SPIN_RECOVERY
timeUs_t yawSpinTimeUs;
bool yawSpinDetected;
#endif // USE_YAW_SPIN_RECOVERY
#ifdef USE_GYRO_DATA_ANALYSE
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
gyroAnalyseState_t gyroAnalyseState;
#endif
flight_dynamics_index_t gyroDebugAxis;
} gyroSensor_t; } gyroSensor_t;
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1; STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1;
@ -175,7 +139,7 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev;
#endif #endif
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz); static void gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz);
#define DEBUG_GYRO_CALIBRATION 3 #define DEBUG_GYRO_CALIBRATION 3
@ -197,6 +161,11 @@ PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 7);
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
#endif #endif
#ifdef USE_GYRO_DATA_ANALYSE
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
#endif
void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig) void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
{ {
gyroConfig->gyroCalibrationDuration = 125; // 1.25 seconds gyroConfig->gyroCalibrationDuration = 125; // 1.25 seconds
@ -436,7 +405,6 @@ static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t
static void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config) static void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
{ {
gyroSensor->gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis;
gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr; gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
gyroSensor->gyroDev.gyroAlign = config->alignment; gyroSensor->gyroDev.gyroAlign = config->alignment;
buildRotationMatrixFromAlignment(&config->customAlignment, &gyroSensor->gyroDev.rotationMatrix); buildRotationMatrixFromAlignment(&config->customAlignment, &gyroSensor->gyroDev.rotationMatrix);
@ -479,10 +447,6 @@ static void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *c
gyroInitSensorFilters(gyroSensor); gyroInitSensorFilters(gyroSensor);
#ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyseStateInit(&gyroSensor->gyroAnalyseState, gyro.targetLooptime);
#endif
} }
void gyroPreInit(void) void gyroPreInit(void)
@ -517,8 +481,6 @@ bool gyroInit(void)
case DEBUG_DYN_LPF: case DEBUG_DYN_LPF:
gyroDebugMode = debugMode; gyroDebugMode = debugMode;
break; break;
case DEBUG_DUAL_GYRO:
case DEBUG_DUAL_GYRO_COMBINE:
case DEBUG_DUAL_GYRO_DIFF: case DEBUG_DUAL_GYRO_DIFF:
case DEBUG_DUAL_GYRO_RAW: case DEBUG_DUAL_GYRO_RAW:
case DEBUG_DUAL_GYRO_SCALED: case DEBUG_DUAL_GYRO_SCALED:
@ -530,6 +492,7 @@ bool gyroInit(void)
gyroDetectionFlags = NO_GYROS_DETECTED; gyroDetectionFlags = NO_GYROS_DETECTED;
gyroToUse = gyroConfig()->gyro_to_use; gyroToUse = gyroConfig()->gyro_to_use;
gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis;
if (gyroDetectSensor(&gyroSensor1, gyroDeviceConfig(0))) { if (gyroDetectSensor(&gyroSensor1, gyroDeviceConfig(0))) {
gyroDetectionFlags |= DETECTED_GYRO_1; gyroDetectionFlags |= DETECTED_GYRO_1;
@ -580,6 +543,20 @@ bool gyroInit(void)
detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor1.gyroDev.gyroHardware; detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor1.gyroDev.gyroHardware;
} }
// Copy the sensor's scale to the high-level gyro object. If running in "BOTH" mode
// then logic above requires both sensors to be the same so we'll use sensor1's scale.
// This will need to be revised if we ever allow different sensor types to be used simultaneously.
// Likewise determine the appropriate raw data for use in DEBUG_GYRO_RAW
gyro.scale = gyroSensor1.gyroDev.scale;
gyro.rawSensorDev = &gyroSensor1.gyroDev;
#if defined(USE_MULTI_GYRO)
if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
gyro.scale = gyroSensor2.gyroDev.scale;
gyro.rawSensorDev = &gyroSensor2.gyroDev;
}
#endif
gyroInitFilters();
return true; return true;
} }
@ -615,20 +592,20 @@ static void dynLpfFilterInit()
} }
#endif #endif
void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz) void gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz)
{ {
filterApplyFnPtr *lowpassFilterApplyFn; filterApplyFnPtr *lowpassFilterApplyFn;
gyroLowpassFilter_t *lowpassFilter = NULL; gyroLowpassFilter_t *lowpassFilter = NULL;
switch (slot) { switch (slot) {
case FILTER_LOWPASS: case FILTER_LOWPASS:
lowpassFilterApplyFn = &gyroSensor->lowpassFilterApplyFn; lowpassFilterApplyFn = &gyro.lowpassFilterApplyFn;
lowpassFilter = gyroSensor->lowpassFilter; lowpassFilter = gyro.lowpassFilter;
break; break;
case FILTER_LOWPASS2: case FILTER_LOWPASS2:
lowpassFilterApplyFn = &gyroSensor->lowpass2FilterApplyFn; lowpassFilterApplyFn = &gyro.lowpass2FilterApplyFn;
lowpassFilter = gyroSensor->lowpass2Filter; lowpassFilter = gyro.lowpass2Filter;
break; break;
default: default:
@ -692,32 +669,32 @@ void gyroInitSlewLimiter(gyroSensor_t *gyroSensor) {
} }
#endif #endif
static void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz) static void gyroInitFilterNotch1(uint16_t notchHz, uint16_t notchCutoffHz)
{ {
gyroSensor->notchFilter1ApplyFn = nullFilterApply; gyro.notchFilter1ApplyFn = nullFilterApply;
notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz); notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz);
if (notchHz != 0 && notchCutoffHz != 0) { if (notchHz != 0 && notchCutoffHz != 0) {
gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; gyro.notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyroSensor->notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH); biquadFilterInit(&gyro.notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
} }
} }
} }
static void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz) static void gyroInitFilterNotch2(uint16_t notchHz, uint16_t notchCutoffHz)
{ {
gyroSensor->notchFilter2ApplyFn = nullFilterApply; gyro.notchFilter2ApplyFn = nullFilterApply;
notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz); notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz);
if (notchHz != 0 && notchCutoffHz != 0) { if (notchHz != 0 && notchCutoffHz != 0) {
gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; gyro.notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyroSensor->notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH); biquadFilterInit(&gyro.notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
} }
} }
} }
@ -728,20 +705,20 @@ static bool isDynamicFilterActive(void)
return featureIsEnabled(FEATURE_DYNAMIC_FILTER); return featureIsEnabled(FEATURE_DYNAMIC_FILTER);
} }
static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) static void gyroInitFilterDynamicNotch()
{ {
gyroSensor->notchFilterDynApplyFn = nullFilterApply; gyro.notchFilterDynApplyFn = nullFilterApply;
gyroSensor->notchFilterDynApplyFn2 = nullFilterApply; gyro.notchFilterDynApplyFn2 = nullFilterApply;
if (isDynamicFilterActive()) { if (isDynamicFilterActive()) {
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2 gyro.notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
if(gyroConfig()->dyn_notch_width_percent != 0) { if(gyroConfig()->dyn_notch_width_percent != 0) {
gyroSensor->notchFilterDynApplyFn2 = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2 gyro.notchFilterDynApplyFn2 = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
} }
const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyroSensor->notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH); biquadFilterInit(&gyro.notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
biquadFilterInit(&gyroSensor->notchFilterDyn2[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH); biquadFilterInit(&gyro.notchFilterDyn2[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
} }
} }
} }
@ -751,8 +728,13 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
{ {
#if defined(USE_GYRO_SLEW_LIMITER) #if defined(USE_GYRO_SLEW_LIMITER)
gyroInitSlewLimiter(gyroSensor); gyroInitSlewLimiter(gyroSensor);
#else
UNUSED(gyroSensor);
#endif #endif
}
void gyroInitFilters(void)
{
uint16_t gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz; uint16_t gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz;
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
@ -762,34 +744,27 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
#endif #endif
gyroInitLowpassFilterLpf( gyroInitLowpassFilterLpf(
gyroSensor,
FILTER_LOWPASS, FILTER_LOWPASS,
gyroConfig()->gyro_lowpass_type, gyroConfig()->gyro_lowpass_type,
gyro_lowpass_hz gyro_lowpass_hz
); );
gyroInitLowpassFilterLpf( gyroInitLowpassFilterLpf(
gyroSensor,
FILTER_LOWPASS2, FILTER_LOWPASS2,
gyroConfig()->gyro_lowpass2_type, gyroConfig()->gyro_lowpass2_type,
gyroConfig()->gyro_lowpass2_hz gyroConfig()->gyro_lowpass2_hz
); );
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); gyroInitFilterNotch1(gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); gyroInitFilterNotch2(gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
gyroInitFilterDynamicNotch(gyroSensor); gyroInitFilterDynamicNotch();
#endif #endif
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
dynLpfFilterInit(); dynLpfFilterInit();
#endif #endif
} #ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyseStateInit(&gyro.gyroAnalyseState, gyro.targetLooptime);
void gyroInitFilters(void)
{
gyroInitSensorFilters(&gyroSensor1);
#ifdef USE_MULTI_GYRO
gyroInitSensorFilters(&gyroSensor2);
#endif #endif
} }
@ -927,50 +902,61 @@ FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
#endif #endif
#ifdef USE_GYRO_OVERFLOW_CHECK #ifdef USE_GYRO_OVERFLOW_CHECK
static FAST_CODE_NOINLINE void handleOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) static FAST_CODE_NOINLINE void handleOverflow(timeUs_t currentTimeUs)
{ {
const float gyroOverflowResetRate = GYRO_OVERFLOW_RESET_THRESHOLD * gyroSensor->gyroDev.scale; // This will need to be revised if we ever allow different sensor types to be
if ((abs(gyroSensor->gyroDev.gyroADCf[X]) < gyroOverflowResetRate) // used simultaneously. In that case the scale might be different between sensors.
&& (abs(gyroSensor->gyroDev.gyroADCf[Y]) < gyroOverflowResetRate) // It's complicated by the fact that we're using filtered gyro data here which is
&& (abs(gyroSensor->gyroDev.gyroADCf[Z]) < gyroOverflowResetRate)) { // after both sensors are scaled and averaged.
const float gyroOverflowResetRate = GYRO_OVERFLOW_RESET_THRESHOLD * gyro.scale;
if ((abs(gyro.gyroADCf[X]) < gyroOverflowResetRate)
&& (abs(gyro.gyroADCf[Y]) < gyroOverflowResetRate)
&& (abs(gyro.gyroADCf[Z]) < gyroOverflowResetRate)) {
// if we have 50ms of consecutive OK gyro vales, then assume yaw readings are OK again and reset overflowDetected // if we have 50ms of consecutive OK gyro vales, then assume yaw readings are OK again and reset overflowDetected
// reset requires good OK values on all axes // reset requires good OK values on all axes
if (cmpTimeUs(currentTimeUs, gyroSensor->overflowTimeUs) > 50000) { if (cmpTimeUs(currentTimeUs, overflowTimeUs) > 50000) {
gyroSensor->overflowDetected = false; overflowDetected = false;
} }
} else { } else {
// not a consecutive OK value, so reset the overflow time // not a consecutive OK value, so reset the overflow time
gyroSensor->overflowTimeUs = currentTimeUs; overflowTimeUs = currentTimeUs;
} }
} }
static FAST_CODE void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) static FAST_CODE void checkForOverflow(timeUs_t currentTimeUs)
{ {
// check for overflow to handle Yaw Spin To The Moon (YSTTM) // check for overflow to handle Yaw Spin To The Moon (YSTTM)
// ICM gyros are specified to +/- 2000 deg/sec, in a crash they can go out of spec. // ICM gyros are specified to +/- 2000 deg/sec, in a crash they can go out of spec.
// This can cause an overflow and sign reversal in the output. // This can cause an overflow and sign reversal in the output.
// Overflow and sign reversal seems to result in a gyro value of +1996 or -1996. // Overflow and sign reversal seems to result in a gyro value of +1996 or -1996.
if (gyroSensor->overflowDetected) { if (overflowDetected) {
handleOverflow(gyroSensor, currentTimeUs); handleOverflow(currentTimeUs);
} else { } else {
#ifndef SIMULATOR_BUILD #ifndef SIMULATOR_BUILD
// check for overflow in the axes set in overflowAxisMask // check for overflow in the axes set in overflowAxisMask
gyroOverflow_e overflowCheck = GYRO_OVERFLOW_NONE; gyroOverflow_e overflowCheck = GYRO_OVERFLOW_NONE;
const float gyroOverflowTriggerRate = GYRO_OVERFLOW_TRIGGER_THRESHOLD * gyroSensor->gyroDev.scale;
if (abs(gyroSensor->gyroDev.gyroADCf[X]) > gyroOverflowTriggerRate) { // This will need to be revised if we ever allow different sensor types to be
// used simultaneously. In that case the scale might be different between sensors.
// It's complicated by the fact that we're using filtered gyro data here which is
// after both sensors are scaled and averaged.
const float gyroOverflowTriggerRate = GYRO_OVERFLOW_TRIGGER_THRESHOLD * gyro.scale;
if (abs(gyro.gyroADCf[X]) > gyroOverflowTriggerRate) {
overflowCheck |= GYRO_OVERFLOW_X; overflowCheck |= GYRO_OVERFLOW_X;
} }
if (abs(gyroSensor->gyroDev.gyroADCf[Y]) > gyroOverflowTriggerRate) { if (abs(gyro.gyroADCf[Y]) > gyroOverflowTriggerRate) {
overflowCheck |= GYRO_OVERFLOW_Y; overflowCheck |= GYRO_OVERFLOW_Y;
} }
if (abs(gyroSensor->gyroDev.gyroADCf[Z]) > gyroOverflowTriggerRate) { if (abs(gyro.gyroADCf[Z]) > gyroOverflowTriggerRate) {
overflowCheck |= GYRO_OVERFLOW_Z; overflowCheck |= GYRO_OVERFLOW_Z;
} }
if (overflowCheck & overflowAxisMask) { if (overflowCheck & overflowAxisMask) {
gyroSensor->overflowDetected = true; overflowDetected = true;
gyroSensor->overflowTimeUs = currentTimeUs; overflowTimeUs = currentTimeUs;
#ifdef USE_YAW_SPIN_RECOVERY #ifdef USE_YAW_SPIN_RECOVERY
gyroSensor->yawSpinDetected = false; yawSpinDetected = false;
#endif // USE_YAW_SPIN_RECOVERY #endif // USE_YAW_SPIN_RECOVERY
} }
#endif // SIMULATOR_BUILD #endif // SIMULATOR_BUILD
@ -979,57 +965,45 @@ static FAST_CODE void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t curren
#endif // USE_GYRO_OVERFLOW_CHECK #endif // USE_GYRO_OVERFLOW_CHECK
#ifdef USE_YAW_SPIN_RECOVERY #ifdef USE_YAW_SPIN_RECOVERY
static FAST_CODE_NOINLINE void handleYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) static FAST_CODE_NOINLINE void handleYawSpin(timeUs_t currentTimeUs)
{ {
const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f; const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f;
if (abs(gyroSensor->gyroDev.gyroADCf[Z]) < yawSpinResetRate) { if (abs(gyro.gyroADCf[Z]) < yawSpinResetRate) {
// testing whether 20ms of consecutive OK gyro yaw values is enough // testing whether 20ms of consecutive OK gyro yaw values is enough
if (cmpTimeUs(currentTimeUs, gyroSensor->yawSpinTimeUs) > 20000) { if (cmpTimeUs(currentTimeUs, yawSpinTimeUs) > 20000) {
gyroSensor->yawSpinDetected = false; yawSpinDetected = false;
} }
} else { } else {
// reset the yaw spin time // reset the yaw spin time
gyroSensor->yawSpinTimeUs = currentTimeUs; yawSpinTimeUs = currentTimeUs;
} }
} }
static FAST_CODE void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) static FAST_CODE void checkForYawSpin(timeUs_t currentTimeUs)
{ {
// if not in overflow mode, handle yaw spins above threshold // if not in overflow mode, handle yaw spins above threshold
#ifdef USE_GYRO_OVERFLOW_CHECK #ifdef USE_GYRO_OVERFLOW_CHECK
if (gyroSensor->overflowDetected) { if (overflowDetected) {
gyroSensor->yawSpinDetected = false; yawSpinDetected = false;
return; return;
} }
#endif // USE_GYRO_OVERFLOW_CHECK #endif // USE_GYRO_OVERFLOW_CHECK
if (gyroSensor->yawSpinDetected) { if (yawSpinDetected) {
handleYawSpin(gyroSensor, currentTimeUs); handleYawSpin(currentTimeUs);
} else { } else {
#ifndef SIMULATOR_BUILD #ifndef SIMULATOR_BUILD
// check for spin on yaw axis only // check for spin on yaw axis only
if (abs(gyroSensor->gyroDev.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) { if (abs(gyro.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) {
gyroSensor->yawSpinDetected = true; yawSpinDetected = true;
gyroSensor->yawSpinTimeUs = currentTimeUs; yawSpinTimeUs = currentTimeUs;
} }
#endif // SIMULATOR_BUILD #endif // SIMULATOR_BUILD
} }
} }
#endif // USE_YAW_SPIN_RECOVERY #endif // USE_YAW_SPIN_RECOVERY
#define GYRO_FILTER_FUNCTION_NAME filterGyro static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor)
#define GYRO_FILTER_DEBUG_SET(mode, index, value) { UNUSED(mode); UNUSED(index); UNUSED(value); }
#include "gyro_filter_impl.c"
#undef GYRO_FILTER_FUNCTION_NAME
#undef GYRO_FILTER_DEBUG_SET
#define GYRO_FILTER_FUNCTION_NAME filterGyroDebug
#define GYRO_FILTER_DEBUG_SET DEBUG_SET
#include "gyro_filter_impl.c"
#undef GYRO_FILTER_FUNCTION_NAME
#undef GYRO_FILTER_DEBUG_SET
static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{ {
if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
return; return;
@ -1056,129 +1030,111 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
} }
} else { } else {
performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold); performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold);
// still calibrating, so no need to further process gyro data
return;
} }
if (gyroDebugMode == DEBUG_NONE) {
filterGyro(gyroSensor);
} else {
filterGyroDebug(gyroSensor);
}
#ifdef USE_GYRO_OVERFLOW_CHECK
if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) {
checkForOverflow(gyroSensor, currentTimeUs);
}
#endif
#ifdef USE_YAW_SPIN_RECOVERY
if (gyroConfig()->yaw_spin_recovery) {
checkForYawSpin(gyroSensor, currentTimeUs);
}
#endif
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn, gyroSensor->notchFilterDyn2);
}
#endif
#if (!defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY))
UNUSED(currentTimeUs);
#endif
} }
#define GYRO_FILTER_FUNCTION_NAME filterGyro
#define GYRO_FILTER_DEBUG_SET(mode, index, value) { UNUSED(mode); UNUSED(index); UNUSED(value); }
#include "gyro_filter_impl.c"
#undef GYRO_FILTER_FUNCTION_NAME
#undef GYRO_FILTER_DEBUG_SET
#define GYRO_FILTER_FUNCTION_NAME filterGyroDebug
#define GYRO_FILTER_DEBUG_SET DEBUG_SET
#include "gyro_filter_impl.c"
#undef GYRO_FILTER_FUNCTION_NAME
#undef GYRO_FILTER_DEBUG_SET
FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
{ {
switch (gyroToUse) { switch (gyroToUse) {
case GYRO_CONFIG_USE_GYRO_1: case GYRO_CONFIG_USE_GYRO_1:
gyroUpdateSensor(&gyroSensor1, currentTimeUs); gyroUpdateSensor(&gyroSensor1);
if (isGyroSensorCalibrationComplete(&gyroSensor1)) { if (isGyroSensorCalibrationComplete(&gyroSensor1)) {
gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X]; gyro.gyroADC[X] = gyroSensor1.gyroDev.gyroADC[X] * gyroSensor1.gyroDev.scale;
gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y]; gyro.gyroADC[Y] = gyroSensor1.gyroDev.gyroADC[Y] * gyroSensor1.gyroDev.scale;
gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z]; gyro.gyroADC[Z] = gyroSensor1.gyroDev.gyroADC[Z] * gyroSensor1.gyroDev.scale;
#ifdef USE_GYRO_OVERFLOW_CHECK
overflowDetected = gyroSensor1.overflowDetected;
#endif
#ifdef USE_YAW_SPIN_RECOVERY
yawSpinDetected = gyroSensor1.yawSpinDetected;
#endif
}
if (useDualGyroDebugging) {
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 0, lrintf(gyro.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyroSensor1.gyroDev.gyroADC[X] * gyroSensor1.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyroSensor1.gyroDev.gyroADC[Y] * gyroSensor1.gyroDev.scale));
} }
break; break;
#ifdef USE_MULTI_GYRO #ifdef USE_MULTI_GYRO
case GYRO_CONFIG_USE_GYRO_2: case GYRO_CONFIG_USE_GYRO_2:
gyroUpdateSensor(&gyroSensor2, currentTimeUs); gyroUpdateSensor(&gyroSensor2);
if (isGyroSensorCalibrationComplete(&gyroSensor2)) { if (isGyroSensorCalibrationComplete(&gyroSensor2)) {
gyro.gyroADCf[X] = gyroSensor2.gyroDev.gyroADCf[X]; gyro.gyroADC[X] = gyroSensor2.gyroDev.gyroADC[X] * gyroSensor2.gyroDev.scale;
gyro.gyroADCf[Y] = gyroSensor2.gyroDev.gyroADCf[Y]; gyro.gyroADC[Y] = gyroSensor2.gyroDev.gyroADC[Y] * gyroSensor2.gyroDev.scale;
gyro.gyroADCf[Z] = gyroSensor2.gyroDev.gyroADCf[Z]; gyro.gyroADC[Z] = gyroSensor2.gyroDev.gyroADC[Z] * gyroSensor2.gyroDev.scale;
#ifdef USE_GYRO_OVERFLOW_CHECK
overflowDetected = gyroSensor2.overflowDetected;
#endif
#ifdef USE_YAW_SPIN_RECOVERY
yawSpinDetected = gyroSensor2.yawSpinDetected;
#endif
}
if (useDualGyroDebugging) {
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 3, lrintf(gyro.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyroSensor2.gyroDev.gyroADC[X] * gyroSensor2.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyroSensor2.gyroDev.gyroADC[Y] * gyroSensor2.gyroDev.scale));
} }
break; break;
case GYRO_CONFIG_USE_GYRO_BOTH: case GYRO_CONFIG_USE_GYRO_BOTH:
gyroUpdateSensor(&gyroSensor1, currentTimeUs); gyroUpdateSensor(&gyroSensor1);
gyroUpdateSensor(&gyroSensor2, currentTimeUs); gyroUpdateSensor(&gyroSensor2);
if (isGyroSensorCalibrationComplete(&gyroSensor1) && isGyroSensorCalibrationComplete(&gyroSensor2)) { if (isGyroSensorCalibrationComplete(&gyroSensor1) && isGyroSensorCalibrationComplete(&gyroSensor2)) {
gyro.gyroADCf[X] = (gyroSensor1.gyroDev.gyroADCf[X] + gyroSensor2.gyroDev.gyroADCf[X]) / 2.0f; gyro.gyroADC[X] = ((gyroSensor1.gyroDev.gyroADC[X] * gyroSensor1.gyroDev.scale) + (gyroSensor2.gyroDev.gyroADC[X] * gyroSensor2.gyroDev.scale)) / 2.0f;
gyro.gyroADCf[Y] = (gyroSensor1.gyroDev.gyroADCf[Y] + gyroSensor2.gyroDev.gyroADCf[Y]) / 2.0f; gyro.gyroADC[Y] = ((gyroSensor1.gyroDev.gyroADC[Y] * gyroSensor1.gyroDev.scale) + (gyroSensor2.gyroDev.gyroADC[Y] * gyroSensor2.gyroDev.scale)) / 2.0f;
gyro.gyroADCf[Z] = (gyroSensor1.gyroDev.gyroADCf[Z] + gyroSensor2.gyroDev.gyroADCf[Z]) / 2.0f; gyro.gyroADC[Z] = ((gyroSensor1.gyroDev.gyroADC[Z] * gyroSensor1.gyroDev.scale) + (gyroSensor2.gyroDev.gyroADC[Z] * gyroSensor2.gyroDev.scale)) / 2.0f;
#ifdef USE_GYRO_OVERFLOW_CHECK }
overflowDetected = gyroSensor1.overflowDetected || gyroSensor2.overflowDetected; break;
#endif
#ifdef USE_YAW_SPIN_RECOVERY
yawSpinDetected = gyroSensor1.yawSpinDetected || gyroSensor2.yawSpinDetected;
#endif #endif
} }
if (gyroDebugMode == DEBUG_NONE) {
filterGyro();
} else {
filterGyroDebug();
}
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
gyroDataAnalyse(&gyro.gyroAnalyseState, gyro.notchFilterDyn, gyro.notchFilterDyn2);
}
#endif
if (useDualGyroDebugging) { if (useDualGyroDebugging) {
switch (gyroToUse) {
case GYRO_CONFIG_USE_GYRO_1:
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyroSensor1.gyroDev.gyroADC[X] * gyroSensor1.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyroSensor1.gyroDev.gyroADC[Y] * gyroSensor1.gyroDev.scale));
break;
#ifdef USE_MULTI_GYRO
case GYRO_CONFIG_USE_GYRO_2:
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyroSensor2.gyroDev.gyroADC[X] * gyroSensor2.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyroSensor2.gyroDev.gyroADC[Y] * gyroSensor2.gyroDev.scale));
break;
case GYRO_CONFIG_USE_GYRO_BOTH:
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X] - gyroSensor2.gyroDev.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y] - gyroSensor2.gyroDev.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf(gyroSensor1.gyroDev.gyroADCf[Z] - gyroSensor2.gyroDev.gyroADCf[Z]));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyroSensor1.gyroDev.gyroADC[X] * gyroSensor1.gyroDev.scale)); DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyroSensor1.gyroDev.gyroADC[X] * gyroSensor1.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyroSensor1.gyroDev.gyroADC[Y] * gyroSensor1.gyroDev.scale)); DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyroSensor1.gyroDev.gyroADC[Y] * gyroSensor1.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyroSensor2.gyroDev.gyroADC[X] * gyroSensor2.gyroDev.scale)); DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyroSensor2.gyroDev.gyroADC[X] * gyroSensor2.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyroSensor2.gyroDev.gyroADC[Y] * gyroSensor2.gyroDev.scale)); DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyroSensor2.gyroDev.gyroADC[Y] * gyroSensor2.gyroDev.scale));
} DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0, lrintf((gyroSensor1.gyroDev.gyroADC[X] * gyroSensor1.gyroDev.scale) - (gyroSensor2.gyroDev.gyroADC[X] * gyroSensor2.gyroDev.scale)));
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf((gyroSensor1.gyroDev.gyroADC[Y] * gyroSensor1.gyroDev.scale) - (gyroSensor2.gyroDev.gyroADC[Y] * gyroSensor2.gyroDev.scale)));
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf((gyroSensor1.gyroDev.gyroADC[Z] * gyroSensor1.gyroDev.scale) - (gyroSensor2.gyroDev.gyroADC[Z] * gyroSensor2.gyroDev.scale)));
break; break;
#endif #endif
} }
}
#ifdef USE_GYRO_OVERFLOW_CHECK
if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) {
checkForOverflow(currentTimeUs);
}
#endif
#ifdef USE_YAW_SPIN_RECOVERY
if (gyroConfig()->yaw_spin_recovery) {
checkForYawSpin(currentTimeUs);
}
#endif
if (!overflowDetected) { if (!overflowDetected) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@ -1189,6 +1145,9 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
accumulatedMeasurementCount++; accumulatedMeasurementCount++;
} }
#if !defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY)
UNUSED(currentTimeUs);
#endif
} }
bool gyroGetAccumulationAverage(float *accumulationAverage) bool gyroGetAccumulationAverage(float *accumulationAverage)
@ -1290,30 +1249,12 @@ void dynLpfGyroUpdate(float throttle)
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq); DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
const float gyroDt = gyro.targetLooptime * 1e-6f; const float gyroDt = gyro.targetLooptime * 1e-6f;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
#ifdef USE_MULTI_GYRO pt1FilterUpdateCutoff(&gyro.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
}
if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
pt1FilterUpdateCutoff(&gyroSensor2.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
}
#else
pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
#endif
} }
} else if (dynLpfFilter == DYN_LPF_BIQUAD) { } else if (dynLpfFilter == DYN_LPF_BIQUAD) {
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq); DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
#ifdef USE_MULTI_GYRO biquadFilterUpdateLPF(&gyro.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
}
if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
biquadFilterUpdateLPF(&gyroSensor2.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
}
#else
biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
#endif
} }
} }
} }

40
src/main/sensors/gyro.h
View file

@ -24,16 +24,54 @@
#include "common/filter.h" #include "common/filter.h"
#include "common/time.h" #include "common/time.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/bus.h" #include "drivers/bus.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#ifdef USE_GYRO_DATA_ANALYSE
#include "flight/gyroanalyse.h"
#endif
#include "pg/pg.h" #include "pg/pg.h"
#define FILTER_FREQUENCY_MAX 4000 // maximum frequency for filter cutoffs (nyquist limit of 8K max sampling) #define FILTER_FREQUENCY_MAX 4000 // maximum frequency for filter cutoffs (nyquist limit of 8K max sampling)
typedef union gyroLowpassFilter_u {
pt1Filter_t pt1FilterState;
biquadFilter_t biquadFilterState;
} gyroLowpassFilter_t;
typedef struct gyro_s { typedef struct gyro_s {
uint32_t targetLooptime; uint32_t targetLooptime;
float gyroADCf[XYZ_AXIS_COUNT]; float scale;
float gyroADC[XYZ_AXIS_COUNT]; // aligned, calibrated, scaled, but unfiltered data from the sensor(s)
float gyroADCf[XYZ_AXIS_COUNT]; // filtered gyro data
gyroDev_t *rawSensorDev; // pointer to the sensor providing the raw data for DEBUG_GYRO_RAW
// lowpass gyro soft filter
filterApplyFnPtr lowpassFilterApplyFn;
gyroLowpassFilter_t lowpassFilter[XYZ_AXIS_COUNT];
// lowpass2 gyro soft filter
filterApplyFnPtr lowpass2FilterApplyFn;
gyroLowpassFilter_t lowpass2Filter[XYZ_AXIS_COUNT];
// notch filters
filterApplyFnPtr notchFilter1ApplyFn;
biquadFilter_t notchFilter1[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilter2ApplyFn;
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilterDynApplyFn;
filterApplyFnPtr notchFilterDynApplyFn2;
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT];
#ifdef USE_GYRO_DATA_ANALYSE
gyroAnalyseState_t gyroAnalyseState;
#endif
} gyro_t; } gyro_t;
extern gyro_t gyro; extern gyro_t gyro;

26
src/main/sensors/gyro_filter_impl.c
View file

@ -20,18 +20,18 @@
#include "platform.h" #include "platform.h"
static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
{ {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_RAW, axis, gyroSensor->gyroDev.gyroADCRaw[axis]); GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_RAW, axis, gyro.rawSensorDev->gyroADCRaw[axis]);
// scale gyro output to degrees per second // scale gyro output to degrees per second
float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; float gyroADCf = gyro.gyroADC[axis];
// DEBUG_GYRO_SCALED records the unfiltered, scaled gyro output // DEBUG_GYRO_SCALED records the unfiltered, scaled gyro output
GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroADCf));
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) { if (isDynamicFilterActive()) {
if (axis == gyroSensor->gyroDebugAxis) { if (axis == gyroDebugAxis) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf));
@ -45,27 +45,27 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor)
// apply static notch filters and software lowpass filters // apply static notch filters and software lowpass filters
gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); gyroADCf = gyro.notchFilter1ApplyFn((filter_t *)&gyro.notchFilter1[axis], gyroADCf);
gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); gyroADCf = gyro.notchFilter2ApplyFn((filter_t *)&gyro.notchFilter2[axis], gyroADCf);
gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf); gyroADCf = gyro.lowpassFilterApplyFn((filter_t *)&gyro.lowpassFilter[axis], gyroADCf);
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); gyroADCf = gyro.lowpass2FilterApplyFn((filter_t *)&gyro.lowpass2Filter[axis], gyroADCf);
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) { if (isDynamicFilterActive()) {
if (axis == gyroSensor->gyroDebugAxis) { if (axis == gyroDebugAxis) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 3, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 3, lrintf(gyroADCf));
} }
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf); gyroDataAnalysePush(&gyro.gyroAnalyseState, axis, gyroADCf);
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); gyroADCf = gyro.notchFilterDynApplyFn((filter_t *)&gyro.notchFilterDyn[axis], gyroADCf);
gyroADCf = gyroSensor->notchFilterDynApplyFn2((filter_t *)&gyroSensor->notchFilterDyn2[axis], gyroADCf); gyroADCf = gyro.notchFilterDynApplyFn2((filter_t *)&gyro.notchFilterDyn2[axis], gyroADCf);
} }
#endif #endif
// DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied. // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));
gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf; gyro.gyroADCf[axis] = gyroADCf;
} }
} }