From ff8d1a842a5b3a1f2c8020d726d5eb724a0cd9da Mon Sep 17 00:00:00 2001 From: Kenneth Mitchell Date: Thu, 25 Oct 2018 20:17:08 -0400 Subject: [PATCH] Fixes from reviews. --- src/main/flight/pid.c | 46 ++++++++++++++++-------------------- src/main/sensors/gyro.c | 38 ++++++++++++++--------------- src/main/sensors/gyro.h | 2 +- src/main/target/common_pre.h | 2 +- 4 files changed, 41 insertions(+), 47 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e8813e84f..823d4362f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -121,16 +121,11 @@ void resetPidProfile(pidProfile_t *pidProfile) [PID_LEVEL] = { 50, 50, 75, 0 }, [PID_MAG] = { 40, 0, 0, 0 }, }, - .pidSumLimit = PIDSUM_LIMIT, .pidSumLimitYaw = PIDSUM_LIMIT_YAW, .yaw_lowpass_hz = 0, - .dterm_lowpass_hz = 100, // dual PT1 filtering ON by default - .dterm_lowpass2_hz = 200, // second Dterm LPF ON by default .dterm_notch_hz = 0, .dterm_notch_cutoff = 0, - .dterm_filter_type = FILTER_PT1, - .dterm_filter2_type = FILTER_PT1, .itermWindupPointPercent = 40, .vbatPidCompensation = 0, .pidAtMinThrottle = PID_STABILISATION_ON, @@ -169,9 +164,13 @@ void resetPidProfile(pidProfile_t *pidProfile) .antiGravityMode = ANTI_GRAVITY_SMOOTH, .dyn_lpf_dterm_max_hz = 200, .dyn_lpf_dterm_idle = 20, + .dterm_lowpass_hz = 100, // dual PT1 filtering ON by default + .dterm_lowpass2_hz = 200, // second Dterm LPF ON by default + .dterm_filter_type = FILTER_PT1, + .dterm_filter2_type = FILTER_PT1, ); #ifdef USE_DYN_LPF - pidProfile->dterm_lowpass_hz = 150; + pidProfile->dterm_lowpass_hz = 120; pidProfile->dterm_lowpass2_hz = 180; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_filter2_type = FILTER_BIQUAD; @@ -452,8 +451,7 @@ void pidUpdateAntiGravityThrottleFilter(float throttle) 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 float dynLpfInvIdlePointScaled; static FAST_RAM_ZERO_INIT uint16_t dynLpfMin; #endif @@ -537,15 +535,15 @@ void pidInitConfig(const pidProfile_t *pidProfile) 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; + case FILTER_PT1: + dynLpfFilter = DYN_LPF_PT1; + break; + case FILTER_BIQUAD: + dynLpfFilter = DYN_LPF_BIQUAD; + break; + default: + dynLpfFilter = DYN_LPF_NONE; + break; } } } else { @@ -553,8 +551,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) } 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; + dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (pidProfile->dyn_lpf_dterm_max_hz - dynLpfMin); #endif } @@ -1180,21 +1177,20 @@ bool pidAntiGravityEnabled(void) #ifdef USE_DYN_LPF void dynLpfDTermUpdate(float throttle) { - if (dynLpfFilter == DYN_LPF_PT1 || dynLpfFilter == DYN_LPF_BIQUAD) { + if (dynLpfFilter != DYN_LPF_NONE) { uint16_t cutoffFreq = dynLpfMin; if (throttle > dynLpfIdle) { - const float dynThrottle = (throttle - (throttle * throttle * throttle) * 0.333f) * 1.5f; - cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePoint * dynLpfDiff; + const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f; + cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled; } 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)); + pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, dT)); } } else { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, gyro.targetLooptime); + biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime); } } } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 8f4433da8..84e658b57 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -192,7 +192,7 @@ PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5); void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig) { gyroConfig->gyro_align = ALIGN_DEFAULT; - gyroConfig->gyroCalibrationDuration = 125; // 1gyroConfig->25 seconds + gyroConfig->gyroCalibrationDuration = 125; // 1.25 seconds gyroConfig->gyroMovementCalibrationThreshold = 48; gyroConfig->gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT; gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL; @@ -218,7 +218,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig) gyroConfig->dyn_lpf_gyro_max_hz = 400; gyroConfig->dyn_lpf_gyro_idle = 20; #ifdef USE_DYN_LPF - gyroConfig->gyro_lowpass_hz = 150; + gyroConfig->gyro_lowpass_hz = 120; #endif } @@ -543,8 +543,7 @@ bool gyroInit(void) 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 float dynLpfInvIdlePointScaled; static FAST_RAM_ZERO_INIT uint16_t dynLpfMin; static void dynLpfFilterInit() @@ -553,15 +552,15 @@ static void dynLpfFilterInit() 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; + case FILTER_PT1: + dynLpfFilter = DYN_LPF_PT1; + break; + case FILTER_BIQUAD: + dynLpfFilter = DYN_LPF_BIQUAD; + break; + default: + dynLpfFilter = DYN_LPF_NONE; + break; } } } else { @@ -569,8 +568,7 @@ static void dynLpfFilterInit() } 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; + dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (gyroConfig()->dyn_lpf_gyro_max_hz - dynLpfMin); } #endif @@ -1202,13 +1200,13 @@ uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg) #ifdef USE_DYN_LPF void dynLpfGyroUpdate(float throttle) { - if (dynLpfFilter == DYN_LPF_PT1 || dynLpfFilter == DYN_LPF_BIQUAD) { + if (dynLpfFilter != DYN_LPF_NONE) { 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 dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f; + cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled; + } + 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 diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 17086b7a3..7d35e8681 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -57,7 +57,7 @@ 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 diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index ec5f09a43..e75ba96ad 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -189,7 +189,7 @@ #define USE_THROTTLE_BOOST #define USE_RC_SMOOTHING_FILTER #define USE_ITERM_RELAX -//#define USE_DYN_LPF +#define USE_DYN_LPF #ifdef USE_SERIALRX_SPEKTRUM #define USE_SPEKTRUM_BIND