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

Dynamic notch refactoring

Browse source 
+ added new parameter group for Dyn Notch
This commit is contained in:
KarateBrot 2021-08-27 08:42:38 +02:00 • committed by Michael Keller
parent 4c9eb210a9
commit 8649f1003f
19 changed files with 297 additions and 240 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -63,8 +63,8 @@
#include "fc/runtime_config.h"
#ifdef USE_GYRO_DATA_ANALYSE
#include "flight/gyroanalyse.h"
#ifdef USE_DYN_NOTCH_FILTER
#include "flight/dyn_notch_filter.h"
#endif
#include "pg/gyrodev.h"
@ -72,11 +72,6 @@
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#ifdef USE_GYRO_DATA_ANALYSE
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
#endif
#ifdef USE_MULTI_GYRO
#define ACTIVE_GYRO ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2) ? &gyro.gyroSensor2 : &gyro.gyroSensor1)
#else
@ -129,25 +124,6 @@ static void gyroInitFilterNotch2(uint16_t notchHz, uint16_t notchCutoffHz)
}
}
#ifdef USE_GYRO_DATA_ANALYSE
static void gyroInitFilterDynamicNotch()
{
gyro.notchFilterDynApplyFn = nullFilterApply;
if (isDynamicFilterActive()) {
gyro.notchFilterDynApplyFn = (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 (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, 1.0f);
}
}
}
}
#endif
static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_t looptime)
{
filterApplyFnPtr *lowpassFilterApplyFn;
@ -279,14 +255,11 @@ void gyroInitFilters(void)
gyroInitFilterNotch1(gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
gyroInitFilterNotch2(gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
#ifdef USE_GYRO_DATA_ANALYSE
gyroInitFilterDynamicNotch();
#endif
#ifdef USE_DYN_LPF
dynLpfFilterInit();
#endif
#ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyseInit(&gyro.gyroAnalyseState, gyro.targetLooptime);
#ifdef USE_DYN_NOTCH_FILTER
dynNotchInit(dynNotchConfig(), gyro.targetLooptime);
#endif
}