diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index e02fc2c774..0b30a15f73 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -815,7 +815,11 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa } pidUpdateAntiGravityThrottleFilter(throttle); - +#ifdef USE_DYN_LPF + dynLpfGyroUpdate(throttle); + dynLpfDTermUpdate(throttle); +#endif + #if defined(USE_THROTTLE_BOOST) if (throttleBoost > 0.0f) { const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 89d57ebcf3..e8813e84f3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -167,7 +167,15 @@ void resetPidProfile(pidProfile_t *pidProfile) .abs_control_limit = 90, .abs_control_error_limit = 20, .antiGravityMode = ANTI_GRAVITY_SMOOTH, + .dyn_lpf_dterm_max_hz = 200, + .dyn_lpf_dterm_idle = 20, ); +#ifdef USE_DYN_LPF + pidProfile->dterm_lowpass_hz = 150; + pidProfile->dterm_lowpass2_hz = 180; + pidProfile->dterm_filter_type = FILTER_BIQUAD; + pidProfile->dterm_filter2_type = FILTER_BIQUAD; +#endif } void pgResetFn_pidProfiles(pidProfile_t *pidProfiles) @@ -280,7 +288,11 @@ void pidInitFilters(const pidProfile_t *pidProfile) } break; case FILTER_BIQUAD: +#ifdef USE_DYN_LPF + dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; +#else dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply; +#endif for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime); } @@ -436,6 +448,15 @@ void pidUpdateAntiGravityThrottleFilter(float throttle) } } +#ifdef USE_DYN_LPF +static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE; +static FAST_RAM_ZERO_INIT float dynLpfIdle; +static FAST_RAM_ZERO_INIT float dynLpfIdlePoint; +static FAST_RAM_ZERO_INIT float dynLpfInvIdlePoint; +static FAST_RAM_ZERO_INIT int16_t dynLpfDiff; +static FAST_RAM_ZERO_INIT uint16_t dynLpfMin; +#endif + void pidInitConfig(const pidProfile_t *pidProfile) { if (pidProfile->feedForwardTransition == 0) { @@ -510,6 +531,31 @@ void pidInitConfig(const pidProfile_t *pidProfile) acLimit = (float)pidProfile->abs_control_limit; acErrorLimit = (float)pidProfile->abs_control_error_limit; #endif + +#ifdef USE_DYN_LPF + if (pidProfile->dyn_lpf_dterm_idle > 0 && pidProfile->dyn_lpf_dterm_max_hz > 0) { + if (pidProfile->dterm_lowpass_hz > 0 ) { + dynLpfMin = pidProfile->dterm_lowpass_hz; + switch (pidProfile->dterm_filter_type) { + case FILTER_PT1: + dynLpfFilter = DYN_LPF_PT1; + break; + case FILTER_BIQUAD: + dynLpfFilter = DYN_LPF_BIQUAD; + break; + default: + dynLpfFilter = DYN_LPF_NONE; + break; + } + } + } else { + dynLpfFilter = DYN_LPF_NONE; + } + dynLpfIdle = pidProfile->dyn_lpf_dterm_idle / 100.0f; + dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f; + dynLpfInvIdlePoint = 1 / (1 - dynLpfIdlePoint); + dynLpfDiff = pidProfile->dyn_lpf_dterm_max_hz - dynLpfMin; +#endif } void pidInit(const pidProfile_t *pidProfile) @@ -1130,3 +1176,27 @@ bool pidAntiGravityEnabled(void) { return antiGravityEnabled; } + +#ifdef USE_DYN_LPF +void dynLpfDTermUpdate(float throttle) +{ + if (dynLpfFilter == DYN_LPF_PT1 || dynLpfFilter == DYN_LPF_BIQUAD) { + uint16_t cutoffFreq = dynLpfMin; + if (throttle > dynLpfIdle) { + const float dynThrottle = (throttle - (throttle * throttle * throttle) * 0.333f) * 1.5f; + cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePoint * dynLpfDiff; + } + + if (dynLpfFilter == DYN_LPF_PT1) { + const float gyroDt = gyro.targetLooptime * 1e-6f; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, gyroDt)); + } + } else { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, gyro.targetLooptime); + } + } + } +} +#endif diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 25806bdcd4..d5c6536883 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -149,6 +149,8 @@ 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_max_hz; + uint8_t dyn_lpf_dterm_idle; } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles); @@ -214,3 +216,5 @@ float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint); float calcHorizonLevelStrength(void); #endif +void dynLpfDTermUpdate(float throttle); + diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 4d5d41d689..280c12fc5a 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -550,6 +550,12 @@ const clivalue_t valueTable[] = { { "dyn_filter_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) }, { "dyn_filter_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_range) }, #endif +#ifdef USE_DYN_LPF + { "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_idle", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_idle) }, + { "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_idle", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_idle) }, +#endif // PG_ACCELEROMETER_CONFIG { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index bf396e58a0..8f4433da89 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -183,38 +183,44 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ #define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro) #define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro) -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5); +PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5); #ifndef GYRO_CONFIG_USE_GYRO_DEFAULT #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 #endif -PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, - .gyro_align = ALIGN_DEFAULT, - .gyroCalibrationDuration = 125, // 1.25 seconds - .gyroMovementCalibrationThreshold = 48, - .gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT, - .gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL, - .gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL, - .gyro_lowpass_type = FILTER_PT1, - .gyro_lowpass_hz = 100, - .gyro_lowpass2_type = FILTER_PT1, - .gyro_lowpass2_hz = 300, - .gyro_high_fsr = false, - .gyro_use_32khz = false, - .gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT, - .gyro_soft_notch_hz_1 = 0, - .gyro_soft_notch_cutoff_1 = 0, - .gyro_soft_notch_hz_2 = 0, - .gyro_soft_notch_cutoff_2 = 0, - .checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES, - .gyro_offset_yaw = 0, - .yaw_spin_recovery = true, - .yaw_spin_threshold = 1950, - .dyn_filter_width_percent = 40, - .dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS, - .dyn_filter_range = DYN_FILTER_RANGE_MEDIUM, -); +void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig) +{ + gyroConfig->gyro_align = ALIGN_DEFAULT; + gyroConfig->gyroCalibrationDuration = 125; // 1gyroConfig->25 seconds + gyroConfig->gyroMovementCalibrationThreshold = 48; + gyroConfig->gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT; + gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL; + gyroConfig->gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL; + gyroConfig->gyro_lowpass_type = FILTER_PT1; + gyroConfig->gyro_lowpass_hz = 100; + gyroConfig->gyro_lowpass2_type = FILTER_PT1; + gyroConfig->gyro_lowpass2_hz = 300; + gyroConfig->gyro_high_fsr = false; + gyroConfig->gyro_use_32khz = false; + gyroConfig->gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT; + gyroConfig->gyro_soft_notch_hz_1 = 0; + gyroConfig->gyro_soft_notch_cutoff_1 = 0; + gyroConfig->gyro_soft_notch_hz_2 = 0; + gyroConfig->gyro_soft_notch_cutoff_2 = 0; + gyroConfig->checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES; + gyroConfig->gyro_offset_yaw = 0; + gyroConfig->yaw_spin_recovery = true; + gyroConfig->yaw_spin_threshold = 1950; + gyroConfig->dyn_filter_width_percent = 40; + gyroConfig->dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS; + gyroConfig->dyn_filter_range = DYN_FILTER_RANGE_MEDIUM; + gyroConfig->dyn_lpf_gyro_max_hz = 400; + gyroConfig->dyn_lpf_gyro_idle = 20; +#ifdef USE_DYN_LPF + gyroConfig->gyro_lowpass_hz = 150; +#endif +} #ifdef USE_MULTI_GYRO #define ACTIVE_GYRO ((gyroToUse == GYRO_CONFIG_USE_GYRO_2) ? &gyroSensor2 : &gyroSensor1) @@ -533,6 +539,41 @@ bool gyroInit(void) return ret; } +#ifdef USE_DYN_LPF +static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE; +static FAST_RAM_ZERO_INIT float dynLpfIdle; +static FAST_RAM_ZERO_INIT float dynLpfIdlePoint; +static FAST_RAM_ZERO_INIT float dynLpfInvIdlePoint; +static FAST_RAM_ZERO_INIT int16_t dynLpfDiff; +static FAST_RAM_ZERO_INIT uint16_t dynLpfMin; + +static void dynLpfFilterInit() +{ + if (gyroConfig()->dyn_lpf_gyro_idle > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > 0) { + if (gyroConfig()->gyro_lowpass_hz > 0 ) { + dynLpfMin = gyroConfig()->gyro_lowpass_hz; + switch (gyroConfig()->gyro_lowpass_type) { + case FILTER_PT1: + dynLpfFilter = DYN_LPF_PT1; + break; + case FILTER_BIQUAD: + dynLpfFilter = DYN_LPF_BIQUAD; + break; + default: + dynLpfFilter = DYN_LPF_NONE; + break; + } + } + } else { + dynLpfFilter = DYN_LPF_NONE; + } + dynLpfIdle = gyroConfig()->dyn_lpf_gyro_idle / 100.0f; + dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f; + dynLpfInvIdlePoint = 1 / (1 - dynLpfIdlePoint); + dynLpfDiff = gyroConfig()->dyn_lpf_gyro_max_hz - dynLpfMin; +} +#endif + void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz) { filterApplyFnPtr *lowpassFilterApplyFn; @@ -574,7 +615,11 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint } break; case FILTER_BIQUAD: +#ifdef USE_DYN_LPF + *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1; +#else *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply; +#endif for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime); } @@ -682,6 +727,9 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) #ifdef USE_GYRO_DATA_ANALYSE gyroInitFilterDynamicNotch(gyroSensor); #endif +#ifdef USE_DYN_LPF + dynLpfFilterInit(); +#endif } void gyroInitFilters(void) @@ -1150,3 +1198,44 @@ uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg) return mpuGyroReadRegister(gyroSensorBusByDevice(whichSensor), reg); } #endif // USE_GYRO_REGISTER_DUMP + +#ifdef USE_DYN_LPF +void dynLpfGyroUpdate(float throttle) +{ + if (dynLpfFilter == DYN_LPF_PT1 || dynLpfFilter == DYN_LPF_BIQUAD) { + uint16_t cutoffFreq = dynLpfMin; + if (throttle > dynLpfIdle) { + const float dynThrottle = (throttle - (throttle * throttle * throttle) * 0.333f) * 1.5f; + cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePoint * dynLpfDiff; + } + if (dynLpfFilter == DYN_LPF_PT1) { + const float gyroDt = gyro.targetLooptime * 1e-6f; + 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) { + pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt)); + } + if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) { + pt1FilterUpdateCutoff(&gyroSensor2.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt)); + } +#else + pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt)); +#endif + } + } else { + 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) { + biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime); + } + if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) { + biquadFilterUpdateLPF(&gyroSensor2.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime); + } +#else + biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime); +#endif + } + } + } +} +#endif diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 90f520e849..17086b7a39 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -53,6 +53,12 @@ enum { DYN_FILTER_RANGE_LOW } ; +enum { + DYN_LPF_NONE = 0, + DYN_LPF_PT1, + DYN_LPF_BIQUAD +} ; + #define GYRO_CONFIG_USE_GYRO_1 0 #define GYRO_CONFIG_USE_GYRO_2 1 #define GYRO_CONFIG_USE_GYRO_BOTH 2 @@ -95,6 +101,8 @@ typedef struct gyroConfig_s { uint8_t dyn_filter_width_percent; uint8_t dyn_fft_location; // before or after static filters uint8_t dyn_filter_range; // ignore any FFT bin below this threshold + uint16_t dyn_lpf_gyro_max_hz; + uint8_t dyn_lpf_gyro_idle; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); @@ -117,3 +125,6 @@ bool gyroOverflowDetected(void); bool gyroYawSpinDetected(void); uint16_t gyroAbsRateDps(int axis); uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg); +#ifdef USE_DYN_LPF +void dynLpfGyroUpdate(float throttle); +#endif diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 49dd029380..ec5f09a433 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -189,6 +189,7 @@ #define USE_THROTTLE_BOOST #define USE_RC_SMOOTHING_FILTER #define USE_ITERM_RELAX +//#define USE_DYN_LPF #ifdef USE_SERIALRX_SPEKTRUM #define USE_SPEKTRUM_BIND