Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 15:25:36 +03:00
Code Issues Wiki Activity

Fix TPA (#11969)

Browse source
This commit is contained in:
haslinghuis 2022-11-09 11:18:14 +01:00 • committed by GitHub
parent b44129f38d
commit 139210e403
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 18 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/blackbox/blackbox.c
View file

@ -1327,6 +1327,7 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PID_PROCESS_DENOM, "%d", activePidLoopDenom);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THR_MID, "%d", currentControlRateProfile->thrMid8);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THR_EXPO, "%d", currentControlRateProfile->thrExpo8);
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("rc_rates", "%d,%d,%d", currentControlRateProfile->rcRates[ROLL],

6
src/main/cli/settings.c
View file

@ -1059,9 +1059,6 @@ const clivalue_t valueTable[] = {
{ "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
{ "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
#endif
#ifdef USE_TPA_MODE
{ PARAM_NAME_TPA_MODE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TPA_MODE }, PG_PID_CONFIG, offsetof(pidConfig_t, tpaMode) },
#endif
// PG_PID_PROFILE
#ifdef USE_PROFILE_NAMES
@ -1216,6 +1213,9 @@ const clivalue_t valueTable[] = {
{ 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) },
{ 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) },
#endif
#ifdef USE_TPA_MODE
{ PARAM_NAME_TPA_MODE, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TPA_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_mode) },
#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_PULSE_MIN, PWM_PULSE_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_breakpoint) },

8
src/main/cms/cms_menu_imu.c
View file

@ -530,6 +530,9 @@ static uint8_t cmsx_feedforward_smooth_factor;
static uint8_t cmsx_feedforward_jitter_factor;
#endif
static uint8_t cmsx_tpa_rate;
static uint16_t cmsx_tpa_breakpoint;
static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
{
UNUSED(pDisp);
@ -575,6 +578,8 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
#ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
cmsx_vbat_sag_compensation = pidProfile->vbat_sag_compensation;
#endif
cmsx_tpa_rate = pidProfile->tpa_rate;
cmsx_tpa_breakpoint = pidProfile->tpa_breakpoint;
return NULL;
}
@ -624,7 +629,6 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
#ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
pidProfile->vbat_sag_compensation = cmsx_vbat_sag_compensation;
#endif
pidProfile->tpa_rate = cmsx_tpa_rate;
pidProfile->tpa_breakpoint = cmsx_tpa_breakpoint;
@ -678,7 +682,7 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
{ "VBAT_SAG_COMP", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_vbat_sag_compensation, 0, 150, 1 } },
#endif
{ "THRPID ATT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_tpa_rate, 0, 100, 1, 10} },
{ "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} },
{ "BACK", OME_Back, NULL, NULL },

7
src/main/flight/pid.c
View file

@ -89,7 +89,7 @@ FAST_DATA_ZERO_INIT float throttleBoost;
pt1Filter_t throttleLpf;
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 4);
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 3);
#if defined(STM32F411xE)
#define PID_PROCESS_DENOM_DEFAULT 2
@ -103,12 +103,10 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
.runaway_takeoff_prevention = true,
.runaway_takeoff_deactivate_throttle = 20, // throttle level % needed to accumulate deactivation time
.runaway_takeoff_deactivate_delay = 500, // Accumulated time (in milliseconds) before deactivation in successful takeoff
.tpaMode = TPA_MODE_D
);
#else
PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
.pid_process_denom = PID_PROCESS_DENOM_DEFAULT,
.tpaMode = TPA_MODE_D
);
#endif
@ -222,6 +220,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.simplified_dterm_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT,
.anti_gravity_cutoff_hz = 5,
.anti_gravity_p_gain = 100,
.tpa_mode = TPA_MODE_D,
.tpa_rate = 65,
.tpa_breakpoint = 1350,
);
@ -835,7 +834,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
static float previousRawGyroRateDterm[XYZ_AXIS_COUNT];
#ifdef USE_TPA_MODE
const float tpaFactorKp = (pidConfig()->tpaMode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
const float tpaFactorKp = (pidProfile->tpa_mode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
#else
const float tpaFactorKp = pidRuntime.tpaFactor;
#endif

4
src/main/flight/pid.h
View file

@ -230,6 +230,7 @@ typedef struct pidProfile_s {
uint8_t anti_gravity_cutoff_hz;
uint8_t anti_gravity_p_gain;
uint8_t tpa_mode; // Controls which PID terms TPA effects
uint8_t tpa_rate; // Percent reduction in P or D at full throttle
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
} pidProfile_t;
@ -241,7 +242,6 @@ typedef struct pidConfig_s {
uint8_t runaway_takeoff_prevention; // off, on - enables pidsum runaway disarm logic
uint16_t runaway_takeoff_deactivate_delay; // delay in ms for "in-flight" conditions before deactivation (successful flight)
uint8_t runaway_takeoff_deactivate_throttle; // minimum throttle percent required during deactivation phase
uint8_t tpaMode; // Controls which PID terms TPA effects
} pidConfig_t;
PG_DECLARE(pidConfig_t, pidConfig);
@ -400,8 +400,6 @@ typedef struct pidRuntime_s {
#ifdef USE_ACC
pt3Filter_t attitudeFilter[2]; // Only for ROLL and PITCH
#endif
uint8_t tpa_rate;
uint16_t tpa_breakpoint;
} pidRuntime_t;
extern pidRuntime_t pidRuntime;

7
src/main/msp/msp.c
View file

@ -2020,6 +2020,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
#else
sbufWriteU8(dst, 0);
#endif
sbufWriteU8(dst, currentPidProfile->tpa_mode);
sbufWriteU8(dst, currentPidProfile->tpa_rate);
sbufWriteU16(dst, currentPidProfile->tpa_breakpoint); // was currentControlRateProfile->tpa_breakpoint
break;
@ -3168,10 +3169,10 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
sbufReadU8(src);
#endif
}
if (sbufBytesRemaining(src) >= 3) {
if (sbufBytesRemaining(src) >= 4) {
// Added in API 1.45
value = sbufReadU8(src);
currentPidProfile->tpa_rate = MIN(value, TPA_MAX);
currentPidProfile->tpa_mode = sbufReadU8(src);
currentPidProfile->tpa_rate = MIN(sbufReadU8(src), TPA_MAX);
currentPidProfile->tpa_breakpoint = sbufReadU16(src);
}