terrain data from SD card functionality to inav

Author: error414

.gitignore

@@ -46,6 +46,9 @@ launch.json
 # Assitnow token and files for test script
 tokens.yaml
 *.ubx
+/.idea
+
+/testing/
 
 # Local development files
 .semgrepignore

docs/Settings.md

@@ -6372,6 +6372,16 @@ 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 |
+
+---
+
 ### 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,73 @@
+# 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** (preferably SDIO). Due to the high CPU load
+and SD card read speed requirements of terrain data processing, it is not supported on weaker hardware.
+
+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
+
+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. Formatting should be done using the official 
+[**SD Memory Card Formatter**](https://www.sdcard.org/downloads/formatter/sd-memory-card-formatter-for-windows-download/) tool from the SD Association,
+which ensures correct alignment and a compatible file system structure. Using this tool minimizes the risk of terrain data
+read issues during flight and is considered the recommended method for preparing an SD card for iNav.
+
+# Data Generation and Copying
+
+To generate elevation maps, use the terrain generator web tool available at https://terrain.ardupilot.org/
+
+In iNav, **only 30 m resolution (SRTM1)** is currently supported, so this option must be selected during data generation.
+The generated files are then copied to the SD card into the appropriate directory structure.
+
+Copying can be done via **iNav MSC (Mass Storage Class)**, but this method is very slow, so using an **external SD card reader**
+is strongly recommended. Before copying the data, the file **`FREESPAC.E`** must be deleted from the root directory of the SD
+card. iNav uses this file to track available disk space, and without deleting it, the card may appear to be full. After the
+copying process is complete, iNav will automatically recreate this file on the next startup.
+
+# 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.
+
+# TL;DR
+
+- **H7/F4 flight controller** with SD card required
+- Format SD card using [SD Memory Card Formatter](https://www.sdcard.org/downloads/formatter/sd-memory-card-formatter-for-windows-download/) (max 4 GB partition)
+- Generate terrain data at https://terrain.ardupilot.org/ — select **SRTM1 (30 m)**
+- Delete `FREESPAC.E` from SD card root before copying files (use external card reader, not MSC)
+- Enable via CLI: `set terrain_enabled = ON` + `save`
+- Enable **Rangefinder distance** OSD element to see altitude above terrain
+- ⚠️ Displayed value is **informational only** — not used for navigation or altitude control
+- Use only **high-quality branded SD cards**

src/main/CMakeLists.txt

@@ -626,6 +626,15 @@ 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
 )
 
 add_subdirectory(target)

src/main/blackbox/blackbox.c

@@ -65,6 +65,8 @@
 
 #include "io/beeper.h"
 #include "io/gps.h"
+#include "io/asyncfatfs/asyncfatfs.h"
+
 
 #include "navigation/navigation.h"
 
@@ -84,6 +86,8 @@
 #include "flight/wind_estimator.h"
 #include "sensors/temperature.h"
 
+#include "terrain/terrain.h"
+
 
 #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT)
 #define DEFAULT_BLACKBOX_DEVICE     BLACKBOX_DEVICE_FLASH
@@ -196,6 +200,54 @@ typedef struct blackboxDeltaFieldDefinition_s {
     uint8_t condition; // Decide whether this field should appear in the log
 } blackboxDeltaFieldDefinition_t;
 
+static bool canUseBlackboxWithCurrentConfiguration(void)
+{
+    return feature(FEATURE_BLACKBOX);
+}
+
+#ifdef USE_TERRAIN
+//state machine to get access to other device, it's designed only for one other device, in this case for terrain
+//if you would like to have more devices, some queue must be implemented
+static struct blackboxSDCardAccessStatus_s {
+    bool blackboxAccessToSDGrantedToOtherDevice;
+    bool requestToSdCardAccessState;
+
+} blackboxSDCardAccessStatus = {
+        .blackboxAccessToSDGrantedToOtherDevice = false,
+        .requestToSdCardAccessState = false
+};
+
+
+/**
+ * request access from terrain subsystem
+ * @return
+ */
+bool requestToSdCardAccess(void)
+{
+    if(blackboxConfig()->device != BLACKBOX_DEVICE_SDCARD || !canUseBlackboxWithCurrentConfiguration()){
+        return true;
+    }
+
+    if(blackboxSDCardAccessStatus.blackboxAccessToSDGrantedToOtherDevice){
+        return true;
+    }
+
+    blackboxSDCardAccessStatus.requestToSdCardAccessState = true;
+    return false;
+}
+
+/**
+ * release access from terrain subsystem
+ * @return
+ */
+void releaseSdCardAccess(void)
+{
+    blackboxSDCardAccessStatus.blackboxAccessToSDGrantedToOtherDevice = false;
+    blackboxSDCardAccessStatus.requestToSdCardAccessState = false;
+}
+#endif
+
+
 /**
  * Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is
  * written into the flight log header so the log can be properly interpreted (but these definitions don't actually cause
@@ -2176,6 +2228,24 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
  */
 void blackboxUpdate(timeUs_t currentTimeUs)
 {
+#ifdef USE_TERRAIN
+    if(blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD){
+        //access to SD card is given to other device
+        if(blackboxSDCardAccessStatus.blackboxAccessToSDGrantedToOtherDevice){
+            return;
+        }
+
+        //incooming request to get access to SD card
+        if(blackboxSDCardAccessStatus.requestToSdCardAccessState && (blackboxState == BLACKBOX_STATE_RUNNING || blackboxState == BLACKBOX_STATE_STOPPED)){
+            //we have to be sure that all writes are already processed and SD card is in idle
+            if(afatfs_isIdle()){
+                blackboxSDCardAccessStatus.requestToSdCardAccessState = false;
+                blackboxSDCardAccessStatus.blackboxAccessToSDGrantedToOtherDevice = true;
+            }
+        }
+    }
+#endif
+
     if (blackboxState >= BLACKBOX_FIRST_HEADER_SENDING_STATE && blackboxState <= BLACKBOX_LAST_HEADER_SENDING_STATE) {
         blackboxReplenishHeaderBudget();
     }
@@ -2306,11 +2376,6 @@ void blackboxUpdate(timeUs_t currentTimeUs)
     }
 }
 
-static bool canUseBlackboxWithCurrentConfiguration(void)
-{
-    return feature(FEATURE_BLACKBOX);
-}
-
 BlackboxState getBlackboxState(void)
 {
     return blackboxState;

src/main/blackbox/blackbox.h

@@ -75,3 +75,6 @@ void blackboxIncludeFlagSet(uint32_t mask);
 void blackboxIncludeFlagClear(uint32_t mask);
 bool blackboxIncludeFlag(uint32_t mask);
 BlackboxState getBlackboxState(void);
+
+bool requestToSdCardAccess(void);
+void releaseSdCardAccess(void);

src/main/blackbox/blackbox_io.c

@@ -435,7 +435,16 @@ static bool blackboxSDCardBeginLog(void)
         if (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY) {
             blackboxSDCard.state = BLACKBOX_SDCARD_WAITING;
 
-            afatfs_mkdir("logs", blackboxLogDirCreated);
+            if(afatfs_isCurrentDirRoot()){
+                //we are in root of SD card, we have to create or move to log directory
+                afatfs_mkdir("logs", blackboxLogDirCreated);
+            }
+            else
+            {
+                //we are already in log directory
+                blackboxSDCard.logDirectory = NULL;
+                blackboxSDCard.state = BLACKBOX_SDCARD_READY_TO_CREATE_LOG;
+            }
         }
         break;
 
@@ -444,6 +453,12 @@ static bool blackboxSDCardBeginLog(void)
         break;
 
     case BLACKBOX_SDCARD_ENUMERATE_FILES:
+
+        if (blackboxSDCard.logDirectory == NULL) {
+            blackboxSDCard.state = BLACKBOX_SDCARD_READY_TO_CREATE_LOG;
+            break;
+        }
+
         while (afatfs_findNext(blackboxSDCard.logDirectory, &blackboxSDCard.logDirectoryFinder, &directoryEntry) == AFATFS_OPERATION_SUCCESS) {
             if (directoryEntry && !fat_isDirectoryEntryTerminator(directoryEntry)) {
                 // If this is a log file, parse the log number from the filename
@@ -469,6 +484,12 @@ static bool blackboxSDCardBeginLog(void)
         break;
 
     case BLACKBOX_SDCARD_CHANGE_INTO_LOG_DIRECTORY:
+        //if logDirectory is NULL, it would mean change directory to ROOT, we don't want to do that
+        if (blackboxSDCard.logDirectory == NULL) {
+            blackboxSDCard.state = BLACKBOX_SDCARD_READY_TO_CREATE_LOG;
+            break;
+        }
+
         // Change into the log directory:
         if (afatfs_chdir(blackboxSDCard.logDirectory)) {
             // We no longer need our open handle on the log directory
@@ -535,7 +556,7 @@ bool blackboxDeviceEndLog(bool retainLog)
         ) {
             // Don't bother waiting the for the close to complete, it's queued now and will complete eventually
             blackboxSDCard.logFile = NULL;
-            blackboxSDCard.state = BLACKBOX_SDCARD_READY_TO_CREATE_LOG;
+            blackboxSDCard.state = BLACKBOX_SDCARD_INITIAL;
             return true;
         }
         return false;

src/main/config/parameter_group_ids.h

@@ -77,6 +77,7 @@
 // #define PG_ELERES_CONFIG 55
 #define PG_TEMP_SENSOR_CONFIG 56
 #define PG_CF_END 56
+#define PG_TERRAIN_CONFIG 57
 
 // Driver configuration
 //#define PG_DRIVER_PWM_RX_CONFIG 100

src/main/fc/fc_init.c

@@ -151,6 +151,8 @@
 
 #include "telemetry/telemetry.h"
 
+#include "terrain/terrain.h"
+
 #if defined(SITL_BUILD)
 #include "target/SITL/serial_proxy.h"
 #endif
@@ -611,6 +613,19 @@ void init(void)
     }
 #endif
 
+#ifdef USE_SDCARD
+
+#ifdef USE_TERRAIN
+    if (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD)
+#endif
+    {
+        sdcardInsertionDetectInit();
+        sdcard_init();
+        afatfs_init();
+    }
+#endif
+
+
 #ifdef USE_BLACKBOX
 
     //Do not allow blackbox to be run faster that 1kHz. It can cause UAV to drop dead when digital ESC protocol is used
@@ -635,20 +650,15 @@ void init(void)
             }
             break;
 #endif
-
-#ifdef USE_SDCARD
-        case BLACKBOX_DEVICE_SDCARD:
-            sdcardInsertionDetectInit();
-            sdcard_init();
-            afatfs_init();
-            break;
-#endif
         default:
             break;
     }
 
     blackboxInit();
 #endif
+#ifdef USE_TERRAIN
+    terrainInit();
+#endif
 
     gyroStartCalibration();
 

src/main/fc/fc_tasks.c

@@ -95,6 +95,9 @@
 #include "telemetry/telemetry.h"
 #include "telemetry/sbus2.h"
 
+#include "terrain/terrain.h"
+#include "terrain/terrain_io.h"
+
 #include "config/feature.h"
 
 #if defined(SITL_BUILD)
@@ -370,6 +373,10 @@ void fcTasksInit(void)
 #ifdef USE_GPS
     setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
 #endif
+#ifdef USE_TERRAIN
+    setTaskEnabled(TASK_TERRAIN, terrainConfig()->terrainEnabled);
+    setTaskEnabled(TASK_TERRAIN_IO, terrainConfig()->terrainEnabled);
+#endif
 #ifdef USE_MAG
     setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
 #if defined(USE_MAG_MPU9250)
@@ -760,4 +767,19 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#ifdef USE_TERRAIN
+    [TASK_TERRAIN] = {
+        .taskName = "TERRAIN",
+        .taskFunc = terrainUpdateTask,
+        .desiredPeriod = TASK_PERIOD_HZ(TERRAIN_TASK_RATE_HZ),
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+    [TASK_TERRAIN_IO] = {
+            .taskName = "TERRAIN_IO",
+            .taskFunc = loadGridToCacheTask,
+            .desiredPeriod = TASK_PERIOD_HZ(TERRAIN_IO_TASK_RATE_HZ),
+            .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
+
 };