Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity

Fixed defaults for lowpass filters.

Browse source
This commit is contained in:
mikeller 2019-03-28 22:58:22 +13:00
parent 8e10e73580
commit d16f35d9cb
3 changed files with 41 additions and 27 deletions
Download patch file Download diff file Expand all files Collapse all files

56
src/main/fc/config.c
View file

@ -190,34 +190,30 @@ static void validateAndFixConfig(void)
featureDisable(FEATURE_GPS); featureDisable(FEATURE_GPS);
} }
if (systemConfig()->activeRateProfile >= CONTROL_RATE_PROFILE_COUNT) { for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) {
systemConfigMutable()->activeRateProfile = 0; // Prevent invalid notch cutoff
} if (pidProfilesMutable(i)->dterm_notch_cutoff >= pidProfilesMutable(i)->dterm_notch_hz) {
loadControlRateProfile(); pidProfilesMutable(i)->dterm_notch_hz = 0;
}
if (systemConfig()->pidProfileIndex >= PID_PROFILE_COUNT) {
systemConfigMutable()->pidProfileIndex = 0;
}
loadPidProfile();
// Prevent invalid notch cutoff
if (currentPidProfile->dterm_notch_cutoff >= currentPidProfile->dterm_notch_hz) {
currentPidProfile->dterm_notch_hz = 0;
}
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
//PRevent invalid dynamic lowpass //Prevent invalid dynamic lowpass
if (currentPidProfile->dyn_lpf_dterm_min_hz > currentPidProfile->dyn_lpf_dterm_max_hz) { if (pidProfilesMutable(i)->dyn_lpf_dterm_min_hz > pidProfilesMutable(i)->dyn_lpf_dterm_max_hz) {
currentPidProfile->dyn_lpf_dterm_min_hz = 0; pidProfilesMutable(i)->dyn_lpf_dterm_min_hz = 0;
} }
if (pidProfilesMutable(i)->dyn_lpf_dterm_min_hz > 0) {
pidProfilesMutable(i)->dterm_lowpass_hz = 0;
}
#endif #endif
if (currentPidProfile->motor_output_limit > 100 || currentPidProfile->motor_output_limit == 0) { if (pidProfilesMutable(i)->motor_output_limit > 100 || pidProfilesMutable(i)->motor_output_limit == 0) {
currentPidProfile->motor_output_limit = 100; pidProfilesMutable(i)->motor_output_limit = 100;
} }
if (currentPidProfile->auto_profile_cell_count > MAX_AUTO_DETECT_CELL_COUNT || currentPidProfile->auto_profile_cell_count < AUTO_PROFILE_CELL_COUNT_CHANGE) { if (pidProfilesMutable(i)->auto_profile_cell_count > MAX_AUTO_DETECT_CELL_COUNT || pidProfilesMutable(i)->auto_profile_cell_count < AUTO_PROFILE_CELL_COUNT_CHANGE) {
currentPidProfile->auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY; pidProfilesMutable(i)->auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY;
}
} }
if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) { if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
@ -487,6 +483,10 @@ void validateAndFixGyroConfig(void)
if (gyroConfig()->dyn_lpf_gyro_min_hz > gyroConfig()->dyn_lpf_gyro_max_hz) { if (gyroConfig()->dyn_lpf_gyro_min_hz > gyroConfig()->dyn_lpf_gyro_max_hz) {
gyroConfigMutable()->dyn_lpf_gyro_min_hz = 0; gyroConfigMutable()->dyn_lpf_gyro_min_hz = 0;
} }
if (gyroConfig()->dyn_lpf_gyro_min_hz > 0) {
gyroConfigMutable()->gyro_lowpass_hz = 0;
}
#endif #endif
if (gyroConfig()->gyro_hardware_lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) { if (gyroConfig()->gyro_hardware_lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
@ -574,6 +574,16 @@ void validateAndFixGyroConfig(void)
} }
#endif // USE_SDCARD #endif // USE_SDCARD
#endif // USE_BLACKBOX #endif // USE_BLACKBOX
if (systemConfig()->activeRateProfile >= CONTROL_RATE_PROFILE_COUNT) {
systemConfigMutable()->activeRateProfile = 0;
}
loadControlRateProfile();
if (systemConfig()->pidProfileIndex >= PID_PROFILE_COUNT) {
systemConfigMutable()->pidProfileIndex = 0;
}
loadPidProfile();
} }
bool readEEPROM(void) bool readEEPROM(void)

6
src/main/flight/pid.c
View file

@ -203,7 +203,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.transient_throttle_limit = 15, .transient_throttle_limit = 15,
); );
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
pidProfile->dterm_lowpass_hz = 150; pidProfile->dterm_lowpass_hz = 0;
pidProfile->dterm_lowpass2_hz = 150; pidProfile->dterm_lowpass2_hz = 150;
pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_filter_type = FILTER_BIQUAD;
pidProfile->dterm_filter2_type = FILTER_BIQUAD; pidProfile->dterm_filter2_type = FILTER_BIQUAD;
@ -339,7 +339,9 @@ void pidInitFilters(const pidProfile_t *pidProfile)
uint16_t dterm_lowpass_hz = pidProfile->dterm_lowpass_hz; uint16_t dterm_lowpass_hz = pidProfile->dterm_lowpass_hz;
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
dterm_lowpass_hz = MAX(pidProfile->dterm_lowpass_hz, pidProfile->dyn_lpf_dterm_min_hz); if (pidProfile->dyn_lpf_dterm_min_hz) {
dterm_lowpass_hz = pidProfile->dyn_lpf_dterm_min_hz;
}
#endif #endif
if (dterm_lowpass_hz > 0 && dterm_lowpass_hz < pidFrequencyNyquist) { if (dterm_lowpass_hz > 0 && dterm_lowpass_hz < pidFrequencyNyquist) {

6
src/main/sensors/gyro.c
View file

@ -222,7 +222,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->dyn_notch_q = 120; gyroConfig->dyn_notch_q = 120;
gyroConfig->dyn_notch_min_hz = 150; gyroConfig->dyn_notch_min_hz = 150;
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
gyroConfig->gyro_lowpass_hz = 150; gyroConfig->gyro_lowpass_hz = 0;
gyroConfig->gyro_lowpass_type = FILTER_BIQUAD; gyroConfig->gyro_lowpass_type = FILTER_BIQUAD;
gyroConfig->gyro_lowpass2_hz = 0; gyroConfig->gyro_lowpass2_hz = 0;
#endif #endif
@ -744,7 +744,9 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
uint16_t gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz; uint16_t gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz;
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
gyro_lowpass_hz = MAX(gyroConfig()->gyro_lowpass_hz, gyroConfig()->dyn_lpf_gyro_min_hz); if (gyroConfig()->dyn_lpf_gyro_min_hz > 0) {
gyro_lowpass_hz = gyroConfig()->dyn_lpf_gyro_min_hz;
}
#endif #endif
gyroInitLowpassFilterLpf( gyroInitLowpassFilterLpf(