diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 61f0117ab4..e3fc4d7095 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -666,6 +666,7 @@ const clivalue_t valueTable[] = { #ifdef USE_DYN_LPF { "dyn_lpf_gyro_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_min_hz) }, { "dyn_lpf_gyro_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_max_hz) }, + { "dyn_lpf_gyro_curve_expo", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 10 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_curve_expo) }, #endif { "gyro_filter_debug_axis", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_FILTER_DEBUG }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_debug_axis) }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fa06b4e48d..e7c40d8253 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1189,7 +1189,7 @@ void dynLpfDTermUpdate(float throttle) static unsigned int cutoffFreq; if (pidRuntime.dynLpfFilter != DYN_LPF_NONE) { if (pidRuntime.dynLpfCurveExpo > 0) { - cutoffFreq = dynDtermLpfCutoffFreq(throttle, pidRuntime.dynLpfMin, pidRuntime.dynLpfMax, pidRuntime.dynLpfCurveExpo); + cutoffFreq = dynLpfCutoffFreq(throttle, pidRuntime.dynLpfMin, pidRuntime.dynLpfMax, pidRuntime.dynLpfCurveExpo); } else { cutoffFreq = fmax(dynThrottle(throttle) * pidRuntime.dynLpfMax, pidRuntime.dynLpfMin); } @@ -1207,7 +1207,7 @@ void dynLpfDTermUpdate(float throttle) } #endif -float dynDtermLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) { +float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) { const float expof = expo / 10.0f; static float curve; curve = throttle * (1 - throttle) * expof + throttle; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index dfc59fa282..887938092c 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -414,4 +414,4 @@ float pidGetPidFrequency(); float pidGetFfBoostFactor(); float pidGetFfSmoothFactor(); float pidGetSpikeLimitInverse(); -float dynDtermLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo); +float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index ed78bebb7d..53a6cba4b6 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -132,6 +132,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig) gyroConfig->dyn_notch_q = 120; gyroConfig->dyn_notch_min_hz = 150; gyroConfig->gyro_filter_debug_axis = FD_ROLL; + gyroConfig->dyn_lpf_curve_expo = 0; } #ifdef USE_GYRO_DATA_ANALYSE @@ -625,8 +626,12 @@ float dynThrottle(float throttle) { void dynLpfGyroUpdate(float throttle) { if (gyro.dynLpfFilter != DYN_LPF_NONE) { - const unsigned int cutoffFreq = (gyro.dynLpfMax - gyro.dynLpfMin) * dynThrottle(throttle) + gyro.dynLpfMin; - + static unsigned int cutoffFreq; + if (gyro.dynLpfCurveExpo > 0) { + cutoffFreq = dynLpfCutoffFreq(throttle, gyro.dynLpfMin, gyro.dynLpfMax, gyro.dynLpfCurveExpo); + } else { + cutoffFreq = fmax(dynThrottle(throttle) * gyro.dynLpfMax, gyro.dynLpfMin); + } if (gyro.dynLpfFilter == DYN_LPF_PT1) { DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq); const float gyroDt = gyro.targetLooptime * 1e-6f; diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 6539150783..a697d540ed 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -122,6 +122,7 @@ typedef struct gyro_s { uint8_t dynLpfFilter; uint16_t dynLpfMin; uint16_t dynLpfMax; + uint8_t dynLpfCurveExpo; #endif #ifdef USE_GYRO_OVERFLOW_CHECK @@ -197,6 +198,7 @@ typedef struct gyroConfig_s { uint8_t gyro_filter_debug_axis; uint8_t gyrosDetected; // What gyros should detection be attempted for on startup. Automatically set on first startup. + uint8_t dyn_lpf_curve_expo; // set the curve for dynamic gyro lowpass filter } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index 6e8d56f2a5..87222a36d9 100644 --- a/src/main/sensors/gyro_init.c +++ b/src/main/sensors/gyro_init.c @@ -228,6 +228,7 @@ static void dynLpfFilterInit() } gyro.dynLpfMin = gyroConfig()->dyn_lpf_gyro_min_hz; gyro.dynLpfMax = gyroConfig()->dyn_lpf_gyro_max_hz; + gyro.dynLpfCurveExpo = gyroConfig()->dyn_lpf_curve_expo; } #endif