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 <Rm2k-Freak@web.de>

---------

Co-authored-by: Jan Post <Rm2k-Freak@web.de>

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;

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

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;
 

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"

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) {

src/main/flight/pid.h

@@ -93,6 +93,14 @@ typedef enum {
     TPA_MODE_D
 } 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,
@@ -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