Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 11:59:58 +03:00
Code Issues Wiki Activity

use Throttle Setpoint, not rcDATA(throttle), for TPA

Browse source
This commit is contained in:
ctzsnooze 2022-01-24 15:25:16 +11:00
parent f2559fbfd8
commit 2262dfc60e
16 changed files with 37 additions and 43 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1322,7 +1322,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_RATE, "%d", currentControlRateProfile->dynThrPID);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_RATE, "%d", currentControlRateProfile->tpa_rate);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_BREAKPOINT, "%d", currentControlRateProfile->tpa_breakpoint);
BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d", currentControlRateProfile->rcRates[ROLL],
currentControlRateProfile->rcRates[PITCH],

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

@ -972,7 +972,7 @@ const clivalue_t valueTable[] = {
{ "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]) },
{ "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]) },
{ "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]) },
{ PARAM_NAME_TPA_RATE, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_TPA_MAX}, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, dynThrPID) },
{ 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) },
{ 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) },
#ifdef USE_TPA_MODE
{ "tpa_mode", VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TPA_MODE }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, tpaMode) },

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

@ -410,7 +410,7 @@ static const OSD_Entry cmsx_menuRateProfileEntries[] =
{ "THR MID", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.thrMid8, 0, 100, 1} },
{ "THR EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.thrExpo8, 0, 100, 1} },
{ "THRPID ATT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.dynThrPID, 0, 100, 1, 10} },
{ "THRPID ATT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.tpa_rate, 0, 100, 1, 10} },
{ "TPA BRKPT", OME_UINT16, NULL, &(OSD_UINT16_t){ &rateProfile.tpa_breakpoint, 1000, 2000, 10} },
{ "THR LIM TYPE",OME_TAB, NULL, &(OSD_TAB_t) { &rateProfile.throttle_limit_type, THROTTLE_LIMIT_TYPE_COUNT - 1, osdTableThrottleLimitType} },

2
src/main/fc/controlrate_profile.c
View file

@ -45,7 +45,7 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
RESET_CONFIG(controlRateConfig_t, &controlRateConfig[i],
.thrMid8 = 50,
.thrExpo8 = 0,
.dynThrPID = 65,
.tpa_rate = 65,
.tpa_breakpoint = 1350,
.rates_type = RATES_TYPE_ACTUAL,
.rcRates[FD_ROLL] = 7,

2
src/main/fc/controlrate_profile.h
View file

@ -60,7 +60,7 @@ typedef struct controlRateConfig_s {
uint8_t rcRates[3];
uint8_t rcExpo[3];
uint8_t rates[3];
uint8_t dynThrPID;
uint8_t tpa_rate; // Percent reduction in P or D at full throttle
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
uint8_t throttle_limit_type; // Sets the throttle limiting type - off, scale or clip
uint8_t throttle_limit_percent; // Sets the maximum pilot commanded throttle limit

20
src/main/fc/rc.c
View file

@ -65,7 +65,6 @@ float rcCommandDelta[XYZ_AXIS_COUNT];
static float rawSetpoint[XYZ_AXIS_COUNT];
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
static float rcDeflectionSmoothed[3];
static float throttlePIDAttenuation;
static bool reverseMotors = false;
static applyRatesFn *applyRates;
static uint16_t currentRxRefreshRate;
@ -131,11 +130,6 @@ float getRcDeflectionAbs(int axis)
return rcDeflectionAbs[axis];
}
float getThrottlePIDAttenuation(void)
{
return throttlePIDAttenuation;
}
#ifdef USE_FEEDFORWARD
float getRawSetpoint(int axis)
{
@ -633,20 +627,6 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
{
isRxDataNew = true;
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
int32_t prop;
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop = 100;
throttlePIDAttenuation = 1.0f;
} else {
if (rcData[THROTTLE] < 2000) {
prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
} else {
prop = 100 - currentControlRateProfile->dynThrPID;
}
throttlePIDAttenuation = prop / 100.0f;
}
for (int axis = 0; axis < 3; axis++) {
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.

1
src/main/fc/rc.h
View file

@ -33,7 +33,6 @@ void processRcCommand(void);
float getSetpointRate(int axis);
float getRcDeflection(int axis);
float getRcDeflectionAbs(int axis);
float getThrottlePIDAttenuation(void);
void updateRcCommands(void);
void resetYawAxis(void);
void initRcProcessing(void);

3
src/main/flight/mixer.c
View file

@ -518,6 +518,9 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
// use scaled throttle, without dynamic idle throttle offset, as the input to antigravity
pidUpdateAntiGravityThrottleFilter(throttle);
// and for TPA
pidUpdateTpaFactor(throttle);
#ifdef USE_DYN_LPF
// keep the changes to dynamic lowpass clean, without unnecessary dynamic changes
updateDynLpfCutoffs(currentTimeUs, throttle);

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

@ -291,6 +291,20 @@ void pidResetIterm(void)
}
}
void pidUpdateTpaFactor(float throttle)
{
const float tpaBreakpoint = (currentControlRateProfile->tpa_breakpoint - 1000) / 1000.0f;
float tpaRate = currentControlRateProfile->tpa_rate / 100.0f;
if (throttle > tpaBreakpoint) {
if (throttle < 1.0f) {
tpaRate *= (throttle - tpaBreakpoint) / (1.0f - tpaBreakpoint);
}
} else {
tpaRate = 0.0f;
}
pidRuntime.tpaFactor = 1.0f - tpaRate;
}
void pidUpdateAntiGravityThrottleFilter(float throttle)
{
if (pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) {
@ -816,8 +830,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
static bool gpsRescuePreviousState = false;
#endif
const float tpaFactor = getThrottlePIDAttenuation();
#if defined(USE_ACC)
const rollAndPitchTrims_t *angleTrim = &accelerometerConfig()->accelerometerTrims;
#else
@ -826,9 +838,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
#endif
#ifdef USE_TPA_MODE
const float tpaFactorKp = (currentControlRateProfile->tpaMode == TPA_MODE_PD) ? tpaFactor : 1.0f;
const float tpaFactorKp = (currentControlRateProfile->tpaMode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
#else
const float tpaFactorKp = tpaFactor;
const float tpaFactorKp = pidRuntime.tpaFactor;
#endif
#ifdef USE_YAW_SPIN_RECOVERY
@ -1079,7 +1091,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// Apply the dMinFactor
preTpaD *= dMinFactor;
#endif
pidData[axis].D = preTpaD * tpaFactor;
pidData[axis].D = preTpaD * pidRuntime.tpaFactor;
// Log the value of D pre application of TPA
preTpaD *= D_LPF_FILT_SCALE;

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

@ -310,6 +310,7 @@ typedef struct pidRuntime_s {
bool itermRotation;
bool zeroThrottleItermReset;
bool levelRaceMode;
float tpaFactor;
#ifdef USE_ITERM_RELAX
pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
@ -411,6 +412,7 @@ void pidSetItermAccelerator(float newItermAccelerator);
bool crashRecoveryModeActive(void);
void pidAcroTrainerInit(void);
void pidSetAcroTrainerState(bool newState);
void pidUpdateTpaFactor(float throttle);
void pidUpdateAntiGravityThrottleFilter(float throttle);
bool pidOsdAntiGravityActive(void);
bool pidOsdAntiGravityMode(void);

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

@ -1284,7 +1284,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
for (int i = 0 ; i < 3; i++) {
sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
}
sbufWriteU8(dst, currentControlRateProfile->dynThrPID);
sbufWriteU8(dst, currentControlRateProfile->tpa_rate);
sbufWriteU8(dst, currentControlRateProfile->thrMid8);
sbufWriteU8(dst, currentControlRateProfile->thrExpo8);
sbufWriteU16(dst, currentControlRateProfile->tpa_breakpoint);
@ -2566,7 +2566,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
}
value = sbufReadU8(src);
currentControlRateProfile->dynThrPID = MIN(value, CONTROL_RATE_CONFIG_TPA_MAX);
currentControlRateProfile->tpa_rate = MIN(value, CONTROL_RATE_CONFIG_TPA_MAX);
currentControlRateProfile->thrMid8 = sbufReadU8(src);
currentControlRateProfile->thrExpo8 = sbufReadU8(src);
currentControlRateProfile->tpa_breakpoint = sbufReadU16(src);

2
src/main/target/ALIENWHOOP/config.c
View file

@ -159,7 +159,7 @@ void targetConfiguration(void)
controlRateConfig->rates[FD_YAW] = 0;
/* Throttle PID Attenuation (TPA) */
controlRateConfig->dynThrPID = 0; // tpa_rate off
controlRateConfig->tpa_rate = 0; // tpa_rate off
controlRateConfig->tpa_breakpoint = 1600;
/* Force the clipping mixer at 100% seems better for brushed than default (off) and scaling)? */

2
src/main/target/COLIBRI/config.c
View file

@ -63,7 +63,7 @@ void targetConfiguration(void)
for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) {
controlRateConfig_t *controlRateConfig = controlRateProfilesMutable(rateProfileIndex);
controlRateConfig->dynThrPID = 45;
controlRateConfig->tpa_rate = 45;
controlRateConfig->tpa_breakpoint = 1700;
}

4
src/main/target/COLIBRI_RACE/i2c_bst.c
View file

@ -322,7 +322,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
for (i = 0 ; i < 3; i++) {
bstWrite8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
}
bstWrite8(currentControlRateProfile->dynThrPID);
bstWrite8(currentControlRateProfile->tpa_rate);
bstWrite8(currentControlRateProfile->thrMid8);
bstWrite8(currentControlRateProfile->thrExpo8);
bstWrite16(currentControlRateProfile->tpa_breakpoint);
@ -460,7 +460,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
currentControlRateProfile->rates[i] = bstRead8();
}
rate = bstRead8();
currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX);
currentControlRateProfile->tpa_rate = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX);
currentControlRateProfile->thrMid8 = bstRead8();
currentControlRateProfile->thrExpo8 = bstRead8();
currentControlRateProfile->tpa_breakpoint = bstRead16();

4
src/test/unit/pid_unittest.cc
View file

@ -28,7 +28,6 @@ bool simulatedAirmodeEnabled = true;
float simulatedSetpointRate[3] = { 0,0,0 };
float simulatedPrevSetpointRate[3] = { 0,0,0 };
float simulatedRcDeflection[3] = { 0,0,0 };
float simulatedThrottlePIDAttenuation = 1.0f;
float simulatedMotorMixRange = 0.0f;
int16_t debug[DEBUG16_VALUE_COUNT];
@ -78,7 +77,6 @@ extern "C" {
bool unitLaunchControlActive = false;
launchControlMode_e unitLaunchControlMode = LAUNCH_CONTROL_MODE_NORMAL;
float getThrottlePIDAttenuation(void) { return simulatedThrottlePIDAttenuation; }
float getMotorMixRange(void) { return simulatedMotorMixRange; }
float getSetpointRate(int axis) { return simulatedSetpointRate[axis]; }
bool isAirmodeActivated() { return simulatedAirmodeEnabled; }
@ -180,7 +178,7 @@ timeUs_t currentTestTime(void) {
void resetTest(void) {
loopIter = 0;
simulatedThrottlePIDAttenuation = 1.0f;
pidRuntime.tpaFactor = 1.0f;
simulatedMotorMixRange = 0.0f;
pidStabilisationState(PID_STABILISATION_OFF);

6
src/test/unit/rc_controls_unittest.cc
View file

@ -259,7 +259,7 @@ protected:
.thrMid8 = 0,
.thrExpo8 = 0,
.rates = {0, 0, 0},
.dynThrPID = 0,
.tpa_rate = 0,
.rcExpo[FD_YAW] = 0,
.tpa_breakpoint = 0
};
@ -287,7 +287,7 @@ protected:
controlRateConfig.rates[0] = 0;
controlRateConfig.rates[1] = 0;
controlRateConfig.rates[2] = 0;
controlRateConfig.dynThrPID = 0;
controlRateConfig.tpa_rate = 0;
controlRateConfig.tpa_breakpoint = 0;
PG_RESET(adjustmentRanges);
@ -365,7 +365,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp
.thrMid8 = 0,
.thrExpo8 = 0,
.rates = {0,0,0},
.dynThrPID = 0,
.tpa_rate = 0,
.rcExpo[FD_YAW] = 0,
.tpa_breakpoint = 0
};