Skip to content

Commit da48910

Browse files
haslinghuishaslinghuis
committed
Move TPA to PID profile
1 parent d1bf5db commit da48910

File tree

12 files changed

+51
-45
lines changed

12 files changed

+51
-45
lines changed

‎src/main/blackbox/blackbox.c‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1325,8 +1325,8 @@ static bool blackboxWriteSysinfo(void)
13251325
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PID_PROCESS_DENOM, "%d", activePidLoopDenom);
13261326
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THR_MID, "%d", currentControlRateProfile->thrMid8);
13271327
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THR_EXPO, "%d", currentControlRateProfile->thrExpo8);
1328-
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_RATE, "%d", currentControlRateProfile->tpa_rate);
1329-
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_BREAKPOINT, "%d", currentControlRateProfile->tpa_breakpoint);
1328+
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_RATE, "%d", currentPidProfile->tpa_rate);
1329+
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_BREAKPOINT, "%d", currentPidProfile->tpa_breakpoint);
13301330
BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d", currentControlRateProfile->rcRates[ROLL],
13311331
currentControlRateProfile->rcRates[PITCH],
13321332
currentControlRateProfile->rcRates[YAW]);

‎src/main/cli/settings.c‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -973,11 +973,6 @@ const clivalue_t valueTable[] = {
973973
{ "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RATE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rates[FD_ROLL]) },
974974
{ "pitch_srate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RATE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rates[FD_PITCH]) },
975975
{ "yaw_srate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RATE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rates[FD_YAW]) },
976-
{ PARAM_NAME_TPA_RATE, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_TPA_MAX}, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, tpa_rate) },
977-
{ PARAM_NAME_TPA_BREAKPOINT, VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, tpa_breakpoint) },
978-
#ifdef USE_TPA_MODE
979-
{ "tpa_mode", VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TPA_MODE }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, tpaMode) },
980-
#endif
981976
{ PARAM_NAME_THROTTLE_LIMIT_TYPE, VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_THROTTLE_LIMIT_TYPE }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, throttle_limit_type) },
982977
{ PARAM_NAME_THROTTLE_LIMIT_PERCENT, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 25, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, throttle_limit_percent) },
983978
{ "roll_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_ROLL]) },
@@ -1058,6 +1053,9 @@ const clivalue_t valueTable[] = {
10581053
{ "runaway_takeoff_deactivate_delay", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 1000 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_deactivate_delay) }, // deactivate time in ms
10591054
{ "runaway_takeoff_deactivate_throttle_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_deactivate_throttle) }, // minimum throttle percentage during deactivation phase
10601055
#endif
1056+
#ifdef USE_TPA_MODE
1057+
{ PARAM_NAME_TPA_MODE, VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TPA_MODE }, PG_PID_CONFIG, offsetof(pidConfig_t, tpaMode) },
1058+
#endif
10611059

10621060
// PG_PID_PROFILE
10631061
#ifdef USE_PROFILE_NAMES
@@ -1213,6 +1211,8 @@ const clivalue_t valueTable[] = {
12131211
{ PARAM_NAME_SIMPLIFIED_GYRO_FILTER, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, simplified_gyro_filter) },
12141212
{ PARAM_NAME_SIMPLIFIED_GYRO_FILTER_MULTIPLIER, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { SIMPLIFIED_TUNING_FILTERS_MIN, SIMPLIFIED_TUNING_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, simplified_gyro_filter_multiplier) },
12151213
#endif
1214+
{ PARAM_NAME_TPA_RATE, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, TPA_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_rate) },
1215+
{ PARAM_NAME_TPA_BREAKPOINT, VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_breakpoint) },
12161216

12171217
// PG_TELEMETRY_CONFIG
12181218
#ifdef USE_TELEMETRY

‎src/main/cms/cms_menu_imu.c‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -246,6 +246,8 @@ static uint8_t cmsx_simplified_dterm_filter;
246246
static uint8_t cmsx_simplified_dterm_filter_multiplier;
247247
static uint8_t cmsx_simplified_gyro_filter;
248248
static uint8_t cmsx_simplified_gyro_filter_multiplier;
249+
static uint8_t cmsx_tpa_rate;
250+
static uint16_t cmsx_tpa_breakpoint;
249251

250252
static const void *cmsx_simplifiedTuningOnEnter(displayPort_t *pDisp)
251253
{
@@ -410,8 +412,6 @@ static const OSD_Entry cmsx_menuRateProfileEntries[] =
410412

411413
{ "THR MID", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.thrMid8, 0, 100, 1} },
412414
{ "THR EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.thrExpo8, 0, 100, 1} },
413-
{ "THRPID ATT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.tpa_rate, 0, 100, 1, 10} },
414-
{ "TPA BRKPT", OME_UINT16, NULL, &(OSD_UINT16_t){ &rateProfile.tpa_breakpoint, 1000, 2000, 10} },
415415

416416
{ "THR LIM TYPE",OME_TAB, NULL, &(OSD_TAB_t) { &rateProfile.throttle_limit_type, THROTTLE_LIMIT_TYPE_COUNT - 1, osdTableThrottleLimitType} },
417417
{ "THR LIM %", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.throttle_limit_percent, 25, 100, 1} },
@@ -625,6 +625,9 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
625625
pidProfile->vbat_sag_compensation = cmsx_vbat_sag_compensation;
626626
#endif
627627

628+
pidProfile->tpa_rate = cmsx_tpa_rate;
629+
pidProfile->tpa_breakpoint = cmsx_tpa_breakpoint;
630+
628631
initEscEndpoints();
629632
return NULL;
630633
}
@@ -675,6 +678,9 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
675678
{ "VBAT_SAG_COMP", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_vbat_sag_compensation, 0, 150, 1 } },
676679
#endif
677680

681+
{ "THRPID ATT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_tpa_rate, 0, 100, 1, 10} },
682+
{ "TPA BRKPT", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_tpa_breakpoint, 1000, 2000, 10} },
683+
678684
{ "BACK", OME_Back, NULL, NULL },
679685
{ NULL, OME_END, NULL, NULL}
680686
};

‎src/main/fc/controlrate_profile.c‎

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -37,16 +37,14 @@
3737

3838
controlRateConfig_t *currentControlRateProfile;
3939

40-
PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 4);
40+
PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 5);
4141

4242
void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
4343
{
4444
for (int i = 0; i < CONTROL_RATE_PROFILE_COUNT; i++) {
4545
RESET_CONFIG(controlRateConfig_t, &controlRateConfig[i],
4646
.thrMid8 = 50,
4747
.thrExpo8 = 0,
48-
.tpa_rate = 65,
49-
.tpa_breakpoint = 1350,
5048
.rates_type = RATES_TYPE_ACTUAL,
5149
.rcRates[FD_ROLL] = 7,
5250
.rcRates[FD_PITCH] = 7,
@@ -62,7 +60,6 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
6260
.rate_limit[FD_ROLL] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX,
6361
.rate_limit[FD_PITCH] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX,
6462
.rate_limit[FD_YAW] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX,
65-
.tpaMode = TPA_MODE_D,
6663
.profileName = { 0 },
6764
.quickRatesRcExpo = 0,
6865
.levelExpo[FD_ROLL] = 0,

‎src/main/fc/controlrate_profile.h‎

Lines changed: 0 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -46,11 +46,6 @@ typedef enum {
4646
THROTTLE_LIMIT_TYPE_COUNT // must be the last entry
4747
} throttleLimitType_e;
4848

49-
typedef enum {
50-
TPA_MODE_PD,
51-
TPA_MODE_D
52-
} tpaMode_e;
53-
5449
#define MAX_RATE_PROFILE_NAME_LENGTH 8u
5550

5651
typedef struct controlRateConfig_s {
@@ -60,12 +55,9 @@ typedef struct controlRateConfig_s {
6055
uint8_t rcRates[3];
6156
uint8_t rcExpo[3];
6257
uint8_t rates[3];
63-
uint8_t tpa_rate; // Percent reduction in P or D at full throttle
64-
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
6558
uint8_t throttle_limit_type; // Sets the throttle limiting type - off, scale or clip
6659
uint8_t throttle_limit_percent; // Sets the maximum pilot commanded throttle limit
6760
uint16_t rate_limit[3]; // Sets the maximum rate for the axes
68-
uint8_t tpaMode; // Controls which PID terms TPA effects
6961
char profileName[MAX_RATE_PROFILE_NAME_LENGTH + 1]; // Descriptive name for rate profile
7062
uint8_t quickRatesRcExpo; // Sets expo on rc command for quick rates
7163
uint8_t levelExpo[2]; // roll/pitch level mode expo

‎src/main/fc/parameter_names.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,7 @@
5454
#define PARAM_NAME_RATES_TYPE "rates_type"
5555
#define PARAM_NAME_TPA_RATE "tpa_rate"
5656
#define PARAM_NAME_TPA_BREAKPOINT "tpa_breakpoint"
57+
#define PARAM_NAME_TPA_MODE "tpa_mode"
5758
#define PARAM_NAME_THROTTLE_LIMIT_TYPE "throttle_limit_type"
5859
#define PARAM_NAME_THROTTLE_LIMIT_PERCENT "throttle_limit_percent"
5960
#define PARAM_NAME_GYRO_CAL_ON_FIRST_ARM "gyro_cal_on_first_arm"

‎src/main/fc/rc_controls.h‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -82,8 +82,6 @@ typedef enum {
8282
// (Super) rates are constrained to [0, 100] for Betaflight rates, so values higher than 100 won't make a difference. Range extended for RaceFlight rates.
8383
#define CONTROL_RATE_CONFIG_RATE_MAX 255
8484

85-
#define CONTROL_RATE_CONFIG_TPA_MAX 100
86-
8785
extern float rcCommand[4];
8886

8987
typedef struct rcSmoothingFilterTraining_s {

‎src/main/flight/mixer.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -519,7 +519,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
519519
pidUpdateAntiGravityThrottleFilter(throttle);
520520

521521
// and for TPA
522-
pidUpdateTpaFactor(throttle);
522+
pidUpdateTpaFactor(throttle, currentPidProfile);
523523

524524
#ifdef USE_DYN_LPF
525525
// keep the changes to dynamic lowpass clean, without unnecessary dynamic changes

‎src/main/flight/pid.c‎

Lines changed: 12 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -87,7 +87,7 @@ FAST_DATA_ZERO_INIT float throttleBoost;
8787
pt1Filter_t throttleLpf;
8888
#endif
8989

90-
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 3);
90+
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 4);
9191

9292
#if defined(STM32F411xE)
9393
#define PID_PROCESS_DENOM_DEFAULT 2
@@ -100,11 +100,13 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
100100
.pid_process_denom = PID_PROCESS_DENOM_DEFAULT,
101101
.runaway_takeoff_prevention = true,
102102
.runaway_takeoff_deactivate_throttle = 20, // throttle level % needed to accumulate deactivation time
103-
.runaway_takeoff_deactivate_delay = 500 // Accumulated time (in milliseconds) before deactivation in successful takeoff
103+
.runaway_takeoff_deactivate_delay = 500, // Accumulated time (in milliseconds) before deactivation in successful takeoff
104+
.tpaMode = TPA_MODE_D
104105
);
105106
#else
106107
PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
107-
.pid_process_denom = PID_PROCESS_DENOM_DEFAULT
108+
.pid_process_denom = PID_PROCESS_DENOM_DEFAULT,
109+
.tpaMode = TPA_MODE_D
108110
);
109111
#endif
110112

@@ -117,7 +119,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
117119

118120
#define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
119121

120-
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 4);
122+
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 5);
121123

122124
void resetPidProfile(pidProfile_t *pidProfile)
123125
{
@@ -218,6 +220,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
218220
.simplified_dterm_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT,
219221
.anti_gravity_cutoff_hz = 5,
220222
.anti_gravity_p_gain = 100,
223+
.tpa_rate = 65,
224+
.tpa_breakpoint = 1350,
221225
);
222226

223227
#ifndef USE_D_MIN
@@ -287,10 +291,10 @@ void pidResetIterm(void)
287291
}
288292
}
289293

290-
void pidUpdateTpaFactor(float throttle)
294+
void pidUpdateTpaFactor(float throttle, pidProfile_t *currentPidProfile)
291295
{
292-
const float tpaBreakpoint = (currentControlRateProfile->tpa_breakpoint - 1000) / 1000.0f;
293-
float tpaRate = currentControlRateProfile->tpa_rate / 100.0f;
296+
const float tpaBreakpoint = (currentPidProfile->tpa_breakpoint - 1000) / 1000.0f;
297+
float tpaRate = currentPidProfile->tpa_rate / 100.0f;
294298
if (throttle > tpaBreakpoint) {
295299
if (throttle < 1.0f) {
296300
tpaRate *= (throttle - tpaBreakpoint) / (1.0f - tpaBreakpoint);
@@ -824,7 +828,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
824828
static float previousRawGyroRateDterm[XYZ_AXIS_COUNT];
825829

826830
#ifdef USE_TPA_MODE
827-
const float tpaFactorKp = (currentControlRateProfile->tpaMode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
831+
const float tpaFactorKp = (pidConfig()->tpaMode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
828832
#else
829833
const float tpaFactorKp = pidRuntime.tpaFactor;
830834
#endif

‎src/main/flight/pid.h‎

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -68,6 +68,13 @@
6868
#define DTERM_LPF1_DYN_MAX_HZ_DEFAULT 150
6969
#define DTERM_LPF2_HZ_DEFAULT 150
7070

71+
#define TPA_MAX 100
72+
73+
typedef enum {
74+
TPA_MODE_PD,
75+
TPA_MODE_D
76+
} tpaMode_e;
77+
7178
typedef enum {
7279
PID_ROLL,
7380
PID_PITCH,
@@ -223,15 +230,18 @@ typedef struct pidProfile_s {
223230

224231
uint8_t anti_gravity_cutoff_hz;
225232
uint8_t anti_gravity_p_gain;
233+
uint8_t tpa_rate; // Percent reduction in P or D at full throttle
234+
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
226235
} pidProfile_t;
227236

228237
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
229238

230239
typedef struct pidConfig_s {
231-
uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate
240+
uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate
232241
uint8_t runaway_takeoff_prevention; // off, on - enables pidsum runaway disarm logic
233242
uint16_t runaway_takeoff_deactivate_delay; // delay in ms for "in-flight" conditions before deactivation (successful flight)
234243
uint8_t runaway_takeoff_deactivate_throttle; // minimum throttle percent required during deactivation phase
244+
uint8_t tpaMode; // Controls which PID terms TPA effects
235245
} pidConfig_t;
236246

237247
PG_DECLARE(pidConfig_t, pidConfig);
@@ -411,7 +421,7 @@ void pidSetItermAccelerator(float newItermAccelerator);
411421
bool crashRecoveryModeActive(void);
412422
void pidAcroTrainerInit(void);
413423
void pidSetAcroTrainerState(bool newState);
414-
void pidUpdateTpaFactor(float throttle);
424+
void pidUpdateTpaFactor(float throttle, pidProfile_t *profile);
415425
void pidUpdateAntiGravityThrottleFilter(float throttle);
416426
bool pidOsdAntiGravityActive(void);
417427
void pidSetAntiGravityState(bool newState);

0 commit comments

Comments
 (0)