rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Cascaded notch filters.

This commit is contained in:
Kenneth Mitchell 2018-10-27 09:16:55 -04:00
parent 3736a2486d
commit 011eae93c6
No known key found for this signature in database
GPG Key ID: E27133AAF586AB21
10 changed files with 128 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/build/debug.c
View File

@ -76,4 +76,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"RX_SIGNAL_LOSS",
"RC_SMOOTHING_RATE",
"ANTI_GRAVITY",
"DYN_LPF",
};

1
src/main/build/debug.h
View File

@ -94,6 +94,7 @@ typedef enum {
DEBUG_RX_SIGNAL_LOSS,
DEBUG_RC_SMOOTHING_RATE,
DEBUG_ANTI_GRAVITY,
DEBUG_DYN_LPF,
DEBUG_COUNT
} debugType_e;

51
src/main/flight/pid.c
View File

@ -167,8 +167,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.abs_control_limit = 90,
.abs_control_error_limit = 20,
.antiGravityMode = ANTI_GRAVITY_SMOOTH,
.dyn_lpf_dterm_max_hz = 200,
.dyn_lpf_dterm_idle = 20,
.dyn_lpf_dterm_max_hz = 250,
.dterm_lowpass_hz = 100, // dual PT1 filtering ON by default
.dterm_lowpass2_hz = 200, // second Dterm LPF ON by default
.dterm_filter_type = FILTER_PT1,
@ -180,8 +179,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
.launchControlAllowTriggerReset = true,
);
#ifdef USE_DYN_LPF
pidProfile->dterm_lowpass_hz = 120;
pidProfile->dterm_lowpass2_hz = 180;
pidProfile->dterm_lowpass_hz = 150;
pidProfile->dterm_lowpass2_hz = 150;
pidProfile->dterm_filter_type = FILTER_BIQUAD;
pidProfile->dterm_filter2_type = FILTER_BIQUAD;
#endif
@ -464,11 +463,9 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
}
#ifdef USE_DYN_LPF
static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE;
static FAST_RAM_ZERO_INIT float dynLpfIdle;
static FAST_RAM_ZERO_INIT float dynLpfIdlePoint;
static FAST_RAM_ZERO_INIT float dynLpfInvIdlePointScaled;
static FAST_RAM uint8_t dynLpfFilter = DYN_LPF_NONE;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
#endif
void pidInitConfig(const pidProfile_t *pidProfile)
@ -547,27 +544,23 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#endif
#ifdef USE_DYN_LPF
if (pidProfile->dyn_lpf_dterm_idle > 0 && pidProfile->dyn_lpf_dterm_max_hz > 0) {
if (pidProfile->dterm_lowpass_hz > 0 ) {
dynLpfMin = pidProfile->dterm_lowpass_hz;
switch (pidProfile->dterm_filter_type) {
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;
break;
case FILTER_BIQUAD:
dynLpfFilter = DYN_LPF_BIQUAD;
break;
default:
dynLpfFilter = DYN_LPF_NONE;
break;
}
if (pidProfile->dterm_lowpass_hz > 0 && pidProfile->dyn_lpf_dterm_max_hz > pidProfile->dterm_lowpass_hz) {
switch (pidProfile->dterm_filter_type) {
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;
break;
case FILTER_BIQUAD:
dynLpfFilter = DYN_LPF_BIQUAD;
break;
default:
dynLpfFilter = DYN_LPF_NONE;
break;
}
} else {
dynLpfFilter = DYN_LPF_NONE;
}
dynLpfIdle = pidProfile->dyn_lpf_dterm_idle / 100.0f;
dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f;
dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (pidProfile->dyn_lpf_dterm_max_hz - dynLpfMin);
dynLpfMin = pidProfile->dterm_lowpass_hz;
dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz;
#endif
#ifdef USE_LAUNCH_CONTROL
@ -1299,17 +1292,13 @@ bool pidAntiGravityEnabled(void)
void dynLpfDTermUpdate(float throttle)
{
if (dynLpfFilter != DYN_LPF_NONE) {
uint16_t cutoffFreq = dynLpfMin;
if (throttle > dynLpfIdle) {
const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f;
cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled;
}
const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
if (dynLpfFilter == DYN_LPF_PT1) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, dT));
}
} else {
} else if (dynLpfFilter == DYN_LPF_BIQUAD) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime);
}

1
src/main/flight/pid.h
View File

@ -150,7 +150,6 @@ typedef struct pidProfile_s {
uint8_t abs_control_error_limit; // Limit to the accumulated error
uint8_t dterm_filter2_type; // Filter selection for 2nd dterm
uint16_t dyn_lpf_dterm_max_hz;
uint8_t dyn_lpf_dterm_idle;
uint8_t launchControlMode; // Whether launch control is limited to pitch only (launch stand or top-mount) or all axes (on battery)
uint8_t launchControlThrottlePercent; // Throttle percentage to trigger launch for launch control
uint8_t launchControlAngleLimit; // Optional launch control angle limit (requires ACC)

11
src/main/interface/settings.c
View File

@ -386,7 +386,7 @@ static const char * const lookupTableDynamicFftLocation[] = {
"BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS"
};
static const char * const lookupTableDynamicFilterRange[] = {
"HIGH", "MEDIUM", "LOW"
"HIGH", "MEDIUM", "LOW", "AUTO"
};
#endif // USE_GYRO_DATA_ANALYSE
@ -565,15 +565,14 @@ const clivalue_t valueTable[] = {
{ "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
#endif
#if defined(USE_GYRO_DATA_ANALYSE)
{ "dyn_fft_location", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FFT_LOCATION }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_fft_location) },
{ "dyn_filter_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) },
{ "dyn_filter_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_range) },
{ "dyn_notch_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_range) },
{ "dyn_notch_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 20 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width_percent) },
{ "dyn_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) },
{ "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) },
#endif
#ifdef USE_DYN_LPF
{ "dyn_lpf_gyro_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_max_hz) },
{ "dyn_lpf_gyro_idle", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_idle) },
{ "dyn_lpf_dterm_max_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_max_hz) },
{ "dyn_lpf_dterm_idle", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_idle) },
#endif
// PG_ACCELEROMETER_CONFIG

72
src/main/sensors/gyro.c
View File

@ -141,6 +141,7 @@ typedef struct gyroSensor_s {
filterApplyFnPtr notchFilterDynApplyFn;
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT];
// overflow and recovery
timeUs_t overflowTimeUs;
@ -185,7 +186,7 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ
#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, 5);
PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 6);
#ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
@ -214,13 +215,15 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->gyro_offset_yaw = 0;
gyroConfig->yaw_spin_recovery = true;
gyroConfig->yaw_spin_threshold = 1950;
gyroConfig->dyn_filter_width_percent = 40;
gyroConfig->dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS;
gyroConfig->dyn_filter_range = DYN_FILTER_RANGE_MEDIUM;
gyroConfig->dyn_lpf_gyro_max_hz = 400;
gyroConfig->dyn_lpf_gyro_idle = 20;
gyroConfig->dyn_notch_range = DYN_NOTCH_RANGE_AUTO;
gyroConfig->dyn_notch_width_percent = 8;
gyroConfig->dyn_notch_q = 120;
gyroConfig->dyn_notch_min_hz = 150;
gyroConfig->dyn_lpf_gyro_max_hz = 450;
#ifdef USE_DYN_LPF
gyroConfig->gyro_lowpass_hz = 120;
gyroConfig->gyro_lowpass_hz = 150;
gyroConfig->gyro_lowpass_type = FILTER_BIQUAD;
gyroConfig->gyro_lowpass2_hz = 0;
#endif
}
@ -499,6 +502,7 @@ bool gyroInit(void)
case DEBUG_GYRO_RAW:
case DEBUG_GYRO_SCALED:
case DEBUG_GYRO_FILTERED:
case DEBUG_DYN_LPF:
gyroDebugMode = debugMode;
break;
case DEBUG_DUAL_GYRO:
@ -547,35 +551,29 @@ bool gyroInit(void)
}
#ifdef USE_DYN_LPF
static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE;
static FAST_RAM_ZERO_INIT float dynLpfIdle;
static FAST_RAM_ZERO_INIT float dynLpfIdlePoint;
static FAST_RAM_ZERO_INIT float dynLpfInvIdlePointScaled;
static FAST_RAM uint8_t dynLpfFilter = DYN_LPF_NONE;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
static void dynLpfFilterInit()
{
if (gyroConfig()->dyn_lpf_gyro_idle > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > 0) {
if (gyroConfig()->gyro_lowpass_hz > 0 ) {
dynLpfMin = gyroConfig()->gyro_lowpass_hz;
switch (gyroConfig()->gyro_lowpass_type) {
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;
break;
case FILTER_BIQUAD:
dynLpfFilter = DYN_LPF_BIQUAD;
break;
default:
dynLpfFilter = DYN_LPF_NONE;
break;
}
if (gyroConfig()->gyro_lowpass_hz > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > gyroConfig()->gyro_lowpass_hz) {
switch (gyroConfig()->gyro_lowpass_type) {
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;
break;
case FILTER_BIQUAD:
dynLpfFilter = DYN_LPF_BIQUAD;
break;
default:
dynLpfFilter = DYN_LPF_NONE;
break;
}
} else {
dynLpfFilter = DYN_LPF_NONE;
}
dynLpfIdle = gyroConfig()->dyn_lpf_gyro_idle / 100.0f;
dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f;
dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (gyroConfig()->dyn_lpf_gyro_max_hz - dynLpfMin);
dynLpfMin = gyroConfig()->gyro_lowpass_hz;
dynLpfMax = gyroConfig()->dyn_lpf_gyro_max_hz;
}
#endif
@ -701,6 +699,7 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
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(&gyroSensor->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);
}
}
}
@ -1022,7 +1021,7 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn);
gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn, gyroSensor->notchFilterDyn2);
}
#endif
@ -1211,14 +1210,17 @@ uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg)
#endif // USE_GYRO_REGISTER_DUMP
#ifdef USE_DYN_LPF
float dynThrottle(float throttle) {
return throttle * (1 - (throttle * throttle) / 3.0f) * 1.5f;
}
void dynLpfGyroUpdate(float throttle)
{
if (dynLpfFilter != DYN_LPF_NONE) {
uint16_t cutoffFreq = dynLpfMin;
if (throttle > dynLpfIdle) {
const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f;
cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled;
}
const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
if (dynLpfFilter == DYN_LPF_PT1) {
const float gyroDt = gyro.targetLooptime * 1e-6f;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@ -1233,7 +1235,7 @@ void dynLpfGyroUpdate(float throttle)
pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
#endif
}
} else {
} else if (dynLpfFilter == DYN_LPF_BIQUAD) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
#ifdef USE_MULTI_GYRO
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {

25
src/main/sensors/gyro.h
View File

@ -43,15 +43,15 @@ typedef enum {
} gyroOverflowCheck_e;
enum {
DYN_FFT_BEFORE_STATIC_FILTERS = 0,
DYN_FFT_AFTER_STATIC_FILTERS
DYN_NOTCH_RANGE_HIGH = 0,
DYN_NOTCH_RANGE_MEDIUM,
DYN_NOTCH_RANGE_LOW,
DYN_NOTCH_RANGE_AUTO
} ;
enum {
DYN_FILTER_RANGE_HIGH = 0,
DYN_FILTER_RANGE_MEDIUM,
DYN_FILTER_RANGE_LOW
} ;
#define DYN_NOTCH_RANGE_HZ_HIGH 2000
#define DYN_NOTCH_RANGE_HZ_MEDIUM 1333
#define DYN_NOTCH_RANGE_HZ_LOW 1000
enum {
DYN_LPF_NONE = 0,
@ -96,13 +96,13 @@ typedef struct gyroConfig_s {
uint8_t yaw_spin_recovery;
int16_t yaw_spin_threshold;
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
uint8_t dyn_filter_width_percent;
uint8_t dyn_fft_location; // before or after static filters
uint8_t dyn_filter_range; // ignore any FFT bin below this threshold
uint8_t dyn_notch_range; // ignore any FFT bin below this threshold
uint16_t dyn_lpf_gyro_max_hz;
uint8_t dyn_lpf_gyro_idle;
uint8_t dyn_notch_width_percent;
uint16_t dyn_notch_q;
uint16_t dyn_notch_min_hz;
} gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig);
@ -126,5 +126,6 @@ bool gyroYawSpinDetected(void);
uint16_t gyroAbsRateDps(int axis);
uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg);
#ifdef USE_DYN_LPF
float dynThrottle(float throttle);
void dynLpfGyroUpdate(float throttle);
#endif

16
src/main/sensors/gyro_filter_impl.h
View File

@ -12,10 +12,9 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor)
if (axis == X) {
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));
}
}
float gyroDataForAnalysis = gyroADCf;
#endif
// apply static notch filters and software lowpass filters
@ -26,17 +25,14 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor)
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
if (gyroConfig()->dyn_fft_location == DYN_FFT_AFTER_STATIC_FILTERS) {
gyroDataForAnalysis = gyroADCf;
}
if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroDataForAnalysis));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroDataForAnalysis));
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(&gyroSensor->gyroAnalyseState, axis, gyroDataForAnalysis);
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn2[axis], gyroADCf);
}
#endif

77
src/main/sensors/gyroanalyse.c
View File

@ -51,23 +51,18 @@
#define FFT_BIN_OFFSET 2
// smoothing frequency for FFT centre frequency
#define DYN_NOTCH_SMOOTH_FREQ_HZ 50
// notch centre point will not go below sample rate divided by these dividers, resulting in range limits:
// HIGH : 133/166-1000Hz, MEDIUM -> 89/111-666Hz, LOW -> 67/83-500Hz
#define DYN_NOTCH_MIN_CENTRE_DIV 12
// divider to get lowest allowed notch cutoff frequency
// otherwise cutoff is user configured percentage below centre frequency
#define DYN_NOTCH_MIN_CUTOFF_DIV 15
// we need 4 steps for each axis
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4)
static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz;
static float FAST_RAM_ZERO_INIT fftResolution;
static uint8_t FAST_RAM_ZERO_INIT fftBinOffset;
static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMinCenterHz;
static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMaxCenterHz;
static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMinCutoffHz;
static float FAST_RAM_ZERO_INIT dynamicFilterWidthFactor;
static uint8_t dynamicFilterRange;
static float FAST_RAM_ZERO_INIT dynamicFilterNotchQ;
static float FAST_RAM_ZERO_INIT dynamicFilterNotch1Center;
static float FAST_RAM_ZERO_INIT dynamicFilterNotch2Center;
static uint16_t FAST_RAM_ZERO_INIT dynFilterNotchMinHz;
// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE];
@ -82,14 +77,30 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
gyroAnalyseInitialized = true;
#endif
dynamicFilterRange = gyroConfig()->dyn_filter_range;
dynamicFilterRange = gyroConfig()->dyn_notch_range;
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
fftBinOffset = FFT_BIN_OFFSET;
dynamicFilterNotch1Center = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f;
dynamicFilterNotch2Center = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f;
dynamicFilterNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
dynFilterNotchMinHz = gyroConfig()->dyn_notch_min_hz;
fftSamplingRateHz = 1000;
if (dynamicFilterRange == DYN_FILTER_RANGE_HIGH) {
fftSamplingRateHz = 2000;
}
else if (dynamicFilterRange == DYN_FILTER_RANGE_MEDIUM) {
fftSamplingRateHz = 1333;
if (dynamicFilterRange == DYN_NOTCH_RANGE_AUTO) {
if (gyroConfig()->dyn_lpf_gyro_max_hz > 333) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
}
if (gyroConfig()->dyn_lpf_gyro_max_hz > 610) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
fftBinOffset = 1;
}
} else {
if (dynamicFilterRange == DYN_NOTCH_RANGE_HIGH) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
fftBinOffset = 1;
}
else if (dynamicFilterRange == DYN_NOTCH_RANGE_MEDIUM) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
}
}
// If we get at least 3 samples then use the default FFT sample frequency
// otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K)
@ -98,13 +109,8 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
fftSamplingRateHz = MIN((gyroLoopRateHz / 3), fftSamplingRateHz);
fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE;
fftBinOffset = FFT_BIN_OFFSET;
dynamicNotchMaxCenterHz = fftSamplingRateHz / 2; //Nyquist
dynamicNotchMinCenterHz = fftSamplingRateHz / DYN_NOTCH_MIN_CENTRE_DIV;
dynamicNotchMinCutoffHz = fftSamplingRateHz / DYN_NOTCH_MIN_CUTOFF_DIV;
dynamicFilterWidthFactor = (100.0f - gyroConfig()->dyn_filter_width_percent) / 100;
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
@ -140,12 +146,12 @@ void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float
state->oversampledGyroAccumulator[axis] += sample;
}
static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn);
static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2);
/*
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
*/
void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
{
// samples should have been pushed by `gyroDataAnalysePush`
// if gyro sampling is > 1kHz, accumulate multiple samples
@ -174,7 +180,7 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
// calculate FFT and update filters
if (state->updateTicks > 0) {
gyroDataAnalyseUpdate(state, notchFilterDyn);
gyroDataAnalyseUpdate(state, notchFilterDyn, notchFilterDyn2);
--state->updateTicks;
}
}
@ -188,7 +194,7 @@ void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t
/*
* Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
*/
static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
{
enum {
STEP_ARM_CFFT_F32,
@ -307,15 +313,15 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
} else {
centerFreq = state->prevCenterFreq[state->updateAxis];
}
// constrain and low-pass smooth centre frequency
centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz);
centerFreq = fmax(centerFreq, dynFilterNotchMinHz);
centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz);
state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
state->centerFreq[state->updateAxis] = centerFreq;
if (state->updateAxis == 0) {
DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]);
DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]);
DEBUG_SET(DEBUG_DYN_LPF, 1, state->centerFreq[state->updateAxis]);
}
if (state->updateAxis == 1) {
DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]);
@ -327,11 +333,11 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
case STEP_UPDATE_FILTERS:
{
// 7us
// calculate cutoffFreq and notch Q, update notch filter
const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicFilterWidthFactor, dynamicNotchMinCutoffHz);
const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq);
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH);
// calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004)
if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynamicFilterNotch1Center, gyro.targetLooptime, dynamicFilterNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&notchFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynamicFilterNotch2Center, gyro.targetLooptime, dynamicFilterNotchQ, FILTER_NOTCH);
}
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
@ -355,4 +361,5 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
state->updateStep = (state->updateStep + 1) % STEP_COUNT;
}
#endif // USE_GYRO_DATA_ANALYSE

2
src/main/sensors/gyroanalyse.h
View File

@ -58,4 +58,4 @@ STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlyi
void gyroDataAnalyseStateInit(gyroAnalyseState_t *gyroAnalyse, uint32_t targetLooptime);
void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample);
void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn);
void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2);