diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 23855c0ed..8b0413d2f 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 aace0fe4c..010d0c023 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 4813d862d..3de6465c4 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 b5d667746..ac9110b1c 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 20ad9b1b6..19546bbb7 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