From 2262dfc60e14bd9cf3a8cad99ae33c2113f52178 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Mon, 24 Jan 2022 15:25:16 +1100 Subject: [PATCH] use Throttle Setpoint, not rcDATA(throttle), for TPA --- src/main/blackbox/blackbox.c | 2 +- src/main/cli/settings.c | 2 +- src/main/cms/cms_menu_imu.c | 2 +- src/main/fc/controlrate_profile.c | 2 +- src/main/fc/controlrate_profile.h | 2 +- src/main/fc/rc.c | 20 -------------------- src/main/fc/rc.h | 1 - src/main/flight/mixer.c | 3 +++ src/main/flight/pid.c | 22 +++++++++++++++++----- src/main/flight/pid.h | 2 ++ src/main/msp/msp.c | 4 ++-- src/main/target/ALIENWHOOP/config.c | 2 +- src/main/target/COLIBRI/config.c | 2 +- src/main/target/COLIBRI_RACE/i2c_bst.c | 4 ++-- src/test/unit/pid_unittest.cc | 4 +--- src/test/unit/rc_controls_unittest.cc | 6 +++--- 16 files changed, 37 insertions(+), 43 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 96e1bb79b2..552760f141 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -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], diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 38eb2760e3..893a131066 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -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) }, diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index f3f601f463..fd246f1ec1 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -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} }, diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index 382d079db7..8809dc5259 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -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, diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 107e162662..fac4129efa 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -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 diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 62357b325f..f09b95514a 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -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. diff --git a/src/main/fc/rc.h b/src/main/fc/rc.h index 4fe55b9b12..4babf5ebc8 100644 --- a/src/main/fc/rc.h +++ b/src/main/fc/rc.h @@ -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); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index b9e0ad992d..8d73134fa2 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9fbdef792e..6722dab7df 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 7b49f56039..a38e698a3a 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 612a4a7dcf..03c135fa16 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -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); diff --git a/src/main/target/ALIENWHOOP/config.c b/src/main/target/ALIENWHOOP/config.c index a85553d541..dd20b6af9c 100644 --- a/src/main/target/ALIENWHOOP/config.c +++ b/src/main/target/ALIENWHOOP/config.c @@ -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)? */ diff --git a/src/main/target/COLIBRI/config.c b/src/main/target/COLIBRI/config.c index abd7d401d5..6ae7899772 100644 --- a/src/main/target/COLIBRI/config.c +++ b/src/main/target/COLIBRI/config.c @@ -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; } diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 80d7fac22f..a5ef9e70a2 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -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(); diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 7e3b6d6630..93cc769817 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -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); diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 9ba4b80c54..cb2482247b 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -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 };