mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 16:25:31 +03:00
Allow dyn lpf to initialize if static lpf config is 0.
Change min > max per review. Fix missing ifdef.
This commit is contained in:
parent
7f58ecc77f
commit
f6e456cd06
3 changed files with 35 additions and 9 deletions
|
@ -195,6 +195,13 @@ static void validateAndFixConfig(void)
|
||||||
currentPidProfile->dterm_notch_hz = 0;
|
currentPidProfile->dterm_notch_hz = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifdef USE_DYN_LPF
|
||||||
|
//PRevent invalid dynamic lowpass
|
||||||
|
if (currentPidProfile->dyn_lpf_dterm_min_hz > currentPidProfile->dyn_lpf_dterm_max_hz) {
|
||||||
|
currentPidProfile->dyn_lpf_dterm_min_hz = 0;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
|
if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
|
||||||
featureDisable(FEATURE_3D);
|
featureDisable(FEATURE_3D);
|
||||||
|
|
||||||
|
@ -432,6 +439,12 @@ void validateAndFixGyroConfig(void)
|
||||||
if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
|
if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
|
||||||
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
|
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
|
||||||
}
|
}
|
||||||
|
#ifdef USE_DYN_LPF
|
||||||
|
//Prevent invalid dynamic lowpass filter
|
||||||
|
if (gyroConfig()->dyn_lpf_gyro_min_hz > gyroConfig()->dyn_lpf_gyro_max_hz) {
|
||||||
|
gyroConfigMutable()->dyn_lpf_gyro_min_hz = 0;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
if (gyroConfig()->gyro_hardware_lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
|
if (gyroConfig()->gyro_hardware_lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
|
||||||
pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
|
pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
|
||||||
|
|
|
@ -315,14 +315,18 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
||||||
}
|
}
|
||||||
|
|
||||||
//1st Dterm Lowpass Filter
|
//1st Dterm Lowpass Filter
|
||||||
if (pidProfile->dterm_lowpass_hz == 0 || pidProfile->dterm_lowpass_hz > pidFrequencyNyquist) {
|
uint16_t dterm_lowpass_hz = pidProfile->dterm_lowpass_hz;
|
||||||
dtermLowpassApplyFn = nullFilterApply;
|
|
||||||
} else {
|
#ifdef USE_DYN_LPF
|
||||||
|
dterm_lowpass_hz = MAX(pidProfile->dterm_lowpass_hz, pidProfile->dyn_lpf_dterm_min_hz);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
if (dterm_lowpass_hz > 0 && dterm_lowpass_hz < pidFrequencyNyquist) {
|
||||||
switch (pidProfile->dterm_filter_type) {
|
switch (pidProfile->dterm_filter_type) {
|
||||||
case FILTER_PT1:
|
case FILTER_PT1:
|
||||||
dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
||||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||||
pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass_hz, dT));
|
pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(dterm_lowpass_hz, dT));
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case FILTER_BIQUAD:
|
case FILTER_BIQUAD:
|
||||||
|
@ -332,13 +336,15 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
||||||
dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||||
#endif
|
#endif
|
||||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||||
biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime);
|
biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
dtermLowpassApplyFn = nullFilterApply;
|
dtermLowpassApplyFn = nullFilterApply;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
} else {
|
||||||
|
dtermLowpassApplyFn = nullFilterApply;
|
||||||
}
|
}
|
||||||
|
|
||||||
//2nd Dterm Lowpass Filter
|
//2nd Dterm Lowpass Filter
|
||||||
|
@ -609,7 +615,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_DYN_LPF
|
#ifdef USE_DYN_LPF
|
||||||
if (pidProfile->dyn_lpf_dterm_min_hz > 0 && pidProfile->dyn_lpf_dterm_max_hz > pidProfile->dyn_lpf_dterm_min_hz) {
|
if (pidProfile->dyn_lpf_dterm_min_hz > 0) {
|
||||||
switch (pidProfile->dterm_filter_type) {
|
switch (pidProfile->dterm_filter_type) {
|
||||||
case FILTER_PT1:
|
case FILTER_PT1:
|
||||||
dynLpfFilter = DYN_LPF_PT1;
|
dynLpfFilter = DYN_LPF_PT1;
|
||||||
|
|
|
@ -578,7 +578,7 @@ static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
|
||||||
|
|
||||||
static void dynLpfFilterInit()
|
static void dynLpfFilterInit()
|
||||||
{
|
{
|
||||||
if (gyroConfig()->dyn_lpf_gyro_min_hz > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > gyroConfig()->dyn_lpf_gyro_min_hz) {
|
if (gyroConfig()->dyn_lpf_gyro_min_hz > 0) {
|
||||||
switch (gyroConfig()->gyro_lowpass_type) {
|
switch (gyroConfig()->gyro_lowpass_type) {
|
||||||
case FILTER_PT1:
|
case FILTER_PT1:
|
||||||
dynLpfFilter = DYN_LPF_PT1;
|
dynLpfFilter = DYN_LPF_PT1;
|
||||||
|
@ -736,11 +736,17 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
|
||||||
gyroInitSlewLimiter(gyroSensor);
|
gyroInitSlewLimiter(gyroSensor);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
uint16_t gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz;
|
||||||
|
|
||||||
|
#ifdef USE_DYN_LPF
|
||||||
|
gyro_lowpass_hz = MAX(gyroConfig()->gyro_lowpass_hz, gyroConfig()->dyn_lpf_gyro_min_hz);
|
||||||
|
#endif
|
||||||
|
|
||||||
gyroInitLowpassFilterLpf(
|
gyroInitLowpassFilterLpf(
|
||||||
gyroSensor,
|
gyroSensor,
|
||||||
FILTER_LOWPASS,
|
FILTER_LOWPASS,
|
||||||
gyroConfig()->gyro_lowpass_type,
|
gyroConfig()->gyro_lowpass_type,
|
||||||
gyroConfig()->gyro_lowpass_hz
|
gyro_lowpass_hz
|
||||||
);
|
);
|
||||||
|
|
||||||
gyroInitLowpassFilterLpf(
|
gyroInitLowpassFilterLpf(
|
||||||
|
@ -1244,8 +1250,8 @@ void dynLpfGyroUpdate(float throttle)
|
||||||
if (dynLpfFilter != DYN_LPF_NONE) {
|
if (dynLpfFilter != DYN_LPF_NONE) {
|
||||||
const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
|
const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
|
||||||
|
|
||||||
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
|
|
||||||
if (dynLpfFilter == DYN_LPF_PT1) {
|
if (dynLpfFilter == DYN_LPF_PT1) {
|
||||||
|
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
|
||||||
const float gyroDt = gyro.targetLooptime * 1e-6f;
|
const float gyroDt = gyro.targetLooptime * 1e-6f;
|
||||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||||
#ifdef USE_MULTI_GYRO
|
#ifdef USE_MULTI_GYRO
|
||||||
|
@ -1260,6 +1266,7 @@ void dynLpfGyroUpdate(float throttle)
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
} else if (dynLpfFilter == DYN_LPF_BIQUAD) {
|
} else if (dynLpfFilter == DYN_LPF_BIQUAD) {
|
||||||
|
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
|
||||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||||
#ifdef USE_MULTI_GYRO
|
#ifdef USE_MULTI_GYRO
|
||||||
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
|
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue