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

Added tracking of multiple dynamic notches per axis and replaced FFT with SDFT

Browse source
This commit is contained in:
KarateBrot 2020-09-22 18:15:06 +02:00
parent 7b46440056
commit d02af7334c
16 changed files with 504 additions and 326 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -98,7 +98,7 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyro.gyroSensor1.gyroDev;
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro)
#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro)
PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 8);
PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 9);
#ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
@ -129,8 +129,8 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->dyn_lpf_gyro_min_hz = DYN_LPF_GYRO_MIN_HZ_DEFAULT;
gyroConfig->dyn_lpf_gyro_max_hz = DYN_LPF_GYRO_MAX_HZ_DEFAULT;
gyroConfig->dyn_notch_max_hz = 600;
gyroConfig->dyn_notch_width_percent = 8;
gyroConfig->dyn_notch_q = 120;
gyroConfig->dyn_notch_count = 1;
gyroConfig->dyn_notch_bandwidth_hz = 45;
gyroConfig->dyn_notch_min_hz = 150;
gyroConfig->gyro_filter_debug_axis = FD_ROLL;
gyroConfig->dyn_lpf_curve_expo = 5;
@ -484,7 +484,7 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
gyroDataAnalyse(&gyro.gyroAnalyseState, gyro.notchFilterDyn, gyro.notchFilterDyn2);
gyroDataAnalyse(&gyro.gyroAnalyseState);
}
#endif

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

@ -107,12 +107,11 @@ typedef struct gyro_s {
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
filterApplyFnPtr notchFilterDynApplyFn;
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
uint8_t notchFilterDynCount;
gyroAnalyseState_t gyroAnalyseState;
#endif
@ -196,8 +195,8 @@ typedef struct gyroConfig_s {
uint16_t dyn_lpf_gyro_max_hz;
uint16_t dyn_notch_max_hz;
uint8_t dyn_notch_width_percent;
uint16_t dyn_notch_q;
uint8_t dyn_notch_count;
uint16_t dyn_notch_bandwidth_hz;
uint16_t dyn_notch_min_hz;
uint8_t gyro_filter_debug_axis;

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

@ -49,16 +49,6 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
// DEBUG_GYRO_SAMPLE(1) Record the post-downsample value for the selected debug axis
GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 1, lrintf(gyroADCf));
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
if (axis == gyro.gyroDebugAxis) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf));
}
}
#endif
#ifdef USE_RPM_FILTER
gyroADCf = rpmFilterGyro(axis, gyroADCf);
#endif
@ -76,14 +66,21 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
if (axis == gyro.gyroDebugAxis) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf));
}
gyroDataAnalysePush(&gyro.gyroAnalyseState, axis, gyroADCf);
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
gyroADCf = gyro.notchFilterDynApplyFn((filter_t*)&gyro.notchFilterDyn[axis][p], gyroADCf);
}
if (axis == gyro.gyroDebugAxis) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 3, lrintf(gyroADCf));
}
gyroDataAnalysePush(&gyro.gyroAnalyseState, axis, gyroADCf);
gyroADCf = gyro.notchFilterDynApplyFn((filter_t *)&gyro.notchFilterDyn[axis], gyroADCf);
gyroADCf = gyro.notchFilterDynApplyFn2((filter_t *)&gyro.notchFilterDyn2[axis], gyroADCf);
}
#endif

13
src/main/sensors/gyro_init.c
View file

@ -133,17 +133,16 @@ static void gyroInitFilterNotch2(uint16_t notchHz, uint16_t notchCutoffHz)
static void gyroInitFilterDynamicNotch()
{
gyro.notchFilterDynApplyFn = nullFilterApply;
gyro.notchFilterDynApplyFn2 = nullFilterApply;
if (isDynamicFilterActive()) {
gyro.notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
if(gyroConfig()->dyn_notch_width_percent != 0) {
gyro.notchFilterDynApplyFn2 = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
}
gyro.notchFilterDynCount = gyroConfig()->dyn_notch_count;
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++) {
biquadFilterInit(&gyro.notchFilterDyn[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);
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
biquadFilterInit(&gyro.notchFilterDyn[axis][p], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
}
}
}
}