Terrain AGL Hold — terrain following for fixed wings (builds on #11438) by MartinovEm · Pull Request #11785 · iNavFlight/inav

MartinovEm · 2026-08-16T20:44:05Z

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 (now rebased onto #11438's current head — 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)

Build this branch — it builds as INAV 9.1 on top of Error414/feature/inav terrain #11438's current head (10-bit .TER tiles; terrain is enabled by default only on F7/H7/AT32-class targets per Error414/feature/inav terrain #11438). Enable error414's data layer as in Error414/feature/inav terrain #11438: set terrain_enabled = ON (it defaults OFF), with .TER terrain tiles on the SD card — you can generate them for your area with this map generator (.TER (INAV Terrain) is its default output); the Configurator's built-in Map Generator tab switches to .TER in feat(map-generator): switch terrain output to .TER version 50 (10-bit packed) inav-configurator#2708.
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> 70 <AUX channel - 1> 1700 2100    # TERRAIN AGL HOLD (permanentId 70)
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 are CLI-only (simple enough that a Configurator page isn't planned). 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 — the flight evidence below was flown on the earlier int16/9.1 base; on the current rebased base the layer is verified by clean build, unit tests 120/120 and SITL (incl. the firmware AGL matching an independent int16 reference 519/519 on .TER tiles). 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.
Rebased onto Error414/feature/inav terrain #11438's current head (c177c2e27: direct-read cache, 10-bit .TER tiles, 2 m steps). Re-verified there: clean build, unit 120/120, SITL scenarios incl. the engage decode check (519/519 vs an independent int16 reference). Hardware/field evidence on this base is still pending — F4 no longer builds terrain by default, so it will come from an H7.
maintenance-10.x holds boxId 60 / permanentId 69 for AUTO SPEED; this PR's box is boxId 61 / permanentId 70.
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.

…ature/inav_terrain # Conflicts: # docs/Settings.md

…ature/inav_terrain

…ature/inav_terrain # Conflicts: # .gitignore

…ature/inav_terrain

…rd is not in idle

…add new DEBUG LOG type "terrain"

# Conflicts: # src/test/unit/CMakeLists.txt

…in-unit-test fix(terrain): build terrain unit test

Adds a third sdcardVTable_t implementation for the SITL target that reads/writes 512-byte blocks in a disk-image file on the host (--sdcard=<image>), following the SPI/SDIO drivers contract (deferred completion callbacks from poll()). Enables USE_SDCARD and USE_TERRAIN for SITL, so asyncfatfs and the terrain tile reader run unchanged in the simulator. Without --sdcard the virtual FC behaves like one with an empty card slot.

…es and climb lookahead Adds terrain_nav.{c,h}: a strict wrapper around the terrain module for future navigation consumers. Success is always an explicit boolean and values are written only on success - no sentinel, stale or substituted value ever reaches a caller. Provides current AGL, terrain height at an arbitrary location (scheduling the block load on cache miss), terrain height at the GPS origin, and a bounded climb-lookahead that walks the grid along a bearing, accumulates achievable climb and reports the worst height deficit together with an explicit count of unavailable samples. Built entirely on the terrain module's public utilities; no existing files changed apart from the build list. Nothing in flight code calls this API yet - flight behavior is unchanged.

FW 3D Cruise only, opt-in RC box, health-gated, all commands through the single altitude-target funnel. Engage captures current AGL (below minimum: slew-limited climb to it); data loss freezes the target with an OSD warning, resume after 3 s healthy; pitch stick pauses the hold and re-captures on release; directional lookahead climbs early for rising terrain within the global per-cycle block budget (cache - 2); nav_max_altitude always wins with TERRAIN VS MAX ALT / TERRAIN PULL UP warnings. New settings: terrain_nav_min_agl (60 m default), terrain_nav_lookahead (1000 m, 0=off). SITL tile cache set to 5 to mirror the small-cache board tier.

…ning The climb lookahead scans along the course over ground; when the heading estimate is invalid the cog can be stale, so the lookahead now switches itself off (same skip as the config and low-speed gates) and the pilot gets a new lowest-priority OSD warning - the ceiling and data-loss warnings always override it. The reactive hold keeps tracking unchanged (the same degraded mode as terrain_nav_lookahead = 0). No new settings.

…LIMB info + TERR flymode indicator

…mand step, no deadband-edge writer flap)

…capped lookahead horizon

…d threat only, PULL UP survives below-min captures, pass-at-minimum clear rule, TAWS callout texts Below the minimum the escape-test shortfall now counts only the real terrain-relative deficit (the altitude term is clamped out there; above the minimum it keeps acting as the honest cushion), so the automatic climb shows TERRAIN AUTO CLIMB TO MIN instead of a false TERRAIN AHEAD. A capture below the minimum no longer extinguishes an active PULL UP - the alarm clears only at/above the minimum, as always. TERRAIN AHEAD clears when the escape test passes with the aircraft at/above the minimum, both sustained 2 s; a returning threat re-fires the series (the spare-margin clear could never be met riding exactly at the floor). OSD texts become "TERRAIN AHEAD!" and "TERRAIN! PULL UP!".

…-min capture hands over to the auto-climb info A capture below the minimum (low engagement or a deep stick release) starts a clean alarm phase again: the automatic climb shows TERRAIN AUTO CLIMB TO MIN, and TERRAIN! PULL UP! returns only when the climb is failing (losing height against the best achieved), when the pilot pushes below the margin, or when the floor is breached after having been reached.

… cap-uncover fast clear

…annel + auto-climb info alternation

… info text

…rtfall

At a 30 m floor the alarm point (floor - 5 m) minus the worst-case map error budget (30 m on steep slopes) minus the tracking band leaves no real clearance. 50 m is the lowest floor where the worst-case stack still clears with margin. Default unchanged at 6000.

…format The terrain data layer now stores heights as a 10-bit packed heightOffset[] plus an int16 heightBase instead of a plain int16 height[x][y] grid. Read the four interpolation corners through getHeightOffsetByIndex() + heightBase, scaled by TERRAIN_HEIGHT_OFFSET_RESOLUTION_M (2 m), the same way terrain.c decodes a sample; the interpolation math is unchanged.

The blackbox log-number stat compiles on SITL only once USE_SDCARD is enabled (added for SITL SD-card support in this branch). On 64-bit hosts int32_t is not long, and clang on macOS errors on the format mismatch with warnings-as-errors. Cast the argument to match the format.

github-actions · 2026-08-20T23:36:34Z

RAM / Flash usage vs. base branch — commit bfa9a96

No size baseline is available yet for this PR's base branch (first run after this feature shipped, or a new branch). This comment will show deltas once a baseline exists.

Target	Flash Δ	RAM Δ
MATEKF405	660419 B (no baseline)	143712 B (no baseline)
MATEKF722	462983 B (no baseline)	126284 B (no baseline)
MATEKF765	698699 B (no baseline)	160932 B (no baseline)
MATEKH743	729275 B (no baseline)	163068 B (no baseline)

github-actions · 2026-08-20T23:36:52Z

Test firmware build ready — commit bfa9a96

Download firmware for PR #11785

244 targets built. Find your board's .hex file by name on that page (e.g. MATEKF405SE.hex). Files are individually downloadable — no GitHub login required.

Development build for testing only. Use Full Chip Erase when flashing.

qodo-code-review · 2026-08-20T23:43:12Z

ⓘ Qodo reviews are paused because the subscription is no longer active. Ask your workspace admin to reactivate the subscription to resume reviews. Manage billing

qodo-free-for-open-source-projects · 2026-08-20T23:46:57Z

PR Summary by Qodo

Add Terrain AGL Hold (terrain-following Cruise) backed by SD terrain tiles

✨ Enhancement 📝 Documentation 🧪 Tests ⚙️ Configuration changes 🕐 40+ Minutes

AI Description

• Add SD-backed terrain subsystem with cached grid blocks and async IO task.
• Add fixed-wing TERRAIN AGL HOLD for 3D Cruise with floor, lookahead, freeze-on-loss.
• Expose terrain AGL/AMSL in OSD/Blackbox; add SITL support, unit tests, docs.

Diagram

graph TD
  NAV["Navigation loop"] --> HOLD["Terrain AGL Hold gate"] --> TNAV["TerrainNav API"] --> TIO["Terrain cache + IO task"] --> SD[("SD + AsyncFATFS")]
  HOLD --> OSD["OSD warnings"]
  TIO --> BB["Blackbox SD lock + fields"] --> SD

High-Level Assessment

The following are alternative approaches to this PR:

1. Fold into existing SURFACE / rangefinder terrain-follow naming

➕ Single pilot-facing “terrain follow” concept across platforms
➕ Potential UI/message reuse
➖ Map-based terrain has a different error/failure model than a rangefinder
➖ Higher risk of confusing safety assumptions; must preserve no-stacking guarantees

2. Make Terrain AGL Hold a standalone NAV mode (not a Cruise modifier)

➕ Cleaner “one switch = one behavior” model
➕ Less dependency on Cruise state machine details
➖ More invasive changes to nav mode switching and transitions
➖ Loses the very safe failure mode of “box off = stock CRUZ immediately”

3. Abstract a generic “terrain-relative altitude target” layer in the altitude funnel

➕ Reusable foundation for future terrain-relative WP/RTH work
➕ Centralizes clamps/slew semantics
➖ Larger blast radius in core altitude path; harder to keep stock behavior when disabled
➖ Adds coupling before requirements for terrain-aware RTH/WP are proven

Recommendation: The current strategy (opt-in modifier box, hard cruise whitelist, cache-only reads, and a single gate into the existing altitude-target funnel) is a strong safety-first integration with limited controller intrusion. Keep this design while the feature matures; revisit deeper abstractions only when terrain-aware WP/RTH requirements solidify.

Files changed (44) +3762 / -26

Enhancement (29) +3328 / -15

blackbox.cLog terrain AGL/AMSL and implement SD access handoff state +87/-5

Log terrain AGL/AMSL and implement SD access handoff state

• Adds terrainAGL and terrainAMSL to Blackbox slow fields and populates them from terrain getters. Implements a simple SD access grant mechanism so terrain IO can safely borrow the SD card when Blackbox logs to SD.

src/main/blackbox/blackbox.c

blackbox.hExpose SD access request/release API for terrain IO +3/-0

Expose SD access request/release API for terrain IO
• Adds requestToSdCardAccess() and releaseSdCardAccess() declarations used by terrain IO to coordinate with Blackbox SD logging.
src/main/blackbox/blackbox.h

log.hAdd LOG_TOPIC_TERRAIN debug topic +1/-0

Add LOG_TOPIC_TERRAIN debug topic
• Introduces LOG_TOPIC_TERRAIN to support targeted debug logging from terrain IO and related components.
src/main/common/log.h

sdcard.cSelect SITL SD backend when configured +2/-0

Select SITL SD backend when configured
• Adds USE_SDCARD_SITL vtable selection so SITL can emulate SD blocks from a host image file. Leaves SPI/SDIO selection intact.
src/main/drivers/sdcard/sdcard.c

sdcard_impl.hDeclare SITL SD vtable symbol +4/-0

Declare SITL SD vtable symbol
• Declares sdcardSitlVTable under USE_SDCARD_SITL so the core SD driver can link the SITL backend.
src/main/drivers/sdcard/sdcard_impl.h

sdcard_sitl.cImplement FAT-image-backed SD card for SITL +215/-0

Implement FAT-image-backed SD card for SITL
• Implements a block-level SD backend reading/writing 512-byte sectors from a host file, driven via sdcard.poll(). Enables terrain/Blackbox SD paths to run in SITL.
src/main/drivers/sdcard/sdcard_sitl.c

sdcard_sitl.hAdd SITL SD image path configuration API +22/-0

Add SITL SD image path configuration API
• Adds sdcardSitlSetPath() to configure the SD image file used by the SITL SD backend.
src/main/drivers/sdcard/sdcard_sitl.h

fc_msp_box.cAdd TERRAIN AGL HOLD mode box and engagement reporting +14/-0

Add TERRAIN AGL HOLD mode box and engagement reporting

• Registers the TERRAIN AGL HOLD box (permanentId 70) and advertises it only for fixed-wing when terrain is enabled. Reports box as active based on hold engagement state, not just switch position.

src/main/fc/fc_msp_box.c

fc_tasks.cAdd terrain update and terrain IO tasks +22/-0

Add terrain update and terrain IO tasks

• Adds TASK_TERRAIN and TASK_TERRAIN_IO definitions and enables them based on terrain_enabled. Wires terrainUpdateTask and loadGridToCacheTask at low scheduler priority.

src/main/fc/fc_tasks.c

asyncfatfs.cIncrease open file limit and add filesystem idle/root helpers +52/-1

Increase open file limit and add filesystem idle/root helpers

• Increases AFATFS_MAX_OPEN_FILES to accommodate a kept-open terrain tile file alongside Blackbox and directory operations. Adds afatfs_isIdle() and afatfs_isCurrentDirRoot() helpers used to safely arbitrate SD access and directory context.

src/main/io/asyncfatfs/asyncfatfs.c

asyncfatfs.hExpose new asyncfatfs helper APIs +3/-0

Expose new asyncfatfs helper APIs
• Adds declarations for afatfs_isIdle() and afatfs_isCurrentDirRoot().
src/main/io/asyncfatfs/asyncfatfs.h

osd.cShow terrain AGL in Rangefinder element and add terrain hold warnings +89/-9

Show terrain AGL in Rangefinder element and add terrain hold warnings

• Extends OSD_RANGEFINDER to fall back to terrain-derived AGL when no valid rangefinder reading is available. Adds “TERR” mode label while engaged and integrates Terrain AGL Hold warning ladder with blink for urgent floor warnings.

src/main/io/osd.c

osd.hDefine Terrain AGL Hold OSD message strings +11/-0

Define Terrain AGL Hold OSD message strings

• Adds OSD message constants for terrain readiness, data loss freeze, max-alt conflict, urgent floor warnings, lookahead-off, auto-climb, and terrain-ahead messages.

src/main/io/osd.h

navigation.cCall Terrain AGL Hold gate each navigation cycle +11/-0

Call Terrain AGL Hold gate each navigation cycle

• Invokes terrainNavCruiseHoldUpdate() from applyWaypointNavigationAndAltitudeHold() so engagement/disengagement/freeze all happen in the primary nav loop that writes altitude targets.

src/main/navigation/navigation.c

target.cAdd --sdcard option to load a FAT image in SITL +8/-0

Add --sdcard option to load a FAT image in SITL

• Adds a command line option to provide the path to an SD-card image file and wires it to the SITL SD backend. Enables terrain tiles and SD-based logging paths in simulation.

src/main/target/SITL/target.c

terrain.cImplement terrain height tracking, home anchoring, and freshness gating +214/-0

Implement terrain height tracking, home anchoring, and freshness gating
• Computes terrain AMSL from map data and derives AGL using estimated altitude and a home reference. Applies freshness limits and returns explicit sentinels when data is stale or terrain is disabled.
src/main/terrain/terrain.c

terrain.hDefine terrain grid format, cache types, config, and getters +166/-0

Define terrain grid format, cache types, config, and getters

• Defines the packed .TER grid block format (10-bit offsets + base), cache structures/state, sentinels, and terrain configuration. Declares terrain update task and APIs to read last AGL/AMSL values.

src/main/terrain/terrain.h

terrain_io.cAdd async terrain tile loader with SD/Blackbox arbitration and validation +575/-0

Add async terrain tile loader with SD/Blackbox arbitration and validation

• Implements the async IO state machine to open .TER tiles, seek/read blocks into cache, validate CRC/version/spacing/indices, and keep files open to avoid repeated directory scans. Coordinates SD access with Blackbox and releases the card between sector reads to reduce logging interference.

src/main/terrain/terrain_io.c

terrain_io.hDefine terrain IO state machine types and APIs +86/-0

Define terrain IO state machine types and APIs

• Adds terrain IO task rate/constants, state enums, state struct (including SD access flags), callbacks, and isTerrainIoFailure() used by higher layers to health-gate outputs.

src/main/terrain/terrain_io.h

terrain_location.cImplement coordinate stepping and NE distance utilities +91/-0

Implement coordinate stepping and NE distance utilities
• Adds offsetLatlng() and gpsGetDistanceNE() to support terrain sampling and lookahead stepping. Handles longitude wrapping and latitude limiting for robustness.
src/main/terrain/terrain_location.c

terrain_location.hDeclare terrain location math helpers and constants +44/-0

Declare terrain location math helpers and constants
• Defines scaling constants, a lat/lon tolerance macro, and declarations for coordinate offset/distance helpers used by terrain grid and lookahead logic.
src/main/terrain/terrain_location.h

terrain_nav.cAdd navigation-facing terrain API and bounded lookahead scan +190/-0

Add navigation-facing terrain API and bounded lookahead scan

• Implements health-gated AGL retrieval, cache-only height sampling at arbitrary GPS points, home sampling, and a bounded-step lookahead walk returning climb-needed and escape-deficit metrics while scheduling missing blocks for async loading.

src/main/terrain/terrain_nav.c

terrain_nav.hDefine TerrainNav API contract for navigation consumers +70/-0

Define TerrainNav API contract for navigation consumers
• Introduces a boolean-success, out-parameter API for terrain access that avoids leaking sentinels/stale values. Defines lookahead result structure with completeness statistics.
src/main/terrain/terrain_nav.h

terrain_nav_hold.cImplement Terrain AGL Hold gate and altitude-target funnel integration +292/-0

Implement Terrain AGL Hold gate and altitude-target funnel integration

• Implements eligibility gating (fixed-wing, 3D Cruise only, no launch/land/emerg/VTOL, GPS fix, no SURFACE), periodic terrain queries, and a single output gate into updateClimbRateToAltitudeController(). Exposes status/warning/auto-climb and held AGL for OSD/MSP.

src/main/terrain/terrain_nav_hold.c

terrain_nav_hold.hDefine Terrain AGL Hold config and status APIs +65/-0

Define Terrain AGL Hold config and status APIs
• Defines the terrainNavConfig parameter group (min floor AGL and lookahead distance) and declares the per-cycle gate and status/warning helpers used by OSD and MSP box reporting.
src/main/terrain/terrain_nav_hold.h

terrain_nav_hold_core.cAdd pure decision core for Terrain AGL Hold (alarms, blend, freeze/resume) +490/-0

Add pure decision core for Terrain AGL Hold (alarms, blend, freeze/resume)
• Implements the firmware-independent state machine for engagement/capture, floor logic, predictive escape alarm persistence/clear, urgent floor alarm ladder (pull-up vs turn-away), stick handover blend, and freeze-on-data-loss with resume hysteresis.
src/main/terrain/terrain_nav_hold_core.c

terrain_nav_hold_core.hDefine Terrain AGL Hold core inputs/outputs/state and timing thresholds +167/-0

Define Terrain AGL Hold core inputs/outputs/state and timing thresholds
• Defines core structs, status/warning enums, and constants controlling freeze grace, resume hysteresis, floor hysteresis, blend timing, retake dwell, and escape alarm persistence/depth.
src/main/terrain/terrain_nav_hold_core.h

terrain_utils.cImplement grid cache, direct-read safety, bitmap checks, and 10-bit decode +291/-0

Implement grid cache, direct-read safety, bitmap checks, and 10-bit decode
• Adds LRU cache allocation/lookup that avoids evicting blocks under read and prevents duplicates during direct-read overwrites. Implements bitmap availability checks, packed 10-bit offset decoding, and CRC calculation for block validation.
src/main/terrain/terrain_utils.c

terrain_utils.hDeclare terrain cache and grid decoding helpers +43/-0

Declare terrain cache and grid decoding helpers
• Declares grid info calculation, bitmap checks, packed offset decoding, CRC helper, cache find, and cache status management functions used by terrain and navigation layers.
src/main/terrain/terrain_utils.h

Bug fix (1) +23 / -2

blackbox_io.cHandle log directory state more safely and reset SD log state +23/-2

Handle log directory state more safely and reset SD log state

• Avoids redundant mkdir/chdir when already in the log directory and prevents unintended chdir to root via NULL handles. Resets SD state to INITIAL after ending a log to support re-init workflows with other SD users.

src/main/blackbox/blackbox_io.c

Tests (2) +89 / -0

CMakeLists.txtAdd build wiring for terrain unit tests +3/-0

Add build wiring for terrain unit tests

• Adds terrain_unittest.cc dependencies and compile-time defines (including USE_TERRAIN and cache sizing) so terrain utilities can be unit-tested in the existing gtest setup.

src/test/unit/CMakeLists.txt

terrain_unittest.ccAdd unit test for calculateGridInfo() +86/-0

Add unit test for calculateGridInfo()

• Introduces a gtest validating calculateGridInfo() outputs for representative GPS coordinates. Provides a correctness check for grid indexing and reference computations used by sampling/lookahead.

src/test/unit/terrain_unittest.cc

Documentation (3) +215 / -0

Navigation Terrain Following.mdDocument Terrain AGL Hold behavior, setup, and warnings +87/-0

Document Terrain AGL Hold behavior, setup, and warnings
• Adds user documentation for Terrain AGL Hold, including requirements, setup, settings, and alarm ladder semantics. Clarifies differences vs multirotor SURFACE mode and safety limitations.
docs/Navigation Terrain Following.md

Settings.mdAdd CLI settings docs for terrain and Terrain AGL Hold +30/-0

Add CLI settings docs for terrain and Terrain AGL Hold
• Documents terrain_enabled plus terrain_nav_min_agl and terrain_nav_lookahead (defaults/ranges). Keeps Settings.md aligned with settings.yaml.
docs/Settings.md

Terrain.mdAdd terrain tile generation, SD layout, and troubleshooting guide +98/-0

Add terrain tile generation, SD layout, and troubleshooting guide

• Documents .TER generation, SD preparation, file naming/layout, enabling terrain, OSD display behavior, and troubleshooting steps. Notes home-tile requirement for terrain initialization.

docs/Terrain.md

Other (9) +107 / -9

sitl.cmakeEnable SD/AsyncFATFS sources for SITL builds +5/-0

Enable SD/AsyncFATFS sources for SITL builds

• Adds sdcard and asyncfatfs sources to SITL so terrain can be exercised in simulation. Enables end-to-end terrain IO without hardware SD.

cmake/sitl.cmake

CMakeLists.txtAdd terrain and terrain-nav modules to firmware build +15/-0

Add terrain and terrain-nav modules to firmware build
• Registers the new terrain subsystem, navigation API, and Terrain AGL Hold modules in common sources. Ensures compilation when USE_TERRAIN is enabled.
src/main/CMakeLists.txt

parameter_group_ids.hRegister terrain parameter group IDs +3/-1

Register terrain parameter group IDs
• Adds PG_TERRAIN_CONFIG and PG_TERRAIN_NAV_CONFIG and updates PG_INAV_END. Enables persistence and tooling support for the new terrain settings.
src/main/config/parameter_group_ids.h

fc_init.cInitialize SD stack when required by Blackbox or terrain +22/-8

Initialize SD stack when required by Blackbox or terrain
• Refactors SD init to run when either Blackbox uses SD or terrain is enabled. Adds terrainInit() call when USE_TERRAIN is built in.
src/main/fc/fc_init.c

rc_modes.hAdd BOXTERRAINAGLHOLD checkbox ID +1/-0

Add BOXTERRAINAGLHOLD checkbox ID
• Introduces a new checkbox ID for assigning TERRAIN AGL HOLD via Modes and evaluating its RC activation.
src/main/fc/rc_modes.h

settings.yamlAdd terrain_enabled and Terrain AGL Hold settings +27/-0

Add terrain_enabled and Terrain AGL Hold settings

• Defines PG_TERRAIN_CONFIG (terrain_enabled) and PG_TERRAIN_NAV_CONFIG (terrain_nav_min_agl, terrain_nav_lookahead) with defaults and ranges. Enables CLI config and documentation generation for these settings.

src/main/fc/settings.yaml

scheduler.hAdd terrain tasks to scheduler task IDs +5/-0

Add terrain tasks to scheduler task IDs
• Adds TASK_TERRAIN and TASK_TERRAIN_IO enum values under USE_TERRAIN. Enables consistent scheduling and task enablement for the terrain subsystem.
src/main/scheduler/scheduler.h

target.hEnable terrain and SD simulation in SITL target config +7/-0

Enable terrain and SD simulation in SITL target config
• Enables USE_SDCARD, USE_SDCARD_SITL, and USE_TERRAIN for SITL and sets a small terrain cache size. Provides an integration test target for terrain IO + nav behavior.
src/main/target/SITL/target.h

common_post.hEnable USE_TERRAIN only on higher-RAM MCU families and set cache tiers +22/-0

Enable USE_TERRAIN only on higher-RAM MCU families and set cache tiers
• Enables USE_TERRAIN by default only on H7, select F7, and AT32F43x families when baro+SD are present. Defines TERRAIN_GRID_BLOCK_CACHE_SIZE (4 or 8) based on MCU flash-size tiers as a heuristic.
src/main/target/common_post.h

qodo-free-for-open-source-projects · 2026-08-20T23:49:43Z

Code Review by Qodo

🐞 Bugs (1) 📘 Rule violations (0) 📜 Skill insights (0)

1. Async seek mis-handled 🐞 Bug ≡ Correctness

Description

terrain_io advances from TERRAIN_IO_SEEK to TERRAIN_IO_READ on any non-FAILURE fseek result,
including AFATFS_OPERATION_IN_PROGRESS. asyncfatfs documents that reads will fail until the seek
completes, but terrain_io counts those 0-byte reads as errors and can invalidate the grid block /
file incorrectly, degrading or disabling terrain data during flight.

Code

src/main/terrain/terrain_io.c[R441-444]

+        afatfsOperationStatus_e seekState = afatfs_fseek(terrainIoState.datFile, (int32_t)fileOffset64, AFATFS_SEEK_SET);
+        if(seekState != AFATFS_OPERATION_FAILURE){
+            LOG_DEBUG(TERRAIN, "TERRAIN SEEK OK -> READ");
+            //the read overwrites the destination block, stash the expected idx for the completion check

Evidence

terrain_io treats any non-FAILURE seek as ready-to-read and immediately switches to READ. asyncfatfs
explicitly documents that when seek is IN_PROGRESS, subsequent read/write attempts will fail until
the seek completes. terrain_io then increments readsZeroBytesCount when bytesRead==0, so these
expected failures can trip the “too many zero reads” guard and incorrectly invalidate terrain data.

src/main/terrain/terrain_io.c[413-502]
src/main/io/asyncfatfs/asyncfatfs.c[2112-2122]

Agent prompt

The issue below was found during a code review. Follow the provided context and guidance below and implement a solution

### Issue description
`loadGridToCacheTask()` transitions from `TERRAIN_IO_SEEK` to `TERRAIN_IO_READ` when `afatfs_fseek()` returns anything other than `AFATFS_OPERATION_FAILURE`. This includes `AFATFS_OPERATION_IN_PROGRESS`, where asyncfatfs explicitly states reads/writes will fail until the seek completes. The subsequent `TERRAIN_IO_READ` logic treats initial 0-byte `afatfs_fread()` results as IO/file errors (`readsZeroBytesCount`), which can incorrectly mark blocks/files invalid.

### Issue Context
- `afatfs_fseek()` can return `AFATFS_OPERATION_IN_PROGRESS`; during that time, reads are expected to fail.
- Terrain IO currently treats those expected failures as repeated “zero reads” and can cross the error threshold.

### Fix Focus Areas
- src/main/terrain/terrain_io.c[415-520]

### What to change
- Distinguish `AFATFS_OPERATION_SUCCESS` vs `AFATFS_OPERATION_IN_PROGRESS` from `afatfs_fseek()`.
- If `AFATFS_OPERATION_IN_PROGRESS`, do **not** start counting 0-byte reads as IO errors. Options:
 - Add a dedicated state like `TERRAIN_IO_SEEK_PENDING` and only enter `TERRAIN_IO_READ` once the seek is complete.
 - Or track a `seekInProgress` flag; while it is true, if `afatfs_fread()` returns 0 and `bytesRead == 0`, just return (no `readsZeroBytesCount++`). Clear the flag once the first non-zero read occurs.
- Ensure SD-access locking semantics remain safe while the seek is still in progress (don’t hand SD access back to blackbox mid-seek unless you can prove the filesystem/driver is idle and the seek operation is not pending).

ⓘ Copy this prompt and use it to remediate the issue with your preferred AI generation tools

Tip of the day

💡 Did you know, you can copy the agent prompt from any finding and feed it to your IDE agent

More tips ↗ | Customize Qodo ↗ | Qodo docs ↗

error414 added 30 commits April 12, 2026 20:30

terrain data from SD card functionality to inav

e51e158

update documentation

3ca41fa

add terrain data to blackbox

c848248

fix tests: update terrain

6a8ed40

update manual

26db385

update manual, Troubleshooting

b4f054c

update settings.md

f872711

remove condition in target files, each board has its own

913a9b0

Merge remote-tracking branch 'inav/maintenance-10.x' into error414/fe…

50a92fa

…ature/inav_terrain # Conflicts: # docs/Settings.md

Merge remote-tracking branch 'inav/maintenance-10.x' into error414/fe…

015005e

…ature/inav_terrain

update Settings.md

37e0a3b

move "define" for terrain to common_post.h

4c53777

clean changes in target.h files

e562665

Merge remote-tracking branch 'inav/maintenance-10.x' into error414/fe…

fe43cdb

…ature/inav_terrain # Conflicts: # .gitignore

Merge remote-tracking branch 'inav/maintenance-10.x' into error414/fe…

5cd0a34

…ature/inav_terrain

terrain: fix blackbox int type, fix closing file, allow negative AMSL

ce07edc

add to link to new tile generator

3715444

terrain: make better return error status

d141bfc

terrain - fix missing return and add extra guard not to read if SD ca…

cd87a8b

…rd is not in idle

Merge branch 'maintenance-10.x' into error414/feature/inav_terrain

b4844ff

terrain - fix SDcard init if blackbox use diferent device than SD card

989ce2c

terrain - improve lock releasing, keep DAT file opened to save time, …

f4fdd5e

…add new DEBUG LOG type "terrain"

Merge branch 'maintenance-10.x' into error414/feature/inav_terrain

2f6e314

# Conflicts: # src/test/unit/CMakeLists.txt

Merge branch 'maintenance-10.x' into error414/feature/inav_terrain

276464c

Merge branch 'maintenance-10.x' into error414/feature/inav_terrain

200a697

Merge branch 'maintenance-10.x' into error414/feature/inav_terrain

b9ee6ea

Merge branch 'maintenance-10.x' into error414/feature/inav_terrain

5a8354a

Merge branch 'maintenance-10.x' into error414/feature/inav_terrain

0305a3a

Merge branch 'maintenance-10.x' into error414/feature/inav_terrain

c82b3c4

Terrain - direct reading data from SD card to cache

99e4889

Merge pull request iNavFlight#1 from mart1npetroff/fix/error414-terra…

c177c2e

…in-unit-test fix(terrain): build terrain unit test

MartinovEm force-pushed the terrain-nav-main branch from 4b9e4ef to 3b656e4 Compare August 20, 2026 18:23

MartinovEm added 19 commits August 21, 2026 02:10

feat(terrain-nav): unconditional floor alarm with hysteresis + AUTO C…

4e235a3

…LIMB info + TERR flymode indicator

feat(terrain-nav): smooth stick handover blend + retake dwell (no com…

ed28de2

…mand step, no deadband-edge writer flap)

feat(terrain-nav): predictive TERRAIN AHEAD escape-test alarm + time-…

b6fe3a7

…capped lookahead horizon

feat(terrain-nav): honest red - TURN AWAY where pulling cannot help +…

44809ae

… cap-uncover fast clear

feat(terrain-nav): honest red v2 - TURN AWAY on exhausted vertical ch…

a51e0d7

…annel + auto-climb info alternation

feat(terrain-nav): blink the terrain floor warnings like the failsafe…

8bf3dbf

… info text

feat(terrain-nav): born-TURN-AWAY requires a deep (>=10 m) escape sho…

0de1c84

…rtfall

docs: update Settings.md (terrain_nav settings)

06377fc

feat(SITL): mirror the new small-RAM terrain cache tier (4)

c2ce3b4

docs: add Terrain AGL Hold pilot guide

daea1b7

MartinovEm force-pushed the terrain-nav-main branch from 4e7ec2e to bfa9a96 Compare August 20, 2026 23:13

MartinovEm marked this pull request as ready for review August 20, 2026 23:43

qodo-free-for-open-source-projects Bot reviewed Aug 20, 2026

View reviewed changes

Comment thread src/main/terrain/terrain_io.c

MartinovEm mentioned this pull request Aug 22, 2026

FW nav: replace roll PT1 smoothing with a triggered S-curve (no steady-state lag) #11804

Open

Navigation Menu

Uh oh!

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

Terrain AGL Hold — terrain following for fixed wings (builds on #11438)#11785
MartinovEm wants to merge 55 commits into
iNavFlight:maintenance-10.xfrom
MartinovEm:terrain-nav-main

MartinovEm commented Aug 16, 2026 •

edited

Loading

Uh oh!

github-actions Bot commented Aug 20, 2026

Uh oh!

github-actions Bot commented Aug 20, 2026

Uh oh!

qodo-code-review Bot commented Aug 20, 2026

Uh oh!

qodo-free-for-open-source-projects Bot commented Aug 20, 2026

Uh oh!

qodo-free-for-open-source-projects Bot commented Aug 20, 2026

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

Uh oh!

Conversation

MartinovEm commented Aug 16, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

What it does

Safety design

Why a separate mode, and not SURFACE

New settings (only two)

How to test (bench + first flight)

Verification — honest levels

Integration notes

Roadmap (after this proves itself)

Uh oh!

github-actions Bot commented Aug 20, 2026

Uh oh!

github-actions Bot commented Aug 20, 2026

Uh oh!

qodo-code-review Bot commented Aug 20, 2026

Uh oh!

qodo-free-for-open-source-projects Bot commented Aug 20, 2026

PR Summary by Qodo

Uh oh!

qodo-free-for-open-source-projects Bot commented Aug 20, 2026

Code Review by Qodo

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

MartinovEm commented Aug 16, 2026 •

edited

Loading