Fix inverted TPA pitch-compensation sign in fixed-wing throttle

[sensei-hacker]

Summary

calculateTPAThtrottle() in src/main/flight/pid.c applies tpa_pitch_compensation with the wrong sign, so fixed-wing TPA pitch compensation currently reduces throttle when climbing and increases it when diving — the opposite of the documented behavior ("Positive values will increase throttle when pitching up, and decrease throttle when pitching down").

Root cause

groundCos is the NED down-component of the aircraft's forward vector (vForward = {x, -y, -z} of HeadVecEFFiltered), so it is positive while diving and negative while climbing. tpa_pitch_compensation is range-limited to [0, 20] (always non-negative), so multiplying it directly by groundCos decreases throttle on climb.

This earth-frame Z sign convention (HeadVecEFFiltered.z is up-positive in the raw frame, not NED) was independently settled in #11869 while auditing the wind estimator's equation 12. That PR's "Follow-ups" section flagged this exact TPA inversion:

Possible sign inversion in fixed-wing TPA pitch compensation (pid.c:659-662, 1349-1351). groundCos evaluates to -HeadVecEF.z, which is negative in a climb, so throttle is reduced when pitching up — contradicting both the tpa_pitch_compensation description and the code's own inline comment.

Two corrections to that follow-up / to this task's original assignment, found during investigation:

• There is only one call site, calculateTPAThtrottle() — no second function (calculateAutoTransitionTargetTPAThrottle()) exists anywhere in the codebase; that name appears to be stale.
• tpa_pitch_compensation's min: 0 in settings.yaml means no user could have "compensated" for the inversion with a negative value — the setting has never accepted one.

The vForward/groundCos construction itself is correct and shared with wind_estimator.c's fuselageDirection, so the fix negates only at the point of use (throttleAdjustment), not the vector construction.

Testing

• SITL build verified clean (no new warnings) with this change.
• No unit test exists for this function; the sign's real-world effect can only be confirmed in flight. Behavior change: any fixed-wing craft currently tuned with tpa_pitch_compensation > 0 will see the compensation direction reverse after this fix (previously reduced throttle on climb; will now increase it, as documented). Flight testing is needed to confirm the corrected direction feels right and to re-tune if needed.

Related

Follow-up to #11855 / #11869.

https://claude.ai/code/session_017mXYKN2RCu49ZtumhXfutw

[qodo-code-review]

ⓘ Qodo reviews are paused because the subscription is no longer active. Ask your workspace admin to reactivate the subscription to resume reviews. Manage billing

[qodo-free-for-open-source-projects]

PR Summary by Qodo

Correct fixed-wing TPA pitch-compensation direction

🐞 Bug fix 🕐 10-20 Minutes

AI Description

• Reverses fixed-wing pitch compensation to add throttle during climbs and reduce it during dives.
• Documents NED sign semantics while preserving the wind estimator's shared forward-vector
 convention.

Diagram

graph TD
  A["Attitude Vector"] -->|NED transform| B["Forward Vector"] -->|dot down axis| C["Pitch Direction"] -->|negated compensation| D["Throttle Adjustment"] -->|clamp and filter| E["TPA Throttle"]

Loading

High-Level Assessment

Negating groundCos at the compensation point is the safest and most localized correction. Inverting the shared forward-vector construction would violate the established NED convention and risk changing wind-estimator behavior, while accepting negative configuration values would preserve incorrect semantics and require user retuning.

Files changed (1) +6 / -1

Bug fix (1) +6 / -1

pid.cCorrect fixed-wing pitch-compensation sign +6/-1

Correct fixed-wing pitch-compensation sign

• Negates the NED down-component when applying fixed-wing TPA pitch compensation, increasing throttle during climbs and decreasing it during dives as documented. Adds comments explaining the coordinate convention and why the shared forward-vector construction remains unchanged.

src/main/flight/pid.c

[qodo-free-for-open-source-projects]

Code Review by Qodo

🐞 Bugs (0) 📘 Rule violations (0) 📎 Requirement gaps (0)

Great, no issues found!

Qodo reviewed your code and found no material issues that require review

Tip of the day

💡 Did you know, you can reply 'qodo' on any finding to push back, ask questions, or dig deeper

More tips ↗ | Customize Qodo ↗ | Qodo docs ↗

[github-actions]

RAM / Flash usage vs. base commit 13485eb — commit df24806

Target	Flash Δ	RAM Δ
MATEKF405	±0 B (±0.00%)	CCM: ±0 B (±0.00%) RAM: ±0 B (±0.00%)
MATEKF722	±0 B (±0.00%)	ITCM_RAM: ±0 B (±0.00%) RAM: ±0 B (±0.00%) TCM: ±0 B (±0.00%)
MATEKF765	+16 B (+0.00%)	DTCM_RAM: ±0 B (±0.00%) SRAM1: ±0 B (±0.00%)
MATEKH743	±0 B (±0.00%)	D2_RAM: ±0 B (±0.00%) DTCM_RAM: ±0 B (±0.00%) ITCM_RAM: ±0 B (±0.00%) RAM: ±0 B (±0.00%)

See RAM/flash optimization guide for techniques to reduce usage.

[github-actions]

Test firmware build ready — commit df24806

Download firmware for PR #11952

245 targets built. Find your board's .hex file by name on that page (e.g. MATEKF405SE.hex). Files are individually downloadable — no GitHub login required.

Development build for testing only. Use Full Chip Erase when flashing.

[Jetrell]

This is an interesting one. Because the mention of increasing the throttle or decreasing it seems very misleading. It was even written this way in the explanation of the setting.

with the wrong sign, so fixed-wing TPA pitch compensation currently reduces throttle when climbing and increases it when diving

This is what it does currently. And would be totally wrong if it is altering the actual throttle output. That would be a backwards move.

However, from what I remember when I was reviewing tpa_pitch_compensation last year. It didn't alter the throttle output to the motor, which is what it sounds like it does.. . But instead its altering the tpaThrottle throttle calculation which is part of the TPA function.

I think the complexity comes by the way the wording appears to be a double inversion of the throttle.
Below is the way TPA has always worked -
Throttle command goes up and the gains strength goes down.
Throttle command reduced and the gains strength goes up.
So in both cases it reverses the condition to produce a dynamic PID gain adjustment.

But when you add pitch angle up or down to either of those conditions. It then sets another reverse condition on top of that. So we effectively end up with.
Throttle command goes up and the gains strength goes up
Throttle command reduced and the gains strength goes down.

Plane is flying level.

1. Throttle command increase = higher thrust = airspeed increasing = tpaThrottle reducing = gain attenuation.
2. Throttle command reduction = reduced thrust = airspeed reducing = tpaThrottle increasing = gain boost.

Plane is commanded to climb (e.g. 20degs)

3. Throttle command increase = higher thrust = airspeed normally reduced due to gravity/lack of power = tpaThrottle increasing (reversing TPA throttle calculation by pitch angle) = gains slightly boosted.

Plane is commanded to dive (e.g. 20degs)

4. Throttle commanded reduced = reduced thrust = airspeed normally increasing due to gravity = tpaThrottle reducing (reversing TPA throttle calculation by pitch angle) = gains slightly attenuated.

Notice how 1 and 3 both command the throttle to increase. But the tpaThrottle is reversed by pitch UP for 3. So it keeps the gains higher as the plane slows in a climb.

Then noticed how 2 and 4 both command the throttle to reduce. But the tpaThrottle is reverse by pitch DOWN for 4. So it lowers the gains as the planes speed increases by gravity in a dive.

Granted I could be wrong. But I haven't seen the control surfaces oscillate on any of my planes when using tpa_pitch_compensation in a low throttle, 90deg dive at over 120km/h, as I used to before this function was implemented. And this is when TPA is in use, not APA.. I also have my PID gains tuned noticeably tighter than I used to before TPA+PA and APA was implemented.

Does that make any sense ?

[breadoven]

I think calculateTPAThtrottle is probably correct as it is without this change if it's supposed to work as follows.

I take it the idea is to "virtually" adjust the throttle passed to calculateFixedWingTPAFactor to adjust the tpaFactor to take account of slowing during a climb and speeding up during a dive. In that case surely you need to reduce the throttle input to calculateFixedWingTPAFactor when climbing to boost PIDs and increase the throttle input when diving, attenuating PIDs. So groundCos should be -ve when climbing which it will be because of the -ve sign on HeadVecEFFiltered.z (which is +ve during climb). I think the comments are confusing/wrong if this how this works, also in the setting for tpa_pitch_compensation .

As an aside I don't really understand the need for vDown and vForward given groundCos is just -HeadVecEFFiltered.z due to the zero x and y in vDown. Also the constraints could be simplified to just:
uint16_t throttleAdjusted = constain(rcCommand[THROTTLE] + throttleAdjustment, 1000, 2000);

And perhaps better to use constants to convert rads to degrees rather than using raw numbers.

[sensei-hacker]

Yes, sorry guys. This shouldn't be here.

It was a process error that you guys even ended up seeing this.

I take it the idea is to "virtually" adjust the throttle passed to calculateFixedWingTPAFactor

Yeah the machine got confused there, I think. (But I will double-check just in case).

That's similar to I remember for APA initially it estimated a "virtual throttle" based on the airspeed, then fed that to the essentially TPA. But throttle is itself a proxy for airspeed, so it was using the actual airspeed as a proxy for a virtual throttle, as a proxy for the airspeed.
I think we simplified that to just using airspeed for airspeed, without producing it through a "virtual throttle".

If I had unlimited time and energy, I'd apply the same treatment to TPA pitch, eliminating "virtual throttle".

[sensei-hacker]

There is a different PR attempting to fix the ACTUAL problem. A comment in the code said:

//when 1deg pitch up, increase throttle by pitch(deg)_to_throttle.

And settings.yaml said:

Positive values will increase throttle when pitching up, and decrease throttle when pitching down.

The machine saw that the actual behavior didn't match the "documented intent". It wanted to fix the code to match the documentation and comment.
Documentation bug! 🙂

Settings.yaml hopefully fixed in:
#11956