diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f41a291f1c..9b39237324 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1664,6 +1664,10 @@ static bool blackboxWriteSysinfo(void) #endif #endif +#ifdef USE_WING + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_DELAY_MS, "%d", currentPidProfile->tpa_delay_ms); +#endif + default: return true; } diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 30bd2d7c2d..66c94e1366 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -117,4 +117,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "MAG_CALIB", "MAG_TASK_RATE", "EZLANDING", + "TPA", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index ab4c0169ae..2feb381ad6 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -119,6 +119,7 @@ typedef enum { DEBUG_MAG_CALIB, DEBUG_MAG_TASK_RATE, DEBUG_EZLANDING, + DEBUG_TPA, DEBUG_COUNT } debugType_e; diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index ce56acb97d..4d71995280 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1259,6 +1259,10 @@ const clivalue_t valueTable[] = { { PARAM_NAME_TPA_LOW_BREAKPOINT, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { PWM_RANGE_MIN, PWM_RANGE_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_low_breakpoint) }, { PARAM_NAME_TPA_LOW_ALWAYS, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_low_always) }, +#ifdef USE_WING + { PARAM_NAME_TPA_DELAY_MS, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_delay_ms) }, +#endif + { PARAM_NAME_EZ_LANDING_THRESHOLD, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, ez_landing_threshold) }, { PARAM_NAME_EZ_LANDING_LIMIT, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 75 }, PG_PID_PROFILE, offsetof(pidProfile_t, ez_landing_limit) }, { PARAM_NAME_EZ_LANDING_SPEED, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, ez_landing_speed) }, diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 5984b2c619..c159930b79 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -30,6 +30,9 @@ #define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - 2nd order butterworth*/ +// PTn cutoff correction = 1 / sqrt(2^(1/n) - 1) +#define CUTOFF_CORRECTION_PT2 1.553773974f +#define CUTOFF_CORRECTION_PT3 1.961459177f // NULL filter @@ -48,6 +51,17 @@ FAST_CODE_NOINLINE float pt1FilterGain(float f_cut, float dT) return omega / (omega + 1.0f); } +// Calculates filter gain based on delay (time constant of filter) - time it takes for filter response to reach 63.2% of a step input. +float pt1FilterGainFromDelay(float delay, float dT) +{ + if (delay <= 0) { + return 1.0f; // gain = 1 means no filtering + } + + const float cutoffHz = 1.0f / (2.0f * M_PIf * delay); + return pt1FilterGain(cutoffHz, dT); +} + void pt1FilterInit(pt1Filter_t *filter, float k) { filter->state = 0.0f; @@ -70,13 +84,21 @@ FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input) FAST_CODE float pt2FilterGain(float f_cut, float dT) { - // PTn cutoff correction = 1 / sqrt(2^(1/n) - 1) - #define CUTOFF_CORRECTION_PT2 1.553773974f - // shift f_cut to satisfy -3dB cutoff condition return pt1FilterGain(f_cut * CUTOFF_CORRECTION_PT2, dT); } +// Calculates filter gain based on delay (time constant of filter) - time it takes for filter response to reach 63.2% of a step input. +float pt2FilterGainFromDelay(float delay, float dT) +{ + if (delay <= 0) { + return 1.0f; // gain = 1 means no filtering + } + + const float cutoffHz = 1.0f / (M_PIf * delay * CUTOFF_CORRECTION_PT2); + return pt2FilterGain(cutoffHz, dT); +} + void pt2FilterInit(pt2Filter_t *filter, float k) { filter->state = 0.0f; @@ -101,13 +123,21 @@ FAST_CODE float pt2FilterApply(pt2Filter_t *filter, float input) FAST_CODE float pt3FilterGain(float f_cut, float dT) { - // PTn cutoff correction = 1 / sqrt(2^(1/n) - 1) - #define CUTOFF_CORRECTION_PT3 1.961459177f - // shift f_cut to satisfy -3dB cutoff condition return pt1FilterGain(f_cut * CUTOFF_CORRECTION_PT3, dT); } +// Calculates filter gain based on delay (time constant of filter) - time it takes for filter response to reach 63.2% of a step input. +float pt3FilterGainFromDelay(float delay, float dT) +{ + if (delay <= 0) { + return 1.0f; // gain = 1 means no filtering + } + + const float cutoffHz = 1.0f / (M_PIf * delay * CUTOFF_CORRECTION_PT3); + return pt3FilterGain(cutoffHz, dT); +} + void pt3FilterInit(pt3Filter_t *filter, float k) { filter->state = 0.0f; diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 9ff87804c0..cc1bb9c43c 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -98,16 +98,19 @@ typedef struct meanAccumulator_s { float nullFilterApply(filter_t *filter, float input); float pt1FilterGain(float f_cut, float dT); +float pt1FilterGainFromDelay(float delay, float dT); void pt1FilterInit(pt1Filter_t *filter, float k); void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k); float pt1FilterApply(pt1Filter_t *filter, float input); float pt2FilterGain(float f_cut, float dT); +float pt2FilterGainFromDelay(float delay, float dT); void pt2FilterInit(pt2Filter_t *filter, float k); void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k); float pt2FilterApply(pt2Filter_t *filter, float input); float pt3FilterGain(float f_cut, float dT); +float pt3FilterGainFromDelay(float delay, float dT); void pt3FilterInit(pt3Filter_t *filter, float k); void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k); float pt3FilterApply(pt3Filter_t *filter, float input); diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 73b9240b9f..898b6f9395 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -58,6 +58,7 @@ #define PARAM_NAME_TPA_LOW_BREAKPOINT "tpa_low_breakpoint" #define PARAM_NAME_TPA_LOW_ALWAYS "tpa_low_always" #define PARAM_NAME_TPA_MODE "tpa_mode" +#define PARAM_NAME_TPA_DELAY_MS "tpa_delay_ms" #define PARAM_NAME_MIXER_TYPE "mixer_type" #define PARAM_NAME_EZ_LANDING_THRESHOLD "ez_landing_threshold" #define PARAM_NAME_EZ_LANDING_LIMIT "ez_landing_limit" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0660209cd8..116c9a2549 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -88,7 +88,7 @@ FAST_DATA_ZERO_INIT float throttleBoost; pt1Filter_t throttleLpf; #endif -PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 4); #ifndef DEFAULT_PID_PROCESS_DENOM #define DEFAULT_PID_PROCESS_DENOM 1 @@ -230,6 +230,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .ez_landing_threshold = 25, .ez_landing_limit = 15, .ez_landing_speed = 50, + .tpa_delay_ms = 0, ); #ifndef USE_D_MIN @@ -292,7 +293,18 @@ void pidUpdateTpaFactor(float throttle) } else { tpaRate = pidRuntime.tpaLowMultiplier * (pidRuntime.tpaLowBreakpoint - throttle); } - pidRuntime.tpaFactor = 1.0f - tpaRate; + + float tpaFactor = 1.0f - tpaRate; + DEBUG_SET(DEBUG_TPA, 0, lrintf(tpaFactor * 1000)); + +#ifdef USE_WING + if (isFixedWing()) { + tpaFactor = pt2FilterApply(&pidRuntime.tpaLpf, tpaFactor); + } +#endif + + DEBUG_SET(DEBUG_TPA, 1, lrintf(tpaFactor * 1000)); + pidRuntime.tpaFactor = tpaFactor; } void pidUpdateAntiGravityThrottleFilter(float throttle) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index f50935029b..e20725e754 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -245,6 +245,7 @@ typedef struct pidProfile_s { uint8_t ez_landing_threshold; // Threshold stick position below which motor output is limited uint8_t ez_landing_limit; // Maximum motor output when all sticks centred and throttle zero uint8_t ez_landing_speed; // Speed below which motor output is limited + uint16_t tpa_delay_ms; // TPA delay for fixed wings using pt2 filter (time constant) } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles); @@ -423,6 +424,10 @@ typedef struct pidRuntime_s { bool axisInAngleMode[3]; float maxRcRateInv[2]; #endif + +#ifdef USE_WING + pt2Filter_t tpaLpf; +#endif } pidRuntime_t; extern pidRuntime_t pidRuntime; diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index a949e7e86b..9ff1007831 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -257,6 +257,9 @@ void pidInitFilters(const pidProfile_t *pidProfile) #endif pt2FilterInit(&pidRuntime.antiGravityLpf, pt2FilterGain(pidProfile->anti_gravity_cutoff_hz, pidRuntime.dT)); +#ifdef USE_WING + pt2FilterInit(&pidRuntime.tpaLpf, pt2FilterGainFromDelay(pidProfile->tpa_delay_ms / 1000.0f, pidRuntime.dT)); +#endif } void pidInit(const pidProfile_t *pidProfile)