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

cmake/sitl.cmake

@@ -14,8 +14,13 @@ main_sources(SITL_COMMON_SRC_EXCLUDES
 main_sources(SITL_SRC
     config/config_streamer_file.c
     drivers/dronecan/libcanard/canard_sitl_driver.c
+    drivers/sdcard/sdcard.c
+    drivers/sdcard/sdcard_sitl.c
+    drivers/sdcard/sdcard_sitl.h
     drivers/serial_tcp.c
     drivers/serial_tcp.h
+    io/asyncfatfs/asyncfatfs.c
+    io/asyncfatfs/fat_standard.c
     target/SITL/sim/realFlight.c
     target/SITL/sim/realFlight.h
     target/SITL/sim/simHelper.c

docs/Navigation Terrain Following.md

@@ -0,0 +1,87 @@
+# Terrain AGL Hold
+
+Terrain AGL Hold is INAV's **terrain-following** mode for fixed wing. It is a **3D Cruise** enhancement that holds a set height **above the ground** instead of above the take-off point — *Cruise with a moving target*. As the ground rises and falls under you, the altitude target rides with it, so the aircraft keeps its clearance over hills and valleys with no stick input: downhill it descends with the valley, uphill it climbs with the ridge.
+
+It reads the ground elevation from the terrain elevation data on the SD card (see **[Terrain](Terrain.md)** for generating and loading tiles); it never uses a physical rangefinder. With the mode switched off, the aircraft flies exactly like stock 3D Cruise — everything here is opt-in, and **the pitch stick always wins** (the mode box is an instant in-flight kill switch).
+
+## How it works
+
+![How Terrain AGL Hold works](assets/images/Terrain-AGL-Hold-diagram.png)
+
+*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).*
+
+## Requirements
+
+- A flight controller with terrain support (SD card + barometer): **STM32H7, STM32F7 (F745 / F746 / F765), or AT32F43x**. F4 boards are not enabled for terrain by default.
+- **Fixed-wing only.** A valid **GPS fix** and a **barometer**.
+- **Terrain tiles** (`.TER`) on the SD card, **including the tile for your take-off site** — see [Terrain](Terrain.md).
+- **3D Cruise must be active.** At this stage Terrain AGL Hold works **only together with NAV CRUISE** — this is a mandatory safety condition for now, not just a recommendation.
+
+## Not the same as copter SURFACE mode
+
+Multirotor **SURFACE** mode follows the ground *reactively* from a physical rangefinder — the instantaneous distance straight below. Terrain AGL Hold is the fixed-wing equivalent taken from the SD terrain map, and crucially it is **predictive**: it scans **up to 2 km ahead** along your track and climbs/warns *early* for terrain rising in front of you — something a downward rangefinder cannot do. The two never stack (this mode refuses to engage while SURFACE is active), and SURFACE mode itself is multirotor-only.
+
+## What it does
+
+- **Captures** your current height above ground when you switch it on, and holds it. Move the **pitch stick** to set a new height; centre the stick and it holds the new one.
+- Engage **below the safety floor** → it climbs smoothly up to the floor first.
+- **Looks ahead** along your track (up to 2 km, 1 km by default) with a per-airframe **escape test** — *"at this speed, with your configured climb rate, will you clear what's ahead?"* — and warns you **early, while there's still time**.
+- If the terrain data is lost mid-flight, it **holds the last valid altitude target and warns** on the OSD — it never descends on dead data.
+- The stock 3D Cruise altitude controller is **untouched**; this only feeds it a moving target.
+- **Not used for autonomous navigation.** RTH and waypoint missions do **not** use it yet — the mode yields to them and they fly stock (terrain-blind).
+
+## Setup
+
+1. **Prepare and enable terrain data** — see **[Terrain](Terrain.md)**: generate `.TER` tiles for your area (include the take-off tile), copy them to the SD card root, then in the CLI:
+   ```
+   set terrain_enabled = ON
+   save
+   ```
+2. **Assign the mode.** In the Configurator **Modes** tab (or via CLI `aux`), put **TERRAIN AGL HOLD** on a spare, **deliberately guarded** switch — not next to ARM. It is active only inside 3D Cruise.
+   > **Hint (once you are confident with the mode):** you *can* put **NAV CRUISE** and **TERRAIN AGL HOLD** on the **same switch** so one flip arms both. This is **not** the recommended default — keeping them on **separate** switches lets you enter Cruise first, confirm it, and engage terrain hold as a deliberate second step.
+3. **Set the floor and look-ahead** if you want (defaults are sensible — see *Settings*).
+4. **OSD:** add the **Rangefinder** element to see your height above ground (AGL) in flight. The flight-mode field shows **TERR** while the hold is engaged.
+5. **Fly** in 3D Cruise, then flip the switch — it captures and holds your current AGL. **Panic rule: box off = stock cruise instantly; the pitch stick always wins.**
+
+## Settings
+
+| Setting | Default | Range | Meaning |
+|---|---|---|---|
+| `terrain_nav_min_agl` | 60 m | 50 – 120 m | The **safety floor** — the lowest height above ground the mode will hold. Engage below it and it climbs up to it. (Stored in centimetres: 6000 = 60 m.) |
+| `terrain_nav_lookahead` | 1000 m | 0 – 2000 m | How far ahead along your track the **forward scan** looks for rising terrain. `0` disables the early *TERRAIN AHEAD!* warning, leaving only the reactive floor. |
+
+> **NB — keep your climb rates honest.** The climb authority is your existing **`nav_fw_auto_climb_rate`** — there is no separate terrain climb setting, and the *TERRAIN AHEAD!* escape warning **trusts that number**, so set it to a rate your model genuinely sustains. Keep **`nav_fw_manual_climb_rate` at least equal to `nav_fw_auto_climb_rate`** (they default to 300 / 500) — or **greater**, if you want pulling the stick to actually add climb. If `nav_fw_manual_climb_rate` is **lower** than `nav_fw_auto_climb_rate`, grabbing pitch during an auto-climb commands the lower rate: the nose visibly eases and it can even **slow** the climb.
+
+## Behaviour in the air
+
+- **Engage at or above the floor** → captures your current AGL and holds it.
+- **Engage below the floor** → climbs to the floor (*TERRAIN AUTO CLIMB TO MIN*).
+- **Pitch stick** → sets a new held AGL. The stick always wins: it pauses the hold the moment it moves (smooth blend, no jump); centre it and the current height is re-captured.
+- **Hands off over a hill** → the target rides the terrain up and back down.
+- **Sharp turns while riding the floor** clip the safety margin — bank with height in hand.
+
+## OSD messages — the alarm ladder
+
+The mode narrates itself on the OSD. Higher-priority messages take the slot; while the autopilot is auto-climbing under a warning, the two alternate so you always see both the danger and the action.
+
+| Message | Kind | Meaning | What to do |
+|---|---|---|---|
+| **TERRAIN! TURN AWAY!** | Warning | Too low **and** climbing can no longer save you (full climb still loses, or you're sinking, or pinned at your altitude ceiling). | **Turn away immediately** (manual + full throttle). Don't count on pulling up. |
+| **TERRAIN! PULL UP!** | Warning | Too low, but climbing **still works**. | Release the stick — the autopilot climbs at full rate — or pull **if `nav_fw_manual_climb_rate` > `nav_fw_auto_climb_rate`**; if in doubt, turn as well. |
+| **TERRAIN AHEAD!** | Caution | The forward scan says that at this speed the slope ahead beats a full-rate climb — **you have time now**. | Turn, slow down, or climb early — while it's cheap. |
+| **TERRAIN VS MAX ALT** | Caution | The terrain needs more altitude than your `nav_max_altitude` ceiling allows (only if a ceiling is set). | Raise/remove the ceiling, or accept reduced clearance at the wall. |
+| **TERRAIN AUTO CLIMB TO MIN** | Info | The autopilot is already climbing you to the floor. | Nothing — it's handled. |
+| **TERRAIN NOT READY** | Info | Engagement refused — no usable terrain data yet. | Fly stock cruise; check tiles / SD card if it persists. |
+| **TERRAIN LOST - ALT FROZEN** | Caution | Data lost mid-hold — the altitude target is **frozen at the last valid value**. | Never descend blind; navigate out on the frozen altitude (it resumes after a few seconds of good data). |
+| **TERRAIN LOOKAHEAD OFF** | Info | The forward scan is unavailable (no heading estimate). | Only the reactive floor guards you now. |
+
+> **About `nav_max_altitude`:** that ceiling is **barometric altitude above your Home point** — it is *not* terrain-AGL aware. So over rising ground the terrain can legitimately need more altitude than the ceiling allows; that is exactly when *TERRAIN VS MAX ALT* appears.
+
+## Safety & limitations
+
+- **Fixed-wing only.** It never runs on multirotors, rovers, or boats, and it works **only inside 3D Cruise** (mandatory at this stage).
+- **Not for autonomous navigation yet.** RTH and waypoint missions do not use it — the mode **yields cleanly to stock behaviour** (hands the aircraft back) during launch, autoland, emergency landing, RTH, and waypoint missions, and on **GPS loss**. Stock RTH is terrain-blind: it does not follow the ground.
+- The **floor is deliberately high** (default 60 m, minimum 50 m). Terrain data and GPS each carry a few metres of error, so keep clearance in hand — don't set the floor low.
+- **Not a rangefinder replacement.** The value is the ground elevation at your GPS position from the map — it does **not** see trees, buildings, wires, or the actual nearest object below you.
+- **Never stacks with the rangefinder** — it refuses to engage while SURFACE mode is active.
+- If terrain data is lost while engaged, the target **freezes at the last valid altitude** (*TERRAIN LOST - ALT FROZEN*) — it never descends on dead data.

docs/Settings.md

@@ -6938,6 +6938,36 @@ Which aux channel to use to change serial output & baud rate (MSP / Telemetry).
 
 ---
 
+### terrain_enabled
+
+Enable load terrain data from SD card
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
+### terrain_nav_lookahead
+
+TERRAIN AGL HOLD: check terrain up to this distance ahead along the flight path and climb early for rising ground [m]. 0 disables the lookahead. The effective distance is also limited by the terrain block cache of the flight controller
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1000 | 0 | 2000 |
+
+---
+
+### terrain_nav_min_agl
+
+TERRAIN AGL HOLD: minimum held height above ground [cm]. Engaging below it commands a gentle climb to it. The floor absorbs worst-case terrain map error on steep slopes plus canopy - the minimum is the lowest value where that error budget still clears
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 6000 | 5000 | 12000 |
+
+---
+
 ### thr_comp_weight
 
 Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage)

docs/Terrain.md

@@ -0,0 +1,98 @@
+# Terrain
+```
+┌─────────────────────────────────────────────────┐
+│  ===Experimental===                             │
+│  This feature is experimental. Use it with      │
+│  caution.                                       │
+└─────────────────────────────────────────────────┘
+```
+
+This feature is available **only on H7-based and F4-based flight controllers with an SD card**.
+
+This feature in iNav determines the model’s altitude above ground level using preloaded elevation maps
+(terrain / SRTM data) stored on an SD card, without requiring a physical rangefinder. Based on the current GPS position, the
+flight controller identifies the corresponding point in the terrain map, calculates the ground elevation, and derives the
+altitude above terrain.
+
+In this first implementation, the calculated value is used **only for informational display in the OSD**. It is indicative
+only, does not yet behave as a true virtual rangefinder, and is not used for navigation or automatic altitude control. If the
+terrain data are unavailable or a read error occurs, the feature is automatically disabled.
+
+# SD Card Preparation
+
+Use only quality SD cards from reputable brands. Note that some combinations of flight controllers and SD cards can cause issues, 
+so if you experience any problems, try a different card before troubleshooting further. Compatibility problems have been 
+observed especially with F4-based flight controllers.
+
+For proper operation, the SD card must be prepared in advance. It is recommended to create a **partition with a maximum size
+of 4 GB** and format it to FAT32. 
+
+# Data Generation and Copying
+
+To generate elevation maps, use the terrain generator web tool available at https://martinovem.github.io/High-Resolution-Map-Generator/
+
+The generated `.TER` files use an iNav-specific format and are **not compatible with ArduPilot terrain data**. Do not use files from https://terrain.ardupilot.org/ or copy `.DAT` files from an ArduPilot setup — they will not be read correctly.
+
+**Before copying any terrain data files, the SD card must be formatted. Always format the card before each new terrain data
+installation to avoid file system errors.**
+
+The generator always produces **30 m resolution** data, which is the only resolution iNav supports — there is no resolution setting to choose.
+The generated files are then copied to the SD card into the root directory structure.
+
+For example
+```
+SDCARD:\
+├── N47E014.TER
+├── N47E015.TER
+├── N47E016.TER
+├── N49E015.TER
+├── N49E016.TER
+├── N49E017.TER
+└── N50E016.TER
+```
+
+Copying can be done via **iNav MSC (Mass Storage Class)** is not recommended.
+
+> **Important:** The `.TER` file covering your **home position** (the location where you arm) must always be present on the SD
+> card. The terrain system first reads the ground elevation at the home position to establish an altitude reference. Until this
+> succeeds, terrain data will not be displayed for any position — even if tiles for your entire flight area are present. Make
+> sure the tile for your take-off site is included when generating and copying terrain files.
+
+# Enabling and Displaying Terrain Data
+
+To display altitude above terrain in iNav, the OSD element **“Rangefinder distance”** must be enabled. If terrain data are
+available on the SD card and no valid data are available from a dedicated rangefinder, the value calculated by the terrain
+system will be displayed. If a rangefinder is present and providing valid data, its measurements always take priority and the
+actual distance to the ground will be shown.
+
+Loading terrain data from the SD card is enabled via the CLI using the following command:
+
+```text
+set terrain_enabled = ON
+save
+```
+
+After restarting the flight controller, iNav will automatically start loading terrain data and, when conditions are met, use
+them to display altitude above terrain.
+
+Finally, it is **strongly recommended to use only high-quality, branded SD cards** from reputable manufacturers. The terrain
+system is sensitive to SD card read speed and reliability, and low-quality or counterfeit cards may cause read errors,
+display dropouts, or automatic disabling of the feature during flight. Using a quality SD card significantly improves the
+stability and reliability of the terrain feature.
+
+# Troubleshooting
+- OSD shows no terrain value / "Rangefinder distance" is dash:
+  - Verify `terrain_enabled = ON` is set and saved (`set terrain_enabled = ON` → `save` → reboot)
+  - Confirm the SD card is mounted: check `status` in the CLI for SD card state
+  - Confirm a valid GPS fix is present before expecting any terrain value to appear
+  - Terrain data is only loaded **after the first valid GPS fix**. If the fix is acquired after a long wait, allow a few seconds for the first read to complete
+  - Check that the `.TER` file covering your **home position** exists on the SD card — see note in *Data Generation and Copying* above
+- Feature disables itself mid-flight:
+  - The terrain subsystem disables itself on any SD card read failure. This is most commonly caused by a slow or low-quality SD card. Replace the card with a branded, high-speed card
+  - On F4-based flight controllers, SD card compatibility issues are more common. If problems persist, test with a different card model or switch to an H7-based flight controller
+  - Ensure the SD card partition is ≤ 4 GB and formatted FAT32. Larger partitions or exFAT formatting can cause intermittent read failures
+- Value appears but is clearly wrong:
+  - Reformat the SD card and recopy the terrain files. A corrupted or partially overwritten FAT32 filesystem can produce plausible but incorrect altitude values
+  - The displayed value is altitude **above terrain at the current GPS position**, not distance to the nearest object below the aircraft. Trees, buildings, and local obstacles are not accounted for
+- Terrain data was not loaded before arming:
+  - If the flight controller is armed before terrain data for the home position is successfully read from the SD card, the system will not attempt to load data during the flight (to avoid SD card access latency while airborne). Disarm, wait for the OSD value to appear, then arm

src/main/CMakeLists.txt

@@ -669,6 +669,21 @@ main_sources(COMMON_SRC
     telemetry/sim.h
     telemetry/telemetry.c
     telemetry/telemetry.h
+
+    terrain/terrain.h
+    terrain/terrain.c
+    terrain/terrain_utils.h
+    terrain/terrain_utils.c
+    terrain/terrain_io.h
+    terrain/terrain_io.c
+    terrain/terrain_location.h
+    terrain/terrain_location.c
+    terrain/terrain_nav.h
+    terrain/terrain_nav.c
+    terrain/terrain_nav_hold_core.h
+    terrain/terrain_nav_hold_core.c
+    terrain/terrain_nav_hold.h
+    terrain/terrain_nav_hold.c
 )
 
 list(APPEND COMMON_SRC ${DSDLC_GENERATED_SRC})