diff --git a/docs/Settings.md b/docs/Settings.md index 628a144399e..4a2b4fd8b03 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -7208,7 +7208,7 @@ Fixed wing only. Pitch angle based bias for TPA. Used as a proxy for airspeed wh ### tpa_rate -Throttle based PID attenuation(TPA) reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. On multirotor, For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. for fixedwing modifies PIDFF. See **PID Attenuation and scaling** Wiki for full details. +Throttle PID attenuation (TPA) changes affect of PDFF on ROLL and PITCH of multi-rotor or PIDFF on ROLL, PITCH and YAW of fixed_wing as throttle changes. On multirotor PDFF is reduced by the TPA rate for every 1% throttle increase after the TPA breakpoint (Note: tpa_rate is limited to max of 100 for multirotor). For fixed wing it modifies PIDFF with additional pitch related throttle compensation if tpa_pitch_compensation set. See **PID Attenuation and scaling** Wiki for full details. | Default | Min | Max | | --- | --- | --- | diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index f35a79211b4..07d310f4acd 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -1994,7 +1994,7 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: | `dynamicThrottlePID` | `uint8_t` | 1 | Dynamic Throttle PID (TPA) value (`currentControlRateProfile->throttle.dynPID`) | | `throttleMid` | `uint8_t` | 1 | Throttle Midpoint (`currentControlRateProfile->throttle.rcMid8`) | | `throttleExpo` | `uint8_t` | 1 | Throttle Expo (`currentControlRateProfile->throttle.rcExpo8`) | -| `tpaBreakpoint` | `uint16_t` | 2 | Throttle PID Attenuation (TPA) breakpoint (`currentControlRateProfile->throttle.pa_breakpoint`) | +| `tpaBreakpoint` | `uint16_t` | 2 | Throttle PID Attenuation (TPA) breakpoint (`currentControlRateProfile->throttle.tpa_breakpoint`) | | `rcYawExpo` | `uint8_t` | 1 | Yaw RC Expo (`currentControlRateProfile->stabilized.rcYawExpo8`) | **Notes:** Superseded by `MSP2_INAV_RATE_PROFILE` which includes manual rates/expos. @@ -2421,7 +2421,7 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: | `dynamicThrottlePID` | `uint8_t` | 1 | Sets `currentControlRateProfile->throttle.dynPID` (constrained) | | `throttleMid` | `uint8_t` | 1 | Sets `currentControlRateProfile->throttle.rcMid8` | | `throttleExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile->throttle.rcExpo8` | -| `tpaBreakpoint` | `uint16_t` | 2 | Sets `currentControlRateProfile->throttle.pa_breakpoint` | +| `tpaBreakpoint` | `uint16_t` | 2 | Sets `currentControlRateProfile->throttle.tpa_breakpoint` | | `rcYawExpo` | `uint8_t` | 1 | (Optional) Sets `currentControlRateProfile->stabilized.rcYawExpo8` | **Reply Payload:** **None** @@ -3315,7 +3315,7 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: | `throttleMid` | `uint8_t` | 1 | Throttle Midpoint (`currentControlRateProfile->throttle.rcMid8`) | | `throttleExpo` | `uint8_t` | 1 | Throttle Expo (`currentControlRateProfile->throttle.rcExpo8`) | | `dynamicThrottlePID` | `uint8_t` | 1 | TPA value (`currentControlRateProfile->throttle.dynPID`) | -| `tpaBreakpoint` | `uint16_t` | 2 | TPA breakpoint (`currentControlRateProfile->throttle.pa_breakpoint`) | +| `tpaBreakpoint` | `uint16_t` | 2 | TPA breakpoint (`currentControlRateProfile->throttle.tpa_breakpoint`) | | `stabRcExpo` | `uint8_t` | 1 | Stabilized Roll/Pitch Expo (`currentControlRateProfile->stabilized.rcExpo8`) | | `stabRcYawExpo` | `uint8_t` | 1 | Stabilized Yaw Expo (`currentControlRateProfile->stabilized.rcYawExpo8`) | | `stabRollRate` | `uint8_t` | 1 | Stabilized Roll Rate (`currentControlRateProfile->stabilized.rates[FD_ROLL]`) | @@ -3336,7 +3336,7 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: | `throttleMid` | `uint8_t` | 1 | Sets `currentControlRateProfile->throttle.rcMid8` | | `throttleExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile->throttle.rcExpo8` | | `dynamicThrottlePID` | `uint8_t` | 1 | Sets `currentControlRateProfile->throttle.dynPID` | -| `tpaBreakpoint` | `uint16_t` | 2 | Sets `currentControlRateProfile->throttle.pa_breakpoint` | +| `tpaBreakpoint` | `uint16_t` | 2 | Sets `currentControlRateProfile->throttle.tpa_breakpoint` | | `stabRcExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rcExpo8` | | `stabRcYawExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rcYawExpo8` | | `stabRollRate` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rates[FD_ROLL]` (constrained) | diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 176833915ad..c471c665eed 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -4022,7 +4022,7 @@ { "name": "tpaBreakpoint", "ctype": "uint16_t", - "desc": "Throttle PID Attenuation (TPA) breakpoint (`currentControlRateProfile->throttle.pa_breakpoint`)", + "desc": "Throttle PID Attenuation (TPA) breakpoint (`currentControlRateProfile->throttle.tpa_breakpoint`)", "units": "" }, { @@ -5127,7 +5127,7 @@ { "name": "tpaBreakpoint", "ctype": "uint16_t", - "desc": "Sets `currentControlRateProfile->throttle.pa_breakpoint`", + "desc": "Sets `currentControlRateProfile->throttle.tpa_breakpoint`", "units": "" }, { @@ -7447,7 +7447,7 @@ { "name": "tpaBreakpoint", "ctype": "uint16_t", - "desc": "TPA breakpoint (`currentControlRateProfile->throttle.pa_breakpoint`)", + "desc": "TPA breakpoint (`currentControlRateProfile->throttle.tpa_breakpoint`)", "units": "" }, { @@ -7541,7 +7541,7 @@ { "name": "tpaBreakpoint", "ctype": "uint16_t", - "desc": "Sets `currentControlRateProfile->throttle.pa_breakpoint`", + "desc": "Sets `currentControlRateProfile->throttle.tpa_breakpoint`", "units": "" }, { diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 711a57a94c7..fcf095b5bac 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -2057,7 +2057,7 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlProfile->throttle.rcMid8); BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlProfile->throttle.rcExpo8); BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlProfile->throttle.dynPID); - BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlProfile->throttle.pa_breakpoint); + BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlProfile->throttle.tpa_breakpoint); BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlProfile->stabilized.rates[ROLL], currentControlProfile->stabilized.rates[PITCH], currentControlProfile->stabilized.rates[YAW]); diff --git a/src/main/fc/control_profile.c b/src/main/fc/control_profile.c index 316643b343f..74d5c5894bf 100644 --- a/src/main/fc/control_profile.c +++ b/src/main/fc/control_profile.c @@ -44,7 +44,7 @@ void pgResetFn_controlProfiles(controlConfig_t *instance) .rcExpo8 = SETTING_THR_EXPO_DEFAULT, .dynPID = SETTING_TPA_RATE_DEFAULT, .dynPID_on_YAW = SETTING_TPA_ON_YAW_DEFAULT, - .pa_breakpoint = SETTING_TPA_BREAKPOINT_DEFAULT, + .tpa_breakpoint = SETTING_TPA_BREAKPOINT_DEFAULT, .fixedWingTauMs = SETTING_FW_TPA_TIME_CONSTANT_DEFAULT, .apa_pow = SETTING_APA_POW_DEFAULT, .tpa_pitch_compensation = SETTING_TPA_PITCH_COMPENSATION_DEFAULT diff --git a/src/main/fc/control_profile_config_struct.h b/src/main/fc/control_profile_config_struct.h index 9300858fe36..d3e3da1ecac 100644 --- a/src/main/fc/control_profile_config_struct.h +++ b/src/main/fc/control_profile_config_struct.h @@ -30,9 +30,9 @@ typedef struct controlConfig_s { uint8_t rcExpo8; uint8_t dynPID; bool dynPID_on_YAW; - uint16_t pa_breakpoint; // Breakpoint where TPA is activated - uint16_t fixedWingTauMs; // Time constant of airplane TPA PT1-filter - uint16_t apa_pow; // Use airspeed instead of throttle position for TPA calculation,0 to disable + uint16_t tpa_breakpoint; // Breakpoint where TPA is activated + uint16_t fixedWingTauMs; // Time constant of airplane TPA PT1-filter + uint16_t apa_pow; // Use airspeed instead of throttle position for TPA calculation,0 to disable uint8_t tpa_pitch_compensation; // Pitch angle based throttle compensation for fixed wing } throttle; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 9f13f904302..c4e49546cb4 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -811,7 +811,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, currentControlProfile->throttle.dynPID); sbufWriteU8(dst, currentControlProfile->throttle.rcMid8); sbufWriteU8(dst, currentControlProfile->throttle.rcExpo8); - sbufWriteU16(dst, currentControlProfile->throttle.pa_breakpoint); + sbufWriteU16(dst, currentControlProfile->throttle.tpa_breakpoint); sbufWriteU8(dst, currentControlProfile->stabilized.rcYawExpo8); break; @@ -820,7 +820,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, currentControlProfile->throttle.rcMid8); sbufWriteU8(dst, currentControlProfile->throttle.rcExpo8); sbufWriteU8(dst, currentControlProfile->throttle.dynPID); - sbufWriteU16(dst, currentControlProfile->throttle.pa_breakpoint); + sbufWriteU16(dst, currentControlProfile->throttle.tpa_breakpoint); // stabilized sbufWriteU8(dst, currentControlProfile->stabilized.rcExpo8); @@ -2150,7 +2150,7 @@ typedef struct PACKED { uint8_t dynPID; uint8_t throttleRcMid8; uint8_t throttleRcExpo8; - uint16_t throttlePaBreakpoint; + uint16_t throttleTpaBreakpoint; } mspSetRcTuning_t; STATIC_ASSERT(sizeof(mspSetRcTuning_t) == 10, mspSetRcTuning_t_size); @@ -2158,7 +2158,7 @@ typedef struct PACKED { uint8_t throttleRcMid8; uint8_t throttleRcExpo8; uint8_t throttleDynPID; - uint16_t throttlePaBreakpoint; + uint16_t throttleTpaBreakpoint; uint8_t stabilizedRcExpo8; uint8_t stabilizedRcYawExpo8; uint8_t stabilizedRollRate; @@ -2312,7 +2312,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) currentControlProfile_p->throttle.dynPID = MIN(pkt.dynPID, SETTING_TPA_RATE_MAX); currentControlProfile_p->throttle.rcMid8 = pkt.throttleRcMid8; currentControlProfile_p->throttle.rcExpo8 = pkt.throttleRcExpo8; - currentControlProfile_p->throttle.pa_breakpoint = pkt.throttlePaBreakpoint; + currentControlProfile_p->throttle.tpa_breakpoint = pkt.throttleTpaBreakpoint; if (dataSize > sizeof(mspSetRcTuning_t)) { uint8_t rcYawExpo8; @@ -2343,7 +2343,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) currentControlProfile_p->throttle.rcMid8 = pkt.throttleRcMid8; currentControlProfile_p->throttle.rcExpo8 = pkt.throttleRcExpo8; currentControlProfile_p->throttle.dynPID = pkt.throttleDynPID; - currentControlProfile_p->throttle.pa_breakpoint = pkt.throttlePaBreakpoint; + currentControlProfile_p->throttle.tpa_breakpoint = pkt.throttleTpaBreakpoint; // stabilized currentControlProfile_p->stabilized.rcExpo8 = pkt.stabilizedRcExpo8; diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 99d884cbce3..2ae4dbbfc56 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -597,7 +597,7 @@ static void applyStepAdjustment(controlConfig_t *controlConfig, uint8_t adjustme applyAdjustmentU8(ADJUSTMENT_TPA, &controlConfig->throttle.dynPID, delta, 0, SETTING_TPA_RATE_MAX); break; case ADJUSTMENT_TPA_BREAKPOINT: - applyAdjustmentU16(ADJUSTMENT_TPA_BREAKPOINT, &controlConfig->throttle.pa_breakpoint, delta, PWM_RANGE_MIN, PWM_RANGE_MAX); + applyAdjustmentU16(ADJUSTMENT_TPA_BREAKPOINT, &controlConfig->throttle.tpa_breakpoint, delta, PWM_RANGE_MIN, PWM_RANGE_MAX); break; case ADJUSTMENT_FW_TPA_TIME_CONSTANT: applyAdjustmentU16(ADJUSTMENT_FW_TPA_TIME_CONSTANT, &controlConfig->throttle.fixedWingTauMs, delta, SETTING_FW_TPA_TIME_CONSTANT_MIN, SETTING_FW_TPA_TIME_CONSTANT_MAX); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c41daa4dfd9..0e2652d3472 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1457,7 +1457,7 @@ groups: min: 0 max: 200 - name: tpa_rate - description: "Throttle based PID attenuation(TPA) reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. On multirotor, For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. for fixedwing modifies PIDFF. See **PID Attenuation and scaling** Wiki for full details." + description: "Throttle PID attenuation (TPA) changes affect of PDFF on ROLL and PITCH of multi-rotor or PIDFF on ROLL, PITCH and YAW of fixed_wing as throttle changes. On multirotor PDFF is reduced by the TPA rate for every 1% throttle increase after the TPA breakpoint (Note: tpa_rate is limited to max of 100 for multirotor). For fixed wing it modifies PIDFF with additional pitch related throttle compensation if tpa_pitch_compensation set. See **PID Attenuation and scaling** Wiki for full details." default_value: 0 field: throttle.dynPID min: 0 @@ -1465,7 +1465,7 @@ groups: - name: tpa_breakpoint description: "See tpa_rate." default_value: 1500 - field: throttle.pa_breakpoint + field: throttle.tpa_breakpoint min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: tpa_pitch_compensation diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a9c8afc90cf..ce1703b3d48 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -673,11 +673,11 @@ static float calculateAutoTransitionTargetTPAFactor(const controlConfig_t *contr float tpaFactor; if (controlProfile->throttle.dynPID != 0 && - controlProfile->throttle.pa_breakpoint > getThrottleIdleValue() && + controlProfile->throttle.tpa_breakpoint > getThrottleIdleValue() && !FLIGHT_MODE(AUTO_TUNE) && ARMING_FLAG(ARMED)) { if (throttle > getThrottleIdleValue()) { - tpaFactor = 0.5f + ((float)(controlProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f); + tpaFactor = 0.5f + ((float)(controlProfile->throttle.tpa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f); } else { tpaFactor = 2.0f; } @@ -695,12 +695,12 @@ static float calculateAutoTransitionTargetMultirotorTPAFactor(const controlConfi { const uint16_t throttle = rcCommand[THROTTLE]; - if (controlProfile->throttle.dynPID == 0 || throttle < controlProfile->throttle.pa_breakpoint) { + if (controlProfile->throttle.dynPID == 0 || throttle < controlProfile->throttle.tpa_breakpoint) { return 1.0f; } if (throttle < getMaxThrottle()) { - return (100 - (uint16_t)controlProfile->throttle.dynPID * (throttle - controlProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - controlProfile->throttle.pa_breakpoint)) / 100.0f; + return (100 - (uint16_t)controlProfile->throttle.dynPID * (throttle - controlProfile->throttle.tpa_breakpoint) / (float)(getMaxThrottle() - controlProfile->throttle.tpa_breakpoint)) / 100.0f; } return (100 - constrain(controlProfile->throttle.dynPID, 0, 100)) / 100.0f; @@ -1297,67 +1297,62 @@ static float calculateFixedWingAirspeedITermFactor(void){ return iTermFactor; } +static int16_t tpaPitchThrottleAdjustment(void) +{ + /* Applies correction to throttle input for throttle based tpa to compensate for speed decrease in climb or increase during descent. + * +ve correction during descent, -ve correction during climb. + * e.g. +ve correction during descent increases tpa throttle input attenuating PIDS for increased speed during descent */ + + int16_t tpaThrottleAdjustment = 0; + + const uint8_t tpaPitchCompensationValue = currentControlProfile->throttle.tpa_pitch_compensation; + + if (currentControlProfile->throttle.fixedWingTauMs && tpaPitchCompensationValue) { + tpaThrottleAdjustment = constrain(tpaPitchCompensationValue * RADIANS_TO_DEGREES(-HeadVecEFFiltered.z), -PWM_RANGE_MIN, PWM_RANGE_MIN); + } + + return tpaThrottleAdjustment; +} + static float calculateFixedWingTPAFactor(uint16_t throttle) { - float tpaFactor; + const uint8_t dynamicPID = currentControlProfile->throttle.dynPID; + const uint16_t tpaBreakpoint = currentControlProfile->throttle.tpa_breakpoint; + const uint16_t throttleIdleValue = getThrottleIdleValue(); + float tpaFactor = 1.0f; // tpa_rate is amount of curve TPA applied to PIDs // tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned) - if (currentControlProfile->throttle.dynPID != 0 && currentControlProfile->throttle.pa_breakpoint > getThrottleIdleValue() && !FLIGHT_MODE(AUTO_TUNE) && ARMING_FLAG(ARMED)) { - if (throttle > getThrottleIdleValue()) { - // Calculate TPA according to throttle - tpaFactor = 0.5f + ((float)(currentControlProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f); - } - else { - tpaFactor = 2.0f; - } + if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(AUTO_TUNE) && dynamicPID && tpaBreakpoint > throttleIdleValue) { + // throttleIdleValue + 1 to avoid div zero + uint16_t pitchThrottleSpeedFactor = constrain(throttle + tpaPitchThrottleAdjustment(), throttleIdleValue + 1, getMaxThrottle()); + pitchThrottleSpeedFactor = pt1FilterApply(&fixedWingTpaFilter, pitchThrottleSpeedFactor); + + // Calculate TPA according to throttle with compensation for pitch attitude + tpaFactor = 0.5f + 0.5f * ((tpaBreakpoint - throttleIdleValue) / (float)(pitchThrottleSpeedFactor - throttleIdleValue)); // Attenuate TPA curve according to configured amount - tpaFactor = 1.0f + (tpaFactor - 1.0f) * (currentControlProfile->throttle.dynPID / 100.0f); - // Limit to [0.5; 2] range + tpaFactor = 1.0f + (tpaFactor - 1.0f) * (0.01f * dynamicPID); tpaFactor = constrainf(tpaFactor, 0.3f, 2.0f); } - else { - tpaFactor = 1.0f; - } return tpaFactor; } static float calculateMultirotorTPAFactor(uint16_t throttle) { - float tpaFactor; + const uint8_t dynamicPID = constrain(currentControlProfile->throttle.dynPID, 0, 100); + const uint16_t tpaBreakpoint = currentControlProfile->throttle.tpa_breakpoint; + float tpaFactor = 1.0f; // TPA should be updated only when TPA is actually set - if (currentControlProfile->throttle.dynPID == 0 || throttle < currentControlProfile->throttle.pa_breakpoint) { - tpaFactor = 1.0f; - } else if (throttle < getMaxThrottle()) { - tpaFactor = (100 - (uint16_t)currentControlProfile->throttle.dynPID * (throttle - currentControlProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlProfile->throttle.pa_breakpoint)) / 100.0f; - } else { - tpaFactor = (100 - constrain(currentControlProfile->throttle.dynPID, 0, 100)) / 100.0f; + if (dynamicPID && throttle > tpaBreakpoint) { + tpaFactor = 0.01f * (100.0f - (dynamicPID * (throttle - tpaBreakpoint) / (float)(getMaxThrottle() - tpaBreakpoint))); } return tpaFactor; } -static float calculateTPAThtrottle(void) -{ - uint16_t tpaThrottle = 0; - static const fpVector3_t vDown = { .v = { 0.0f, 0.0f, 1.0f } }; - - if (usedPidControllerType == PID_TYPE_PIFF && (currentControlProfile->throttle.fixedWingTauMs > 0)) { //fixed wing TPA with filtering - fpVector3_t vForward = { .v = { HeadVecEFFiltered.x, -HeadVecEFFiltered.y, -HeadVecEFFiltered.z } }; - float groundCos = vectorDotProduct(&vForward, &vDown); - int16_t throttleAdjustment = currentControlProfile->throttle.tpa_pitch_compensation * groundCos * 90.0f / 1.57079632679f; //groundCos is positive while diving; this raises the virtual throttle (and so attenuates PID gains) when pitching down, since diving increases airspeed. cos(89 deg)*90/(pi/2)=0.99995,cos(80 deg)*90/(pi/2)=9.9493, - uint16_t throttleAdjusted = rcCommand[THROTTLE] + constrain(throttleAdjustment, -1000, 1000); - tpaThrottle = pt1FilterApply(&fixedWingTpaFilter, constrain(throttleAdjusted, 1000, 2000)); - } - else { - tpaThrottle = rcCommand[THROTTLE]; //multirotor TPA without filtering - } - return tpaThrottle; -} - void schedulePidGainsUpdate(void) { pidGainsUpdateRequired = true; @@ -1381,32 +1376,31 @@ void updatePIDCoefficients(void) pidState[axis].stickPosition = constrain(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE, -500, 500) / 500.0f; } - float tpaFactor=1.0f; - float iTermFactor=1.0f; // Separate factor for I-term scaling - if(usedPidControllerType == PID_TYPE_PIFF){ // Fixed wing TPA calculation - if(currentControlProfile->throttle.apa_pow>0 && pitotGetValidForAirspeed()){ + float tpaFactor = 1.0f; + float iTermFactor = 1.0f; // Separate factor for I-term scaling + if (usedPidControllerType == PID_TYPE_PIFF) { // Fixed wing TPA calculation + if (currentControlProfile->throttle.apa_pow > 0 && pitotGetValidForAirspeed()) { tpaFactor = calculateFixedWingAirspeedTPAFactor(); iTermFactor = calculateFixedWingAirspeedITermFactor(); // Less aggressive I-term scaling - }else{ - tpaFactor = calculateFixedWingTPAFactor(calculateTPAThtrottle()); + } else { + tpaFactor = calculateFixedWingTPAFactor(rcCommand[THROTTLE]); iTermFactor = tpaFactor; // Use same factor for throttle-based TPA } } else { - tpaFactor = calculateMultirotorTPAFactor(calculateTPAThtrottle()); + tpaFactor = calculateMultirotorTPAFactor(rcCommand[THROTTLE]); iTermFactor = tpaFactor; // Multirotor uses same factor } + if (tpaFactor != tpaFactorprev) { pidGainsUpdateRequired = true; } tpaFactorprev = tpaFactor; - // If nothing changed - don't waste time recalculating coefficients if (!pidGainsUpdateRequired) { return; } - // PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change for (int axis = 0; axis < 3; axis++) { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 0543f8e6c36..7aa4ab020ab 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3943,7 +3943,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, "BP"); attr = TEXT_ATTRIBUTES_NONE; - osdFormatIntUnit(buff, 4, currentControlProfile->throttle.pa_breakpoint, 0); + osdFormatIntUnit(buff, 4, currentControlProfile->throttle.tpa_breakpoint, 0); if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA_BREAKPOINT)) { TEXT_ATTRIBUTES_ADD_BLINK(attr); } diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 110430e167c..3d87addbb8d 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -961,7 +961,7 @@ static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction) tfp_sprintf(buff, "TPA %3d", currentControlProfile->throttle.dynPID); break; case ADJUSTMENT_TPA_BREAKPOINT: - tfp_sprintf(buff, "TPABP %4d", currentControlProfile->throttle.pa_breakpoint); + tfp_sprintf(buff, "TPABP %4d", currentControlProfile->throttle.tpa_breakpoint); break; case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS: tfp_sprintf(buff, "CSM %3d", navConfigMutable()->fw.control_smoothness); @@ -1443,7 +1443,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms sbufWriteU8(dst, currentControlProfile->throttle.dynPID); sbufWriteU8(dst, currentControlProfile->throttle.rcMid8); sbufWriteU8(dst, currentControlProfile->throttle.rcExpo8); - sbufWriteU16(dst, currentControlProfile->throttle.pa_breakpoint); + sbufWriteU16(dst, currentControlProfile->throttle.tpa_breakpoint); sbufWriteU8(dst, currentControlProfile->stabilized.rcYawExpo8); sbufWriteU8(dst, 100); // INAV doesn't use rcRate sbufWriteU8(dst, 100); // INAV doesn't use rcRate