Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
Code Issues Wiki Activity

Gyro filtering restructure

Browse source 
Refactor the gyro filtering to move the filters from the individual gyro sensor and make them part of the logical gyro object. For dual-gyro mode the sensors are averaged BEFORE and then filtering is applied. This differs from the current structure where filtering is applied individually to each sensor and the results were averaged at the end.

Significantly reduces filtering processing load and enables RPM Filtering for dual-gyro mode.
This commit is contained in:
Bruce Luckcuck 2019-06-05 10:53:22 -04:00
parent e38d460acf
commit 86f9960987
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

409
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) { #define GYRO_FILTER_FUNCTION_NAME filterGyro
filterGyro(gyroSensor); #define GYRO_FILTER_DEBUG_SET(mode, index, value) { UNUSED(mode); UNUSED(index); UNUSED(value); }
} else { #include "gyro_filter_impl.c"
filterGyroDebug(gyroSensor); #undef GYRO_FILTER_FUNCTION_NAME
} #undef GYRO_FILTER_DEBUG_SET
#ifdef USE_GYRO_OVERFLOW_CHECK #define GYRO_FILTER_FUNCTION_NAME filterGyroDebug
if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) { #define GYRO_FILTER_DEBUG_SET DEBUG_SET
checkForOverflow(gyroSensor, currentTimeUs); #include "gyro_filter_impl.c"
} #undef GYRO_FILTER_FUNCTION_NAME
#endif #undef GYRO_FILTER_DEBUG_SET
#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
}
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;
} }
} }