From e16ef1db4f9c12f00a2c56e7a989a9ff37b305ca Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Sat, 26 Jan 2019 12:28:51 +0100 Subject: [PATCH] First attempt low throttle airmode oscillation fix raise dc offset immediately Add airmode noise fix no filter when inactive and slightly more throttle hpf throttle to avoid large moves increasing throttle persistently fix ws fix ununsed variable and default to off include airmode throttle offset in loggingThrottle --- src/main/cli/settings.c | 3 +++ src/main/flight/mixer.c | 13 +++++++++++ src/main/flight/pid.c | 44 ++++++++++++++++++++++++++++++++++++ src/main/flight/pid.h | 5 ++++ src/main/target/common_pre.h | 1 + 5 files changed, 66 insertions(+) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 23855c0ed6..8b0413d2f0 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1012,6 +1012,9 @@ const clivalue_t valueTable[] = { #ifdef USE_THRUST_LINEARIZATION { "thrust_linear", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, thrustLinearization) }, #endif +#ifdef USE_AIRMODE_LPF + { "transient_throttle_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, transient_throttle_limit) }, +#endif // PG_TELEMETRY_CONFIG #ifdef USE_TELEMETRY diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index aace0fe4ca..010d0c0235 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -926,7 +926,13 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa } #endif +#ifdef USE_AIRMODE_LPF + const float unadjustedThrottle = throttle; + throttle += pidGetAirmodeThrottleOffset(); + float airmodeThrottleChange = 0; +#endif loggingThrottle = throttle; + motorMixRange = motorMixMax - motorMixMin; if (motorMixRange > 1.0f) { for (int i = 0; i < motorCount; i++) { @@ -939,9 +945,16 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa } else { if (airmodeEnabled || throttle > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle throttle = constrainf(throttle, -motorMixMin, 1.0f - motorMixMax); +#ifdef USE_AIRMODE_LPF + airmodeThrottleChange = constrainf(unadjustedThrottle, -motorMixMin, 1.0f - motorMixMax) - unadjustedThrottle; +#endif } } +#ifdef USE_AIRMODE_LPF + pidUpdateAirmodeLpf(airmodeThrottleChange); +#endif + if (featureIsEnabled(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !featureIsEnabled(FEATURE_3D) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 4813d862d4..3de6465c49 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -117,6 +117,10 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, #define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint #endif // USE_ACRO_TRAINER +#ifdef USE_AIRMODE_LPF +static FAST_RAM_ZERO_INIT float airmodeThrottleOffsetLimit; +#endif + #define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff #define CRASH_RECOVERY_DETECTION_DELAY_US 1000000 // 1 second delay before crash recovery detection is active after entering a self-level mode @@ -196,6 +200,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .d_min_advance = 20, .motor_output_limit = 100, .auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY, + .transient_throttle_limit = 0, ); #ifdef USE_DYN_LPF pidProfile->dterm_lowpass_hz = 150; @@ -287,6 +292,11 @@ static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis; static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType; #endif // USE_RC_SMOOTHING_FILTER +#ifdef USE_AIRMODE_LPF +static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf1; +static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf2; +#endif + static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf; void pidInitFilters(const pidProfile_t *pidProfile) @@ -415,6 +425,12 @@ void pidInitFilters(const pidProfile_t *pidProfile) pt1FilterInit(&dMinLowpass[axis], pt1FilterGain(D_MIN_LOWPASS_HZ, dT)); } #endif +#if defined(USE_AIRMODE_LPF) + if (pidProfile->transient_throttle_limit) { + pt1FilterInit(&airmodeThrottleLpf1, pt1FilterGain(7.0f, dT)); + pt1FilterInit(&airmodeThrottleLpf2, pt1FilterGain(20.0f, dT)); + } +#endif pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT)); } @@ -674,6 +690,9 @@ void pidInitConfig(const pidProfile_t *pidProfile) dMinSetpointGain = pidProfile->d_min_gain * D_MIN_SETPOINT_GAIN_FACTOR * pidProfile->d_min_advance * pidFrequency / (100 * D_MIN_LOWPASS_HZ); // lowpass included inversely in gain since stronger lowpass decreases peak effect #endif +#if defined(USE_AIRMODE_LPF) + airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f; +#endif } void pidInit(const pidProfile_t *pidProfile) @@ -1117,6 +1136,31 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm, } #endif +#ifdef USE_AIRMODE_LPF +void pidUpdateAirmodeLpf(float currentOffset) +{ + if (airmodeThrottleOffsetLimit == 0.0f) { + return; + } + + float offsetHpf = currentOffset * 2.5f; + offsetHpf = offsetHpf - pt1FilterApply(&airmodeThrottleLpf2, offsetHpf); + + // During high frequency oscillation 2 * currentOffset averages to the offset required to avoid mirroring of the waveform + pt1FilterApply(&airmodeThrottleLpf1, offsetHpf); + // Bring offset up immediately so the filter only applies to the decline + if (currentOffset * airmodeThrottleLpf1.state >= 0 && fabsf(currentOffset) > airmodeThrottleLpf1.state) { + airmodeThrottleLpf1.state = currentOffset; + } + airmodeThrottleLpf1.state = constrainf(airmodeThrottleLpf1.state, -airmodeThrottleOffsetLimit, airmodeThrottleOffsetLimit); +} + +float pidGetAirmodeThrottleOffset() +{ + return airmodeThrottleLpf1.state; +} +#endif + #ifdef USE_LAUNCH_CONTROL #define LAUNCH_CONTROL_MAX_RATE 100.0f #define LAUNCH_CONTROL_MIN_RATE 5.0f diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index b5d6677464..ac9110b1c8 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -165,6 +165,7 @@ typedef struct pidProfile_s { uint8_t d_min_advance; // Percentage multiplier for setpoint input to boost algorithm uint8_t motor_output_limit; // Upper limit of the motor output (percent) int8_t auto_profile_cell_count; // Cell count for this profile to be used with if auto PID profile switching is used + uint8_t transient_throttle_limit; // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles); @@ -220,6 +221,10 @@ bool pidAntiGravityEnabled(void); float pidApplyThrustLinearization(float motorValue); float pidCompensateThrustLinearization(float throttle); #endif +#ifdef USE_AIRMODE_LPF +void pidUpdateAirmodeLpf(float currentOffset); +float pidGetAirmodeThrottleOffset(); +#endif #ifdef UNIT_TEST #include "sensors/acceleration.h" diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 20ad9b1b65..19546bbb79 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -258,6 +258,7 @@ #endif // FLASH_SIZE > 128 #if (FLASH_SIZE > 256) +#define USE_AIRMODE_LPF #define USE_DASHBOARD #define USE_GPS #define USE_GPS_NMEA