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

Merge pull request #7393 from kmitchel/dyn_filt_init 

Allow dyn lpf to initialize if static lpf config is 0.
This commit is contained in:
Michael Keller 2019-01-21 11:32:20 +13:00 committed by GitHub
parent f8934c357b f6e456cd06
commit 8740e484f7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 35 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/main/fc/config.c
View File

@ -201,6 +201,13 @@ static void validateAndFixConfig(void)
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) {
featureDisable(FEATURE_3D);
@ -438,6 +445,12 @@ void validateAndFixGyroConfig(void)
if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
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) {
pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed

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

@ -319,14 +319,18 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
//1st Dterm Lowpass Filter
if (pidProfile->dterm_lowpass_hz == 0 || pidProfile->dterm_lowpass_hz > pidFrequencyNyquist) {
dtermLowpassApplyFn = nullFilterApply;
} else {
uint16_t dterm_lowpass_hz = pidProfile->dterm_lowpass_hz;
#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) {
case FILTER_PT1:
dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
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;
case FILTER_BIQUAD:
@ -336,13 +340,15 @@ void pidInitFilters(const pidProfile_t *pidProfile)
dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
#endif
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;
default:
dtermLowpassApplyFn = nullFilterApply;
break;
}
} else {
dtermLowpassApplyFn = nullFilterApply;
}
//2nd Dterm Lowpass Filter
@ -613,7 +619,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#endif
#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) {
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;

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

@ -578,7 +578,7 @@ static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
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) {
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;
@ -736,11 +736,17 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
gyroInitSlewLimiter(gyroSensor);
#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(
gyroSensor,
FILTER_LOWPASS,
gyroConfig()->gyro_lowpass_type,
gyroConfig()->gyro_lowpass_hz
gyro_lowpass_hz
);
gyroInitLowpassFilterLpf(
@ -1244,8 +1250,8 @@ void dynLpfGyroUpdate(float throttle)
if (dynLpfFilter != DYN_LPF_NONE) {
const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
if (dynLpfFilter == DYN_LPF_PT1) {
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
const float gyroDt = gyro.targetLooptime * 1e-6f;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
#ifdef USE_MULTI_GYRO
@ -1260,6 +1266,7 @@ void dynLpfGyroUpdate(float throttle)
#endif
}
} else if (dynLpfFilter == DYN_LPF_BIQUAD) {
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
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) {