Add airspeed TPA support (backport from #11042) via merge from master#11197
Merged
sensei-hacker merged 16 commits intomaintenance-9.xfrom Dec 22, 2025
Merged
Add airspeed TPA support (backport from #11042) via merge from master#11197sensei-hacker merged 16 commits intomaintenance-9.xfrom
sensei-hacker merged 16 commits intomaintenance-9.xfrom
Conversation
Merge Maintenance 9.x to master
Add airspeed TPA support
Contributor
|
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[bot]](https://avatars.githubusercontent.com/in/484649?s=60&v=4){kind=small}
Comment on lines
+487
to
491
| } 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; | ||
| } |
Contributor
There was a problem hiding this comment.
Suggestion: Clamp the multirotor tpaFactor to the range [0.0f, 1.0f] to prevent negative values when dynPID is greater than 100. [possible issue, importance: 8]
Suggested change
| } 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; | |
| } | |
| if (throttle < getMaxThrottle()) { | |
| tpaFactor = (100 - (uint16_t)currentControlProfile->throttle.dynPID | |
| * (throttle - currentControlProfile->throttle.pa_breakpoint) | |
| / (float)(getMaxThrottle() - currentControlProfile->throttle.pa_breakpoint)) | |
| / 100.0f; | |
| tpaFactor = constrainf(tpaFactor, 0.0f, 1.0f); | |
| } else { | |
| tpaFactor = (100 - constrain(currentControlProfile->throttle.dynPID, 0, 100)) / 100.0f; | |
| tpaFactor = constrainf(tpaFactor, 0.0f, 1.0f); | |
| } |
Comment on lines
+445
to
+451
| static float calculateFixedWingAirspeedTPAFactor(void){ | ||
| const float airspeed = getAirspeedEstimate(); // in cm/s | ||
| const float referenceAirspeed = pidProfile()->fixedWingReferenceAirspeed; // in cm/s | ||
| float tpaFactor= powf(referenceAirspeed/(airspeed+0.01f), currentControlProfile->throttle.apa_pow/100.0f); | ||
| tpaFactor= constrainf(tpaFactor, 0.3f, 2.0f); | ||
| return tpaFactor; | ||
| } |
Contributor
There was a problem hiding this comment.
Suggestion: Validate airspeed and referenceAirspeed are positive (and apa_pow is in-range) and return a safe neutral factor (1.0) when invalid, instead of relying on an arbitrary epsilon that can still yield extreme results. [Learned best practice, importance: 6]
Suggested change
| static float calculateFixedWingAirspeedTPAFactor(void){ | |
| const float airspeed = getAirspeedEstimate(); // in cm/s | |
| const float referenceAirspeed = pidProfile()->fixedWingReferenceAirspeed; // in cm/s | |
| float tpaFactor= powf(referenceAirspeed/(airspeed+0.01f), currentControlProfile->throttle.apa_pow/100.0f); | |
| tpaFactor= constrainf(tpaFactor, 0.3f, 2.0f); | |
| return tpaFactor; | |
| } | |
| static float calculateFixedWingAirspeedTPAFactor(void) | |
| { | |
| const float airspeed = getAirspeedEstimate(); // cm/s | |
| const float referenceAirspeed = pidProfile()->fixedWingReferenceAirspeed; // cm/s | |
| if (airspeed <= 0.0f || referenceAirspeed <= 0.0f) { | |
| return 1.0f; | |
| } | |
| const float apaPow = constrainf(currentControlProfile->throttle.apa_pow / 100.0f, 0.0f, 2.0f); | |
| float tpaFactor = powf(referenceAirspeed / airspeed, apaPow); | |
| return constrainf(tpaFactor, 0.3f, 2.0f); | |
| } |
Comment on lines
+503
to
+505
| 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); |
Contributor
There was a problem hiding this comment.
Suggestion: groundCos is a cosine, not an angle; compute a pitch/tilt angle in radians (e.g., via acosf with clamping), then convert to degrees once before applying tpa_pitch_compensation. [Learned best practice, importance: 5]
Suggested change
| 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); | |
| float groundCos = vectorDotProduct(&vForward, &vDown); | |
| groundCos = constrainf(groundCos, -1.0f, 1.0f); | |
| const float tiltRad = acosf(groundCos); | |
| const float tiltDegFromLevel = (tiltRad - (float)M_PI_2) * (180.0f / (float)M_PI); | |
| const int16_t throttleAdjustment = | |
| (int16_t)(currentControlProfile->throttle.tpa_pitch_compensation * tiltDegFromLevel); | |
| uint16_t throttleAdjusted = rcCommand[THROTTLE] + constrain(throttleAdjustment, -1000, 1000); |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
User description
Merges #11042 from master into maintenance-9.x
PR Type
Enhancement, New Feature
Description
Add airspeed-based TPA (Throttle PID Attenuation) support for fixed-wing aircraft
apa_powparameter to enable airspeed-based PID scaling instead of throttle positionRefactor TPA calculation logic for improved flexibility
Enhance PID coefficient update mechanism
pidGainsUpdateRequiredto true for proper startup behaviorExpand TPA range and add validation
tpa_ratemax value from 100 to 200pitotValidForAirspeed()validation function for sensor health checksDiagram Walkthrough
flowchart LR A["Throttle/Airspeed Input"] --> B{"PID Type?"} B -->|Fixed Wing| C["Check apa_pow enabled"] B -->|Multirotor| D["Multirotor TPA"] C -->|Airspeed Valid| E["Airspeed-based TPA"] C -->|Disabled/Invalid| F["Throttle-based TPA"] F --> G["Apply Pitch Compensation"] G --> H["Filter & Constrain"] E --> I["Calculate TPA Factor"] D --> I H --> I I --> J["Update PID Coefficients"]File Walkthrough
control_profile.c
Add airspeed TPA and pitch compensation config fieldssrc/main/fc/control_profile.c
apa_powandtpa_pitch_compensationfunction
settings.yaml
Configure airspeed TPA and pitch compensation settingssrc/main/fc/settings.yaml
apa_powsetting definition with default value 120 and range 0-200tpa_pitch_compensationsetting definition with default value 8 andrange 0-20
tpa_ratemax value from 100 to 200tpa_ratedescription to include airspeed TPA detailscontrol_profile_config_struct.h
Define new throttle control structure fieldssrc/main/fc/control_profile_config_struct.h
apa_powfield (uint16_t) for airspeed-based TPA power parametertpa_pitch_compensationfield (uint8_t) for pitch angle throttlecompensation
pid.c
Implement airspeed TPA and pitch compensation logicsrc/main/flight/pid.c
pidGainsUpdateRequiredto true for proper startup behaviorcalculateFixedWingAirspeedTPAFactor()function for airspeed-basedTPA calculation
calculateTPAThtrottle()function for filtered throttle with pitchcompensation
calculateMultirotorTPAFactor()to accept throttle parameterupdatePIDCoefficients()to use TPA factor comparison insteadof throttle comparison
pitotmeter.c
Add airspeed validation function for TPAsrc/main/sensors/pitotmeter.c
pitotValidForAirspeed()function to validate pitot sensor healthsensors
pitotmeter.h
Declare airspeed validation functionsrc/main/sensors/pitotmeter.h
pitotValidForAirspeed()Settings.md
Document airspeed TPA and pitch compensation settingsdocs/Settings.md
apa_powparameter with formula and range (0-200)tpa_pitch_compensationparameter with range(0-20)
tpa_ratedescription to clarify throttle-based PID attenuationbehavior
tpa_ratemax value from 100 to 200