Repository navigation
Limit fixed-wing climb pitch when thrust cannot hold the climb - #12194
sensei-hacker wants to merge 1 commit into
Conversation
The fixed-wing altitude controller has no airspeed input, so when the motor cannot deliver the demanded climb (low battery, thrust-limited airframe) it holds the full nav_fw_climb_angle while the aircraft sinks and slows toward a stall. Extend the energy bank guard to also lower the climb-pitch ceiling when speed falls against a slow peak-following reference while nose-up demand is sustained and the climb deficit is latched, or when speed has dropped well below the reference. The ceiling is held until the deficit clears, then restored gradually. No stall speed and no new settings are needed. Without a pitot the large-loss trigger also requires the latched deficit, so wind-driven ground speed changes do not cut the ceiling.
|
ⓘ Qodo reviews are paused because the subscription is no longer active. Ask your workspace admin to reactivate the subscription to resume reviews. Manage billing |
PR Summary by QodoLimit fixed-wing climb pitch during under-thrust speed loss
AI Description
Diagram
High-Level Assessment
Files changed (1)
|
Code Review by Qodo
1. Auto-speed climbs never receive a pitch cut
|
![qodo-free-for-open-source-projects[bot]](https://avatars.githubusercontent.com/in/1068000?s=60&v=4){kind=small}
| const bool underThrust = veryLowSpeedTrigger || | ||
| (pitchPinned && (fwGuard.speedLow || (fwGuard.speedFalling && speed < NAV_FW_GUARD_DECEL_SPEED_FRAC * fwGuard.speedRef)) && fwGuard.deficitLatched && fwGuard.pinnedSec >= NAV_FW_GUARD_PINNED_MIN_S); |
There was a problem hiding this comment.
1. Auto-speed climbs never receive a pitch cut 🐞 Bug ≡ Correctness
updateFwEnergyBankGuard() contains all updates to climbPitchCut, but its caller skips it whenever isFixedwingAutoSpeedActive() is true. In an altitude-controlled waypoint climb using auto speed, falling speed and an unmet climb demand therefore cannot lower the pitch ceiling, and an existing cut cannot recover until auto speed stops.
Agent Prompt
## Issue description
The new pitch-cut logic is bypassed whenever auto speed owns throttle, including altitude-controlled waypoint climbs.
## Fix Focus Areas
- src/main/navigation/navigation_fixedwing.c[1825-1870]
- src/main/navigation/navigation_fixedwing.c[633-716]
## Recommended Fix
Run the speed, climb-deficit, and pitch-cut updates on both throttle paths. Keep any bank-guard logic that requires the normal auto-throttle demand conditional on that demand being available.
ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools
| float speed = posControl.actualState.velXY; // cm/s | ||
| bool speedIsAirspeed = false; |
There was a problem hiding this comment.
2. Lost gps speed can cut climb pitch 🐞 Bug ☼ Reliability
updateFwEnergyBankGuard() uses posControl.actualState.velXY as ground speed without checking whether the horizontal velocity estimate is usable. Without a valid pitot, loss of horizontal estimation publishes zero speed; a latched climb deficit and nose-up demand then turn that unavailable measurement into a pitch cut while altitude control can still run.
Agent Prompt
## Issue description
An invalid horizontal velocity estimate is published as zero and is treated as evidence of lost flight speed when no valid airspeed is available.
## Fix Focus Areas
- src/main/navigation/navigation_fixedwing.c[589-619]
- src/main/navigation/navigation_fixedwing.c[671-716]
## Recommended Fix
Gate ground-speed tracking and pitch-cut triggering on a usable horizontal velocity estimate. When valid speed returns, resynchronize the speed reference, previous sample, and deceleration state rather than comparing it against invalid samples.
ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools
| } else if (!fwGuard.deficitLatched && (currentTimeUs - fwGuard.lastPitchTriggerUs) > ((timeUs_t)NAV_FW_GUARD_RECOVER_HOLDOFF_MS * 1000)) { | ||
| // Hold the ceiling until the climb demand is being met again, otherwise pitch returns to the limit and speed bleeds off again | ||
| fwGuard.climbPitchCut -= DEGREES_TO_DECIDEGREES(NAV_FW_GUARD_PITCH_RECOVER_DPS) * dtSec; |
There was a problem hiding this comment.
3. A cut can persist after climb is cancelled 🐞 Bug ≡ Correctness
climbPitchCut recovers only when deficitLatched clears, although the pitch trigger can remain active through climbPitchCut > 0 without checking whether a climb is still commanded. After switching to level flight or descent, an aircraft descending faster than its new target can retain the deficit latch and remain unable to demand its full positive pitch to arrest that descent.
Agent Prompt
## Issue description
The pitch ceiling remains reduced while a vertical-rate deficit persists, even after the commanded climb ends.
## Fix Focus Areas
- src/main/navigation/navigation_fixedwing.c[633-653]
- src/main/navigation/navigation_fixedwing.c[700-716]
## Recommended Fix
Separate an unmet commanded climb from a generic target-minus-actual vertical-rate deficit. Stop extending the cut when climb is no longer requested, and allow the ceiling to recover for level or descent demands rather than waiting for the climb-deficit latch alone.
ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools
| if (speedIsAirspeed != fwGuard.speedIsAirspeed) { | ||
| // Airspeed and ground speed are not comparable: restart the reference on the new source | ||
| fwGuard.speedIsAirspeed = speedIsAirspeed; | ||
| fwGuard.speedRef = speed; | ||
| fwGuard.prevSpeed = speed; |
There was a problem hiding this comment.
4. Pitot fallback can cause a false pitch cut 🐞 Bug ☼ Reliability
speedIsAirspeed treats hardware pitot and virtual airspeed as the same source, so their transition does not reset speedRef or prevSpeed. When pitot validation fails but virtual airspeed remains available, a lower virtual reading is interpreted as speed loss against the hardware-derived reference and can reduce climb pitch without a measured deceleration.
Agent Prompt
## Issue description
Hardware and virtual airspeed can replace one another while the new source flag remains unchanged, leaving discontinuous speed samples in the pitch guard.
## Fix Focus Areas
- src/main/navigation/navigation_fixedwing.c[589-595]
- src/main/navigation/navigation_fixedwing.c[671-681]
- src/main/sensors/pitotmeter.c[397-413]
## Recommended Fix
Track the actual speed producer as well as airspeed-versus-ground-speed selection. Reset the speed reference, prior sample, deceleration filter, and speed-loss latches whenever the hardware-to-virtual producer changes.
ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools
|
RAM / Flash usage vs. base commit
See RAM/flash optimization guide for techniques to reduce usage. |
|
Test firmware build ready — commit Download firmware for PR #12194 251 targets built. Find your board's
|
| #define NAV_FW_GUARD_REDUCE_RATE_DPS 20.0f // bank-limit reduce rate | ||
| #define NAV_FW_GUARD_RECOVER_RATE_DPS 5.0f // bank-limit recover rate | ||
| #define NAV_FW_GUARD_RECOVER_HOLDOFF_MS 1000 // healthy time before recovery starts | ||
| #define NAV_FW_GUARD_PITCH_REDUCE_DPS 20.0f // climb-pitch ceiling reduce rate when thrust is insufficient |
There was a problem hiding this comment.
Does it make sense to include these constant defines when they're only used once in the code ? If used more than once then it's a necessity otherwise it just makes the code more difficult to read having to find the defined value somewhere else. Why not just have the actual value where it's used and add a comment if need be to clarify what it is ?
Just a general quibble ... nothing more.
There was a problem hiding this comment.
Agreed, thank you.
It would be better for them to be local const, which will be optimized away by the compiler, but descriptive for those reading the code.
Or have it as a literal with comments as needed.
I don't want to read too much into what you did NOT say.
Since it seems you have at least begun reading the code, do you have feedback, good or bad, on the physics etc?
Problem
updateAltitudeVelocityAndPitchController_FWhas no airspeed or thrust input. When the motor cannot deliver the demanded climb (sagging battery, thrust-limited airframe) it keeps the pitch demand pinned atnav_fw_climb_anglewhile the aircraft sinks and slows toward a stall. Reproduced in JSBSim SITL in ALTHOLD, ALTHOLD + climb, RTH and WP, on a C172 and a T-6 (airspeed down to 51.7 kt against a stall of about 48 kt).Change
Extends
updateFwEnergyBankGuard()to also lower the climb-pitch ceiling (climbPitchCut, fed into the altitude PID limit so anti-windup applies) when:The ceiling is held until the deficit clears, then restored at 2 deg/s. No stall speed and no new settings. The cut clears on an altitude-controller reset and on a guard resync.
Testing (JSBSim SITL, C172 and T-6, 4 modes, climb angle 20 and 35)
Pass criteria per run: min IAS >= 1.2 x Vs1, and at most 3.0 s with the motor at the cap, pitch demand above +1 deg and IAS more than 5 kt below its pre-cut value.
t6 rthat 20 and 35 deg (4.2 s each) andt6 altholdat 35 deg (6.3 s). In those the cut engages a few seconds late because 5 kt is only about 3% of T-6 cruise speed.Known limits
nearPitchLimitstill compares against the full climb angle; follow-up.Cost
Text +736 B (F722SE) to +944 B (F405SE) against baseline, with about 28 B of bss; roughly +0.2% of the image.