Additional TPA breakpoint for low Throttle (#13006)

* Removed white spaces and everything that is not new

Rebase on master and update comment

Make TPA lower independent from air mode

Included tpa_breakpoint_lower_vanish option

Changes according to PR comments

Corrected comment for API version

Bugfix in msp.c

Additional TPA breakpoint for low throttle

* Changes according to PR comments

* Update src/main/cms/cms_menu_imu.c

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

* Update src/main/flight/pid_init.c

src/main/blackbox/blackbox.c

@@ -1402,6 +1402,9 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_MODE, "%d",               currentPidProfile->tpa_mode);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_RATE, "%d",               currentPidProfile->tpa_rate);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_BREAKPOINT, "%d",         currentPidProfile->tpa_breakpoint);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_RATE_LOWER, "%d",         currentPidProfile->tpa_rate_lower);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_BREAKPOINT_LOWER, "%d",   currentPidProfile->tpa_breakpoint_lower);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_BREAKPOINT_LOWER_FADE, "%d", currentPidProfile->tpa_breakpoint_lower_fade);
         BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d",                  currentControlRateProfile->rcRates[ROLL],
                                                                             currentControlRateProfile->rcRates[PITCH],
                                                                             currentControlRateProfile->rcRates[YAW]);

src/main/cli/settings.c

@@ -1255,6 +1255,9 @@ const clivalue_t valueTable[] = {
 #endif
     { PARAM_NAME_TPA_RATE,          VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_rate) },
     { PARAM_NAME_TPA_BREAKPOINT,    VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { PWM_RANGE_MIN, PWM_RANGE_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_breakpoint) },
+    { PARAM_NAME_TPA_RATE_LOWER,            VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_rate_lower) },
+    { PARAM_NAME_TPA_BREAKPOINT_LOWER,      VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { PWM_RANGE_MIN, PWM_RANGE_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_breakpoint_lower) },
+    { PARAM_NAME_TPA_BREAKPOINT_LOWER_FADE, VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_breakpoint_lower_fade) },
 
 // PG_TELEMETRY_CONFIG
 #ifdef USE_TELEMETRY

src/main/cms/cms_menu_imu.c

@@ -267,6 +267,9 @@ static uint8_t cmsx_simplified_gyro_filter;
 static uint8_t cmsx_simplified_gyro_filter_multiplier;
 static uint8_t cmsx_tpa_rate;
 static uint16_t cmsx_tpa_breakpoint;
+static uint8_t cmsx_tpa_rate_lower;
+static uint16_t cmsx_tpa_breakpoint_lower;
+static uint8_t cmsx_tpa_breakpoint_lower_fade;
 
 static const void *cmsx_simplifiedTuningOnEnter(displayPort_t *pDisp)
 {
@@ -553,6 +556,9 @@ static uint8_t cmsx_feedforward_jitter_factor;
 
 static uint8_t cmsx_tpa_rate;
 static uint16_t cmsx_tpa_breakpoint;
+static uint8_t cmsx_tpa_rate_lower;
+static uint16_t cmsx_tpa_breakpoint_lower;
+static uint8_t cmsx_tpa_breakpoint_lower_fade;
 
 static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
 {
@@ -605,6 +611,9 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
 #endif
     cmsx_tpa_rate = pidProfile->tpa_rate;
     cmsx_tpa_breakpoint = pidProfile->tpa_breakpoint;
+    cmsx_tpa_rate_lower = pidProfile->tpa_rate_lower;
+    cmsx_tpa_breakpoint_lower = pidProfile->tpa_breakpoint_lower;
+    cmsx_tpa_breakpoint_lower_fade = pidProfile->tpa_breakpoint_lower_fade;
 
     return NULL;
 }
@@ -660,6 +669,9 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
 #endif
     pidProfile->tpa_rate = cmsx_tpa_rate;
     pidProfile->tpa_breakpoint = cmsx_tpa_breakpoint;
+    pidProfile->tpa_rate_lower = cmsx_tpa_rate_lower;
+    pidProfile->tpa_breakpoint_lower = cmsx_tpa_breakpoint_lower;
+    pidProfile->tpa_breakpoint_lower_fade = cmsx_tpa_breakpoint_lower_fade;
 
     initEscEndpoints();
     return NULL;
@@ -717,6 +729,9 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
 
     { "TPA RATE",  OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &cmsx_tpa_rate,          0,  100,  1, 10} },
     { "TPA BRKPT",   OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_tpa_breakpoint, 1000, 2000, 10} },
+    { "TPA RATE LOW",      OME_FLOAT,  NULL, &(OSD_FLOAT_t) { &cmsx_tpa_rate_lower,        0,   100,    1, 10} },
+    { "TPA BRKPT LOW",     OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_tpa_breakpoint_lower,      1000, 2000, 10} },
+    { "TPA BRKPT LOW FAD", OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_tpa_breakpoint_lower_fade,    0,    1,  1} },
 
     { "BACK", OME_Back, NULL, NULL },
     { NULL, OME_END, NULL, NULL}

src/main/fc/parameter_names.h

@@ -54,6 +54,9 @@
 #define PARAM_NAME_RATES_TYPE "rates_type"
 #define PARAM_NAME_TPA_RATE "tpa_rate"
 #define PARAM_NAME_TPA_BREAKPOINT "tpa_breakpoint"
+#define PARAM_NAME_TPA_RATE_LOWER "tpa_rate_lower"
+#define PARAM_NAME_TPA_BREAKPOINT_LOWER "tpa_breakpoint_lower"
+#define PARAM_NAME_TPA_BREAKPOINT_LOWER_FADE "tpa_breakpoint_lower_fade"
 #define PARAM_NAME_TPA_MODE "tpa_mode"
 #define PARAM_NAME_THROTTLE_LIMIT_TYPE "throttle_limit_type"
 #define PARAM_NAME_THROTTLE_LIMIT_PERCENT "throttle_limit_percent"

src/main/flight/pid.c

@@ -116,7 +116,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 
 #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
 
-PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 7);
+PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 8);
 
 void resetPidProfile(pidProfile_t *pidProfile)
 {
@@ -224,6 +224,9 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .angle_feedforward_smoothing_ms = 80,
         .angle_earth_ref = 100,
         .horizon_delay_ms = 500, // 500ms time constant on any increase in horizon strength
+        .tpa_rate_lower = 20,
+        .tpa_breakpoint_lower = 1050,
+        .tpa_breakpoint_lower_fade = 1,
     );
 
 #ifndef USE_D_MIN
@@ -273,9 +276,20 @@ void pidResetIterm(void)
 
 void pidUpdateTpaFactor(float throttle)
 {
-    const float throttleTemp = fminf(throttle, 1.0f); // don't permit throttle > 1 ? is this needed ? can throttle be > 1 at this point ?
+    static bool isTpaLowerFaded = false;
-    const float throttleDifference = fmaxf(throttleTemp - pidRuntime.tpaBreakpoint, 0.0f);
+    // don't permit throttle > 1 & throttle < 0 ? is this needed ? can throttle be > 1 or < 0 at this point
-    pidRuntime.tpaFactor = 1.0f - throttleDifference * pidRuntime.tpaMultiplier;
+    throttle = constrainf(throttle, 0.0f, 1.0f);
+    bool isThrottlePastTpaBreakpointLower = (throttle < pidRuntime.tpaBreakpointLower && pidRuntime.tpaBreakpointLower > 0.01f) ? false : true;
+    float tpaRate = 0.0f;
+    if (isThrottlePastTpaBreakpointLower || isTpaLowerFaded) {
+        tpaRate = pidRuntime.tpaMultiplier * fmaxf(throttle - pidRuntime.tpaBreakpoint, 0.0f);
+        if (pidRuntime.tpaBreakpointLowerFade && !isTpaLowerFaded) {
+            isTpaLowerFaded = true;
+        }
+    } else {
+        tpaRate = pidRuntime.tpaMultiplierLower * (pidRuntime.tpaBreakpointLower - throttle);
+    }
+    pidRuntime.tpaFactor = 1.0f - tpaRate;
 }
 
 void pidUpdateAntiGravityThrottleFilter(float throttle)

src/main/flight/pid.h

@@ -237,6 +237,9 @@ typedef struct pidProfile_s {
     uint8_t angle_feedforward_smoothing_ms; // Smoothing factor for angle feedforward as time constant in milliseconds
     uint8_t angle_earth_ref;         // Control amount of "co-ordination" from yaw into roll while pitched forward in angle mode
     uint16_t horizon_delay_ms;           // delay when Horizon Strength increases, 50 = 500ms time constant
+    uint8_t tpa_rate_lower;                 // Percent reduction in P or D at zero throttle
+    uint16_t tpa_breakpoint_lower;          // Breakpoint where lower TPA is deactivated
+    uint8_t tpa_breakpoint_lower_fade;      // off, on - if on lower TPA is only active until tpa_breakpoint_lower is reached the first time
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@@ -324,6 +327,9 @@ typedef struct pidRuntime_s {
     float tpaFactor;
     float tpaBreakpoint;
     float tpaMultiplier;
+    float tpaBreakpointLower;
+    float tpaMultiplierLower;
+    bool tpaBreakpointLowerFade;
 
 #ifdef USE_ITERM_RELAX
     pt1Filter_t windupLpf[XYZ_AXIS_COUNT];

src/main/flight/pid_init.c

@@ -428,6 +428,11 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     pidRuntime.tpaBreakpoint = constrainf((pidProfile->tpa_breakpoint - PWM_RANGE_MIN) / 1000.0f, 0.0f, 0.99f);
     // default of 1350 returns 0.35. range limited to 0 to 0.99
     pidRuntime.tpaMultiplier = (pidProfile->tpa_rate / 100.0f) / (1.0f - pidRuntime.tpaBreakpoint);
+    // it is assumed that tpaBreakpointLower is always less than or equal to tpaBreakpoint
+    pidRuntime.tpaBreakpointLower = constrainf((pidProfile->tpa_breakpoint_lower - PWM_RANGE_MIN) / 1000.0f, 0.01f, 1.0f);
+    pidRuntime.tpaBreakpointLower = MIN(pidRuntime.tpaBreakpointLower, pidRuntime.tpaBreakpoint);
+    pidRuntime.tpaMultiplierLower = pidProfile->tpa_rate_lower / (100.0f * pidRuntime.tpaBreakpointLower);
+    pidRuntime.tpaBreakpointLowerFade = pidProfile->tpa_breakpoint_lower_fade;
 }
 
 void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)