Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 22:05:17 +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

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);
}
}
}
}