From df24806487f1032ca64040b0f3a89266357ef06c Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Mon, 14 Sep 2026 20:17:49 -0500 Subject: [PATCH] Fix inverted TPA pitch-compensation sign in fixed-wing throttle calculateTPAThtrottle() computed groundCos as the NED down-component of the aircraft's forward vector (positive diving, negative climbing), then multiplied it directly by tpa_pitch_compensation, which is documented and range-limited (0-20) to *increase* throttle when climbing. The unsigned multiply produced the opposite: throttle was reduced when pitching up and increased when diving. The earth-frame Z sign convention here (raw frame is up-positive, not NED) was independently settled by #11869 while auditing the wind estimator, which flagged this exact TPA inversion as a follow-up. The underlying vForward/groundCos construction is correct and shared with wind_estimator.c's fuselageDirection, so the fix negates only at the point where groundCos is applied to throttleAdjustment, rather than touching the vector construction itself. tpa_pitch_compensation has always been >= 0 (settings.yaml min: 0), so no existing tune could have used a negative value to work around the inversion. Any fixed-wing craft currently relying on the old (inverted) behavior will see TPA pitch compensation reverse direction after this fix - flight testing is needed to confirm. --- src/main/flight/pid.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9beda7ae225..b8796346166 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -516,8 +516,13 @@ static float calculateTPAThtrottle(void) if (usedPidControllerType == PID_TYPE_PIFF && (currentControlProfile->throttle.fixedWingTauMs > 0)) { //fixed wing TPA with filtering fpVector3_t vForward = { .v = { HeadVecEFFiltered.x, -HeadVecEFFiltered.y, -HeadVecEFFiltered.z } }; + // vForward is in NED (see #11869); groundCos is its down-component, so it's + // positive when diving and negative when climbing. tpa_pitch_compensation + // is documented (and range-limited to >= 0) to *increase* throttle when + // climbing, so the sign must be flipped here rather than in groundCos + // itself, which wind_estimator.c's identical construction depends on. float groundCos = vectorDotProduct(&vForward, &vDown); - int16_t throttleAdjustment = currentControlProfile->throttle.tpa_pitch_compensation * groundCos * 90.0f / 1.57079632679f; //when 1deg pitch up, increase throttle by pitch(deg)_to_throttle. cos(89 deg)*90/(pi/2)=0.99995,cos(80 deg)*90/(pi/2)=9.9493, + int16_t throttleAdjustment = -currentControlProfile->throttle.tpa_pitch_compensation * groundCos * 90.0f / 1.57079632679f; //when 1deg pitch up, increase throttle by pitch(deg)_to_throttle. 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)); }