Terrain AGL Hold — terrain following for fixed wings (builds on #11438)

[MartinovEm]

DRAFT — depends on #11438. This PR is built on top of error414's terrain data layer (#11438) and includes its commits underneath; please review only the top commits (the terrain_nav layer). Merge order is deliberate: #11438 lands first, this follows (a rebase onto its current rework is planned — see Integration notes).

As an FPV pilot flying a field with hills around it, I kept coming back to the same worry: plain Cruise holds a fixed altitude — fine over flat ground, but around those hills it can quietly become a problem, since the aircraft holds its height above home, not above the ground rising ahead of it. The terrain data is already on the SD card — so I decided to work on a Cruise that follows it.

Terrain AGL Hold is basically CRUZ with a moving target — in 3D Cruise, one new mode box makes the altitude target follow the terrain data under the aircraft: you hold height above the ground, not above home. Downhill the plane descends with the valley, uphill it climbs with the ridge. Box off — instant stock CRUZ.

▶ Video — Flights 1–2: floor work and turns over the flat field (AtomRC Beluga, SpeedyBee F405 Wing, real SD terrain tiles).

▶ Video — Flight 3: crossing a ridge that rises ~150 m above the takeoff field — three passes, hands off the pitch stick, no warnings needed.

What it does

One navigation cycle — inputs, health gate, the moving target, the alarm ladder, and the single gate into the altitude target path (the stock controller, untouched).

• One new mode box — TERRAIN AGL HOLD — fixed wing only, active only inside 3D Cruise (NAV COURSE HOLD + NAV ALTHOLD). It is a modifier: everything else stays stock Cruise.
• An enforced safety floor — default 60 m, CLI-adjustable 50–120 m (so the hard minimum is 50, not 60). Engaging below it commands a gentle auto-climb to the floor first (OSD: TERRAIN AUTO CLIMB TO MIN). Why 50 m is the lowest the firmware will accept: the floor has to absorb everything going wrong at once — worst-case map error on steep slopes (~±30 m; typical source agreement is ~6 m, measured against ICESat-2 and radar-altimeter data), the aircraft's natural ±5–8 m tracking breathing, and the alarm margin (the floor alarm fires 5 m below the floor). At a 50 m floor that worst-case stack still clears the ground with a few meters in hand.
• Predictive, not just reactive — a forward scan up to 2 km ahead (time-capped at 35 s at current ground speed) climbs early for rising terrain, and a per-airframe escape test — "at this speed, with your configured climb rate, will you clear what's ahead?" — drives TERRAIN AHEAD! tens of seconds before any reactive alarm could fire.
• Honest OSD warnings (worst wins; while an auto-climb runs underneath, the active warning alternates with TERRAIN AUTO CLIMB TO MIN):

Message	Meaning	Pilot action
TERRAIN NOT READY	box on but data not usable	you are in normal cruise
TERRAIN AUTO CLIMB TO MIN	below the floor, already climbing back	hands off (or help it)
TERRAIN AHEAD!	full-rate climb won't clear the path ahead	turn — you have time
TERRAIN! PULL UP! (blinks)	below the floor, pulling works	release any push / pull
TERRAIN! TURN AWAY! (blinks)	below the floor and climbing is not enough	bank away now
TERRAIN VS MAX ALT	terrain demands more than nav_max_altitude	raise the ceiling or turn
TERRAIN LOST - ALT FROZEN	map data gone mid-hold; target frozen, won't descend	take over, disengage when comfortable
TERRAIN LOOKAHEAD OFF	forward scan unavailable; hold still tracks below	give hills margin

• The pilot always wins. The pitch stick pauses the hold the moment it moves (smooth blend, no command step); release re-captures at the current height. The box is an instant in-flight kill switch. Switching to any other nav mode (RTH, WP, …) disengages the layer the same cycle — those modes fly 100 % stock.

Safety design

• Everything is opt-in and off by default. Without the box, every mode is byte-for-byte stock behavior.
• The terrain data layer (Error414/feature/inav terrain #11438) is untouched — this layer only reads its API. Every height comes from the data layer's EXISTING tile cache; it adds no tile buffers or storage of its own — the whole layer costs ~100 bytes of RAM, and staying that small was a design constraint from day one, not an accident.
• The navigation loop never reads the SD card. Ever. All height queries are cache-only, at 10 Hz. When the forward scan needs a block that is not cached yet, it only schedules it — at most one block per cycle — and the actual load happens where Error414/feature/inav terrain #11438 put it: in the async, low-priority terrain IO task. The scan can never evict the block under the aircraft. Measured on hardware: TERRAIN_IO worst case 449 µs with blackbox logging sharing the same card vs 445 µs without — lock contention zero, terrain health 100.00 % over a 14-min flight at up to 57.5 m/s (chart attached).
• One gate into the altitude target path — all commands go through the existing altitude-target funnel, which keeps the stock slew limit and the nav_max_altitude clamp downstream. No PID or controller code is modified anywhere.
• Climb authority is your existing nav_fw_auto_climb_rate — no separate terrain climb setting. Set it to what your aircraft can genuinely sustain: the terrain escape warning trusts that number.
  NB: set nav_fw_manual_climb_rate equal to nav_fw_auto_climb_rate (they default to 300/500). Matched, an auto-climb stays smooth if you touch the pitch stick — and pulling won’t climb any faster, since the auto-climb is already at that rate. Left unmatched, you may see a small, harmless nose-ease.
• Data loss is never silent — health-gated outputs; 0.5 s of bad data freezes the altitude target with TERRAIN LOST - ALT FROZEN (it will not descend blindly), 3 s of healthy data resumes. Proven closed-loop on a deliberately corrupted card.
• Never stacks with the rangefinder — the layer refuses to engage while SURFACE mode is active, and hard-disengages for launch, landing, emergency, GPS loss and VTOL transition states.

Why a separate mode, and not SURFACE

A fair question: INAV already has "terrain following" — SURFACE mode. Why not extend it? Because under the same family name, the two do different jobs with different physics:

	SURFACE	TERRAIN AGL HOLD
platform	multirotor only (the box is MC-gated)	fixed wing (3D Cruise)
source	rangefinder (needs the physical sensor)	terrain map from SD
sees	instantaneous distance below	the ground below and up to 2 km ahead
function	hold above the surface	hold + enforced floor + predictive escape test + freeze-on-data-loss
error model	cm-accurate, short range, blind ahead	~6 m typical / ±30 m worst on steep slopes; tiles can vanish mid-flight

A pilot carrying SURFACE expectations into a map-based mode (or vice versa) would be carrying the wrong safety assumptions — that is exactly what a distinct name prevents. The two never stack: this mode refuses to engage while SURFACE is active. Converging the pilot-facing "terrain following" concept over both sources someday — happy to discuss (see the open question below); the safety rules stay per-source either way.

New settings (only two)

Setting	Default	Range	What it does
terrain_nav_min_agl	6000 (60 m)	5000–12000 [cm]	the safety floor
terrain_nav_lookahead	1000	0–2000 [m], 0 = off	forward scan distance (also capped by the tile cache block budget)

How to test (bench + first flight)

1. Build this branch — as-is it builds as INAV 9.1 (the exact bits all the testing below was done on); the rebase onto the current 10.x base is planned (see Integration notes). Enable error414's data layer as in Error414/feature/inav terrain #11438: set terrain_enabled = ON (it defaults OFF), with terrain tiles on the SD card — you can generate tiles for your area with this map generator (usage explained in the repo).
2. Assign the mode — two ways:
   • Configurator, Modes tab: the box is TERRAIN AGL HOLD — assign it like any other mode: pick the channel your switch is on, drag the slider to the range you want, Save. Put it on a deliberate, guarded switch, not next to ARM. Note: on some boards the mode list can currently mislabel entries — a pre-existing MSP box-names buffer limit that a separate PR is already fixing — so if the name doesn't show correctly, use CLI (below).
   • CLI:

set terrain_nav_min_agl = 6000
set terrain_nav_lookahead = 1000
aux <free row> 69 <AUX channel - 1> 1700 2100    # TERRAIN AGL HOLD (permanentId 69 on this branch)
save

 (`aux` syntax: `<row>` = any free slot 0–39 · `<AUX channel - 1>` = zero-based index among the non-stick channels — radio CH N → N − 5, e.g. CH 13 → 8 (note: the CLI counts differently from the Modes tab, which shows plain CH numbers) · the last two numbers are the activation range in µs — any range works, 1700–2100 is just "switch high". Verify with `aux` readback after save.)

The two terrain_nav_* settings themselves are CLI-only for now — a companion inav-configurator PR adding them to the GUI follows once the firmware side settles. Defaults are sane; for a first test you can skip both set lines entirely.

OSD: the stock Altitude element shows height above your home point, not above the ground. To see the terrain height above ground (AGL) in flight, add the Rangefinder OSD element — with no rangefinder fitted, it shows the terrain-derived AGL. The flight-mode field shows TERR while the hold is engaged.
3. Important: set nav_fw_manual_climb_rate equal to nav_fw_auto_climb_rate (defaults are 300/500 — unequal). If manual < auto, grabbing pitch during an auto-climb commands the lower rate and the nose visibly eases.
4. Ground check outdoors: GPS fix · sd_info = Ready · the OSD height-above-ground element reads ≈ 0 on the ground.
5. First flight: fly 3D Cruise as usual, get comfortably HIGH over FLAT ground, then flip TERRAIN AGL HOLD on — watch it hold height above the ground; flip it off and on; grab the pitch stick (it yields, release re-captures). Only when that is boring, go lower or toward terrain — and never dive at a hill on purpose; the message table above says what each warning wants from you. Panic rule: box off = stock cruise instantly; the pitch stick always wins.

Verification — honest levels

• Unit: 120/120 on the decision core (alarm ladder, capture/floor rules, handover blend, escape test).
• SITL: 8/8 scenario suite (edge, ceiling, failsafe, stick, S-turn, low-engage, hole-tile freeze ladder, cache pressure) on a file-backed SD card with real tiles.
• HITL: weeks on a real SpeedyBee F405 Wing + X-Plane (real SD, real tiles): alarm campaign totals 93 fires ≤ 55.0 m / 52 clears ≥ 60.0 m / 0 exceptions; ceiling clamp exact; handover hand-feel confirmed.
• Field: THREE real flights (AtomRC Beluga): flight 1 — 6 exact captures incl. a below-floor engage (46.2 → 60.0), ±5–8 m tracking; flight 2 — one 14.7-min hold, 11 full alarm ladders, 18 hands-off turns measured (sag ≤ 3.5 m to 25° bank, 7–11 m at 42–44°); flight 3 — a real 855 m ridge crossed 65–69 m above the crest, the target riding the terrain up and down; the ladder at speed (median 95 km/h): 8 fires all ≤ 54.9, escalation only while genuinely losing, every clear 60.0–60.4 — and NO "TERRAIN AHEAD!", because the slope never beat full climb: the honest alarm stayed quiet exactly when it should. Campaign discipline across everything measured: every fire ≤ 55 m, every altitude clear at 60.0–60.6 m, zero exceptions.

Flight 2: floor work, the push-dive ladders, the bank-vs-sag curve.

The SD timing proof: nav never blocks, the data layer’s lock does its job.

The campaign-wide alarm discipline: every fire below the floor, every clear at 60.

Tested on: SPI-SD F405 Wing + SITL. Not yet measured on SDIO-SD boards (H7 class) — the design is driver-independent (cache-only reads, async IO, health gating), but "at least as good on SDIO" is reasoning, not measurement. SDIO testers very welcome.

Integration notes

• Depends on Error414/feature/inav terrain #11438 — merge order: the data layer first, this follows.
• A rebase onto Error414/feature/inav terrain #11438's current rework branch (direct-read cache, .TER tiles) is planned once that branch settles; the full test ladder will be re-run on 2 m tiles before this PR leaves draft.
• Current maintenance-10.x took boxId 60 / permanentId 69 for AUTO SPEED — at the rebase this PR's box moves to boxId 61 / permanentId 70 (a two-file, known-resolution conflict until then).
• Open UX question: keep the two-box modifier design, or fold it into a single "TERR" mode implying cruise — input welcome. For now the two boxes are deliberately separate, for safety: switching the hold off leaves you in stock CRUZ — a known, stable place — instead of dropping you out of navigation entirely; while the feature is experimental, that is the failure mode I want pilots to have. That said, the current design already gives confident users the one-switch feel for free: put the cruise boxes and TERRAIN AGL HOLD on the same switch (one flip = terrain cruise as a whole), or give the hold a full-width channel range for always-on behavior.

Roadmap (after this proves itself)

Terrain-relative waypoints · terrain-aware RTH · geozone awareness — deliberately later, on top of this foundation.

Credits: error414's terrain data layer made all of this possible — this PR only reads what #11438 provides. He saw the feature before anyone else and has been testing it since.

One personal note to close: I fly these hills every week. The flights above were the first time I crossed that ridge with my hands off the pitch stick — the plane just followed the ground, said nothing, and had nothing to say. That quiet is the feature.