From a659189bf03e8923bd7426f056f545b58b4131ae Mon Sep 17 00:00:00 2001 From: Ivan Efimov Date: Mon, 4 Nov 2024 15:03:31 -0600 Subject: [PATCH] Yaw type rudder/diff_thrust for TPA calculations (for wings) (#13929) * Yaw type rudder or diff_thrust for TPA calculations (for wings) * Added yaw_type to blackbox header * KarateBrot review * KarateBrot break Co-authored-by: Jan Post --------- Co-authored-by: Jan Post --- src/main/blackbox/blackbox.c | 3 ++- src/main/cli/settings.c | 10 ++++++++-- src/main/cli/settings.h | 3 ++- src/main/fc/parameter_names.h | 1 + src/main/flight/pid.c | 42 +++++++++++++++++++++++++++++++++++++----- src/main/flight/pid.h | 15 +++++++++++++++ 6 files changed, 65 insertions(+), 9 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 9f029d36b..f8f744afd 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1745,7 +1745,8 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_ADV_THRUST, "%d", currentPidProfile->tpa_speed_adv_thrust); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_MAX_VOLTAGE, "%d", currentPidProfile->tpa_speed_max_voltage); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_PITCH_OFFSET, "%d", currentPidProfile->tpa_speed_pitch_offset); -#endif + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_YAW_TYPE, "%d", currentPidProfile->yaw_type); +#endif // USE_WING default: return true; diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index e6938bc9d..7a96425b6 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -543,7 +543,11 @@ const char* const lookupTableTpaCurveType[] = { const char* const lookupTableTpaSpeedType[] = { "BASIC", "ADVANCED", }; -#endif + +const char* const lookupTableYawType[] = { + "RUDDER", "DIFF_THRUST", +}; +#endif // USE_WING #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } @@ -673,7 +677,8 @@ const lookupTableEntry_t lookupTables[] = { #endif #ifdef USE_WING LOOKUP_TABLE_ENTRY(lookupTableTpaSpeedType), -#endif + LOOKUP_TABLE_ENTRY(lookupTableYawType), +#endif // USE_WING }; #undef LOOKUP_TABLE_ENTRY @@ -1316,6 +1321,7 @@ const clivalue_t valueTable[] = { { PARAM_NAME_SPA_YAW_CENTER, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, spa_center[FD_YAW]) }, { PARAM_NAME_SPA_YAW_WIDTH, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, spa_width[FD_YAW]) }, { PARAM_NAME_SPA_YAW_MODE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SPA_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, spa_mode[FD_YAW]) }, + { PARAM_NAME_YAW_TYPE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_YAW_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_type) }, #endif // PG_TELEMETRY_CONFIG diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h index f0fea366c..55ee1f621 100644 --- a/src/main/cli/settings.h +++ b/src/main/cli/settings.h @@ -148,7 +148,8 @@ typedef enum { #endif #ifdef USE_WING TABLE_TPA_SPEED_TYPE, -#endif + TABLE_YAW_TYPE, +#endif // USE_WING LOOKUP_TABLE_COUNT } lookupTableIndex_e; diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 69e466635..bb99e94a8 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -72,6 +72,7 @@ #define PARAM_NAME_TPA_SPEED_ADV_THRUST "tpa_speed_adv_thrust" #define PARAM_NAME_TPA_SPEED_MAX_VOLTAGE "tpa_speed_max_voltage" #define PARAM_NAME_TPA_SPEED_PITCH_OFFSET "tpa_speed_pitch_offset" +#define PARAM_NAME_YAW_TYPE "yaw_type" #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 febcfa80f..90494e4c9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -251,6 +251,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .tpa_speed_adv_thrust = 2000, .tpa_speed_max_voltage = 2520, .tpa_speed_pitch_offset = 0, + .yaw_type = YAW_TYPE_RUDDER, ); } @@ -363,7 +364,6 @@ float getTpaFactorClassic(float tpaArgument) void pidUpdateTpaFactor(float throttle) { - // don't permit throttle > 1 & throttle < 0 ? is this needed ? can throttle be > 1 or < 0 at this point throttle = constrainf(throttle, 0.0f, 1.0f); float tpaFactor; @@ -388,6 +388,18 @@ void pidUpdateTpaFactor(float throttle) DEBUG_SET(DEBUG_TPA, 0, lrintf(tpaFactor * 1000)); pidRuntime.tpaFactor = tpaFactor; + +#ifdef USE_WING + switch (currentPidProfile->yaw_type) { + case YAW_TYPE_DIFF_THRUST: + pidRuntime.tpaFactorYaw = getTpaFactorClassic(tpaArgument); + break; + case YAW_TYPE_RUDDER: + default: + pidRuntime.tpaFactorYaw = pidRuntime.tpaFactor; + break; + } +#endif // USE_WING } void pidUpdateAntiGravityThrottleFilter(float throttle) @@ -975,6 +987,28 @@ NOINLINE static void applySpa(int axis, const pidProfile_t *pidProfile) #endif // USE_WING } +static float getTpaFactor(const pidProfile_t *pidProfile, int axis, term_e term) +{ + float tpaFactor = pidRuntime.tpaFactor; + +#ifdef USE_WING + if (axis == FD_YAW) { + tpaFactor = pidRuntime.tpaFactorYaw; + } +#else + UNUSED(axis); +#endif + + switch (term) { + case TERM_P: + return (pidProfile->tpa_mode == TPA_MODE_PD) ? tpaFactor : 1.0f; + case TERM_D: + return tpaFactor; + default: + return 1.0f; + } +} + // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. // Based on 2DOF reference design (matlab) void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs) @@ -984,8 +1018,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim calculateSpaValues(pidProfile); - const float tpaFactorKp = (pidProfile->tpa_mode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f; - #ifdef USE_YAW_SPIN_RECOVERY const bool yawSpinActive = gyroYawSpinDetected(); #endif @@ -1160,7 +1192,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim // --------low-level gyro-based PID based on 2DOF PID controller. ---------- // -----calculate P component - pidData[axis].P = pidRuntime.pidCoefficient[axis].Kp * errorRate * tpaFactorKp; + pidData[axis].P = pidRuntime.pidCoefficient[axis].Kp * errorRate * getTpaFactor(pidProfile, axis, TERM_P); if (axis == FD_YAW) { pidData[axis].P = pidRuntime.ptermYawLowpassApplyFn((filter_t *) &pidRuntime.ptermYawLowpass, pidData[axis].P); } @@ -1259,7 +1291,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim preTpaD *= dMaxMultiplier; #endif - pidData[axis].D = preTpaD * pidRuntime.tpaFactor; + pidData[axis].D = preTpaD * getTpaFactor(pidProfile, axis, TERM_D); // Log the value of D pre application of TPA if (axis != FD_YAW) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 6eba82ce0..7ed76af28 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -94,6 +94,14 @@ typedef enum { } tpaMode_e; typedef enum { + TERM_P, + TERM_I, + TERM_D, + TERM_F, + TERM_S, +} term_e; + +typedef enum { SPA_MODE_OFF, SPA_MODE_I_FREEZE, SPA_MODE_I, @@ -168,6 +176,11 @@ typedef enum tpaSpeedType_e { TPA_SPEED_ADVANCED, } tpaSpeedType_t; +typedef enum { + YAW_TYPE_RUDDER, + YAW_TYPE_DIFF_THRUST, +} yawType_e; + #define MAX_PROFILE_NAME_LENGTH 8u typedef struct pidProfile_s { @@ -302,6 +315,7 @@ typedef struct pidProfile_s { uint16_t tpa_speed_adv_thrust; // For wings when tpa_speed_type = ADVANCED: stationary thrust in grams uint16_t tpa_speed_max_voltage; // For wings: theoretical max voltage; used for throttle scailing with voltage for air speed estimation int16_t tpa_speed_pitch_offset; // For wings: pitch offset in degrees*10 for craft speed estimation + uint8_t yaw_type; // For wings: type of yaw (rudder or differential thrust) } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles); @@ -500,6 +514,7 @@ typedef struct pidRuntime_s { #ifdef USE_WING float spa[XYZ_AXIS_COUNT]; // setpoint pid attenuation (0.0 to 1.0). 0 - full attenuation, 1 - no attenuation tpaSpeedParams_t tpaSpeed; + float tpaFactorYaw; #endif // USE_WING #ifdef USE_ADVANCED_TPA -- 2.11.4.GIT