diff --git a/.gitignore b/.gitignore index 6d7138e6bd5..2c99c0b4ad3 100644 --- a/.gitignore +++ b/.gitignore @@ -46,6 +46,9 @@ launch.json # Assitnow token and files for test script tokens.yaml *.ubx +/.idea + +/testing/ # Local development files .semgrepignore diff --git a/docs/Settings.md b/docs/Settings.md index 39e3de90d52..f3f71b3b545 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -6382,6 +6382,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) diff --git a/docs/Terrain.md b/docs/Terrain.md new file mode 100644 index 00000000000..2fc436354ac --- /dev/null +++ b/docs/Terrain.md @@ -0,0 +1,75 @@ +# 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 + +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://terrain.ardupilot.org/ + +**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.** + +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 root directory structure. + +For example +``` +SDCARD:\ +├── N47E014.DAT +├── N47E015.DAT +├── N47E016.DAT +├── N49E015.DAT +├── N49E016.DAT +├── N49E017.DAT +└── N50E016.DAT +``` + +Copying can be done via **iNav MSC (Mass Storage Class)** is not recommended. + +# 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. \ No newline at end of file diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 531d43a92fd..7957dd10822 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -628,6 +628,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) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f5225ca3e1e..ac525609c85 100644 --- a/src/main/blackbox/blackbox.c +++ b/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 @@ -468,6 +520,10 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { {"escRPM", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"escTemperature", -1, SIGNED, PREDICT(PREVIOUS), ENCODING(SIGNED_VB)}, #endif +#ifdef USE_TERRAIN + {"terrainAGL", -1, UNSIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"terrainAMSL", -1, UNSIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, +#endif }; #define BLACKBOX_FIRST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_HEADER @@ -572,6 +628,10 @@ typedef struct blackboxSlowState_s { #ifdef USE_ESC_SENSOR uint32_t escRPM; int8_t escTemperature; +#endif +#ifdef USE_TERRAIN + int32_t terrainAGL; + int32_t terrainAMSL; #endif uint16_t rxUpdateRate; uint8_t activeWpNumber; @@ -1357,6 +1417,10 @@ static void writeSlowFrame(void) blackboxWriteUnsignedVB(slowHistory.escRPM); blackboxWriteSignedVB(slowHistory.escTemperature); #endif +#ifdef USE_TERRAIN + blackboxWriteSignedVB(slowHistory.terrainAGL); + blackboxWriteSignedVB(slowHistory.terrainAMSL); +#endif blackboxSlowFrameIterationTimer = 0; } @@ -1432,6 +1496,10 @@ static void loadSlowState(blackboxSlowState_t *slow) slow->escRPM = escSensor->rpm; slow->escTemperature = escSensor->temperature; #endif +#ifdef USE_TERRAIN + slow->terrainAGL = terrainGetLastDistanceCm(); + slow->terrainAMSL = terrainGetLastAMSL(); +#endif } /** @@ -2177,6 +2245,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(); } @@ -2307,11 +2393,6 @@ void blackboxUpdate(timeUs_t currentTimeUs) } } -static bool canUseBlackboxWithCurrentConfiguration(void) -{ - return feature(FEATURE_BLACKBOX); -} - BlackboxState getBlackboxState(void) { return blackboxState; diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 1901201fa22..97829df9cca 100644 --- a/src/main/blackbox/blackbox.h +++ b/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); diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index b344b0dfff6..e3c6444668b 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/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; diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 2acb9c8172e..99365b7506d 100644 --- a/src/main/config/parameter_group_ids.h +++ b/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 diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index a8ca6c0c199..af35c8c90b1 100644 --- a/src/main/fc/fc_init.c +++ b/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(); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 4ca125d5c6a..b81e2a85b6d 100755 --- a/src/main/fc/fc_tasks.c +++ b/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 + }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 58274b1b660..c2d86791ace 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4507,3 +4507,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 diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index d06fc2b8a76..56555873f41 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -495,6 +495,50 @@ static void afatfs_fileOperationContinue(afatfsFile_t *file); static uint8_t* afatfs_fileLockCursorSectorForWrite(afatfsFilePtr_t file); static uint8_t* afatfs_fileRetainCursorSectorForRead(afatfsFilePtr_t file); +bool afatfs_isIdle(void) +{ + // 1. Check if any sector is being actively flushed or read at the hardware level + if (afatfs.cacheFlushInProgress) { + return false; + } + + // 2. Check all cache descriptors for active I/O states (waiting for SD card) + for (int i = 0; i < AFATFS_NUM_CACHE_SECTORS; i++) { + if (afatfs.cacheDescriptor[i].state == AFATFS_CACHE_STATE_READING || + afatfs.cacheDescriptor[i].state == AFATFS_CACHE_STATE_WRITING) { + return false; + } + } + + // 3. Check if current directory state machine is performing an async task (like chdir or findNext) + if (afatfs.currentDirectory.operation.operation != AFATFS_FILE_OPERATION_NONE) { + return false; + } + +#ifdef AFATFS_USE_FREEFILE + // 4. Check freeFile state (ignore LOCKED as it's a static permission marker, not an operation) + if (afatfs.freeFile.operation.operation != AFATFS_FILE_OPERATION_NONE && + afatfs.freeFile.operation.operation != AFATFS_FILE_OPERATION_LOCKED) { + return false; + } +#endif + + // 5. Check all potentially open file handles for pending background operations + for (int i = 0; i < AFATFS_MAX_OPEN_FILES; i++) { + if (afatfs.openFiles[i].type != AFATFS_FILE_TYPE_NONE && + afatfs.openFiles[i].operation.operation != AFATFS_FILE_OPERATION_NONE) { + return false; + } + } + + // 6. During initialization, we are never "idle" in the operational sense + if (afatfs.filesystemState == AFATFS_FILESYSTEM_STATE_INITIALIZATION) { + return false; + } + + return true; +} + static uint32_t roundUpTo(uint32_t value, uint32_t rounding) { uint32_t remainder = value % rounding; @@ -3622,6 +3666,11 @@ afatfsError_e afatfs_getLastError(void) return afatfs.lastError; } +bool afatfs_isCurrentDirRoot(void) +{ + return afatfs.currentDirectory.directoryEntryPos.sectorNumberPhysical == 0; +} + void afatfs_init(void) { #ifdef STM32H7 diff --git a/src/main/io/asyncfatfs/asyncfatfs.h b/src/main/io/asyncfatfs/asyncfatfs.h index 521bd48fba7..9c15eee346d 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.h +++ b/src/main/io/asyncfatfs/asyncfatfs.h @@ -95,3 +95,6 @@ bool afatfs_isFull(void); afatfsFilesystemState_e afatfs_getFilesystemState(void); afatfsError_e afatfs_getLastError(void); + +bool afatfs_isIdle(void); +bool afatfs_isCurrentDirRoot(void); \ No newline at end of file diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f4fe19a4040..39063c10fbc 100644 --- a/src/main/io/osd.c +++ b/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 @@ -2439,19 +2443,29 @@ static bool osdDrawSingleElement(uint8_t item) break; } -#ifdef USE_RANGEFINDER - case OSD_RANGEFINDER: +#if defined(USE_RANGEFINDER) || defined(USE_TERRAIN) + case OSD_RANGEFINDER: { - int32_t range = rangefinderGetLatestRawAltitude(); + int32_t range = -1; + +#if defined(USE_RANGEFINDER) + range = rangefinderGetLatestRawAltitude(); +#ifdef USE_TERRAIN + if (!rangefinderIsHealthy() || range == RANGEFINDER_OUT_OF_RANGE) { + range = terrainGetLastDistanceCm(); + } +#endif +#elif defined(USE_TERRAIN) + range = terrainGetLastDistanceCm(); +#endif + if (range < 0) { - buff[0] = '-'; - buff[1] = '-'; - buff[2] = '-'; + buff[0] = buff[1] = buff[2] = '-'; } else { - osdFormatDistanceSymbol(buff, range, 1, 3); + osdFormatDistanceSymbol(buff, range, 0, 3); } } - break; + break; #endif case OSD_ONTIME: diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 5d38a6dba59..70134ee45cc 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -141,6 +141,11 @@ typedef enum { TASK_GEOZONE, #endif +#if defined (USE_TERRAIN) + TASK_TERRAIN, + TASK_TERRAIN_IO, +#endif + /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 52cd4cf4a9d..87e4652d478 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -146,6 +146,11 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_UART SERIAL_PORT_USART2 diff --git a/src/main/target/AOCODARCF4V3/target.h b/src/main/target/AOCODARCF4V3/target.h index 0c95564162e..5f32f9577d5 100644 --- a/src/main/target/AOCODARCF4V3/target.h +++ b/src/main/target/AOCODARCF4V3/target.h @@ -151,6 +151,10 @@ #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC14 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif #else #define USE_FLASHFS #define USE_FLASH_M25P16 diff --git a/src/main/target/ATOMRCF405NAVI/target.h b/src/main/target/ATOMRCF405NAVI/target.h index 5e9c9b1bd4e..cc826ab89a8 100644 --- a/src/main/target/ATOMRCF405NAVI/target.h +++ b/src/main/target/ATOMRCF405NAVI/target.h @@ -151,6 +151,11 @@ #define SDCARD_CS_PIN PB6 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + /* * LED Strip */ diff --git a/src/main/target/ATOMRCF405NAVI_DELUX/target.h b/src/main/target/ATOMRCF405NAVI_DELUX/target.h index f04a5d5da6f..9c7b1630197 100644 --- a/src/main/target/ATOMRCF405NAVI_DELUX/target.h +++ b/src/main/target/ATOMRCF405NAVI_DELUX/target.h @@ -149,6 +149,11 @@ #define SDCARD_CS_PIN PC14 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + /* * LED Strip */ diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h index 1ff901b870a..1f30c30881c 100644 --- a/src/main/target/BEEROTORF4/target.h +++ b/src/main/target/BEEROTORF4/target.h @@ -52,6 +52,11 @@ #define SDCARD_SPI_BUS BUS_SPI2 #define SDCARD_CS_PIN SPI2_NSS_PIN +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + #define USE_VCP #define VBUS_SENSING_ENABLED #define VBUS_SENSING_PIN PC5 diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index e62120bdc86..d47753ad9a8 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -58,6 +58,11 @@ #define SDCARD_SPI_BUS BUS_SPI3 #define SDCARD_CS_PIN PA15 +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + #define M25P16_CS_PIN PB7 #define M25P16_SPI_BUS BUS_SPI3 diff --git a/src/main/target/BROTHERHOBBYH743/target.h b/src/main/target/BROTHERHOBBYH743/target.h index b069f0ccfd0..bdacc1a8554 100644 --- a/src/main/target/BROTHERHOBBYH743/target.h +++ b/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_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block +#endif + #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT // *************** ADC ***************************** diff --git a/src/main/target/COREWINGF405WINGV2/target.h b/src/main/target/COREWINGF405WINGV2/target.h index 2176728d84d..dc0884e2602 100644 --- a/src/main/target/COREWINGF405WINGV2/target.h +++ b/src/main/target/COREWINGF405WINGV2/target.h @@ -133,6 +133,11 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_CRSF #define SERIALRX_UART SERIAL_PORT_USART6 diff --git a/src/main/target/CORVON405V2/target.h b/src/main/target/CORVON405V2/target.h index 685acbe0588..a8e7842851c 100644 --- a/src/main/target/CORVON405V2/target.h +++ b/src/main/target/CORVON405V2/target.h @@ -66,6 +66,11 @@ #define SDCARD_CS_PIN PC9 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + // *************** UART ***************************** #define USE_VCP diff --git a/src/main/target/CORVON743V1/target.h b/src/main/target/CORVON743V1/target.h index fa5ba03d735..f777b9209f4 100644 --- a/src/main/target/CORVON743V1/target.h +++ b/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_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block +#endif + // *************** ADC ***************************** #define USE_ADC #define ADC_INSTANCE ADC1 diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 21f91c1d49b..9da7d048312 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -55,6 +55,11 @@ #define SDCARD_SPI_BUS BUS_SPI2 #define SDCARD_CS_PIN PE15 +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + #define USE_VCP #define VBUS_SENSING_PIN PA9 diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index 10e325202e3..1372a7fd182 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -82,6 +82,11 @@ #define SDCARD_SPI_BUS BUS_SPI3 #define SDCARD_CS_PIN PB6 +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + // *************** Flash ***************************** #define M25P16_CS_PIN PA15 #define M25P16_SPI_BUS BUS_SPI3 diff --git a/src/main/target/FLYSPARKF4V4/target.h b/src/main/target/FLYSPARKF4V4/target.h index 9a6619f0793..24b6aedf1a7 100644 --- a/src/main/target/FLYSPARKF4V4/target.h +++ b/src/main/target/FLYSPARKF4V4/target.h @@ -126,6 +126,11 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + // *************** OSD ***************************** #define USE_SPI_DEVICE_2 #define SPI2_SCK_PIN PB13 diff --git a/src/main/target/FLYWOOH743PRO/target.h b/src/main/target/FLYWOOH743PRO/target.h index 47419dc78ff..e98c33799ad 100644 --- a/src/main/target/FLYWOOH743PRO/target.h +++ b/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_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block +#endif + #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT // *************** ADC ***************************** diff --git a/src/main/target/FRSKYF4/target.h b/src/main/target/FRSKYF4/target.h index c014283f33e..eefa86233d5 100755 --- a/src/main/target/FRSKYF4/target.h +++ b/src/main/target/FRSKYF4/target.h @@ -45,6 +45,11 @@ #define SDCARD_SPI_BUS BUS_SPI2 #define SDCARD_CS_PIN SPI2_NSS_PIN +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + #define USE_VCP #define VBUS_SENSING_PIN PC5 diff --git a/src/main/target/IFLIGHT_BLITZ_H7_PRO/target.h b/src/main/target/IFLIGHT_BLITZ_H7_PRO/target.h index 7d93ec91ce6..3c3ce5e7dcf 100644 --- a/src/main/target/IFLIGHT_BLITZ_H7_PRO/target.h +++ b/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_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block +#endif + #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT // *************** ADC ***************************** diff --git a/src/main/target/JHEMCUF405WING/target.h b/src/main/target/JHEMCUF405WING/target.h index fd64ff153d6..348c8e096bc 100644 --- a/src/main/target/JHEMCUF405WING/target.h +++ b/src/main/target/JHEMCUF405WING/target.h @@ -121,6 +121,11 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_CRSF #define SERIALRX_UART SERIAL_PORT_USART1 diff --git a/src/main/target/KAKUTEH7WING/target.h b/src/main/target/KAKUTEH7WING/target.h index 7626b997754..5bd7d9d14a6 100644 --- a/src/main/target/KAKUTEH7WING/target.h +++ b/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_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block +#endif + #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT // *************** I2C /Baro/Mag ********************* diff --git a/src/main/target/KROOZX/target.h b/src/main/target/KROOZX/target.h index ea5ee6a8e0f..f5d8be344a5 100755 --- a/src/main/target/KROOZX/target.h +++ b/src/main/target/KROOZX/target.h @@ -51,6 +51,11 @@ #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC13 +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + #ifdef USE_MSP_DISPLAYPORT #undef USE_MSP_DISPLAYPORT #endif diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index a33807c06a5..8b168fe44b9 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -69,6 +69,12 @@ #define SDCARD_CS_PIN PC1 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + #endif // *************** OSD ***************************** diff --git a/src/main/target/MATEKF405CAN/target.h b/src/main/target/MATEKF405CAN/target.h index d12b5e3e1a3..b7cfbb5b930 100644 --- a/src/main/target/MATEKF405CAN/target.h +++ b/src/main/target/MATEKF405CAN/target.h @@ -84,6 +84,11 @@ #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block +#endif + #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT // *************** UART ***************************** diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h index 38c8f56f5d9..78146f0ed11 100644 --- a/src/main/target/MATEKF405SE/target.h +++ b/src/main/target/MATEKF405SE/target.h @@ -100,6 +100,11 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + // *************** UART ***************************** #define USE_VCP #define VBUS_SENSING_PIN PC13 diff --git a/src/main/target/MATEKF405TE/target.h b/src/main/target/MATEKF405TE/target.h index fc7c1ad0ce9..668db85686c 100644 --- a/src/main/target/MATEKF405TE/target.h +++ b/src/main/target/MATEKF405TE/target.h @@ -63,12 +63,23 @@ #define SPI2_MISO_PIN PC2 #define SPI2_MOSI_PIN PC3 +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + #if defined(MATEKF405TE_SD) # define USE_SDCARD # define USE_SDCARD_SPI # define SDCARD_SPI_BUS BUS_SPI2 # define SDCARD_CS_PIN PC1 # define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +# if defined(USE_SDCARD) && defined(USE_BARO) +# define USE_TERRAIN +# define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +# endif #else # define USE_FLASHFS # define USE_FLASH_M25P16 @@ -83,13 +94,6 @@ #define I2C1_SCL PB8 #define I2C1_SDA PB7 -#define USE_BARO -#define BARO_I2C_BUS BUS_I2C1 -#define USE_BARO_BMP280 -#define USE_BARO_MS5611 -#define USE_BARO_DPS310 -#define USE_BARO_SPL06 - #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 #define USE_MAG_ALL diff --git a/src/main/target/MATEKH743/target.h b/src/main/target/MATEKH743/target.h index 67e76e55b62..2f72fdf6ba1 100644 --- a/src/main/target/MATEKH743/target.h +++ b/src/main/target/MATEKH743/target.h @@ -164,7 +164,7 @@ #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 @@ -172,6 +172,11 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block +#endif + // *************** ADC ***************************** #define USE_ADC #define ADC_INSTANCE ADC1 diff --git a/src/main/target/MICOAIR405MINI/target.h b/src/main/target/MICOAIR405MINI/target.h index 4b7e0b811da..3693904fe6e 100644 --- a/src/main/target/MICOAIR405MINI/target.h +++ b/src/main/target/MICOAIR405MINI/target.h @@ -65,6 +65,11 @@ #define SDCARD_CS_PIN PC9 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + // *************** UART ***************************** #define USE_VCP diff --git a/src/main/target/MICOAIR405V2/target.h b/src/main/target/MICOAIR405V2/target.h index c4a54f003b6..d979c9d3350 100644 --- a/src/main/target/MICOAIR405V2/target.h +++ b/src/main/target/MICOAIR405V2/target.h @@ -66,6 +66,11 @@ #define SDCARD_CS_PIN PC9 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + // *************** UART ***************************** #define USE_VCP diff --git a/src/main/target/MICOAIR743/target.h b/src/main/target/MICOAIR743/target.h index 526cf438d08..51304a7b4c8 100644 --- a/src/main/target/MICOAIR743/target.h +++ b/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_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block +#endif + // *************** ADC ***************************** #define USE_ADC #define ADC_INSTANCE ADC1 diff --git a/src/main/target/MICOAIR743AIO/target.h b/src/main/target/MICOAIR743AIO/target.h index 90ea60f0a0b..6b6174bf894 100755 --- a/src/main/target/MICOAIR743AIO/target.h +++ b/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_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block +#endif + // *************** ADC ***************************** #define USE_ADC #define ADC_INSTANCE ADC1 diff --git a/src/main/target/MICOAIR743V2/target.h b/src/main/target/MICOAIR743V2/target.h index 78de91df4af..2aa22a833a8 100755 --- a/src/main/target/MICOAIR743V2/target.h +++ b/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_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block +#endif + // *************** ADC ***************************** #define USE_ADC #define ADC_INSTANCE ADC1 diff --git a/src/main/target/NEUTRONRCH7BT/target.h b/src/main/target/NEUTRONRCH7BT/target.h index 9f4ffd118d5..d0a66b6c683 100644 --- a/src/main/target/NEUTRONRCH7BT/target.h +++ b/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_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block +#endif + #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT // *************** ADC ***************************** diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index fcd89fb0e46..8210545a809 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -234,6 +234,11 @@ #define SDCARD_DETECT_PIN PB7 #define SDCARD_DETECT_INVERTED + + #if defined(USE_SDCARD) && defined(USE_BARO) + #define USE_TERRAIN + #define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block + #endif #else #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define M25P16_CS_PIN SPI3_NSS_PIN diff --git a/src/main/target/SPEEDYBEEF405V3/target.h b/src/main/target/SPEEDYBEEF405V3/target.h index 6653ea56c0c..c21ff1b67ab 100644 --- a/src/main/target/SPEEDYBEEF405V3/target.h +++ b/src/main/target/SPEEDYBEEF405V3/target.h @@ -115,6 +115,11 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + // *************** OSD ***************************** #define USE_OSD diff --git a/src/main/target/SPEEDYBEEF405V4/target.h b/src/main/target/SPEEDYBEEF405V4/target.h index 1c5c847597f..4c111f80e12 100644 --- a/src/main/target/SPEEDYBEEF405V4/target.h +++ b/src/main/target/SPEEDYBEEF405V4/target.h @@ -126,6 +126,11 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + // *************** OSD ***************************** #define USE_SPI_DEVICE_2 #define SPI2_SCK_PIN PB13 diff --git a/src/main/target/SPEEDYBEEF405WING/target.h b/src/main/target/SPEEDYBEEF405WING/target.h index 9da5b115824..dd21a9f154b 100644 --- a/src/main/target/SPEEDYBEEF405WING/target.h +++ b/src/main/target/SPEEDYBEEF405WING/target.h @@ -134,6 +134,11 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_CRSF #define SERIALRX_UART SERIAL_PORT_USART1 diff --git a/src/main/target/SPEEDYBEEF405WINGV2/target.h b/src/main/target/SPEEDYBEEF405WINGV2/target.h index df7cafcc00a..e79d487cf8e 100644 --- a/src/main/target/SPEEDYBEEF405WINGV2/target.h +++ b/src/main/target/SPEEDYBEEF405WINGV2/target.h @@ -133,6 +133,11 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_CRSF #define SERIALRX_UART SERIAL_PORT_USART6 diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index 8af6981e54f..55c4e0e2f8a 100755 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -115,6 +115,11 @@ #define SDCARD_SPI_BUS BUS_SPI2 #define SDCARD_CS_PIN SPI2_NSS_PIN +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif + #define MPU6500_CS_PIN SPI1_NSS_PIN #define MPU6500_SPI_BUS BUS_SPI1 diff --git a/src/main/target/TBS_LUCID_H7/target.h b/src/main/target/TBS_LUCID_H7/target.h index e9eef8686bf..eb7c4479548 100644 --- a/src/main/target/TBS_LUCID_H7/target.h +++ b/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_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block +#endif + #define USE_ADC #define ADC_INSTANCE ADC1 diff --git a/src/main/target/TBS_LUCID_H7_WING/target.h b/src/main/target/TBS_LUCID_H7_WING/target.h index 8871cee7398..bc91ef1aea1 100644 --- a/src/main/target/TBS_LUCID_H7_WING/target.h +++ b/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_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block +#endif + + #define USE_ADC #define ADC_INSTANCE ADC1 diff --git a/src/main/target/TBS_LUCID_H7_WING_MINI/target.h b/src/main/target/TBS_LUCID_H7_WING_MINI/target.h index f211decb7bd..1fbb7826732 100644 --- a/src/main/target/TBS_LUCID_H7_WING_MINI/target.h +++ b/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_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 // 2048 bytes = 1 grid block +#endif + + #define USE_ADC #define ADC_INSTANCE ADC1 diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 6a7bb7d545f..facc5c95dde 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -78,6 +78,11 @@ #define SDCARD_DETECT_PIN PD2 #define SDCARD_SPI_BUS BUS_SPI3 #define SDCARD_CS_PIN SPI3_NSS_PIN + +#if defined(USE_SDCARD) && defined(USE_BARO) +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 5 // 2048 bytes = 1 grid block +#endif #endif #define USB_IO diff --git a/src/main/terrain/terrain.c b/src/main/terrain/terrain.c new file mode 100644 index 00000000000..97d62d4b26e --- /dev/null +++ b/src/main/terrain/terrain.c @@ -0,0 +1,213 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +/////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////////// +/* + * Terrain subsystem notes: + * This module runs in the main loop without an RTOS. It relies on synchronous SD card + * access for terrain data, which is subject to I/O latency. Because there is no + * background thread to handle blocking operations, increasing the update frequency + * or adding "look-ahead" calculations for multiple GPS positions is not recommended. + */ +/////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////////// + +#include "platform.h" + +#ifdef USE_TERRAIN + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "terrain.h" +#include "terrain_utils.h" +#include "terrain_io.h" + +#include "common/utils.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#include "drivers/sdcard/sdcard.h" +#include "drivers/time.h" + + +PG_REGISTER_WITH_RESET_TEMPLATE(terrainConfig_t, terrainConfig, PG_TERRAIN_CONFIG, 1); + +PG_RESET_TEMPLATE(terrainConfig_t, terrainConfig, + .terrainEnabled = false, +); + + +static struct { + timeMs_t lastUpdate; + int32_t terrainAGL; // cm + int32_t terrainAMSL; // cm +} terrainHeight = { + .terrainAGL = 0, + .lastUpdate = 0, + .terrainAMSL = 0, +}; + +static struct { + bool homeAltitudeFound; + float homeAltitudeM; + gpsLocation_t homeLocation; +} terrainHomePos = { + .homeAltitudeFound = false, + .homeAltitudeM = 0.0f, + .homeLocation = {.lat = 0, .lon = 0}, +}; + +/** + * @brief Get the height above mean sea level (AMSL) in meters for a given GPS location. + */ +static float getHeightAmslMeters(const gpsLocation_t *loc) +{ + if(isTerrainIoFailure()){ + return TERRAIN_STATUS_FAILURE; + } + + gridInfo_t info; + calculateGridInfo(loc, &info); + + if (info.idx_x > TERRAIN_GRID_BLOCK_SIZE_X - 2) { + return TERRAIN_STATUS_WRONG_BLOC_SIZE; + } + if (info.idx_y > TERRAIN_GRID_BLOCK_SIZE_Y - 2) { + return TERRAIN_STATUS_WRONG_BLOC_SIZE; + } + + // find the grid + gridCache_t *cache = findGridCache(&info); + if(cache == NULL){ + return TERRAIN_STATUS_CACHE_FULL; + } + gridBlock_t *grid = &cache->gridBlock; + + // check we have all 4 required heights; it's check if grid is loaded from SD card + if (!checkBitmap(grid, info.idx_x, info.idx_y) || !checkBitmap(grid, info.idx_x, info.idx_y + 1) || !checkBitmap(grid, info.idx_x + 1, info.idx_y) || !checkBitmap(grid, info.idx_x + 1, info.idx_y + 1)) { + markGridBlockNeedRead(grid); + return TERRAIN_STATUS_WRONG_BITMAP; + } + + // hXY are the heights of the 4 surrounding grid points + const int16_t h00 = grid->height[info.idx_x+0][info.idx_y+0]; + const int16_t h01 = grid->height[info.idx_x+0][info.idx_y+1]; + const int16_t h10 = grid->height[info.idx_x+1][info.idx_y+0]; + const int16_t h11 = grid->height[info.idx_x+1][info.idx_y+1]; + + // do a simple dual linear interpolation. We could do something + // fancier, but it probably isn't worth it as long as the + // grid_spacing is kept small enough + const float avg1 = (1.0f-info.frac_x) * h00 + info.frac_x * h10; + const float avg2 = (1.0f-info.frac_x) * h01 + info.frac_x * h11; + const float avg = (1.0f-info.frac_y) * avg1 + info.frac_y * avg2; + + return avg; +} + +void terrainInit(void) +{ +} + +/** + * @brief Update the terrain subsystem. + * + * This function checks the status of the SD card and attempts to load terrain data into the cache. + * It also tries to determine the home altitude based on the GPS origin if it hasn't been found yet. + */ +void terrainUpdateTask(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + if(terrainConfig()->terrainEnabled == false){ + return; + } + + if(STATE(GPS_FIX) == false){ + return; + } + + if(ARMING_FLAG(ARMED) && terrainHomePos.homeAltitudeFound == false){ + //vehicle is armed and we don't have loaded values for home position, don't try load from SD card durting flight if we are not sure SD it's possible to load data + return; + } + + ///////////////////////////////////////////////////////////////////////////////////////////////////// + //try to find home altitude to home offset + if(terrainHomePos.homeAltitudeFound == false && posControl.gpsOrigin.valid && (posControl.gpsOrigin.lat != terrainHomePos.homeLocation.lat || posControl.gpsOrigin.lon != terrainHomePos.homeLocation.lon)) { + + gpsLocation_t homeLoc = { + .lat = posControl.gpsOrigin.lat, + .lon = posControl.gpsOrigin.lon, + }; + + float heightASLHome = getHeightAmslMeters(&homeLoc); + + if(heightASLHome >= 0){ + terrainHomePos.homeAltitudeFound = true; + terrainHomePos.homeAltitudeM = heightASLHome; + terrainHomePos.homeLocation.lat = posControl.gpsOrigin.lat; + terrainHomePos.homeLocation.lon = posControl.gpsOrigin.lon; + } + } + ///////////////////////////////////////////////////////////////////////////////////////////////////// + + + ///////////////////////////////////////////////////////////////////////////////////////////////////// + if(terrainHomePos.homeAltitudeFound) + { + float heightASL = getHeightAmslMeters(&gpsSol.llh); + if(heightASL >= 0){ + terrainHeight.terrainAMSL = (int32_t)(heightASL * 100.0f); + terrainHeight.terrainAGL = MAX(0, ((int32_t)getEstimatedActualPosition(Z) + (int32_t)(terrainHomePos.homeAltitudeM * 100.0f)) - terrainHeight.terrainAMSL); + terrainHeight.lastUpdate = millis(); + } + } + ///////////////////////////////////////////////////////////////////////////////////////////////////// +} + +int32_t terrainGetLastAMSL(void){ + if(!terrainConfig()->terrainEnabled){ + return TERRAIN_STATUS_NO_DATA; + } + + return terrainHeight.lastUpdate + TERRAIN_NO_DATA_DELAY_MS < millis() ? TERRAIN_STATUS_NO_DATA : terrainHeight.terrainAMSL; +} + +int32_t terrainGetLastDistanceCm(void) +{ + //start a terrain system after the first query to terrain height + if(!terrainConfig()->terrainEnabled){ + return TERRAIN_STATUS_NO_DATA; + } + + return terrainHeight.lastUpdate + TERRAIN_NO_DATA_DELAY_MS < millis() ? TERRAIN_STATUS_NO_DATA : terrainHeight.terrainAGL; +} + +#endif \ No newline at end of file diff --git a/src/main/terrain/terrain.h b/src/main/terrain/terrain.h new file mode 100644 index 00000000000..f15764b27e6 --- /dev/null +++ b/src/main/terrain/terrain.h @@ -0,0 +1,153 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ +#pragma once + +#include +#include +#include + +#include "navigation/navigation.h" + +#define TERRAIN_TASK_RATE_HZ 5 +#define TERRAIN_NO_DATA_DELAY_MS 1000 + +#define TERRAIN_STATUS_FAILURE (-1) +#define TERRAIN_STATUS_WRONG_BLOC_SIZE (-2) +#define TERRAIN_STATUS_WRONG_BITMAP (-3) +#define TERRAIN_STATUS_NO_DATA (-4) +#define TERRAIN_STATUS_NO_CARD (-5) +#define TERRAIN_STATUS_CACHE_FULL (-6) + +// MAVLink sends 4x4 grids +#define TERRAIN_GRID_MAVLINK_SIZE 4 + +// a 2k grid_block on disk contains 8x7 of the mavlink grids. Each +// grid block overlaps by one with its neighbour. This ensures that +// the altitude at any point can be calculated from a single grid +// block +#define TERRAIN_GRID_BLOCK_MUL_X 7 +#define TERRAIN_GRID_BLOCK_MUL_Y 8 + +// this is the spacing between 32x28 grid blocks, in grid_spacing units +#define TERRAIN_GRID_BLOCK_SPACING_X ((TERRAIN_GRID_BLOCK_MUL_X-1)*TERRAIN_GRID_MAVLINK_SIZE) +#define TERRAIN_GRID_BLOCK_SPACING_Y ((TERRAIN_GRID_BLOCK_MUL_Y-1)*TERRAIN_GRID_MAVLINK_SIZE) + +// giving a total grid size of a disk grid_block of 32x28 +#define TERRAIN_GRID_BLOCK_SIZE_X (TERRAIN_GRID_MAVLINK_SIZE*TERRAIN_GRID_BLOCK_MUL_X) +#define TERRAIN_GRID_BLOCK_SIZE_Y (TERRAIN_GRID_MAVLINK_SIZE*TERRAIN_GRID_BLOCK_MUL_Y) + + +#define TASK_TERRAIN_RATE_MS 10 +#define TERRAIN_MAX_DISTANCE_CM INT16_MAX + +enum GridCacheState { + GRID_CACHE_INVALID = 0, // when first initialised + GRID_CACHE_DISKWAIT = 1, // when waiting for disk read + GRID_CACHE_VALID = 2, // when at least partially valid +}; + +typedef struct __attribute__((packed)){ + // bitmap of 4x4 grids filled in from GCS (56 bits are used) + uint64_t bitmap; + + // south west corner of block in degrees*10^7 + int32_t lat; + int32_t lon; + + // crc of whole block, taken with crc=0 + uint16_t crc; + + // format version number + uint16_t version; + + // grid spacing in meters + uint16_t spacing; + + // heights in meters over a 32*28 grid + int16_t height[TERRAIN_GRID_BLOCK_SIZE_X][TERRAIN_GRID_BLOCK_SIZE_Y]; + + // indices info 32x28 grids for this degree reference + uint16_t grid_idx_x; + uint16_t grid_idx_y; + + // rounded latitude/longitude in degrees. + int16_t lonDegrees; + int8_t latDegrees; + +} gridBlock_t; + + +typedef union { + gridBlock_t block; + uint8_t buffer[2048]; +} gridIoBlock_t; + +/* + grid_info is a broken down representation of a Location, giving + the index terms for finding the right grid + */ +typedef struct { + // rounded latitude/longitude in degrees. + int8_t latDegrees; + int16_t lonDegrees; + + // lat and lon of SW corner of this 32*28 grid, *10^7 degrees + int32_t gridLat; + int32_t gridLon; + + // indices info 32x28 grids for this degree reference + uint16_t grid_idx_x; + uint16_t grid_idx_y; + + // indexes into 32x28 grid + uint8_t idx_x; // north (0..27) + uint8_t idx_y; // east (0..31) + + // fraction within the grid square + float frac_x; // north (0..1) + float frac_y; // east (0..1) + + // file offset of this grid + uint32_t file_offset; +} gridInfo_t; + +typedef struct { + gridBlock_t gridBlock; + enum GridCacheState state; + // the last time access was requested to this block, used for LRU + timeMs_t lastAccessMs; +} gridCache_t; + + +typedef struct terrainConfig_s { + bool terrainEnabled; +} terrainConfig_t; + +PG_DECLARE(terrainConfig_t, terrainConfig); + +void terrainInit(void); +void terrainUpdateTask(timeUs_t currentTimeUs); +int32_t terrainGetLastAMSL(void); +int32_t terrainGetLastDistanceCm(void); + diff --git a/src/main/terrain/terrain_io.c b/src/main/terrain/terrain_io.c new file mode 100644 index 00000000000..59f7fe79ac5 --- /dev/null +++ b/src/main/terrain/terrain_io.c @@ -0,0 +1,425 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include +#include "platform.h" + +#ifdef USE_TERRAIN + +#include "terrain_io.h" +#include "terrain_utils.h" + +//#include "common/log.h" +#include "common/utils.h" + +#include "drivers/time.h" +#include "drivers/sdcard/sdcard.h" + +#include "blackbox/blackbox.h" + + +#include "io/asyncfatfs/asyncfatfs.h" + +static terrainIoState_t terrainIoState; + +/** + * @brief Marks the terrain IO state machine as failed, disabling further operations. + */ +static void hardFailure(void) +{ + terrainIoState.datFile = NULL; + terrainIoState.status = TERRAIN_IO_FAILURE; + terrainIoState.gridBlock = NULL; + //LOG_DEBUG(SYSTEM, "TERRAIN RELEASE FAILURE"); + releaseSdCardAccess(); +} + +/** + * @brief Resets the terrain IO state machine to its idle state. + */ +static void resetStateMachine(void) +{ + terrainIoState.status = TERRAIN_IO_IDLE; + terrainIoState.datFile = NULL; + terrainIoState.gridBlock = NULL; + terrainIoState.bytesRead = 0; + terrainIoState.readsZeroBytesCount = 0; + terrainIoState.openFileStartTimeMs = 0; + + //LOG_DEBUG(SYSTEM, "TERRAIN RELEASE RESET STATE"); + releaseSdCardAccess(); +} + +/** + * @brief we need to close file and reset state + */ +static void cleanUp(void) +{ + if(terrainIoState.status == TERRAIN_IO_CLOSE || terrainIoState.status == TERRAIN_IO_CLOSE_PENDING){ + return; + } + + //LOG_DEBUG(SYSTEM, "TERRAIN RELEASE RESET STATE"); + if(terrainIoState.datFile != NULL){ + //LOG_DEBUG(SYSTEM, "TERRAIN CLOSE CLEAN UP FILE"); + terrainIoState.status = TERRAIN_IO_CLOSE; + } else { + //LOG_DEBUG(SYSTEM, "TERRAIN CLOSE CLEAN RESET NOW"); + resetStateMachine(); + } +} + +/** + * @brief Retrieves or initializes the file open status for a given latitude and longitude. + */ +static terrainIoFileOpenStatus_t* getFileOpenStatusIndex(int8_t latDegrees, int16_t lonDegrees) +{ + uint16_t oldest_i = 0; + const timeMs_t nowMs = millis(); + + for(uint8_t i = 0; i < TERRAIN_IO_MAX_FILE_OPEN_STATUS; i++){ + if(terrainIoState.fileOpenStatus[i].latDegrees == latDegrees && terrainIoState.fileOpenStatus[i].lonDegrees == lonDegrees){ + terrainIoState.fileOpenStatus[i].lastAccessTimeMs = nowMs; + return &(terrainIoState.fileOpenStatus[i]); + } + + if(terrainIoState.fileOpenStatus[i].lastAccessTimeMs < terrainIoState.fileOpenStatus[oldest_i].lastAccessTimeMs){ + oldest_i = i; + } + } + + //not found, return oldest + terrainIoState.fileOpenStatus[oldest_i].latDegrees = latDegrees; + terrainIoState.fileOpenStatus[oldest_i].lonDegrees = lonDegrees; + terrainIoState.fileOpenStatus[oldest_i].errorOpenCount = 0; + terrainIoState.fileOpenStatus[oldest_i].lastAccessTimeMs = nowMs; + + return &(terrainIoState.fileOpenStatus[oldest_i]); +} + +static void increaseFileStatusErrorCount(int8_t latDegrees, int16_t lonDegrees) +{ + terrainIoFileOpenStatus_t* fileOpenStatus = getFileOpenStatusIndex(latDegrees, lonDegrees); + fileOpenStatus->errorOpenCount++; +} + +/** + * @brief Callback function invoked when a terrain file is opened. + */ +void terrainIoOpenedFileCallback(afatfsFilePtr_t file) +{ + if (terrainIoState.status == TERRAIN_IO_FAILURE) { + return; + } + + if(terrainIoState.status != TERRAIN_IO_OPEN_FILE_PENDING){ + hardFailure(); + return; + } + + if(file == NULL){ + //LOG_DEBUG(SYSTEM, "TERRAIN OPEN FILE NULL, RESET STATE"); + markGridBlockInvalid(terrainIoState.gridBlock); + increaseFileStatusErrorCount(terrainIoState.gridBlock->latDegrees, terrainIoState.gridBlock->lonDegrees); + resetStateMachine(); + return; + } + + terrainIoState.status = TERRAIN_IO_SEEK; + terrainIoState.datFile = file; +} + +/** + * @brief Callback function invoked when a terrain file is closed. + */ +void terrainIoClosedFileCallback(void) +{ + if (terrainIoState.status == TERRAIN_IO_FAILURE) { + return; + } + + if(terrainIoState.status != TERRAIN_IO_CLOSE_PENDING){ + hardFailure(); + return; + } + + //LOG_DEBUG(SYSTEM, "TERRAIN CLOSE FILE OK"); + resetStateMachine(); +} + +/** + * @brief Main task function for loading terrain grid data into cache. + */ +void loadGridToCacheTask(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + if(terrainIoState.status == TERRAIN_IO_FAILURE){ + //LOG_DEBUG(SYSTEM, "TERRAIN IO FAILURE"); + return; + } + + //SD card or file system is not prepared or it's not ready yet + if(!sdcard_isInserted() || !sdcard_isFunctional() || afatfs_getFilesystemState() != AFATFS_FILESYSTEM_STATE_READY){ + //SD card is not ready but IO is in IDLE, probably something heppend with SD card during reading + if(terrainIoState.status != TERRAIN_IO_IDLE){ + hardFailure(); + return; + }else{ + return; + } + } + + /////////////////////////////////////////////////////////////////// + /////// IDLE ///////////////////////////////////////////// + // check if in the cache is some block which is waiting to read from disk + if(terrainIoState.status == TERRAIN_IO_IDLE){ + gridBlock_t* gridBlock = getGridBlockToRead(); + + //nothing to read + if(gridBlock == NULL){ + return; + } + + //check if we had too many errors opening of this file + terrainIoFileOpenStatus_t* fileOpenStatus = getFileOpenStatusIndex(gridBlock->latDegrees, gridBlock->lonDegrees); + if(fileOpenStatus->errorOpenCount >= 3){ + //LOG_DEBUG(SYSTEM, "TERRAIN IO FAILURE TOO MANY OPEN ERRORS"); + markGridBlockInvalid(gridBlock); + return; + } + + /////////////////////////////////////////////////////////////////// + //request access from blackbox + if(requestToSdCardAccess() == false){ + return; + } + //////////////////////////////////////////////////////////////// + + //set grid block for process. open file -> seek -> read -> close + terrainIoState.gridBlock = gridBlock; + terrainIoState.status = TERRAIN_IO_CHANGE_DIR; + + return; + } + + /////////////////////////////////////////////////////////////////// + /////// CHANGE DIR ///////////////////////////////////////////// + if(terrainIoState.status == TERRAIN_IO_CHANGE_DIR){ + //already in the root directory, no need to change the directory + if(afatfs_isCurrentDirRoot()){ + terrainIoState.status = TERRAIN_IO_OPEN_FILE; + return; + } + + //LOG_DEBUG(SYSTEM, "TERRAIN CHANGING DIR"); + //change dir to ROOT (null) + if(!afatfs_chdir(NULL)){ + //LOG_DEBUG(SYSTEM, "TERRAIN CHANGE DIR ERROR"); + markGridBlockInvalid(terrainIoState.gridBlock); + resetStateMachine(); + return; + } + + terrainIoState.status = TERRAIN_IO_OPEN_FILE; + return; + } + /////////////////////////////////////////////////////////////////// + + /////////////////////////////////////////////////////////////////// + /////// OPEN FILE ///////////////////////////////////////////// + // wait to call callback terrainIoOpenedFileCallback + if(terrainIoState.status == TERRAIN_IO_OPEN_FILE){ + //LOG_DEBUG(SYSTEM, "TERRAIN OPENING FILE"); + if(terrainIoState.gridBlock == NULL){ + //LOG_DEBUG(SYSTEM, "TERRAIN OPENING FILE GRID BLOCK NULL"); + markGridBlockInvalid(terrainIoState.gridBlock); + resetStateMachine(); + return; + } + + terrainIoState.status = TERRAIN_IO_OPEN_FILE_PENDING; + terrainIoState.openFileStartTimeMs = millis(); + + char filename[20]; + snprintf(filename, sizeof(filename), + "%c%02ld%c%03ld.DAT", + terrainIoState.gridBlock->latDegrees < 0 ? 'S' : 'N', + labs(terrainIoState.gridBlock->latDegrees), + terrainIoState.gridBlock->lonDegrees < 0 ? 'W' : 'E', + labs(terrainIoState.gridBlock->lonDegrees)); + + //of most of the time is callback terrainIoOpenedFileCallback called immediately, not in next cycle + if(!afatfs_fopen(filename, "r", terrainIoOpenedFileCallback)){ + //LOG_DEBUG(SYSTEM, "TERRAIN OPEN FILE ERROR"); + markGridBlockInvalid(terrainIoState.gridBlock); + resetStateMachine(); + return; + } + + //don't add code here, put it to terrainIoOpenedFileCallback + return; + } + /////////////////////////////////////////////////////////////////// + + ////////////////////////////////////////////////////////////////////// + /////// OPEN FILE PENDING WATCHDOG /////////////////////////////////// + // asynfatfs can be in infinite state "opening" if the card failures during opening. So after a while reset state machine + if(terrainIoState.status == TERRAIN_IO_OPEN_FILE_PENDING){ + if(millis() - terrainIoState.openFileStartTimeMs > 3000){ + //LOG_DEBUG(SYSTEM, "TERRAIN OPEN FILE TIMEOUT"); + markGridBlockInvalid(terrainIoState.gridBlock); + increaseFileStatusErrorCount(terrainIoState.gridBlock->latDegrees, terrainIoState.gridBlock->lonDegrees); + resetStateMachine(); + return; + } + } + /////////////////////////////////////////////////////////////////// + + ////////////////////////////////////////////////////////////////////// + /////// SEEK TO BLOCK /////////////////////////////////////////////// + if(terrainIoState.status == TERRAIN_IO_SEEK){ + if(terrainIoState.datFile == NULL || terrainIoState.gridBlock == NULL){ + markGridBlockInvalid(terrainIoState.gridBlock); + cleanUp(); + return; + } + + //LOG_DEBUG(SYSTEM, "TERRAIN SEEK TO POSITION %d,%d", terrainIoState.gridBlock->grid_idx_x, terrainIoState.gridBlock->grid_idx_y); + + //calculate file offset + uint32_t blocknum = (eastBlocks(terrainIoState.gridBlock) * terrainIoState.gridBlock->grid_idx_x) + terrainIoState.gridBlock->grid_idx_y; + uint64_t fileOffset64 = (uint64_t)blocknum * sizeof(terrainIoState.ioBlock); + + if(fileOffset64 > UINT32_MAX) { + markGridBlockInvalid(terrainIoState.gridBlock); + cleanUp(); + return; + } + + afatfsOperationStatus_e seekState = afatfs_fseek(terrainIoState.datFile, (int32_t)fileOffset64, AFATFS_SEEK_SET); + if(seekState != AFATFS_OPERATION_FAILURE){ + //LOG_DEBUG(SYSTEM, "TERRAIN SEEK OK"); + terrainIoState.status = TERRAIN_IO_READ; // we don't wait to end of seek, after seek done, reading will be available in next task call + return; + } + + //LOG_DEBUG(SYSTEM, "TERRAIN SEEK ERROR"); + markGridBlockInvalid(terrainIoState.gridBlock); + cleanUp(); + return; + } + ////////////////////////////////////////////////////////////////////// + + ///////////////////////////////////////////////////////////////////////// + /////// READING BLOCK DATA ///////////////////////////////////////////// + if(terrainIoState.status == TERRAIN_IO_READ){ + if(terrainIoState.datFile == NULL || terrainIoState.gridBlock == NULL){ + markGridBlockInvalid(terrainIoState.gridBlock); + cleanUp(); + return; + } + + uint32_t readNow = afatfs_fread(terrainIoState.datFile, (uint8_t*)&terrainIoState.ioBlock + terrainIoState.bytesRead, sizeof(terrainIoState.ioBlock) - terrainIoState.bytesRead); + terrainIoState.bytesRead += readNow; + + //LOG_DEBUG(SYSTEM, "TERRAIN READING DATA %d/%d", (int)terrainIoState.bytesRead, sizeof(terrainIoState.ioBlock)); + if(terrainIoState.bytesRead == 0 && !(sdcard_isInserted() && sdcard_isFunctional() && afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY)){ + //LOG_DEBUG(SYSTEM, "TERRAIN SD CARD FAILURE"); + hardFailure(); + return; + } + + if(terrainIoState.bytesRead == 0){ + terrainIoState.readsZeroBytesCount++; + } + + // if readNow is zero, it could mean something bad happen, broken file, or any other problem with SD card + // block is 2048, asyncfatfs reads 512. so we accept (2048 / 512) * 2 errors for a single reading. + if(terrainIoState.readsZeroBytesCount > (sizeof(terrainIoState.ioBlock) / 512) * 2){ + markGridBlockInvalid(terrainIoState.gridBlock); + + //we have to increase error for file, for case if error for file reach threshold, and mark file as invalid + increaseFileStatusErrorCount(terrainIoState.gridBlock->latDegrees, terrainIoState.gridBlock->lonDegrees); + cleanUp(); + return; + } + + if(terrainIoState.bytesRead < sizeof(terrainIoState.ioBlock)){ + //file should be divided by 2048, reading up to end of file and not have all data should never happen + if (afatfs_feof(terrainIoState.datFile)) { + //if it happens we have to close file and increase error count for file + increaseFileStatusErrorCount(terrainIoState.gridBlock->latDegrees, terrainIoState.gridBlock->lonDegrees); + markGridBlockInvalid(terrainIoState.gridBlock); + cleanUp(); + return; + } + + } else { + //check if idx and idy is same for terrainIoState.ioBlock and terrainIoState.gridBlock, to be sure we loaded from file correct block + if(terrainIoState.ioBlock.block.grid_idx_x != terrainIoState.gridBlock->grid_idx_x || terrainIoState.ioBlock.block.grid_idx_y != terrainIoState.gridBlock->grid_idx_y) { + markGridBlockInvalid(terrainIoState.gridBlock); + cleanUp(); + return; + } + + memcpy(terrainIoState.gridBlock, &terrainIoState.ioBlock.block, sizeof(gridBlock_t)); + markGridBlockAsRead(terrainIoState.gridBlock); + cleanUp(); + return; + } + } + ////////////////////////////////////////////////////////////////////// + + ///////////////////////////////////////////////////////////////////////// + /////// CLOSING BLOCK DATA ///////////////////////////////////////////// + // wait to call callback terrainIoClosedFileCallback + if(terrainIoState.status == TERRAIN_IO_CLOSE){ + terrainIoState.status = TERRAIN_IO_CLOSE_PENDING; + + if(terrainIoState.datFile == NULL) { + resetStateMachine(); + } + + if(!afatfs_fclose(terrainIoState.datFile, terrainIoClosedFileCallback)){ + resetStateMachine(); + } + return; + } + ////////////////////////////////////////////////////////////////////// +} + +/** + * @brief Checks if the terrain IO system is in a failure state. + * + * @return true if the terrain IO system has failed, false otherwise. + */ +bool isTerrainIoFailure(void) +{ + return terrainIoState.status == TERRAIN_IO_FAILURE; +} + +#endif + diff --git a/src/main/terrain/terrain_io.h b/src/main/terrain/terrain_io.h new file mode 100644 index 00000000000..d627f34377c --- /dev/null +++ b/src/main/terrain/terrain_io.h @@ -0,0 +1,79 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include "terrain.h" +#include "io/asyncfatfs/asyncfatfs.h" + +#define TERRAIN_IO_TASK_RATE_HZ 50 +#define TERRAIN_IO_MAX_FILE_OPEN_STATUS 4 //supposed max 4 files opened for a single flight + +/** + * @brief Enumeration of terrain IO status states. + */ +typedef enum { + TERRAIN_IO_IDLE, + TERRAIN_IO_CHANGE_DIR, + TERRAIN_IO_OPEN_FILE, + TERRAIN_IO_OPEN_FILE_PENDING, //wait state in async, waiting to call callback + TERRAIN_IO_SEEK, + TERRAIN_IO_READ, + TERRAIN_IO_CLOSE, + TERRAIN_IO_CLOSE_PENDING, //wait state in async, waiting to call callback + TERRAIN_IO_FAILURE, //something bad happened, disable SD card +} terrainIoStatus_e; + +/** + * @brief Structure to track the open status of terrain data files. + */ +typedef struct { + int8_t latDegrees; + int16_t lonDegrees; + uint8_t errorOpenCount; + timeMs_t lastAccessTimeMs; +} terrainIoFileOpenStatus_t; + +/** + * @brief Structure representing the state of the terrain IO system. + */ +typedef struct { + terrainIoStatus_e status; + gridBlock_t *gridBlock; + gridIoBlock_t ioBlock; // 2048 bytes because block in disk is exactly 2048 bytes + afatfsFilePtr_t datFile; + terrainIoFileOpenStatus_t fileOpenStatus[TERRAIN_IO_MAX_FILE_OPEN_STATUS]; + uint32_t bytesRead; + uint32_t readsZeroBytesCount; + timeMs_t openFileStartTimeMs; +} terrainIoState_t; + + + +void loadGridToCacheTask(timeUs_t currentTimeUs); +void terrainIoOpenedDirCallback(afatfsFilePtr_t directory); +void terrainIoOpenedFileCallback(afatfsFilePtr_t file); +void terrainIoClosedFileCallback(void); +void terrainIoClosedDirCallback(void); +bool isTerrainIoFailure(void); diff --git a/src/main/terrain/terrain_location.c b/src/main/terrain/terrain_location.c new file mode 100644 index 00000000000..450ffbd8da1 --- /dev/null +++ b/src/main/terrain/terrain_location.c @@ -0,0 +1,91 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include "platform.h" + +#ifdef USE_TERRAIN + +#include "terrain_location.h" + +static float longitudeScale(int32_t lat) +{ + float scale = cosf(lat * (1.0e-7f * DEG2RAD)); + return MAX(scale, 0.01f); +} + +static int32_t limitLat(int32_t lat) +{ + if (lat > 900000000L) { + lat = 1800000000LL - lat; + } else if (lat < -900000000L) { + lat = -(1800000000LL + lat); + } + return lat; +} + +static int32_t wrapLongitude(int64_t lon) +{ + if (lon > 1800000000L) { + lon = (int32_t)lon-3600000000LL; + } else if (lon < -1800000000L) { + lon = (int32_t)lon+3600000000LL; + } + return (int32_t) lon; +} + +static int32_t diffLongitude(int32_t lon1, int32_t lon2) +{ + if ((lon1 & 0x80000000) == (lon2 & 0x80000000)) { + // common case of same sign + return lon1 - lon2; + } + int64_t dlon = (int64_t) lon1 - (int64_t) lon2; + if (dlon > 1800000000LL) { + dlon -= 3600000000LL; + } else if (dlon < -1800000000LL) { + dlon += 3600000000LL; + } + return (int32_t) dlon; +} + +void offsetLatlng(gpsLocation_t *loc, float north_m, float east_m) +{ + const int32_t dlat = north_m * LOCATION_SCALING_FACTOR_INV; + const int64_t dlon = (east_m * LOCATION_SCALING_FACTOR_INV) / longitudeScale(loc->lat + (dlat / 2)); + + loc->lat += dlat; + loc->lat = limitLat(loc->lat); + loc->lon = wrapLongitude(dlon + loc->lon); +} + +neVector_t gpsGetDistanceNE(const gpsLocation_t *a, const gpsLocation_t *b) +{ + neVector_t v; + v.north = (float)(b->lat - a->lat) * LOCATION_SCALING_FACTOR; + v.east = (float)diffLongitude(b->lon, a->lon) * LOCATION_SCALING_FACTOR * longitudeScale((b->lat + a->lat)/2); + return v; +} + +#endif \ No newline at end of file diff --git a/src/main/terrain/terrain_location.h b/src/main/terrain/terrain_location.h new file mode 100644 index 00000000000..6d1f70ddc3c --- /dev/null +++ b/src/main/terrain/terrain_location.h @@ -0,0 +1,44 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include + +#include "navigation/navigation.h" + +#define LOCATION_SCALING_FACTOR 0.011131884502145034f // m per 1e-7 degree +#define LOCATION_SCALING_FACTOR_INV 89.83204953368922f // 1 / LOCATION_SCALING_FACTOR; + +#define DEG2RAD 0.01745329252f + +#define TERRAIN_LATLON_EQUAL(v1, v2) ((unsigned long)labs((v1) - (v2)) <= 500UL) + +typedef struct { + float north; + float east; +} neVector_t; + +void offsetLatlng(gpsLocation_t *loc, float north_m, float east_m); +neVector_t gpsGetDistanceNE(const gpsLocation_t *a, const gpsLocation_t *b); \ No newline at end of file diff --git a/src/main/terrain/terrain_utils.c b/src/main/terrain/terrain_utils.c new file mode 100644 index 00000000000..04d4ffe2473 --- /dev/null +++ b/src/main/terrain/terrain_utils.c @@ -0,0 +1,245 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include "platform.h" + +#ifdef USE_TERRAIN + +#include "terrain.h" +#include "terrain_utils.h" +#include "terrain_location.h" + +#include "navigation/navigation.h" + +#include "drivers/time.h" + + +static uint8_t grid_spacing = 30; +static gridCache_t cache[TERRAIN_GRID_BLOCK_CACHE_SIZE]; + +/** + given an idx_x and idx_y within a 32x28 grid block, return + the bit number (0..55) corresponding to the 4x4 subgrid + */ +static bool gridBitnum(uint8_t idx_x, uint8_t idx_y, uint8_t *bitnum) +{ + if (idx_x > 27 || idx_y > 31) { + return false; + } + + uint8_t subgrid_x = idx_x / TERRAIN_GRID_MAVLINK_SIZE; + uint8_t subgrid_y = idx_y / TERRAIN_GRID_MAVLINK_SIZE; + + if (subgrid_x >= TERRAIN_GRID_BLOCK_MUL_X || + subgrid_y >= TERRAIN_GRID_BLOCK_MUL_Y) { + return false; + } + + *bitnum = subgrid_y + TERRAIN_GRID_BLOCK_MUL_Y * subgrid_x; + return true; +} + +/** + find a grid block that needs to be read from disk + */ +gridBlock_t* getGridBlockToRead(void) +{ + for (uint16_t i = 0; i < TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) { + if (cache[i].state == GRID_CACHE_DISKWAIT) { + return &(cache[i].gridBlock); + } + } + + return NULL; +} + +/** + mark a grid block as needing to be read from disk + */ +void markGridBlockNeedRead(gridBlock_t *gridBlock) +{ + for (uint16_t i = 0; i < TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) { + if (&(cache[i].gridBlock) == gridBlock) { + cache[i].state = GRID_CACHE_DISKWAIT; + return; + } + } +} + +/** + mark a grid block as having been read from disk + */ +void markGridBlockAsRead(gridBlock_t *gridBlock) +{ + for (uint16_t i = 0; i < TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) { + if (&(cache[i].gridBlock) == gridBlock) { + cache[i].state = GRID_CACHE_VALID; + return; + } + } +} + +/** + mark a grid block as having been read from disk + */ +void markGridBlockInvalid(gridBlock_t *gridBlock) +{ + if (gridBlock == NULL) { + return; + } + + for (uint16_t i = 0; i < TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) { + if (&(cache[i].gridBlock) == gridBlock) { + cache[i].state = GRID_CACHE_INVALID; + return; + } + } +} + +/** + given a gps location, calculate the grid info + */ +bool calculateGridInfo(const gpsLocation_t *loc, gridInfo_t *info) +{ + info->latDegrees = (loc->lat < 0 ? (loc->lat - 9999999L) : loc->lat) / 10000000L; // 10 * 1000 * 1000L + info->lonDegrees = (loc->lon < 0 ? (loc->lon - 9999999L) : loc->lon) / 10000000L; // 10 * 1000 * 1000L + + gpsLocation_t ref; + ref.lat = info->latDegrees * 10000000L; + ref.lon = info->lonDegrees * 10000000L; + + neVector_t offset = gpsGetDistanceNE(&ref, loc); + + uint32_t idx_x = offset.north / grid_spacing; + uint32_t idx_y = offset.east / grid_spacing; + + info->grid_idx_x = idx_x / TERRAIN_GRID_BLOCK_SPACING_X; + info->grid_idx_y = idx_y / TERRAIN_GRID_BLOCK_SPACING_Y; + + info->idx_x = idx_x % TERRAIN_GRID_BLOCK_SPACING_X; + info->idx_y = idx_y % TERRAIN_GRID_BLOCK_SPACING_Y; + + info->frac_x = (offset.north - idx_x * grid_spacing) / grid_spacing; + info->frac_y = (offset.east - idx_y * grid_spacing) / grid_spacing; + + gpsLocation_t gridRef = ref; + offsetLatlng(&gridRef,info->grid_idx_x * TERRAIN_GRID_BLOCK_SPACING_X * grid_spacing,info->grid_idx_y * TERRAIN_GRID_BLOCK_SPACING_Y * grid_spacing); + + info->gridLat = gridRef.lat; + info->gridLon = gridRef.lon; + + return true; +} + +/** + calculate how many blocks east are in a grid block + */ +uint32_t eastBlocks(gridBlock_t *gridBlock) +{ + gpsLocation_t loc1, loc2; + + loc1.lat = gridBlock->latDegrees * 10 * 1000 * 1000L; + loc1.lon = gridBlock->lonDegrees * 10 * 1000 * 1000L; + + loc2.lat = loc1.lat; + loc2.lon = (gridBlock->lonDegrees + 1) * 10 * 1000 * 1000L; + + float east_m = 2.0f * gridBlock->spacing * TERRAIN_GRID_BLOCK_SIZE_Y; + float lat_rad = (loc2.lat * 1e-7f) * DEG2RAD; + float dLon_deg = east_m / (111319.5f * cosf(lat_rad)); + loc2.lon += (int32_t)(dLon_deg * 1e7f); + + neVector_t offset = gpsGetDistanceNE(&loc1, &loc2); + + return offset.east / (gridBlock->spacing * TERRAIN_GRID_BLOCK_SPACING_Y); +} + +/** + find or allocate a grid cache entry for a given grid info + */ +gridCache_t* findGridCache(gridInfo_t *info) +{ + int16_t oldest_i = -1; + + // see if we have that grid + const timeMs_t nowMs = millis(); + uint32_t oldestAccessMs = UINT32_MAX; + + for (uint16_t i = 0; i < TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) { + gridBlock_t *grid = & cache[i].gridBlock; + if (TERRAIN_LATLON_EQUAL(grid->lat, info->gridLat) && + TERRAIN_LATLON_EQUAL(grid->lon , info->gridLon) && + cache[i].gridBlock.spacing == grid_spacing) { + cache[i].lastAccessMs = nowMs; + return &cache[i]; + } + if (cache[i].state != GRID_CACHE_DISKWAIT) { + if (oldest_i == -1 || cache[i].lastAccessMs < oldestAccessMs) { + oldestAccessMs = cache[i].lastAccessMs; + oldest_i = (int16_t)i; + } + } + } + + // Not found. Use the oldest grid and make it this grid, + // initially unpopulated + if(oldest_i < 0){ + return NULL; + } + + gridCache_t *gridCache = &cache[oldest_i]; + memset(gridCache, 0, sizeof(gridCache_t)); + + gridCache->gridBlock.lat = info->gridLat; + gridCache->gridBlock.lon = info->gridLon; + gridCache->gridBlock.spacing = grid_spacing; + gridCache->gridBlock.grid_idx_x = info->grid_idx_x; + gridCache->gridBlock.grid_idx_y = info->grid_idx_y; + gridCache->gridBlock.latDegrees = info->latDegrees; + gridCache->gridBlock.lonDegrees = info->lonDegrees; + gridCache->gridBlock.version = 1; + gridCache->lastAccessMs = nowMs; + + // mark as waiting for disk read + gridCache->state = GRID_CACHE_DISKWAIT; + + return gridCache; +} + +/* + given a grid_info check that a given idx_x/idx_y is available (set + in the bitmap) + */ +bool checkBitmap(gridBlock_t *grid, uint8_t idx_x, uint8_t idx_y) +{ + uint8_t bitnum; + if(!gridBitnum(idx_x, idx_y, &bitnum)){ + return false; + } + + return (grid->bitmap & (((uint64_t)1U) << bitnum)) != 0; +} + +#endif \ No newline at end of file diff --git a/src/main/terrain/terrain_utils.h b/src/main/terrain/terrain_utils.h new file mode 100644 index 00000000000..611c1abef6b --- /dev/null +++ b/src/main/terrain/terrain_utils.h @@ -0,0 +1,42 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include + +#include "terrain.h" +#include "io/gps.h" + + +bool calculateGridInfo(const gpsLocation_t *loc, gridInfo_t *info); +bool checkBitmap(gridBlock_t *grid, uint8_t idx_x, uint8_t idx_y); + +gridCache_t* findGridCache(gridInfo_t *info); +gridBlock_t* getGridBlockToRead(void); + +uint32_t eastBlocks(gridBlock_t *gridBlock); +void markGridBlockAsRead(gridBlock_t *gridBlock); +void markGridBlockNeedRead(gridBlock_t *gridBlock); +void markGridBlockInvalid(gridBlock_t *gridBlock); \ No newline at end of file diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 2cb53b64ecb..845a2f269b0 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -43,6 +43,9 @@ set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TE set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c" "drivers/headtracker_common.c") set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST USE_HEADTRACKER) +set_property(SOURCE terrain_unittest.cc PROPERTY depends "terrain/terrain_utils.c" "terrain/terrain_location.c" "drivers/time.c") +set_property(SOURCE terrain_unittest.cc PROPERTY definitions TERRAIN_UNIT_TEST USE_TERRAIN USE_SDCARD USE_SDCARD_SDIO TERRAIN_GRID_BLOCK_CACHE_SIZE=8) + function(unit_test src) get_filename_component(basename ${src} NAME) string(REPLACE ".cc" "" name ${basename} ) diff --git a/src/test/unit/terrain_unittest.cc b/src/test/unit/terrain_unittest.cc new file mode 100644 index 00000000000..60dfc89a681 --- /dev/null +++ b/src/test/unit/terrain_unittest.cc @@ -0,0 +1,86 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +//Run only this test: +// cmake -DTOOLCHAIN= .. && make terrain_unittest && ./src/test/unit/terrain_unittest --gtest_filter=Terrain* + +#include +#include + +extern "C" { + #include "terrain/terrain_utils.h" + #include "common/time.h" + #include "drivers/time.h" + extern timeUs_t usTicks; + extern volatile timeMs_t sysTickUptime; + extern volatile timeMs_t sysTickValStamp; +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +SysTick_Type SysTickValue; +SysTick_Type *SysTick = &SysTickValue; + + +TEST(TerrainTest, calculateGridInfo) +{ + ///////////////////////////////////////////////////////////////////// + gpsLocation_t loc1 = { .lat = 374221234, .lon = -1220845678, .alt = 0 }; + gridInfo_t info1; + + calculateGridInfo(&loc1, &info1); + + EXPECT_EQ(info1.latDegrees, 37); + EXPECT_EQ(info1.lonDegrees, -123); + + EXPECT_EQ(info1.latDegrees, 37); + EXPECT_EQ(info1.lonDegrees, -123); + + EXPECT_EQ(info1.grid_idx_x, 65); + EXPECT_EQ(info1.grid_idx_y, 96); + + EXPECT_EQ(info1.gridLat, 374204140); + EXPECT_EQ(info1.gridLon, -1220904251); + + //////////////////////////////////////////////////////////////////////// + gpsLocation_t loc2 = { .lat = 491304520, .lon = 165976430, .alt = 0 }; + gridInfo_t info2; + + calculateGridInfo(&loc2, &info2); + + EXPECT_EQ(info2.latDegrees, 49); + EXPECT_EQ(info2.lonDegrees, 16); + + EXPECT_EQ(info2.grid_idx_x, 20); + EXPECT_EQ(info2.grid_idx_y, 51); + + EXPECT_EQ(info2.idx_x, 4); + EXPECT_EQ(info2.idx_y, 24); + + EXPECT_EQ(info2.gridLat, 491293581); + EXPECT_EQ(info2.gridLon, 165873573); + + ///////////////////////////////////////////////////////////////////// +} \ No newline at end of file