Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Additional TPA breakpoint for low Throttle #13006

Merged
merged 4 commits into from
Dec 4, 2023
Merged

Additional TPA breakpoint for low Throttle #13006

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
97405c8
Removed white spaces and everything that is not new
pichim Aug 1, 2023
4b1001b
Changes according to PR comments
pichim Nov 13, 2023
218ca0a
Update src/main/cms/cms_menu_imu.c
pichim Dec 4, 2023
2044889
Update src/main/flight/pid_init.c
pichim Dec 4, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
3 changes: 3 additions & 0 deletions src/main/blackbox/blackbox.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_VANISH, "%d", currentPidProfile->tpa_breakpoint_lower_vanish);
pichim marked this conversation as resolved.
Show resolved Hide resolved
BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d", currentControlRateProfile->rcRates[ROLL],
currentControlRateProfile->rcRates[PITCH],
currentControlRateProfile->rcRates[YAW]);
Expand Down
3 changes: 3 additions & 0 deletions src/main/cli/settings.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_VANISH, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_breakpoint_lower_vanish) },

// PG_TELEMETRY_CONFIG
#ifdef USE_TELEMETRY
Expand Down
15 changes: 15 additions & 0 deletions src/main/cms/cms_menu_imu.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_vanish;

static const void *cmsx_simplifiedTuningOnEnter(displayPort_t *pDisp)
{
Expand Down Expand Up @@ -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_vanish;

static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
{
Expand Down Expand Up @@ -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_vanish = pidProfile->tpa_breakpoint_lower_vanish;

return NULL;
}
Expand Down Expand Up @@ -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_vanish = cmsx_tpa_breakpoint_lower_vanish;

initEscEndpoints();
return NULL;
Expand Down Expand Up @@ -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 VAN", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_tpa_breakpoint_lower_vanish, 0, 1, 1} }, // ToDo: confirm this is correct

{ "BACK", OME_Back, NULL, NULL },
{ NULL, OME_END, NULL, NULL}
Expand Down
3 changes: 3 additions & 0 deletions src/main/fc/parameter_names.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_VANISH "tpa_breakpoint_lower_vanish"
#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
22 changes: 18 additions & 4 deletions src/main/flight/pid.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,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)
{
Expand Down Expand Up @@ -228,6 +228,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_vanish = 1,
);

#ifndef USE_D_MIN
Expand Down Expand Up @@ -277,9 +280,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 ?
const float throttleDifference = fmaxf(throttleTemp - pidRuntime.tpaBreakpoint, 0.0f);
pidRuntime.tpaFactor = 1.0f - throttleDifference * pidRuntime.tpaMultiplier;
static bool isTpaLowerVanished = 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;
float tpaRate = 0.0f;
if (isThrottlePastTpaBreakpointLower || isTpaLowerVanished) {
tpaRate = pidRuntime.tpaMultiplier * fmaxf(throttle - pidRuntime.tpaBreakpoint, 0.0f);
if (pidRuntime.tpaBreakpointLowerVanish && !isTpaLowerVanished) {
isTpaLowerVanished = true;
}
} else {
tpaRate = pidRuntime.tpaMultiplierLower * (pidRuntime.tpaBreakpointLower - throttle);
}
pidRuntime.tpaFactor = 1.0f - tpaRate;
}

void pidUpdateAntiGravityThrottleFilter(float throttle)
Expand Down
6 changes: 6 additions & 0 deletions src/main/flight/pid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please ensure the comment is absolutely clear. For example, does a value of 20 mean that the TPA effect will cut D to 20% of normal, or result in D that is 80% normal, at zero throttle?
Currently, TPA high of 80 will cut the D to 20% of normal.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

hey chris, since tpa lower is just an extension of tpa I basically copied the logic of how it is applied and adjusted the description according to tpa_rate and tpa_breakpoint. default values in the table above are now correct and the red line in the figure represents the defaults. maybe @SupaflyFPV has some clearer explanations?

Copy link
Member

@KarateBrot KarateBrot Dec 6, 2023 •
edited

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Pichi is very clear in his comment here. "Percent reduction" means that with a value of 20 you will get 20% reduction/attenuation, or 80% of your original value.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yeah its the same as TPA_RATE. It follows the same logic, just for the LOWER breakpoint. 👌

uint16_t tpa_breakpoint_lower; // Breakpoint where lower TPA is deactivated
pichim marked this conversation as resolved.
Show resolved Hide resolved
uint8_t tpa_breakpoint_lower_vanish; // 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);
Expand Down Expand Up @@ -324,6 +327,9 @@ typedef struct pidRuntime_s {
float tpaFactor;
float tpaBreakpoint;
float tpaMultiplier;
float tpaBreakpointLower;
float tpaMultiplierLower;
bool tpaBreakpointLowerVanish;

#ifdef USE_ITERM_RELAX
pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
Expand Down
4 changes: 4 additions & 0 deletions src/main/flight/pid_init.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -428,6 +428,10 @@ 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 tpaBreakpoint
pidRuntime.tpaBreakpointLower = constrainf((pidProfile->tpa_breakpoint_lower - PWM_RANGE_MIN) / 1000.0f, 0.01f, 1.0f);
pichim marked this conversation as resolved.
Show resolved Hide resolved
pidRuntime.tpaMultiplierLower = pidProfile->tpa_rate_lower / (100.0f * pidRuntime.tpaBreakpointLower);
pidRuntime.tpaBreakpointLowerVanish = pidProfile->tpa_breakpoint_lower_vanish;
}

void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
Expand Down