diff --git a/.gitignore b/.gitignore index 9b72fa8305a..93ff1cc0c29 100644 --- a/.gitignore +++ b/.gitignore @@ -46,3 +46,6 @@ launch.json # Assitnow token and files for test script tokens.yaml *.ubx +/.idea + +/testing/ diff --git a/docs/Settings.md b/docs/Settings.md index b886f6702ac..f68d8038783 100644 --- a/docs/Settings.md +++ b/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) diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 101c7223372..6b486831e68 100755 --- a/src/main/CMakeLists.txt +++ b/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) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 1027f25379d..fb732361f5c 100644 --- a/src/main/blackbox/blackbox.c +++ b/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) 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..99ede69e683 100755 --- a/src/main/fc/fc_tasks.c +++ b/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 + }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 01ca6149bfb..b507157c497 100644 --- a/src/main/fc/settings.yaml +++ b/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 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c7a40e982cc..9fd6726c40a 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 @@ -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] = '-'; diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 5d38a6dba59..03975e021eb 100755 --- a/src/main/scheduler/scheduler.h +++ b/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, diff --git a/src/main/target/BROTHERHOBBYH743/target.h b/src/main/target/BROTHERHOBBYH743/target.h index b069f0ccfd0..d3ff08412e9 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_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 ***************************** diff --git a/src/main/target/CORVON743V1/target.h b/src/main/target/CORVON743V1/target.h index fa5ba03d735..ee36e015b04 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_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 diff --git a/src/main/target/FLYWOOH743PRO/target.h b/src/main/target/FLYWOOH743PRO/target.h index 47419dc78ff..cf02ec6cd14 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_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 ***************************** diff --git a/src/main/target/IFLIGHT_BLITZ_H7_PRO/target.h b/src/main/target/IFLIGHT_BLITZ_H7_PRO/target.h index 7d93ec91ce6..b5fc1906aac 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_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 ***************************** diff --git a/src/main/target/KAKUTEH7WING/target.h b/src/main/target/KAKUTEH7WING/target.h index 7626b997754..6ce46b6cada 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_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 ********************* diff --git a/src/main/target/MATEKH743/target.h b/src/main/target/MATEKH743/target.h index 67e76e55b62..197943cbfaf 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_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 diff --git a/src/main/target/MICOAIR743/target.h b/src/main/target/MICOAIR743/target.h index 526cf438d08..cb0880974d5 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_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 diff --git a/src/main/target/MICOAIR743AIO/target.h b/src/main/target/MICOAIR743AIO/target.h index 90ea60f0a0b..0b6dec45c06 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_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 diff --git a/src/main/target/MICOAIR743V2/target.h b/src/main/target/MICOAIR743V2/target.h index 78de91df4af..7669a9f84f3 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_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 diff --git a/src/main/target/NEUTRONRCH7BT/target.h b/src/main/target/NEUTRONRCH7BT/target.h index 9f4ffd118d5..8c5d3e85147 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_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 ***************************** diff --git a/src/main/target/TBS_LUCID_H7/target.h b/src/main/target/TBS_LUCID_H7/target.h index e9eef8686bf..c86f7d681e1 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_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 diff --git a/src/main/target/TBS_LUCID_H7_WING/target.h b/src/main/target/TBS_LUCID_H7_WING/target.h index 8871cee7398..5a005a0d0d7 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_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 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..206a9c23589 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_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 diff --git a/src/main/terrain/terrain.c b/src/main/terrain/terrain.c new file mode 100644 index 00000000000..17f1966b4f3 --- /dev/null +++ b/src/main/terrain/terrain.c @@ -0,0 +1,187 @@ +/* + * 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 "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 bool terrainSubsystemSdCardOk = false; + +static struct { + timeMs_t lastUpdate; + int32_t terrainAGL; +} terrainHeight; + +static struct { + bool homeAltitudeFound; + float homeAltitudeM; + gpsLocation_t homeLocation; +} terrainHomePos = { + .homeAltitudeFound = false, + .homeAltitudeM = 0.0f, +}; + +/** + * @brief Get the height above mean sea level (AMSL) in meters for a given GPS location. + */ +static float getGeigHtAmslMeters(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 + gridBlock_t *grid = &(findGridCache(&info)->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); + + //terrain must be enabled + if(terrainConfig()->terrainEnabled == false){ + return; + } + + //must be GPS fix + if(STATE(GPS_FIX) == false){ + return; + } + + ///////////////////////////////////////////////////////////////////////////////////////////////////// + + if(terrainSubsystemSdCardOk == false){ + terrainSubsystemSdCardOk = sdcard_isInserted() && sdcard_isFunctional() && afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY; + } + + if(terrainSubsystemSdCardOk == true) { + //this call IO to load data from SD card + loadGridToCacheTask(); + } + + ///////////////////////////////////////////////////////////////////////////////////////////////////// + //try to find home altitude to safe offset + //save reference only once when gps origin is valid and different than last saved + 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 = getGeigHtAmslMeters(&homeLoc); + + if(heightASLHome >= 0){ + terrainHomePos.homeAltitudeFound = true; + terrainHomePos.homeAltitudeM = heightASLHome; + terrainHomePos.homeLocation.lat = posControl.gpsOrigin.lat; + terrainHomePos.homeLocation.lon = posControl.gpsOrigin.lon; + } + } + ///////////////////////////////////////////////////////////////////////////////////////////////////// + + + ///////////////////////////////////////////////////////////////////////////////////////////////////// + float heightASL = getGeigHtAmslMeters(&gpsSol.llh); + if(heightASL >= 0){ + if(terrainHomePos.homeAltitudeFound){ + terrainHeight.terrainAGL = MAX(0, ((int32_t)getEstimatedActualPosition(Z) + (int32_t)(terrainHomePos.homeAltitudeM * 100.0f)) - (int32_t)(heightASL * 100.0f)); + }else{ + terrainHeight.terrainAGL = MAX(0, ((int32_t)getEstimatedActualPosition(Z) + GPS_home.alt) - (int32_t)(heightASL * 100.0f)); + } + + terrainHeight.lastUpdate = millis(); + } + ///////////////////////////////////////////////////////////////////////////////////////////////////// +} + +int32_t terrainGetLastDistanceCm(void) +{ + return terrainHeight.lastUpdate + 500 < 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..6bccdbafe4f --- /dev/null +++ b/src/main/terrain/terrain.h @@ -0,0 +1,149 @@ +/* + * 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 50 + +#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) + +// 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 terrainGetLastDistanceCm(void); diff --git a/src/main/terrain/terrain_io.c b/src/main/terrain/terrain_io.c new file mode 100644 index 00000000000..70c64dd193d --- /dev/null +++ b/src/main/terrain/terrain_io.c @@ -0,0 +1,385 @@ +/* + * 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 "common/log.h" +#include "drivers/time.h" +#include "terrain_utils.h" +#include "drivers/sdcard/sdcard.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"); +} + +/** + * @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"); +} + +/** + * @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(void) +{ + if(terrainIoState.status == TERRAIN_IO_FAILURE){ + LOG_DEBUG(SYSTEM, "TERRAIN IO FAILURE"); + return; + } + + /////////////////////////////////////////////////////////////////// + /////// IDLE ///////////////////////////////////////////// + // check if in 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; + } + + //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){ + 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 /////////////////////////////////// + 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; + uint32_t fileOffset = blocknum * sizeof(terrainIoState.ioBlock); + + afatfsOperationStatus_e seekState = afatfs_fseek(terrainIoState.datFile, (int32_t)fileOffset, 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..26db6abb261 --- /dev/null +++ b/src/main/terrain/terrain_io.h @@ -0,0 +1,78 @@ +/* + * 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_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(void); +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..e2d988d3a4a --- /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 + b->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..1504edb7c6b --- /dev/null +++ b/src/main/terrain/terrain_utils.c @@ -0,0 +1,233 @@ +/* + * 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) +{ + 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) +{ + uint16_t oldest_i = 0; + + // see if we have that grid + const timeMs_t nowMs = millis(); + + 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].lastAccessMs < cache[oldest_i].lastAccessMs) { + oldest_i = i; + } + } + + // Not found. Use the oldest grid and make it this grid, + // initially unpopulated + 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..4f2438f7478 --- /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, 23); + + EXPECT_EQ(info2.gridLat, 491293581); + EXPECT_EQ(info2.gridLon, 165873573); + + ///////////////////////////////////////////////////////////////////// +} \ No newline at end of file