Allow width=0 to select single notch. Restore dyn lpf mins.
This commit is contained in:
parent
011eae93c6
commit
233ac7fb3f
|
@ -167,11 +167,12 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
||||||
.abs_control_limit = 90,
|
.abs_control_limit = 90,
|
||||||
.abs_control_error_limit = 20,
|
.abs_control_error_limit = 20,
|
||||||
.antiGravityMode = ANTI_GRAVITY_SMOOTH,
|
.antiGravityMode = ANTI_GRAVITY_SMOOTH,
|
||||||
.dyn_lpf_dterm_max_hz = 250,
|
|
||||||
.dterm_lowpass_hz = 100, // dual PT1 filtering ON by default
|
.dterm_lowpass_hz = 100, // dual PT1 filtering ON by default
|
||||||
.dterm_lowpass2_hz = 200, // second Dterm LPF ON by default
|
.dterm_lowpass2_hz = 200, // second Dterm LPF ON by default
|
||||||
.dterm_filter_type = FILTER_PT1,
|
.dterm_filter_type = FILTER_PT1,
|
||||||
.dterm_filter2_type = FILTER_PT1,
|
.dterm_filter2_type = FILTER_PT1,
|
||||||
|
.dyn_lpf_dterm_min_hz = 150,
|
||||||
|
.dyn_lpf_dterm_max_hz = 250,
|
||||||
.launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
|
.launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
|
||||||
.launchControlThrottlePercent = 20,
|
.launchControlThrottlePercent = 20,
|
||||||
.launchControlAngleLimit = 0,
|
.launchControlAngleLimit = 0,
|
||||||
|
@ -544,7 +545,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_DYN_LPF
|
#ifdef USE_DYN_LPF
|
||||||
if (pidProfile->dterm_lowpass_hz > 0 && pidProfile->dyn_lpf_dterm_max_hz > pidProfile->dterm_lowpass_hz) {
|
if (pidProfile->dyn_lpf_dterm_min_hz > 0 && pidProfile->dyn_lpf_dterm_max_hz > pidProfile->dyn_lpf_dterm_min_hz) {
|
||||||
switch (pidProfile->dterm_filter_type) {
|
switch (pidProfile->dterm_filter_type) {
|
||||||
case FILTER_PT1:
|
case FILTER_PT1:
|
||||||
dynLpfFilter = DYN_LPF_PT1;
|
dynLpfFilter = DYN_LPF_PT1;
|
||||||
|
@ -559,7 +560,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
||||||
} else {
|
} else {
|
||||||
dynLpfFilter = DYN_LPF_NONE;
|
dynLpfFilter = DYN_LPF_NONE;
|
||||||
}
|
}
|
||||||
dynLpfMin = pidProfile->dterm_lowpass_hz;
|
dynLpfMin = pidProfile->dyn_lpf_dterm_min_hz;
|
||||||
dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz;
|
dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -149,6 +149,7 @@ typedef struct pidProfile_s {
|
||||||
uint8_t abs_control_limit; // Limit to the correction
|
uint8_t abs_control_limit; // Limit to the correction
|
||||||
uint8_t abs_control_error_limit; // Limit to the accumulated error
|
uint8_t abs_control_error_limit; // Limit to the accumulated error
|
||||||
uint8_t dterm_filter2_type; // Filter selection for 2nd dterm
|
uint8_t dterm_filter2_type; // Filter selection for 2nd dterm
|
||||||
|
uint16_t dyn_lpf_dterm_min_hz;
|
||||||
uint16_t dyn_lpf_dterm_max_hz;
|
uint16_t dyn_lpf_dterm_max_hz;
|
||||||
uint8_t launchControlMode; // Whether launch control is limited to pitch only (launch stand or top-mount) or all axes (on battery)
|
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 launchControlThrottlePercent; // Throttle percentage to trigger launch for launch control
|
||||||
|
|
|
@ -382,9 +382,6 @@ static const char * const lookupTableRcSmoothingDerivativeType[] = {
|
||||||
#endif // USE_RC_SMOOTHING_FILTER
|
#endif // USE_RC_SMOOTHING_FILTER
|
||||||
|
|
||||||
#ifdef USE_GYRO_DATA_ANALYSE
|
#ifdef USE_GYRO_DATA_ANALYSE
|
||||||
static const char * const lookupTableDynamicFftLocation[] = {
|
|
||||||
"BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS"
|
|
||||||
};
|
|
||||||
static const char * const lookupTableDynamicFilterRange[] = {
|
static const char * const lookupTableDynamicFilterRange[] = {
|
||||||
"HIGH", "MEDIUM", "LOW", "AUTO"
|
"HIGH", "MEDIUM", "LOW", "AUTO"
|
||||||
};
|
};
|
||||||
|
@ -504,7 +501,6 @@ const lookupTableEntry_t lookupTables[] = {
|
||||||
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType),
|
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType),
|
||||||
#endif // USE_RC_SMOOTHING_FILTER
|
#endif // USE_RC_SMOOTHING_FILTER
|
||||||
#ifdef USE_GYRO_DATA_ANALYSE
|
#ifdef USE_GYRO_DATA_ANALYSE
|
||||||
LOOKUP_TABLE_ENTRY(lookupTableDynamicFftLocation),
|
|
||||||
LOOKUP_TABLE_ENTRY(lookupTableDynamicFilterRange),
|
LOOKUP_TABLE_ENTRY(lookupTableDynamicFilterRange),
|
||||||
#endif // USE_GYRO_DATA_ANALYSE
|
#endif // USE_GYRO_DATA_ANALYSE
|
||||||
#ifdef USE_VTX_COMMON
|
#ifdef USE_VTX_COMMON
|
||||||
|
@ -571,7 +567,9 @@ const clivalue_t valueTable[] = {
|
||||||
{ "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) },
|
{ "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) },
|
||||||
#endif
|
#endif
|
||||||
#ifdef USE_DYN_LPF
|
#ifdef USE_DYN_LPF
|
||||||
|
{ "dyn_lpf_gyro_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_min_hz) },
|
||||||
{ "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_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_max_hz) },
|
||||||
|
{ "dyn_lpf_dterm_min_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_min_hz) },
|
||||||
{ "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_max_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_max_hz) },
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -113,7 +113,6 @@ typedef enum {
|
||||||
TABLE_RC_SMOOTHING_DERIVATIVE_TYPE,
|
TABLE_RC_SMOOTHING_DERIVATIVE_TYPE,
|
||||||
#endif // USE_RC_SMOOTHING_FILTER
|
#endif // USE_RC_SMOOTHING_FILTER
|
||||||
#ifdef USE_GYRO_DATA_ANALYSE
|
#ifdef USE_GYRO_DATA_ANALYSE
|
||||||
TABLE_DYNAMIC_FFT_LOCATION,
|
|
||||||
TABLE_DYNAMIC_FILTER_RANGE,
|
TABLE_DYNAMIC_FILTER_RANGE,
|
||||||
#endif // USE_GYRO_DATA_ANALYSE
|
#endif // USE_GYRO_DATA_ANALYSE
|
||||||
#ifdef USE_VTX_COMMON
|
#ifdef USE_VTX_COMMON
|
||||||
|
|
|
@ -140,6 +140,7 @@ typedef struct gyroSensor_s {
|
||||||
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
|
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
|
||||||
|
|
||||||
filterApplyFnPtr notchFilterDynApplyFn;
|
filterApplyFnPtr notchFilterDynApplyFn;
|
||||||
|
filterApplyFnPtr notchFilterDynApplyFn2;
|
||||||
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
|
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
|
||||||
biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT];
|
biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT];
|
||||||
|
|
||||||
|
@ -215,11 +216,12 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
|
||||||
gyroConfig->gyro_offset_yaw = 0;
|
gyroConfig->gyro_offset_yaw = 0;
|
||||||
gyroConfig->yaw_spin_recovery = true;
|
gyroConfig->yaw_spin_recovery = true;
|
||||||
gyroConfig->yaw_spin_threshold = 1950;
|
gyroConfig->yaw_spin_threshold = 1950;
|
||||||
|
gyroConfig->dyn_lpf_gyro_min_hz = 150;
|
||||||
|
gyroConfig->dyn_lpf_gyro_max_hz = 450;
|
||||||
gyroConfig->dyn_notch_range = DYN_NOTCH_RANGE_AUTO;
|
gyroConfig->dyn_notch_range = DYN_NOTCH_RANGE_AUTO;
|
||||||
gyroConfig->dyn_notch_width_percent = 8;
|
gyroConfig->dyn_notch_width_percent = 8;
|
||||||
gyroConfig->dyn_notch_q = 120;
|
gyroConfig->dyn_notch_q = 120;
|
||||||
gyroConfig->dyn_notch_min_hz = 150;
|
gyroConfig->dyn_notch_min_hz = 150;
|
||||||
gyroConfig->dyn_lpf_gyro_max_hz = 450;
|
|
||||||
#ifdef USE_DYN_LPF
|
#ifdef USE_DYN_LPF
|
||||||
gyroConfig->gyro_lowpass_hz = 150;
|
gyroConfig->gyro_lowpass_hz = 150;
|
||||||
gyroConfig->gyro_lowpass_type = FILTER_BIQUAD;
|
gyroConfig->gyro_lowpass_type = FILTER_BIQUAD;
|
||||||
|
@ -557,7 +559,7 @@ static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
|
||||||
|
|
||||||
static void dynLpfFilterInit()
|
static void dynLpfFilterInit()
|
||||||
{
|
{
|
||||||
if (gyroConfig()->gyro_lowpass_hz > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > gyroConfig()->gyro_lowpass_hz) {
|
if (gyroConfig()->dyn_lpf_gyro_min_hz > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > gyroConfig()->dyn_lpf_gyro_min_hz) {
|
||||||
switch (gyroConfig()->gyro_lowpass_type) {
|
switch (gyroConfig()->gyro_lowpass_type) {
|
||||||
case FILTER_PT1:
|
case FILTER_PT1:
|
||||||
dynLpfFilter = DYN_LPF_PT1;
|
dynLpfFilter = DYN_LPF_PT1;
|
||||||
|
@ -572,7 +574,7 @@ static void dynLpfFilterInit()
|
||||||
} else {
|
} else {
|
||||||
dynLpfFilter = DYN_LPF_NONE;
|
dynLpfFilter = DYN_LPF_NONE;
|
||||||
}
|
}
|
||||||
dynLpfMin = gyroConfig()->gyro_lowpass_hz;
|
dynLpfMin = gyroConfig()->dyn_lpf_gyro_min_hz;
|
||||||
dynLpfMax = gyroConfig()->dyn_lpf_gyro_max_hz;
|
dynLpfMax = gyroConfig()->dyn_lpf_gyro_max_hz;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
@ -693,9 +695,13 @@ static bool isDynamicFilterActive(void)
|
||||||
static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
|
static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
|
||||||
{
|
{
|
||||||
gyroSensor->notchFilterDynApplyFn = nullFilterApply;
|
gyroSensor->notchFilterDynApplyFn = nullFilterApply;
|
||||||
|
gyroSensor->notchFilterDynApplyFn2 = nullFilterApply;
|
||||||
|
|
||||||
if (isDynamicFilterActive()) {
|
if (isDynamicFilterActive()) {
|
||||||
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
|
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
|
||||||
|
if(gyroConfig()->dyn_notch_width_percent != 0) {
|
||||||
|
gyroSensor->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(&gyroSensor->notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
|
||||||
|
|
|
@ -98,9 +98,10 @@ typedef struct gyroConfig_s {
|
||||||
|
|
||||||
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
|
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
|
||||||
|
|
||||||
uint8_t dyn_notch_range; // ignore any FFT bin below this threshold
|
uint16_t dyn_lpf_gyro_min_hz;
|
||||||
uint16_t dyn_lpf_gyro_max_hz;
|
uint16_t dyn_lpf_gyro_max_hz;
|
||||||
uint8_t dyn_notch_width_percent;
|
uint8_t dyn_notch_range; // ignore any FFT bin below this threshold
|
||||||
|
uint8_t dyn_notch_width_percent;
|
||||||
uint16_t dyn_notch_q;
|
uint16_t dyn_notch_q;
|
||||||
uint16_t dyn_notch_min_hz;
|
uint16_t dyn_notch_min_hz;
|
||||||
} gyroConfig_t;
|
} gyroConfig_t;
|
||||||
|
|
|
@ -32,7 +32,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor)
|
||||||
}
|
}
|
||||||
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
|
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
|
||||||
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
|
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
|
||||||
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn2[axis], gyroADCf);
|
gyroADCf = gyroSensor->notchFilterDynApplyFn2((filter_t *)&gyroSensor->notchFilterDyn2[axis], gyroADCf);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -57,12 +57,13 @@
|
||||||
static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz;
|
static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz;
|
||||||
static float FAST_RAM_ZERO_INIT fftResolution;
|
static float FAST_RAM_ZERO_INIT fftResolution;
|
||||||
static uint8_t FAST_RAM_ZERO_INIT fftBinOffset;
|
static uint8_t FAST_RAM_ZERO_INIT fftBinOffset;
|
||||||
static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMaxCenterHz;
|
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxCtrHz;
|
||||||
static uint8_t dynamicFilterRange;
|
static uint8_t dynamicFilterRange;
|
||||||
static float FAST_RAM_ZERO_INIT dynamicFilterNotchQ;
|
static float FAST_RAM_ZERO_INIT dynNotchQ;
|
||||||
static float FAST_RAM_ZERO_INIT dynamicFilterNotch1Center;
|
static float FAST_RAM_ZERO_INIT dynNotch1Ctr;
|
||||||
static float FAST_RAM_ZERO_INIT dynamicFilterNotch2Center;
|
static float FAST_RAM_ZERO_INIT dynNotch2Ctr;
|
||||||
static uint16_t FAST_RAM_ZERO_INIT dynFilterNotchMinHz;
|
static uint16_t FAST_RAM_ZERO_INIT dynNotchMinHz;
|
||||||
|
static bool FAST_RAM dualNotch = true;
|
||||||
|
|
||||||
// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
|
// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
|
||||||
static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE];
|
static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE];
|
||||||
|
@ -80,10 +81,14 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
|
||||||
dynamicFilterRange = gyroConfig()->dyn_notch_range;
|
dynamicFilterRange = gyroConfig()->dyn_notch_range;
|
||||||
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
|
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
|
||||||
fftBinOffset = FFT_BIN_OFFSET;
|
fftBinOffset = FFT_BIN_OFFSET;
|
||||||
dynamicFilterNotch1Center = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f;
|
dynNotch1Ctr = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f;
|
||||||
dynamicFilterNotch2Center = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f;
|
dynNotch2Ctr = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f;
|
||||||
dynamicFilterNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
|
dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
|
||||||
dynFilterNotchMinHz = gyroConfig()->dyn_notch_min_hz;
|
dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
|
||||||
|
|
||||||
|
if (gyroConfig()->dyn_notch_width_percent == 0) {
|
||||||
|
dualNotch = false;
|
||||||
|
}
|
||||||
|
|
||||||
if (dynamicFilterRange == DYN_NOTCH_RANGE_AUTO) {
|
if (dynamicFilterRange == DYN_NOTCH_RANGE_AUTO) {
|
||||||
if (gyroConfig()->dyn_lpf_gyro_max_hz > 333) {
|
if (gyroConfig()->dyn_lpf_gyro_max_hz > 333) {
|
||||||
|
@ -110,7 +115,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
|
||||||
|
|
||||||
fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE;
|
fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE;
|
||||||
|
|
||||||
dynamicNotchMaxCenterHz = fftSamplingRateHz / 2; //Nyquist
|
dynNotchMaxCtrHz = fftSamplingRateHz / 2; //Nyquist
|
||||||
|
|
||||||
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
|
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)));
|
hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
|
||||||
|
@ -135,8 +140,8 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime
|
||||||
const float looptime = MAX(1000000u / fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
|
const float looptime = MAX(1000000u / fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
|
||||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||||
// any init value
|
// any init value
|
||||||
state->centerFreq[axis] = dynamicNotchMaxCenterHz;
|
state->centerFreq[axis] = dynNotchMaxCtrHz;
|
||||||
state->prevCenterFreq[axis] = dynamicNotchMaxCenterHz;
|
state->prevCenterFreq[axis] = dynNotchMaxCtrHz;
|
||||||
biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
|
biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -303,7 +308,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
|
// get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
|
||||||
float centerFreq = dynamicNotchMaxCenterHz;
|
float centerFreq = dynNotchMaxCtrHz;
|
||||||
float fftMeanIndex = 0;
|
float fftMeanIndex = 0;
|
||||||
// idx was shifted by 1 to start at 1, not 0
|
// idx was shifted by 1 to start at 1, not 0
|
||||||
if (fftSum > 0) {
|
if (fftSum > 0) {
|
||||||
|
@ -313,7 +318,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
|
||||||
} else {
|
} else {
|
||||||
centerFreq = state->prevCenterFreq[state->updateAxis];
|
centerFreq = state->prevCenterFreq[state->updateAxis];
|
||||||
}
|
}
|
||||||
centerFreq = fmax(centerFreq, dynFilterNotchMinHz);
|
centerFreq = fmax(centerFreq, dynNotchMinHz);
|
||||||
centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
|
centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
|
||||||
state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
|
state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
|
||||||
state->centerFreq[state->updateAxis] = centerFreq;
|
state->centerFreq[state->updateAxis] = centerFreq;
|
||||||
|
@ -335,8 +340,12 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
|
||||||
// 7us
|
// 7us
|
||||||
// calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004)
|
// calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004)
|
||||||
if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
|
if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
|
||||||
biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynamicFilterNotch1Center, gyro.targetLooptime, dynamicFilterNotchQ, FILTER_NOTCH);
|
if (dualNotch) {
|
||||||
biquadFilterUpdate(¬chFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynamicFilterNotch2Center, gyro.targetLooptime, dynamicFilterNotchQ, FILTER_NOTCH);
|
biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
|
||||||
|
biquadFilterUpdate(¬chFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
|
||||||
|
} else {
|
||||||
|
biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue