Add airspeed TPA support (backport from #11042)

[sensei-hacker]

User description

Merges #11042 into maintenance-9.x

Summary

Adds airspeed-based TPA (Throttle PID Attenuation) calculation for fixed-wing aircraft. When enabled, uses actual airspeed instead of throttle position for more accurate PID scaling.

Changes

• New airspeed_tpa setting to enable airspeed-based TPA mode
• Calculates equivalent throttle value from airspeed: tpa_breakpoint + (airspeed - fw_reference_airspeed)/fw_reference_airspeed * (tpa_breakpoint - ThrottleIdleValue)
• Falls back to throttle-based TPA if airspeed is unavailable
• Adds airspeed sensor validity check

Testing

SITL testing completed successfully

---

PR Type

Enhancement

---

Description

• Add airspeed-based TPA calculation for fixed-wing aircraft

  • New apa_pow setting enables airspeed instead of throttle for PID scaling
  • Calculates TPA factor using airspeed ratio with fallback to throttle-based method

• Implement pitch angle-aware throttle compensation for fixed-wing

  • New tpa_pitch_compensation setting adjusts throttle based on pitch angle
  • Uses filtered throttle with PT1 filter for smooth transitions

• Refactor TPA factor calculation and update logic

  • Separate throttle calculation into calculateTPAThtrottle() function
  • Change TPA update trigger from throttle comparison to factor comparison
  • Initialize pidGainsUpdateRequired to true for proper PID gain updates

• Expand TPA rate range and improve documentation

  • Increase tpa_rate maximum from 100 to 200
  • Add airspeed validity check function pitotValidForAirspeed()
  • Update Settings.md with new parameters and clarified TPA descriptions

---

Diagram Walkthrough

flowchart LR
  A["Throttle Input"] --> B["calculateTPAThtrottle"]
  B --> C["PT1 Filter + Pitch Compensation"]
  C --> D["TPA Factor Calculation"]
  E["Airspeed Sensor"] --> F["pitotValidForAirspeed"]
  F --> G{Airspeed Valid?}
  G -->|Yes| H["calculateFixedWingAirspeedTPAFactor"]
  G -->|No| I["calculateFixedWingTPAFactor"]
  H --> D
  I --> D
  D --> J["Update PID Coefficients"]

Loading

File Walkthrough

	Relevant files
Configuration changes	control_profile.c Add new throttle configuration fields                                        src/main/fc/control_profile.c Add two new throttle configuration fields: apa_pow and tpa_pitch_compensation Initialize new fields with their default values in profile reset function +3/-1      settings.yaml Add new settings and update TPA configuration                        src/main/fc/settings.yaml Add apa_pow setting definition with type, range, and description Add tpa_pitch_compensation setting definition with range and description Update tpa_rate maximum from 100 to 200 Update tpa_rate description with clarifications for fixed-wing and multirotor +15/-2
Enhancement	control_profile_config_struct.h Extend throttle configuration structure                                    src/main/fc/control_profile_config_struct.h Add apa_pow field for airspeed-based TPA power setting Add tpa_pitch_compensation field for pitch angle throttle compensation +2/-0      pid.c Implement airspeed TPA and pitch compensation logic            src/main/flight/pid.c Initialize pidGainsUpdateRequired to true for proper startup behavior Add calculateFixedWingAirspeedTPAFactor() function using airspeed ratio Add calculateTPAThtrottle() function for filtered throttle with pitch compensation Refactor calculateMultirotorTPAFactor() to accept throttle parameter Change TPA update trigger from throttle comparison to factor comparison Adjust TPA factor constraints from [0.5, 2.0] to [0.3, 2.0] Move TPA constraint application after attenuation calculation +52/-27  pitotmeter.c Add airspeed validity check function                                          src/main/sensors/pitotmeter.c Add pitotValidForAirspeed() function to validate airspeed sensor readiness Check sensor health, calibration completion, and GPS fix for virtual pitot +9/-0      pitotmeter.h Export airspeed validity check function                                    src/main/sensors/pitotmeter.h Declare new pitotValidForAirspeed() function for airspeed validation +1/-0
Documentation	Settings.md Document new TPA settings and update descriptions                docs/Settings.md Add documentation for apa_pow setting with formula and range Add documentation for tpa_pitch_compensation setting Update tpa_rate description to clarify throttle-based vs airspeed-based TPA Increase tpa_rate maximum value from 100 to 200 +22/-2

---

[qodo-code-review]

You are nearing your monthly Qodo Merge usage quota. For more information, please visit here.

PR Compliance Guide 🔍

All compliance sections have been disabled in the configurations.

[qodo-code-review]

High-level Suggestion

The pitch-based throttle compensation logic is incorrectly implemented. It modifies the throttle value used for TPA (PID gain scaling) instead of adjusting the actual motor throttle command, and should be moved to the mixer stage. [High-level, importance: 9]

Solution Walkthrough:

Before:

// in src/main/flight/pid.c

float calculateTPAThtrottle() {
    // ... calculate pitch-based throttleAdjustment
    uint16_t throttleAdjusted = rcCommand[THROTTLE] + throttleAdjustment;
    // Return a throttle value used ONLY for TPA calculation
    return pt1FilterApply(&fixedWingTpaFilter, throttleAdjusted);
}

void updatePIDCoefficients() {
    // ...
    // Get the "compensated" throttle
    float tpaThrottle = calculateTPAThtrottle();
    // Use it to calculate TPA factor
    tpaFactor = calculateFixedWingTPAFactor(tpaThrottle);
    // ...
    // Apply TPA factor to PID gains
    pidRuntime[axis].P = pidProfile()->pid[axis].P * tpaFactor;
}

After:

// in src/main/flight/pid.c

void updatePIDCoefficients() {
    // ...
    // TPA calculation should use the real, filtered throttle
    uint16_t filteredThrottle = pt1FilterApply(&fixedWingTpaFilter, rcCommand[THROTTLE]);
    tpaFactor = calculateFixedWingTPAFactor(filteredThrottle);
    // ...
    // Apply TPA factor to PID gains
    pidRuntime[axis].P = pidProfile()->pid[axis].P * tpaFactor;
}

// in mixer.c or similar stage before motor output
void applyMixer() {
    // ... calculate pitch-based throttleAdjustment
    rcCommand[THROTTLE] += throttleAdjustment;
    // ... continue with mixing using the compensated throttle
}

[qodo-code-review]

Suggestion: Constrain the tpaFactor in calculateMultirotorTPAFactor to be non-negative, preventing PID gain inversion when tpa_rate exceeds 100. [possible issue, importance: 9]

Suggested change

	static float calculateMultirotorTPAFactor(uint16_t throttle)
	{
	float tpaFactor;
	// TPA should be updated only when TPA is actually set
	if (currentControlProfile->throttle.dynPID == 0 || rcCommand[THROTTLE] < currentControlProfile->throttle.pa_breakpoint) {
	if (currentControlProfile->throttle.dynPID == 0 || throttle < currentControlProfile->throttle.pa_breakpoint) {
	tpaFactor = 1.0f;
	} else if (rcCommand[THROTTLE] < getMaxThrottle()) {
	tpaFactor = (100 - (uint16_t)currentControlProfile->throttle.dynPID * (rcCommand[THROTTLE] - currentControlProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlProfile->throttle.pa_breakpoint)) / 100.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 - currentControlProfile->throttle.dynPID) / 100.0f;
	tpaFactor = (100 - constrain(currentControlProfile->throttle.dynPID, 0, 100)) / 100.0f;
	}
	return tpaFactor;
	}
	static float calculateMultirotorTPAFactor(uint16_t throttle)
	{
	float tpaFactor;
	// 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()) {
	float reduction = (uint16_t)currentControlProfile->throttle.dynPID * (throttle - currentControlProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlProfile->throttle.pa_breakpoint);
	tpaFactor = (100 - reduction) / 100.0f;
	} else {
	tpaFactor = (100 - currentControlProfile->throttle.dynPID) / 100.0f;
	}
	return constrainf(tpaFactor, 0.0f, 1.0f);
	}

[qodo-code-review]

Suggestion: Correct the pitch angle compensation logic in calculateTPAThtrottle by properly converting the groundCos value to an angle in degrees using acosf before calculating the throttle adjustment. [possible issue, importance: 9]

Suggested change

	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; //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));
	}
	else {
	tpaThrottle = rcCommand[THROTTLE]; //multirotor TPA without filtering
	}
	return tpaThrottle;
	}
	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);
	float pitchAngle = 90.0f - acosf(constrainf(groundCos, -1.0f, 1.0f)) * RAD_TO_DEG;
	int16_t throttleAdjustment = currentControlProfile->throttle.tpa_pitch_compensation * pitchAngle;
	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;
	}

[qodo-code-review]

Suggestion: Replace the direct float inequality with an epsilon-based comparison to avoid frequent coefficient recalculations caused by tiny floating-point changes. [Learned best practice, importance: 5]

Suggested change

	if (tpaFactor != tpaFactorprev) {
	pidGainsUpdateRequired = true;
	}
	tpaFactorprev = tpaFactor;
	if (fabsf(tpaFactor - tpaFactorprev) > 1e-4f) {
	pidGainsUpdateRequired = true;
	}
	tpaFactorprev = tpaFactor;

[sensei-hacker]

@shota3527 Probably most of what the Qodo bot said may be incorrect, but if you get a minute it wouldn't hurt for you to take a glance at its suggestions and see if you think any of them have any merit.