Skip to content

Commit

Permalink
Renamed tpa....lower to tpa_low_..., + tpa_low_always = OFF by default (
Browse files Browse the repository at this point in the history
betaflight#13206)

Renamed tpa....lower to tpa_low_..., inverted the logic for it being active and renamed to a simple tpa_low_always which is OFF by default now
  • Loading branch information
Ivan Efimov authored Dec 8, 2023
1 parent b2ce402 commit 380d39e
Show file tree
Hide file tree
Showing 7 changed files with 46 additions and 46 deletions.
6 changes: 3 additions & 3 deletions src/main/blackbox/blackbox.c
Original file line number Diff line number Diff line change
Expand Up @@ -1402,9 +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(PARAM_NAME_TPA_LOW_RATE, "%d", currentPidProfile->tpa_low_rate);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_BREAKPOINT, "%d", currentPidProfile->tpa_low_breakpoint);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_ALWAYS, "%d", currentPidProfile->tpa_low_always);
BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d", currentControlRateProfile->rcRates[ROLL],
currentControlRateProfile->rcRates[PITCH],
currentControlRateProfile->rcRates[YAW]);
Expand Down
6 changes: 3 additions & 3 deletions src/main/cli/settings.c
Original file line number Diff line number Diff line change
Expand Up @@ -1255,9 +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) },
{ PARAM_NAME_TPA_LOW_RATE, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_low_rate) },
{ 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) },

// PG_TELEMETRY_CONFIG
#ifdef USE_TELEMETRY
Expand Down
34 changes: 17 additions & 17 deletions src/main/cms/cms_menu_imu.c
Original file line number Diff line number Diff line change
Expand Up @@ -267,9 +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 uint8_t cmsx_tpa_low_rate;
static uint16_t cmsx_tpa_low_breakpoint;
static uint8_t cmsx_tpa_low_always;

static const void *cmsx_simplifiedTuningOnEnter(displayPort_t *pDisp)
{
Expand Down Expand Up @@ -556,9 +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 uint8_t cmsx_tpa_low_rate;
static uint16_t cmsx_tpa_low_breakpoint;
static uint8_t cmsx_tpa_low_always;

static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
{
Expand Down Expand Up @@ -611,9 +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;
cmsx_tpa_low_rate = pidProfile->tpa_low_rate;
cmsx_tpa_low_breakpoint = pidProfile->tpa_low_breakpoint;
cmsx_tpa_low_always = pidProfile->tpa_low_always;

return NULL;
}
Expand Down Expand Up @@ -669,9 +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;
pidProfile->tpa_low_rate = cmsx_tpa_low_rate;
pidProfile->tpa_low_breakpoint = cmsx_tpa_low_breakpoint;
pidProfile->tpa_low_always = cmsx_tpa_low_always;

initEscEndpoints();
return NULL;
Expand Down Expand Up @@ -727,11 +727,11 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
{ "VBAT_SAG_COMP", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_vbat_sag_compensation, 0, 150, 1 } },
#endif

{ "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} },
{ "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 LOW RATE", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_tpa_low_rate, 0, 100, 1, 10} },
{ "TPA LOW BRKPT", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_tpa_low_breakpoint, 1000, 2000, 10} },
{ "TPA LOW ALWYS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_tpa_low_always, 0, 1, 1} },

{ "BACK", OME_Back, NULL, NULL },
{ NULL, OME_END, NULL, NULL}
Expand Down
6 changes: 3 additions & 3 deletions src/main/fc/parameter_names.h
Original file line number Diff line number Diff line change
Expand Up @@ -54,9 +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_LOW_RATE "tpa_low_rate"
#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_THROTTLE_LIMIT_TYPE "throttle_limit_type"
#define PARAM_NAME_THROTTLE_LIMIT_PERCENT "throttle_limit_percent"
Expand Down
18 changes: 9 additions & 9 deletions src/main/flight/pid.c
Original file line number Diff line number Diff line change
Expand Up @@ -224,9 +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,
.tpa_low_rate = 20,
.tpa_low_breakpoint = 1050,
.tpa_low_always = 0,
);

#ifndef USE_D_MIN
Expand Down Expand Up @@ -276,18 +276,18 @@ void pidResetIterm(void)

void pidUpdateTpaFactor(float throttle)
{
static bool isTpaLowerFaded = false;
static bool isTpaLowFaded = false;
// 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);
bool isThrottlePastTpaBreakpointLower = (throttle < pidRuntime.tpaBreakpointLower && pidRuntime.tpaBreakpointLower > 0.01f) ? false : true;
bool isThrottlePastTpaLowBreakpoint = (throttle < pidRuntime.tpaLowBreakpoint && pidRuntime.tpaLowBreakpoint > 0.01f) ? false : true;
float tpaRate = 0.0f;
if (isThrottlePastTpaBreakpointLower || isTpaLowerFaded) {
if (isThrottlePastTpaLowBreakpoint || isTpaLowFaded) {
tpaRate = pidRuntime.tpaMultiplier * fmaxf(throttle - pidRuntime.tpaBreakpoint, 0.0f);
if (pidRuntime.tpaBreakpointLowerFade && !isTpaLowerFaded) {
isTpaLowerFaded = true;
if (!pidRuntime.tpaLowAlways && !isTpaLowFaded) {
isTpaLowFaded = true;
}
} else {
tpaRate = pidRuntime.tpaMultiplierLower * (pidRuntime.tpaBreakpointLower - throttle);
tpaRate = pidRuntime.tpaLowMultiplier * (pidRuntime.tpaLowBreakpoint - throttle);
}
pidRuntime.tpaFactor = 1.0f - tpaRate;
}
Expand Down
12 changes: 6 additions & 6 deletions src/main/flight/pid.h
Original file line number Diff line number Diff line change
Expand Up @@ -237,9 +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
uint8_t tpa_low_rate; // Percent reduction in P or D at zero throttle
uint16_t tpa_low_breakpoint; // Breakpoint where lower TPA is deactivated
uint8_t tpa_low_always; // off, on - if OFF then low TPA is only active until tpa_low_breakpoint is reached the first time
} pidProfile_t;

PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
Expand Down Expand Up @@ -327,9 +327,9 @@ typedef struct pidRuntime_s {
float tpaFactor;
float tpaBreakpoint;
float tpaMultiplier;
float tpaBreakpointLower;
float tpaMultiplierLower;
bool tpaBreakpointLowerFade;
float tpaLowBreakpoint;
float tpaLowMultiplier;
bool tpaLowAlways;

#ifdef USE_ITERM_RELAX
pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
Expand Down
10 changes: 5 additions & 5 deletions src/main/flight/pid_init.c
Original file line number Diff line number Diff line change
Expand Up @@ -428,11 +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;
// it is assumed that tpaLowBreakpoint is always less than or equal to tpaBreakpoint
pidRuntime.tpaLowBreakpoint = constrainf((pidProfile->tpa_low_breakpoint - PWM_RANGE_MIN) / 1000.0f, 0.01f, 1.0f);
pidRuntime.tpaLowBreakpoint = MIN(pidRuntime.tpaLowBreakpoint, pidRuntime.tpaBreakpoint);
pidRuntime.tpaLowMultiplier = pidProfile->tpa_low_rate / (100.0f * pidRuntime.tpaLowBreakpoint);
pidRuntime.tpaLowAlways = pidProfile->tpa_low_always;
}

void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
Expand Down

0 comments on commit 380d39e

Please sign in to comment.