MAVLINKHUD

Terrain Following

Executive Summary

Terrain Following allows a vehicle to maintain a constant height above the ground rather than above the takeoff point (Home). This is critical for low-level mapping or flying in mountainous regions.

ArduPilot manages this via the AP_Terrain library, which handles fetching, caching, and interpolating SRTM (Shuttle Radar Topography Mission) data.

Theory & Concepts

1. SRTM Grids

Terrain data is split into Grids.

  • Grid Point: A single altitude value (int16, meters).
  • Grid Spacing: The distance between points (default 100m, controlled by TERRAIN_SPACING).
  • Block: A 2kB structure stored on the SD card containing an 8x7 array of 4x4 micro-grids.

2. Bilinear Interpolation

The vehicle is rarely exactly on top of a grid point. To find the height at the vehicle's location ($x, y$), AP_Terrain uses bilinear interpolation between the four surrounding grid points.

$$ h(x,y) \approx \frac{1}{(x_2-x_1)(y_2-y_1)} (h_{11}(x_2-x)(y_2-y) + h_{21}(x-x_1)(y_2-y) + \dots) $$

This ensures the ground "slope" is continuous and the drone doesn't "step" up and down as it crosses grid lines.

3. Lookahead

For fixed-wing aircraft, knowing the height now isn't enough. You need to know if there is a mountain ahead. AP_Terrain::lookahead() projects a line along the current bearing to calculate the "climb rate" needed to clear upcoming obstacles.

Codebase Investigation

1. The Cache System: AP_Terrain::allocate()

Located in libraries/AP_Terrain/AP_Terrain.cpp.

  • The system allocates a memory cache (LRU - Least Recently Used) of 12 blocks (TERRAIN_CACHE_SZ).
  • It serves data from RAM if possible. If not, it schedules a background Disk I/O task (schedule_disk_io) to read from the SD card.

2. Height Calculation: height_amsl()

  • Finds the correct grid block.
  • Checks the bitmap to ensure valid data exists for the 4 surrounding points.
  • Performs the bilinear interpolation: 
    float avg1 = (1.0f-info.frac_x) * h00  + info.frac_x * h10;
float avg2 = (1.0f-info.frac_x) * h01  + info.frac_x * h11;
float avg  = (1.0f-info.frac_y) * avg1 + info.frac_y * avg2;

3. Failsafes

If the system cannot find data (e.g., SD card error or GCS disconnect):

  • It returns false.
  • The navigation controller (AC_WPNav) will refuse to start a terrain-following mission or trigger a failsafe (RTL/Land) if already flying.

Source Code Reference

Practical Guide: Safe Terrain Following

1. Pre-Flight Validation

Never launch a terrain mission blindly.

  • Check MAVLink Status: The GCS should show "Terrain: 100%".
  • Check TERRAIN_ENABLE: Must be 1.
  • SD Card: The terrain/ folder on the SD card can grow large. Ensure it's not full.

2. The TERRAIN_SPACING Trap

  • Default: 100m.
  • Problem: If you fly in a canyon that is 50m wide, a 100m grid might completely miss the walls!
  • Solution: For complex terrain, set TERRAIN_SPACING to 30 (meters). Note: This increases download time and SD card usage significantly.

3. "Terrain Failsafe"

What happens if the drone flies off the edge of your downloaded map?

  • Behavior: It stops and hovers (Copter) or circles (Plane).
  • Recovery: Switch to AltHold or Stabilize immediately. The auto-mission is stuck until data arrives.