Altitude calculated from terrain data

.gitignore

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

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)

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

@@ -84,6 +84,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
@@ -2308,7 +2310,11 @@ void blackboxUpdate(timeUs_t currentTimeUs)
 
 static bool canUseBlackboxWithCurrentConfiguration(void)
 {
-    return feature(FEATURE_BLACKBOX);
+    return feature(FEATURE_BLACKBOX)
+#ifdef USE_TERRAIN
+    && terrainConfig()->terrainEnabled == false && blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD
+#endif
+    ;
 }
 
 BlackboxState getBlackboxState(void)

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,8 @@
 #include "telemetry/telemetry.h"
 #include "telemetry/sbus2.h"
 
+#include "terrain/terrain.h"
+
 #include "config/feature.h"
 
 #if defined(SITL_BUILD)
@@ -370,6 +372,9 @@ void fcTasksInit(void)
 #ifdef USE_GPS
     setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
 #endif
+#ifdef USE_TERRAIN
+    setTaskEnabled(TASK_TERRAIN, terrainConfig()->terrainEnabled);
+#endif
 #ifdef USE_MAG
     setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
 #if defined(USE_MAG_MPU9250)
@@ -760,4 +765,13 @@ 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,
+    },
+#endif
+
 };

src/main/fc/settings.yaml

@@ -4501,3 +4501,13 @@ groups:
         field: noWayHomeAction
         table: geozone_rth_no_way_home
         type: uint8_t
+  - name: PG_TERRAIN_CONFIG
+    type: terrainConfig_t
+    headers: ["terrain/terrain.h"]
+    condition: USE_TERRAIN
+    members:
+      - name: terrain_enabled
+        description: "Enable load terrain data from SD card"
+        default_value: OFF
+        field: terrainEnabled
+        type: bool

src/main/io/osd.c

@@ -120,6 +120,10 @@
 #include "blackbox/blackbox_io.h"
 #endif
 
+#ifdef USE_TERRAIN
+#include "terrain/terrain.h"
+#endif
+
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
 #endif
@@ -2438,10 +2442,15 @@ static bool osdDrawSingleElement(uint8_t item)
             break;
         }
 
-#ifdef USE_RANGEFINDER
+#if defined(USE_RANGEFINDER) || defined(USE_TERRAIN)
     case OSD_RANGEFINDER:
         {
             int32_t range = rangefinderGetLatestRawAltitude();
+#ifdef USE_TERRAIN
+            if(!rangefinderIsHealthy()) {
+                range = terrainGetLastDistanceCm();
+            }
+#endif
             if (range < 0) {
                 buff[0] = '-';
                 buff[1] = '-';

src/main/scheduler/scheduler.h

@@ -141,6 +141,10 @@ typedef enum {
     TASK_GEOZONE,
 #endif
 
+#if defined (USE_TERRAIN)
+    TASK_TERRAIN,
+#endif
+
     /* Count of real tasks */
     TASK_COUNT,
 

src/main/target/BROTHERHOBBYH743/target.h

@@ -147,6 +147,11 @@
 #define SDCARD_SDIO_DEVICE      SDIODEV_1
 #define SDCARD_SDIO_4BIT
 
+#if defined(USE_SDCARD) && defined(USE_SDCARD_SDIO) && defined(USE_BARO)
+#define USE_TERRAIN //only SDIO is supported
+#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block
+#endif
+
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
 
 // *************** ADC *****************************

src/main/target/CORVON743V1/target.h

@@ -123,6 +123,11 @@
 #define SDCARD_SDIO_4BIT
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
 
+#if defined(USE_SDCARD) && defined(USE_SDCARD_SDIO) && defined(USE_BARO)
+#define USE_TERRAIN //only SDIO is supported
+#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block
+#endif
+
 // *************** ADC *****************************
 #define USE_ADC
 #define ADC_INSTANCE                ADC1

src/main/target/FLYWOOH743PRO/target.h

@@ -145,6 +145,11 @@
 #define SDCARD_SDIO_DEVICE      SDIODEV_1
 #define SDCARD_SDIO_4BIT
 
+#if defined(USE_SDCARD) && defined(USE_SDCARD_SDIO) && defined(USE_BARO)
+#define USE_TERRAIN //only SDIO is supported
+#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block
+#endif
+
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
 
 // *************** ADC *****************************

src/main/target/IFLIGHT_BLITZ_H7_PRO/target.h

@@ -137,6 +137,11 @@
 #define SDCARD_SDIO_DEVICE      SDIODEV_1
 #define SDCARD_SDIO_4BIT
 
+#if defined(USE_SDCARD) && defined(USE_SDCARD_SDIO) && defined(USE_BARO)
+#define USE_TERRAIN //only SDIO is supported
+#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block
+#endif
+
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
 
 // *************** ADC *****************************

src/main/target/KAKUTEH7WING/target.h

@@ -82,6 +82,11 @@
 #define SDCARD_SDIO_NORMAL_SPEED
 #define SDCARD_SDIO2_CMD_ALT
 
+#if defined(USE_SDCARD) && defined(USE_SDCARD_SDIO) && defined(USE_BARO)
+#define USE_TERRAIN //only SDIO is supported
+#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block
+#endif
+
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
 
 // *************** I2C /Baro/Mag *********************

src/main/target/MATEKH743/target.h

@@ -164,14 +164,19 @@
 #define SERIALRX_PROVIDER       SERIALRX_CRSF
 #define SERIALRX_UART           SERIAL_PORT_USART6
 
-// *************** SDIO SD BLACKBOX*******************
+// *************** SDIO SD BLACKBOX AND TERRAIN *******************
 #define USE_SDCARD
 #define USE_SDCARD_SDIO
 #define SDCARD_SDIO_DEVICE      SDIODEV_1
 #define SDCARD_SDIO_4BIT
 
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
 
+#if defined(USE_SDCARD) && defined(USE_SDCARD_SDIO) && defined(USE_BARO)
+#define USE_TERRAIN //only SDIO is supported
+#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block
+#endif
+
 // *************** ADC *****************************
 #define USE_ADC
 #define ADC_INSTANCE                ADC1

src/main/target/MICOAIR743/target.h

@@ -123,6 +123,11 @@
 #define SDCARD_SDIO_4BIT
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
 
+#if defined(USE_SDCARD) && defined(USE_SDCARD_SDIO) && defined(USE_BARO)
+#define USE_TERRAIN //only SDIO is supported
+#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block
+#endif
+
 // *************** ADC *****************************
 #define USE_ADC
 #define ADC_INSTANCE                ADC1

src/main/target/MICOAIR743AIO/target.h

@@ -117,6 +117,11 @@
 #define SDCARD_SDIO_4BIT
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
 
+#if defined(USE_SDCARD) && defined(USE_SDCARD_SDIO) && defined(USE_BARO)
+#define USE_TERRAIN //only SDIO is supported
+#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block
+#endif
+
 // *************** ADC *****************************
 #define USE_ADC
 #define ADC_INSTANCE                ADC1

src/main/target/MICOAIR743V2/target.h

@@ -138,6 +138,11 @@
 #define SDCARD_SDIO_4BIT
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
 
+#if defined(USE_SDCARD) && defined(USE_SDCARD_SDIO) && defined(USE_BARO)
+#define USE_TERRAIN //only SDIO is supported
+#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block
+#endif
+
 // *************** ADC *****************************
 #define USE_ADC
 #define ADC_INSTANCE                ADC1

src/main/target/NEUTRONRCH7BT/target.h

@@ -139,6 +139,11 @@
 #define SDCARD_SDIO_DEVICE      SDIODEV_1
 #define SDCARD_SDIO_4BIT
 
+#if defined(USE_SDCARD) && defined(USE_SDCARD_SDIO) && defined(USE_BARO)
+#define USE_TERRAIN //only SDIO is supported
+#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block
+#endif
+
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
 
 // *************** ADC *****************************

src/main/target/TBS_LUCID_H7/target.h

@@ -141,6 +141,11 @@
 #define SDCARD_SDIO_DEVICE      SDIODEV_1
 #define SDCARD_SDIO_4BIT
 
+#if defined(USE_SDCARD) && defined(USE_SDCARD_SDIO) && defined(USE_BARO)
+#define USE_TERRAIN //only SDIO is supported
+#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block
+#endif
+
 #define USE_ADC
 #define ADC_INSTANCE                ADC1
 

src/main/target/TBS_LUCID_H7_WING/target.h

@@ -141,6 +141,12 @@
 #define SDCARD_SDIO_DEVICE      SDIODEV_1
 #define SDCARD_SDIO_4BIT
 
+#if defined(USE_SDCARD) && defined(USE_SDCARD_SDIO) && defined(USE_BARO)
+#define USE_TERRAIN //only SDIO is supported
+#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block
+#endif
+
+
 #define USE_ADC
 #define ADC_INSTANCE                ADC1
 

src/main/target/TBS_LUCID_H7_WING_MINI/target.h

@@ -141,6 +141,12 @@
 #define SDCARD_SDIO_DEVICE      SDIODEV_1
 #define SDCARD_SDIO_4BIT
 
+#if defined(USE_SDCARD) && defined(USE_SDCARD_SDIO) && defined(USE_BARO)
+#define USE_TERRAIN //only SDIO is supported
+#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block
+#endif
+
+
 #define USE_ADC
 #define ADC_INSTANCE                ADC1