Error414/feature/inav terrain

[error414]

Terrain AGL

Terrain is a system that can show AGL (Above Ground Level) in the "distance" OSD element. It loads pre-generated data from an SD card and uses GPS data and barometric altitude for AGL calculation.

Features

• Shows AGL in the "distance" OSD element
• Can run alongside a rangefinder — rangefinder data is used first, terrain AGL is used as fallback when the rangefinder is out of range
• Can run with blackbox, but terrain needs approximately 150–250ms to load data from cache, so there may be short gaps in the blackbox log

Limitations

• Works only on H7 and F4 boards with an SD card slot
• Works only with SRTM1 resolution (30m). SRTM3 (90m) is not supported, but can be added via settings
• This feature is marked as EXPERIMENTAL in the documentation

Technical details

Terrain uses two tasks: an IO task and a GPS task.

Task	Frequency	Duration
GPS task	5Hz	1–2µs
IO task	50Hz	~40µs

The GPS task reads GPS data and creates requests for the IO task. The IO task reads requests from cache and loads grid data into cache.

The two-task design is needed because AGL data must already be loaded in cache by the time it is requested. In some conditions, AGL data requests should stop, but the IO task must keep running to close files and directories, free memory, and release the blackbox lock.

The IO task needs approximately 10 cycles to load data from the SD card. Task frequency can be adjusted to reduce gaps in the blackbox log.

Board	Cache size
F4	5 grids
H7	8 grids

Protection

When arming, the system checks whether the SD card can be read. If the read fails due to a faulty SD card or missing grid data, the terrain system is disabled for the entire flight. This prevents the first SD card read from happening mid-flight.

Inav Configurator PR: iNavFlight/inav-configurator#2651

Testing

I tested it for few months on two planes
Hewing T1, MatekH743Wlite (blackbox on)
Hewing T2, MatekH743Wlite + dedicated rangerfinder NRA15 (blackbox on)

other testers
Hewing T1, SpeedyBeeF4Wing (blackbox on)
Beluga, SpeedyBeeF4Wing

For testers

Documentation:
https://github.com/error414/inav/blob/4fb0fdb9e28d899f03293d34b60907cadd1dbd2b/docs/Terrain.md

Use configuratior:
https://github.com/iNavFlight/inav-configurator/tree/maintenance-10.x

[github-actions]

Test firmware build ready — commit f4fdd5e

Download firmware for PR #11438

240 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.

[Jetrell]

Tested again with a H743. Now with the grid loading fix, it works well.
I cross referenced multiple flight points to Google Earth elevation, and they where practically the same.

[Jetrell]

I've been using this on a couple of planes each time I go out flying. And I have to say I find it very useful to know and compare the ground altitude to the takeoff altitude. Especially when flying over a broad valley, that I could only guess how far it was below the plane.

Even the terrain altitude accuracy at speed is good.. e.g. when I have flown across a narrow gully at flight speed. It would register that drop in elevation on the OSD for a moment.

[sensei-hacker]

Definitely looks interesting. Rather than modifying each and every target individually, would it make sense to do something like:

#if defined(USE_SDCARD) && MCU_FLASH_SIZE  > 512
   #USE_TERRAIN
#endif

[error414]

you are right, it would be better, I will do it as you said

[error414]

I tried to add it to src/main/target/common.h, but I have no idea how to do that. I would need check if FC has SD card and Baro, but common.h file is loaded before target.h so there is not information about target.

the order

common.h -> platform.h -> FC_TARGET_NAME/target.h

Do you see any solution?

[sensei-hacker]

I tried to add it to src/main/target/common.h, but I have no idea how to do that. I would need check if FC has SD card and Baro, but common.h file is loaded before target.h so there is not information about target.

the order

common.h -> platform.h -> FC_TARGET_NAME/target.h

Do you see any solution?

I will work on that.

[Jetrell]

@error414 I've also been using this with F405 controllers. And it works just as well as it does with the H743. I look forward to it being merged.

[sensei-hacker]

How about in common_post.h ?

In platform.h I see:

#include "target/common.h"
#include "target.h"
#include "target/sanity_check.h"
#include "target/common_post.h"

So maybe in common_post.h:

#if defined(USE_SDCARD_SDIO) && (MCU_FLASH_SIZE > 512) && !defined(USE_TERRAIN)                                                                         
#define USE_TERRAIN                                                                                                                                
#endif

Then in terrain_utils.h:

#ifndef TERRAIN_GRID_BLOCK_CACHE_SIZE                                                                                                               
#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block                                                                                
#endif                                                                                                                                              

Obviously any ideas or suggestions I have are just ideas without me understanding your code as well as you do.

[error414]

good catch, it works.

targets for which is terrain enabled

- AETH743Basic
  - ALIENFLIGHTF4
  - ANYFCF7
  - ATOMRCF405NAVI
  - ATOMRCF405NAVI_DELUX
  - BEEROTORF4
  - BLUEBERRYF405
  - BLUEBERRYH743
  - BLUEJAYF4
  - BRAHMA_H7
  - BROTHERHOBBYH743
  - COREWINGF405WINGV2
  - CORVON405V2
  - CORVON743V1
  - F4BY
  - FISHDRONEF4
  - FLYWOOH743PRO
  - FRSKYF4
  - FRSKYF405
  - FRSKYPILOT
  - GEPRCF745_BT_HD
  - GEPRC_TAKER_H743
  - HAKRCH743
  - IFLIGHT_2RAW_H743
  - IFLIGHT_BLITZ_H7_PRO
  - JHEMCUF405WING
  - KAKUTEF7
  - KAKUTEH7
  - KAKUTEH7WING
  - KROOZX
  - MATEKF405
  - MATEKF405CAN
  - MATEKF405SE
  - MATEKF405TE_SD
  - MATEKF765
  - MATEKF765SE
  - MATEKH743
  - MICOAIR405MINI
  - MICOAIR405V2
  - MICOAIR743
  - NEUTRONRCH7BT
  - OMNIBUSF7
  - PIXRACER
  - SPEEDYBEEF405V3
  - SPEEDYBEEF405V4
  - SPEEDYBEEF405WING
  - SPEEDYBEEF405WINGV2
  - SPRACINGF4EVO
  - TBS_LUCID_H7
  - TBS_LUCID_H7_WING
  - TBS_LUCID_H7_WING_MINI
  - YUPIF4

[error414]

I improved terrain_io a little bit

1. keep dat file open -> it will reduce calling chdir and fopen. File is automatically closed after 60s inactivity. Opened files does not need any exra memory

2. release lock for blackbox more often. So blacbox is blocked much shorter time.

[sensei-hacker]

Hi @error414 — first off, really nice feature, the grid caching design is clean.

I was digging into why CI is currently failing to link on some targets (BLUEBERRYF405, and likely other "big flash, small RAM" F405-class boards) — USE_TERRAIN auto-enables via (MCU_FLASH_SIZE > 512) in common_post.h, with no check on RAM, so it lands on boards with only 128KB RAM and pushes them over by ~1.4KB (measured: this PR adds ~11.6KB of static RAM vs. maintenance-10.x baseline on BLUEBERRYF405). That prevents it from building.
11.6 KB for this feature is a lot on a board that only has 128KB total for everything.

Most of that is the grid cache (cache[TERRAIN_GRID_BLOCK_CACHE_SIZE], ~9.2KB at the 5-entry tier) plus the separate 2048-byte ioBlock scratch buffer in terrainIoState_t. I had a thought on the second one and wanted to run it by you.

Could the I/O scratch buffer be eliminated?

In terrain_io.c, each block gets read into the static ioBlock buffer, then memcpy'd into the destination once complete:

uint32_t readNow = afatfs_fread(terrainIoState.datFile,
    (uint8_t*)&terrainIoState.ioBlock + terrainIoState.bytesRead,
    sizeof(terrainIoState.ioBlock) - terrainIoState.bytesRead);
...
memcpy(terrainIoState.gridBlock, &terrainIoState.ioBlock.block, sizeof(gridBlock_t));

But terrainIoState.gridBlock already points straight at the destination cache slot, and every seek uses an absolute AFATFS_SEEK_SET offset recomputed from blocknum — so as far as I can tell nothing depends on the file position after a read finishes. That made me wonder whether we even need the intermediate copy at all.

Something like this — read directly into the destination, and stash the expected idx before overwriting it (since the compare currently relies on the destination's old idx still being there):

// before starting the read (e.g. when entering TERRAIN_IO_SEEK):
terrainIoState.expectedIdxX = terrainIoState.gridBlock->grid_idx_x;
terrainIoState.expectedIdxY = terrainIoState.gridBlock->grid_idx_y;

// TERRAIN_IO_READ, reading straight into the cache slot instead of ioBlock:
uint32_t readNow = afatfs_fread(terrainIoState.datFile,
    (uint8_t*)terrainIoState.gridBlock + terrainIoState.bytesRead,
    sizeof(gridBlock_t) - terrainIoState.bytesRead);
terrainIoState.bytesRead += readNow;
...
if (terrainIoState.bytesRead == sizeof(gridBlock_t)) {
    if (terrainIoState.gridBlock->grid_idx_x != terrainIoState.expectedIdxX ||
        terrainIoState.gridBlock->grid_idx_y != terrainIoState.expectedIdxY) {
        markGridBlockInvalid(terrainIoState.gridBlock);
        cleanUp();
        return;
    }
    markGridBlockAsRead(terrainIoState.gridBlock);
    finishGridBlockRead();
}

This only reads sizeof(gridBlock_t) (1821 B) instead of the full 2048-byte on-disk record, which seems fine since the next seek is always absolute — but I could easily be missing a reason the trailing padding needs to be consumed, or a reason the cache slot shouldn't be written mid-flight before it's marked valid. Would something like this work to save ~2048 bytes? Or is there a subtlety I'm not seeing?

Separately, and maybe as a quicker stopgap if that turns out to need more thought: would it be reasonable to drop TERRAIN_GRID_BLOCK_CACHE_SIZE from 5 to 2 on the smaller-RAM tier? That would close the current overflow.

[error414]

F405 had enought room for terrain, as I understand there is not enought room when you merged to maintanence-10.x . It makes sense.

If we need reduce using memory, your suggestion is correct. We can read directly to cache without any problem. I used temp gridIoBlock_t just because I like clean aproach, read all and then to have clean data in cache.

So you understand code very well :)

Even as you mentioned we don't have to read whole 2048 bytes, 1821 B is enought, rest of bytes is only zero. Ardupilot uses 2048 because this value is aligned with some data in telemetry. I don't remember exactly now.

For your solution must be adjusted src/main/terrain/terrain_utils.c:182 findGridCache function as well, not to return dirty block. It's easy change.

For F405 CPUs we can reduce cache to 4. It cover situation if you fly near corners. One block is area cca 900 * 1100m so next reading would be read when you fly 900m.

I'm on vacation now, so I can fix and test it next week, if it's OK.

[sensei-hacker]

Great. I also came up with two other ideas which should be able to further reduce the RAM usage by a further 75% without losing much of anything. I'll probably write those up as pull requests. It would also cut in half the time spent reading the data from flash.

Idea 1 - block point

The first one is cutting the RAM usage (and flash read time) in half by adjusting how the data is stored. It might mean doing a conversion while loading it, but I think it will be worth it, if my understanding is correct. It's my understanding the data is currently stored as int_16, with 1-meter units. Actual accuracy of the source data is around 12 meters. Each point uses 16 bits. Consider this scheme:

Each BLOCK gets a 16-bit value telling the minimum elevation in that block.
Each point gets an 8-bit value telling how much higher it is than the base value for that block, with 4-meter precision.
The actual elevation for a point is:
base + offset x 4

That preserves the full resolution of the source data, with only 8 bits per point instead of 16, right? Cuts RAM usage in half, and cuts in half the number of bytes that need to be read from flash.

Idea 2 - findGridCache() reads overlap data

The other idea is you mentioned you don't suggest read-ahead, which is cool, but what about using the data we already read and already have cached? Suppose I am leaving block 1 and entering block 2. So far, I'm still within the 120-meter overlap zone as my new block coordinate moves to block 2. I think the current code will cache miss and not return any data for that point, even though that point is ALSO within block 1? It's already loaded. findGridCache() could use the data we already have in block 1 while waiting for block 2 to load, right, since the current position is in both blocks?

Suppose I'm flying at 100 km/h. After entering block 2, I still have overlap data for 60 meters (120 meters?), or a little over two seconds. Figure it takes 100-200ms to load the uncached (but now smaller) new block I just entered. That's fine -- I'm in no hurry to load the new block because I have two (or four?) seconds of data in the overlap! Cache hits would be less of an issue, so I could cache maybe two rather than five?

[MartinovEm]

Interesting ideas! Two data points from the tile-accuracy side (I've been measuring these maps against ICESat-2 and a radar altimeter together with error414):

— the 8-bit / 4 m scheme has a hard ceiling of 255 × 4 = 1020 m of relief per block. I just scanned my Bulgarian tiles: Rila already hits 804 m in a single block — 79% of that ceiling. Alpine terrain (Eiger, Lauterbrunnen: 1400+ m of relief per km) will exceed it and clip — exactly the places where terrain data matters most.

— in our measurements the sources agree to ~6 m (P95), and we've been working to make that better; 4 m quantization would eat a good part of it back.

And one important thing if this would change the .DAT format itself: that's the ArduPilot terrain format — every existing tile generator (ArduPilot's own tools, terrain servers, and my generator) produces it, and people already have cards full of these tiles. A new format would break all of that and split the ecosystem — conversion-on-load into whatever RAM layout you like keeps the files compatible.

The overlap idea for boundary crossings sounds neat though. Maybe land the simple buffer fix first so CI is green, and explore the bigger reworks as follow-up PRs?

[error414]

@sensei-hacker

1. it's possible, if you are ok that it will not be compatible with ardupilot and that it will not cover all places on earth. Base + offset × 4 will cover max alt diference 1024m in square 900x1000m. For some mountains it's not enought. Or I miss something. :).

2. I can't verify now that it works like I think it works :). Leaving block 1 does not mean that I'm in overlaping zone. But it means that I'm out of overlaping zone and I need to load block 2.

I didnt read block2 if I'm in overlaping zone in block 1. I tried create image.

I will check it I will return from vacation.

The overlaping should prevent to load blocks if you fly between two blocks, but it's not needed if you have in cache both blocks. At least 4 blocks cache does not need overlaping at all.

[sensei-hacker]

I think 1024 would cover all but maybe two or three places on earth. Thinking about it more though, we may want the base to be the HIGHEST point in the block - we're flying over things. If the low point clips at three specific points on earth - well that accounts for the trees that grow in low (moist) points that aren't represented on the radar scan. 😁

One COULD do FP8 to include martian terrain, but that would mean extra CPU that probably isn't worth it.

Regarding compatibility - we could have a tool that converts on the SD card, BUT we could also do the conversion as it's loaded into RAM. (Where conversion means one subtraction and one divide / shift operation). So the format on the SD card would remain the same. We would just down sample each block as it's read to save RAM.

Four is a good divisor because the hardware can shift by two bits in a single cycle.

What I think is good about the overlap is it kinda gives you a smart lookahead "for free". If you always load the one or TWO blocks you're in, you're never waiting for a load. And therefore don't need to cache more than two on a low-RAM chip?
While traveling from A into B, you could load B while still in the overlap, still using data from A. I think?

[error414]

I'm just thinking laud :).

1. ok, If we would like to use 8bit height resulution, then as you said we can use our own DAT file format or convert data durring reading from SD card.

a) our format
We would need to save somewhere "base" value for each grid. Extra data would brake aligment of heigts in grid, so calculating x/y position (index in array) had to be a little bit adjusted. But its duable. "Base" should be save in grid, other wise it would mean next seek. What is expensive.

b) convering durring reading
We could not use fread function, we would have to create our own function fread + converting data on fly.

Current fread function reads data in 512bits junks, so read 2048 bytes needs 4 cycles. So if we would useb our format, then we would need only two reading cycles.

More over our new fread function would have to find the highest point (the base).

2. I think here is misunderstanding. Speed of loading new data is not problem. It's faster than OSD refresh. I tried in HITL disable cache and always read data from sd. (Read whole grid). It was fast enought. What I wanted to achieve is reduce amount of loading from sd card as possible.

For your suggestion overlaping is not needed. I can just read neighboring grid if I'm close to border of current grid. Or I miss something :D

Do you want to do these changes only for f4 with small RAM or for H7 as well ?

Btw: i can verify if overlaping functionality works properly and then f4 cpu can run only with 1 grid cache. In the worse case if pilot would fly zig-zag then reading new grid would be each 120m.

[error414]

Martinov made some statistics

8bit is not enough, the test is from Alps (moutain in Europe).

9bit is ok, and 10bit has plenty of space.

So what would use 9bit? It would be not so neat, I would not be possible to use just index from array, but still it duable and not so complicated.

[error414]

@sensei-hacker I made two changes which reduces RAM, quite a lot :)

So far it's in extra branch https://github.com/error414/inav/tree/error414/feature/inav_terrain_RAM I have to test on the field first.

1. as you suggested, read is without buffer and readding is directly to cache
2. with Martinov we tested base + 10bits (2m resolution). He prepared generator for that https://github.com/MartinovEm/High-Resolution-Map-Generator/tree/test/uint10-tiles
3. I reduced cache for F4 to 4.

What do you think?

Result:

Configuration	FLASH (kB)	FLASH %	RAM (kB)	RAM %
10-bit / 4 cache	655.40	73.15 %	122.99	96.09 %
16-bit / 4 cache	655.15	73.12 %	127.58	99.67 %
10-bit / 5 cache	655.48	73.16 %	124.13	96.98 %
16-bit / 5 cache	655.29	73.13 %	129.37	101.07 % ⚠️

Differences relative to the baseline (10-bit / 4 cache):

Configuration	Δ FLASH (kB)	Δ RAM (kB)
16-bit / 4 cache	−0.25	+4.59
10-bit / 5 cache	+0.08	+1.14
16-bit / 5 cache	−0.11	+6.38