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

Allow width=0 to select single notch. Restore dyn lpf mins.

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

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

@ -167,11 +167,12 @@ void resetPidProfile(pidProfile_t *pidProfile)
.abs_control_limit = 90,
.abs_control_error_limit = 20,
.antiGravityMode = ANTI_GRAVITY_SMOOTH,
.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,
.dterm_filter2_type = FILTER_PT1,
.dyn_lpf_dterm_min_hz = 150,
.dyn_lpf_dterm_max_hz = 250,
.launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
.launchControlThrottlePercent = 20,
.launchControlAngleLimit = 0,
@ -544,7 +545,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#endif
#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) {
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;
@ -559,7 +560,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
} else {
dynLpfFilter = DYN_LPF_NONE;
}
dynLpfMin = pidProfile->dterm_lowpass_hz;
dynLpfMin = pidProfile->dyn_lpf_dterm_min_hz;
dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz;
#endif

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

@ -149,6 +149,7 @@ typedef struct pidProfile_s {
uint8_t abs_control_limit; // Limit to the correction
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_min_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 launchControlThrottlePercent; // Throttle percentage to trigger launch for launch control

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

@ -382,9 +382,6 @@ static const char * const lookupTableRcSmoothingDerivativeType[] = {
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_GYRO_DATA_ANALYSE
static const char * const lookupTableDynamicFftLocation[] = {
"BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS"
};
static const char * const lookupTableDynamicFilterRange[] = {
"HIGH", "MEDIUM", "LOW", "AUTO"
};
@ -504,7 +501,6 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType),
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_GYRO_DATA_ANALYSE
LOOKUP_TABLE_ENTRY(lookupTableDynamicFftLocation),
LOOKUP_TABLE_ENTRY(lookupTableDynamicFilterRange),
#endif // USE_GYRO_DATA_ANALYSE
#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) },
#endif
#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_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) },
#endif

1
src/main/interface/settings.h
View File

@ -113,7 +113,6 @@ typedef enum {
TABLE_RC_SMOOTHING_DERIVATIVE_TYPE,
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_GYRO_DATA_ANALYSE
TABLE_DYNAMIC_FFT_LOCATION,
TABLE_DYNAMIC_FILTER_RANGE,
#endif // USE_GYRO_DATA_ANALYSE
#ifdef USE_VTX_COMMON

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

@ -140,6 +140,7 @@ typedef struct gyroSensor_s {
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilterDynApplyFn;
filterApplyFnPtr notchFilterDynApplyFn2;
biquadFilter_t notchFilterDyn[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->yaw_spin_recovery = true;
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_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 = 150;
gyroConfig->gyro_lowpass_type = FILTER_BIQUAD;
@ -557,7 +559,7 @@ static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
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) {
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;
@ -572,7 +574,7 @@ static void dynLpfFilterInit()
} else {
dynLpfFilter = DYN_LPF_NONE;
}
dynLpfMin = gyroConfig()->gyro_lowpass_hz;
dynLpfMin = gyroConfig()->dyn_lpf_gyro_min_hz;
dynLpfMax = gyroConfig()->dyn_lpf_gyro_max_hz;
}
#endif
@ -693,9 +695,13 @@ static bool isDynamicFilterActive(void)
static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
{
gyroSensor->notchFilterDynApplyFn = nullFilterApply;
gyroSensor->notchFilterDynApplyFn2 = nullFilterApply;
if (isDynamicFilterActive()) {
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
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyroSensor->notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);

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

@ -98,9 +98,10 @@ typedef struct gyroConfig_s {
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;
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_min_hz;
} gyroConfig_t;

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

@ -32,7 +32,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor)
}
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, 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

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

@ -57,12 +57,13 @@
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 dynamicNotchMaxCenterHz;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxCtrHz;
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;
static float FAST_RAM_ZERO_INIT dynNotchQ;
static float FAST_RAM_ZERO_INIT dynNotch1Ctr;
static float FAST_RAM_ZERO_INIT dynNotch2Ctr;
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
static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE];
@ -80,10 +81,14 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
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;
dynNotch1Ctr = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f;
dynNotch2Ctr = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f;
dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
if (gyroConfig()->dyn_notch_width_percent == 0) {
dualNotch = false;
}
if (dynamicFilterRange == DYN_NOTCH_RANGE_AUTO) {
if (gyroConfig()->dyn_lpf_gyro_max_hz > 333) {
@ -110,7 +115,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE;
dynamicNotchMaxCenterHz = fftSamplingRateHz / 2; //Nyquist
dynNotchMaxCtrHz = fftSamplingRateHz / 2; //Nyquist
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)));
@ -135,8 +140,8 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime
const float looptime = MAX(1000000u / fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// any init value
state->centerFreq[axis] = dynamicNotchMaxCenterHz;
state->prevCenterFreq[axis] = dynamicNotchMaxCenterHz;
state->centerFreq[axis] = dynNotchMaxCtrHz;
state->prevCenterFreq[axis] = dynNotchMaxCtrHz;
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)
float centerFreq = dynamicNotchMaxCenterHz;
float centerFreq = dynNotchMaxCtrHz;
float fftMeanIndex = 0;
// idx was shifted by 1 to start at 1, not 0
if (fftSum > 0) {
@ -313,7 +318,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
} else {
centerFreq = state->prevCenterFreq[state->updateAxis];
}
centerFreq = fmax(centerFreq, dynFilterNotchMinHz);
centerFreq = fmax(centerFreq, dynNotchMinHz);
centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
state->centerFreq[state->updateAxis] = centerFreq;
@ -335,8 +340,12 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
// 7us
// 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);
if (dualNotch) {
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&notchFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
} else {
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
}
}
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);