Skip to content

Limit fixed-wing climb pitch when thrust cannot hold the climb #12194

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
sensei-hacker wants to merge 1 commit into
base: maintenance-10.x
Choose a base branch
from
+103 −1
Open

Limit fixed-wing climb pitch when thrust cannot hold the climb #12194

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
104 changes: 103 additions & 1 deletion src/main/navigation/navigation_fixedwing.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,21 @@
#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

@breadoven breadoven Oct 29, 2026

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

@sensei-hacker sensei-hacker Oct 29, 2026

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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?

#define NAV_FW_GUARD_PITCH_RECOVER_DPS 2.0f // climb-pitch ceiling recover rate
#define NAV_FW_GUARD_SPEED_LOW_FRAC 0.93f // speed below this fraction of the reference = losing energy
#define NAV_FW_GUARD_SPEED_OK_FRAC 0.96f // speed back above this fraction of the reference = recovered
#define NAV_FW_GUARD_SPEED_VLOW_FRAC 0.88f // speed this far below the reference needs no sustained pin (airspeed only)
#define NAV_FW_GUARD_SPEED_VOK_FRAC 0.92f // ...back above this fraction clears it
#define NAV_FW_GUARD_DECEL_ENTER_PCT_S 0.012f // speed falling faster than this fraction of the reference per second = bleeding energy
#define NAV_FW_GUARD_DECEL_EXIT_PCT_S 0.004f // ...and no longer once it slows below this
#define NAV_FW_GUARD_DECEL_FILTER_HZ 0.7f
#define NAV_FW_GUARD_DECEL_SPEED_FRAC 0.97f // deceleration only counts once speed is actually this far below the reference
#define NAV_FW_GUARD_PINNED_MIN_S 1.0f // pitch demand loaded this long separates a stall from a zoom climb
#define NAV_FW_GUARD_LOAD_FRAC 0.5f // "loaded" = demanding at least this fraction of the climb-pitch ceiling...
#define NAV_FW_GUARD_LOAD_MIN_DECIDEG 30.0f // ...and at least this much nose-up
#define NAV_FW_GUARD_SPEED_REF_RISE_S 0.5f // reference speed follows a speed increase with this time constant
#define NAV_FW_GUARD_SPEED_REF_FALL_S 60.0f // ...and a speed decrease with this one: a stall bleeds off over seconds and must stay visible

#define NAV_FW_CROSSTRACK_RATE_CUTOFF_HZ 3.0f // cross-track error rate LPF

Expand Down Expand Up @@ -180,9 +195,19 @@ typedef struct {

typedef struct {
float effectiveBankLimit; // adaptive nav bank limit (energy guard), deg; 0 = not yet initialised
float climbPitchCut; // climb-pitch ceiling reduction, decideg; 0 = full nav_fw_climb_angle
float speedRef; // slow peak-following speed reference, cm/s
float pinnedSec; // time the pitch demand has been above half of the ceiling, s
bool speedLow; // speed below LOW_FRAC of speedRef (moderate loss)
bool speedVeryLow; // speed below VLOW_FRAC of speedRef (large loss)
bool speedFalling; // filtered deceleration above DECEL_ENTER_PCT_S of speedRef
float prevSpeed;
pt1Filter_t decelFilter;
bool speedIsAirspeed; // speedRef is in airspeed (pitot) rather than ground speed
float targetVzBaseline;
timeUs_t lastUpdateUs;
timeUs_t lastTriggerUs;
timeUs_t lastPitchTriggerUs;
pt1Filter_t deficitFilter;
pt1Filter_t riseFilter;
bool deficitLatched;
Expand Down Expand Up @@ -237,6 +262,7 @@ void resetFixedWingAltitudeController(void)
posControl.rcAdjustment[PITCH] = 0;
isPitchAdjustmentValid = false;
throttleSpeedAdjustment = 0;
fwGuard.climbPitchCut = 0.0f;

pt1FilterSetCutoff(&pitchFilterState, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ));
pt1FilterReset(&pitchFilterState, 0.0f);
Expand Down Expand Up @@ -275,7 +301,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
}

// Here we use negative values for dive for better clarity
const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle) - fwGuard.climbPitchCut;
const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);

// Default control based on climb rate (velocity)
Expand Down Expand Up @@ -404,6 +430,11 @@ static void fwTurnStateReset(void)
fwTurn.ff.prevLegBearing = -1;
// 0 = use the full ceiling until the guard re-syncs on its next run
fwGuard.effectiveBankLimit = 0.0f;
// climbPitchCut survives: the altitude controller keeps running through a lateral reset
fwGuard.speedFalling = false;
fwGuard.speedLow = false;
fwGuard.speedVeryLow = false;
fwGuard.pinnedSec = 0.0f;
}

/*
Expand Down Expand Up @@ -555,6 +586,15 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottl
fwGuard.effectiveBankLimit = maxBank;
}

float speed = posControl.actualState.velXY; // cm/s
bool speedIsAirspeed = false;
Comment on lines +589 to +590

@qodo-free-for-open-source-projects qodo-free-for-open-source-projects Bot Oct 29, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Action required

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

#ifdef USE_PITOT
if (pitotGetValidForAirspeed()) {
speed = getAirspeedEstimate();
speedIsAirspeed = true;
}
#endif

const timeDeltaLarge_t dtUs = currentTimeUs - fwGuard.lastUpdateUs;
fwGuard.lastUpdateUs = currentTimeUs;
// First call / gap (controller was inactive): resync, skip integration this step.
Expand All @@ -566,6 +606,17 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottl
pt1FilterReset(&fwGuard.riseFilter, 0.0f);
fwGuard.deficitLatched = false;
fwGuard.lastTriggerUs = currentTimeUs;
fwGuard.climbPitchCut = 0.0f;
fwGuard.speedRef = speed;
fwGuard.speedIsAirspeed = speedIsAirspeed;
fwGuard.prevSpeed = speed;
fwGuard.speedFalling = false;
pt1FilterSetCutoff(&fwGuard.decelFilter, NAV_FW_GUARD_DECEL_FILTER_HZ);
pt1FilterReset(&fwGuard.decelFilter, 0.0f);
fwGuard.speedLow = false;
fwGuard.speedVeryLow = false;
fwGuard.pinnedSec = 0.0f;
fwGuard.lastPitchTriggerUs = currentTimeUs;
return;
}
const float dtSec = US2S(dtUs);
Expand Down Expand Up @@ -613,6 +664,57 @@ static void updateFwEnergyBankGuard(timeUs_t currentTimeUs, uint16_t autoThrottl
}
fwGuard.effectiveBankLimit = constrainf(fwGuard.effectiveBankLimit, NAV_FW_GUARD_PHI_FLOOR_DEG, maxBank);

// Losing speed while demanding a large nose-up: thrust cannot hold altitude, so stop asking for nose-up.
// Speed is judged against a slow peak-following reference, so a dive recovery (excess speed) is left
// alone and no stall speed is needed. A moderate loss needs the unmet climb and a sustained pin; a
// large loss needs only the nose-up demand.
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;
Comment on lines +671 to +675

@qodo-free-for-open-source-projects qodo-free-for-open-source-projects Bot Oct 29, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remediation recommended

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

pt1FilterReset(&fwGuard.decelFilter, 0.0f);
fwGuard.speedFalling = false;
fwGuard.speedLow = false;
fwGuard.speedVeryLow = false;
}
const float decel = -pt1FilterApply3(&fwGuard.decelFilter, (speed - fwGuard.prevSpeed) / dtSec, dtSec); // cm/s^2, positive = slowing
fwGuard.prevSpeed = speed;
const float speedRefTauS = (speed > fwGuard.speedRef) ? NAV_FW_GUARD_SPEED_REF_RISE_S : NAV_FW_GUARD_SPEED_REF_FALL_S;
fwGuard.speedRef += (speed - fwGuard.speedRef) * MIN(1.0f, dtSec / speedRefTauS);
if (speed < NAV_FW_GUARD_SPEED_LOW_FRAC * fwGuard.speedRef) {
fwGuard.speedLow = true;
} else if (speed > NAV_FW_GUARD_SPEED_OK_FRAC * fwGuard.speedRef) {
fwGuard.speedLow = false;
}
if (speed < NAV_FW_GUARD_SPEED_VLOW_FRAC * fwGuard.speedRef) {
fwGuard.speedVeryLow = true;
} else if (speed > NAV_FW_GUARD_SPEED_VOK_FRAC * fwGuard.speedRef) {
fwGuard.speedVeryLow = false;
}
if (decel > NAV_FW_GUARD_DECEL_ENTER_PCT_S * fwGuard.speedRef) {
fwGuard.speedFalling = true;
} else if (decel < NAV_FW_GUARD_DECEL_EXIT_PCT_S * fwGuard.speedRef) {
fwGuard.speedFalling = false;
}
const float pitchDemand = (float)posControl.rcAdjustment[PITCH];
// The minimum applies only to arming; it must not drop out as the ceiling shrinks, or the cut would release mid-recovery
const bool noseUp = pitchDemand >= NAV_FW_GUARD_LOAD_MIN_DECIDEG || fwGuard.climbPitchCut > 0.0f;
const bool pitchPinned = noseUp && pitchDemand >= NAV_FW_GUARD_LOAD_FRAC * (maxClimbDeciDeg - fwGuard.climbPitchCut);
fwGuard.pinnedSec = pitchPinned ? (fwGuard.pinnedSec + dtSec) : 0.0f;
// Ground speed swings with the wind on every turn, so without airspeed a large loss alone is not proof of under-thrust
const bool veryLowSpeedTrigger = noseUp && fwGuard.speedVeryLow && (fwGuard.speedIsAirspeed || fwGuard.deficitLatched);
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);
Comment on lines +707 to +708

@qodo-free-for-open-source-projects qodo-free-for-open-source-projects Bot Oct 29, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Action required

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

if (underThrust) {
fwGuard.climbPitchCut += DEGREES_TO_DECIDEGREES(NAV_FW_GUARD_PITCH_REDUCE_DPS) * dtSec;
fwGuard.lastPitchTriggerUs = currentTimeUs;
} 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;
Comment on lines +712 to +714

@qodo-free-for-open-source-projects qodo-free-for-open-source-projects Bot Oct 29, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Action required

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

}
fwGuard.climbPitchCut = constrainf(fwGuard.climbPitchCut, 0.0f, maxClimbDeciDeg);

DEBUG_SET(DEBUG_FW_TURN, 6, lrintf(fwGuard.effectiveBankLimit)); // energy-guard bank ceiling [deg]
}

Expand Down
Loading