diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index d82e32b4ac3..2b5c5734479 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -14,8 +14,13 @@ main_sources(SITL_COMMON_SRC_EXCLUDES main_sources(SITL_SRC config/config_streamer_file.c drivers/dronecan/libcanard/canard_sitl_driver.c + drivers/sdcard/sdcard.c + drivers/sdcard/sdcard_sitl.c + drivers/sdcard/sdcard_sitl.h drivers/serial_tcp.c drivers/serial_tcp.h + io/asyncfatfs/asyncfatfs.c + io/asyncfatfs/fat_standard.c target/SITL/sim/realFlight.c target/SITL/sim/realFlight.h target/SITL/sim/simHelper.c diff --git a/docs/Navigation Terrain Following.md b/docs/Navigation Terrain Following.md new file mode 100644 index 00000000000..8bcc42c0e11 --- /dev/null +++ b/docs/Navigation Terrain Following.md @@ -0,0 +1,87 @@ +# Terrain AGL Hold + +Terrain AGL Hold is INAV's **terrain-following** mode for fixed wing. It is a **3D Cruise** enhancement that holds a set height **above the ground** instead of above the take-off point — *Cruise with a moving target*. As the ground rises and falls under you, the altitude target rides with it, so the aircraft keeps its clearance over hills and valleys with no stick input: downhill it descends with the valley, uphill it climbs with the ridge. + +It reads the ground elevation from the terrain elevation data on the SD card (see **[Terrain](Terrain.md)** for generating and loading tiles); it never uses a physical rangefinder. With the mode switched off, the aircraft flies exactly like stock 3D Cruise — everything here is opt-in, and **the pitch stick always wins** (the mode box is an instant in-flight kill switch). + +## How it works + +![How Terrain AGL Hold works](assets/images/Terrain-AGL-Hold-diagram.png) + +*One navigation cycle — inputs, health gate, the moving target, the alarm ladder, and the single gate into the altitude target path (the stock controller, untouched).* + +## Requirements + +- A flight controller with terrain support (SD card + barometer): **STM32H7, STM32F7 (F745 / F746 / F765), or AT32F43x**. F4 boards are not enabled for terrain by default. +- **Fixed-wing only.** A valid **GPS fix** and a **barometer**. +- **Terrain tiles** (`.TER`) on the SD card, **including the tile for your take-off site** — see [Terrain](Terrain.md). +- **3D Cruise must be active.** At this stage Terrain AGL Hold works **only together with NAV CRUISE** — this is a mandatory safety condition for now, not just a recommendation. + +## Not the same as copter SURFACE mode + +Multirotor **SURFACE** mode follows the ground *reactively* from a physical rangefinder — the instantaneous distance straight below. Terrain AGL Hold is the fixed-wing equivalent taken from the SD terrain map, and crucially it is **predictive**: it scans **up to 2 km ahead** along your track and climbs/warns *early* for terrain rising in front of you — something a downward rangefinder cannot do. The two never stack (this mode refuses to engage while SURFACE is active), and SURFACE mode itself is multirotor-only. + +## What it does + +- **Captures** your current height above ground when you switch it on, and holds it. Move the **pitch stick** to set a new height; centre the stick and it holds the new one. +- Engage **below the safety floor** → it climbs smoothly up to the floor first. +- **Looks ahead** along your track (up to 2 km, 1 km by default) with a per-airframe **escape test** — *"at this speed, with your configured climb rate, will you clear what's ahead?"* — and warns you **early, while there's still time**. +- If the terrain data is lost mid-flight, it **holds the last valid altitude target and warns** on the OSD — it never descends on dead data. +- The stock 3D Cruise altitude controller is **untouched**; this only feeds it a moving target. +- **Not used for autonomous navigation.** RTH and waypoint missions do **not** use it yet — the mode yields to them and they fly stock (terrain-blind). + +## Setup + +1. **Prepare and enable terrain data** — see **[Terrain](Terrain.md)**: generate `.TER` tiles for your area (include the take-off tile), copy them to the SD card root, then in the CLI: + ``` + set terrain_enabled = ON + save + ``` +2. **Assign the mode.** In the Configurator **Modes** tab (or via CLI `aux`), put **TERRAIN AGL HOLD** on a spare, **deliberately guarded** switch — not next to ARM. It is active only inside 3D Cruise. + > **Hint (once you are confident with the mode):** you *can* put **NAV CRUISE** and **TERRAIN AGL HOLD** on the **same switch** so one flip arms both. This is **not** the recommended default — keeping them on **separate** switches lets you enter Cruise first, confirm it, and engage terrain hold as a deliberate second step. +3. **Set the floor and look-ahead** if you want (defaults are sensible — see *Settings*). +4. **OSD:** add the **Rangefinder** element to see your height above ground (AGL) in flight. The flight-mode field shows **TERR** while the hold is engaged. +5. **Fly** in 3D Cruise, then flip the switch — it captures and holds your current AGL. **Panic rule: box off = stock cruise instantly; the pitch stick always wins.** + +## Settings + +| Setting | Default | Range | Meaning | +|---|---|---|---| +| `terrain_nav_min_agl` | 60 m | 50 – 120 m | The **safety floor** — the lowest height above ground the mode will hold. Engage below it and it climbs up to it. (Stored in centimetres: 6000 = 60 m.) | +| `terrain_nav_lookahead` | 1000 m | 0 – 2000 m | How far ahead along your track the **forward scan** looks for rising terrain. `0` disables the early *TERRAIN AHEAD!* warning, leaving only the reactive floor. | + +> **NB — keep your climb rates honest.** The climb authority is your existing **`nav_fw_auto_climb_rate`** — there is no separate terrain climb setting, and the *TERRAIN AHEAD!* escape warning **trusts that number**, so set it to a rate your model genuinely sustains. Keep **`nav_fw_manual_climb_rate` at least equal to `nav_fw_auto_climb_rate`** (they default to 300 / 500) — or **greater**, if you want pulling the stick to actually add climb. If `nav_fw_manual_climb_rate` is **lower** than `nav_fw_auto_climb_rate`, grabbing pitch during an auto-climb commands the lower rate: the nose visibly eases and it can even **slow** the climb. + +## Behaviour in the air + +- **Engage at or above the floor** → captures your current AGL and holds it. +- **Engage below the floor** → climbs to the floor (*TERRAIN AUTO CLIMB TO MIN*). +- **Pitch stick** → sets a new held AGL. The stick always wins: it pauses the hold the moment it moves (smooth blend, no jump); centre it and the current height is re-captured. +- **Hands off over a hill** → the target rides the terrain up and back down. +- **Sharp turns while riding the floor** clip the safety margin — bank with height in hand. + +## OSD messages — the alarm ladder + +The mode narrates itself on the OSD. Higher-priority messages take the slot; while the autopilot is auto-climbing under a warning, the two alternate so you always see both the danger and the action. + +| Message | Kind | Meaning | What to do | +|---|---|---|---| +| **TERRAIN! TURN AWAY!** | Warning | Too low **and** climbing can no longer save you (full climb still loses, or you're sinking, or pinned at your altitude ceiling). | **Turn away immediately** (manual + full throttle). Don't count on pulling up. | +| **TERRAIN! PULL UP!** | Warning | Too low, but climbing **still works**. | Release the stick — the autopilot climbs at full rate — or pull **if `nav_fw_manual_climb_rate` > `nav_fw_auto_climb_rate`**; if in doubt, turn as well. | +| **TERRAIN AHEAD!** | Caution | The forward scan says that at this speed the slope ahead beats a full-rate climb — **you have time now**. | Turn, slow down, or climb early — while it's cheap. | +| **TERRAIN VS MAX ALT** | Caution | The terrain needs more altitude than your `nav_max_altitude` ceiling allows (only if a ceiling is set). | Raise/remove the ceiling, or accept reduced clearance at the wall. | +| **TERRAIN AUTO CLIMB TO MIN** | Info | The autopilot is already climbing you to the floor. | Nothing — it's handled. | +| **TERRAIN NOT READY** | Info | Engagement refused — no usable terrain data yet. | Fly stock cruise; check tiles / SD card if it persists. | +| **TERRAIN LOST - ALT FROZEN** | Caution | Data lost mid-hold — the altitude target is **frozen at the last valid value**. | Never descend blind; navigate out on the frozen altitude (it resumes after a few seconds of good data). | +| **TERRAIN LOOKAHEAD OFF** | Info | The forward scan is unavailable (no heading estimate). | Only the reactive floor guards you now. | + +> **About `nav_max_altitude`:** that ceiling is **barometric altitude above your Home point** — it is *not* terrain-AGL aware. So over rising ground the terrain can legitimately need more altitude than the ceiling allows; that is exactly when *TERRAIN VS MAX ALT* appears. + +## Safety & limitations + +- **Fixed-wing only.** It never runs on multirotors, rovers, or boats, and it works **only inside 3D Cruise** (mandatory at this stage). +- **Not for autonomous navigation yet.** RTH and waypoint missions do not use it — the mode **yields cleanly to stock behaviour** (hands the aircraft back) during launch, autoland, emergency landing, RTH, and waypoint missions, and on **GPS loss**. Stock RTH is terrain-blind: it does not follow the ground. +- The **floor is deliberately high** (default 60 m, minimum 50 m). Terrain data and GPS each carry a few metres of error, so keep clearance in hand — don't set the floor low. +- **Not a rangefinder replacement.** The value is the ground elevation at your GPS position from the map — it does **not** see trees, buildings, wires, or the actual nearest object below you. +- **Never stacks with the rangefinder** — it refuses to engage while SURFACE mode is active. +- If terrain data is lost while engaged, the target **freezes at the last valid altitude** (*TERRAIN LOST - ALT FROZEN*) — it never descends on dead data. diff --git a/docs/Settings.md b/docs/Settings.md index c8eb8bb0716..b92632ec3ba 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -6938,6 +6938,36 @@ 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 | + +--- + +### terrain_nav_lookahead + +TERRAIN AGL HOLD: check terrain up to this distance ahead along the flight path and climb early for rising ground [m]. 0 disables the lookahead. The effective distance is also limited by the terrain block cache of the flight controller + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | 0 | 2000 | + +--- + +### terrain_nav_min_agl + +TERRAIN AGL HOLD: minimum held height above ground [cm]. Engaging below it commands a gentle climb to it. The floor absorbs worst-case terrain map error on steep slopes plus canopy - the minimum is the lowest value where that error budget still clears + +| Default | Min | Max | +| --- | --- | --- | +| 6000 | 5000 | 12000 | + +--- + ### 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..e9303330eff --- /dev/null +++ b/docs/Terrain.md @@ -0,0 +1,98 @@ +# 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**. + +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://martinovem.github.io/High-Resolution-Map-Generator/ + +The generated `.TER` files use an iNav-specific format and are **not compatible with ArduPilot terrain data**. Do not use files from https://terrain.ardupilot.org/ or copy `.DAT` files from an ArduPilot setup — they will not be read correctly. + +**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.** + +The generator always produces **30 m resolution** data, which is the only resolution iNav supports — there is no resolution setting to choose. +The generated files are then copied to the SD card into the root directory structure. + +For example +``` +SDCARD:\ +├── N47E014.TER +├── N47E015.TER +├── N47E016.TER +├── N49E015.TER +├── N49E016.TER +├── N49E017.TER +└── N50E016.TER +``` + +Copying can be done via **iNav MSC (Mass Storage Class)** is not recommended. + +> **Important:** The `.TER` file covering your **home position** (the location where you arm) must always be present on the SD +> card. The terrain system first reads the ground elevation at the home position to establish an altitude reference. Until this +> succeeds, terrain data will not be displayed for any position — even if tiles for your entire flight area are present. Make +> sure the tile for your take-off site is included when generating and copying terrain files. + +# 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. + +# Troubleshooting +- OSD shows no terrain value / "Rangefinder distance" is dash: + - Verify `terrain_enabled = ON` is set and saved (`set terrain_enabled = ON` → `save` → reboot) + - Confirm the SD card is mounted: check `status` in the CLI for SD card state + - Confirm a valid GPS fix is present before expecting any terrain value to appear + - Terrain data is only loaded **after the first valid GPS fix**. If the fix is acquired after a long wait, allow a few seconds for the first read to complete + - Check that the `.TER` file covering your **home position** exists on the SD card — see note in *Data Generation and Copying* above +- Feature disables itself mid-flight: + - The terrain subsystem disables itself on any SD card read failure. This is most commonly caused by a slow or low-quality SD card. Replace the card with a branded, high-speed card + - On F4-based flight controllers, SD card compatibility issues are more common. If problems persist, test with a different card model or switch to an H7-based flight controller + - Ensure the SD card partition is ≤ 4 GB and formatted FAT32. Larger partitions or exFAT formatting can cause intermittent read failures +- Value appears but is clearly wrong: + - Reformat the SD card and recopy the terrain files. A corrupted or partially overwritten FAT32 filesystem can produce plausible but incorrect altitude values + - The displayed value is altitude **above terrain at the current GPS position**, not distance to the nearest object below the aircraft. Trees, buildings, and local obstacles are not accounted for +- Terrain data was not loaded before arming: + - If the flight controller is armed before terrain data for the home position is successfully read from the SD card, the system will not attempt to load data during the flight (to avoid SD card access latency while airborne). Disarm, wait for the OSD value to appear, then arm diff --git a/docs/assets/images/Terrain-AGL-Hold-diagram.png b/docs/assets/images/Terrain-AGL-Hold-diagram.png new file mode 100644 index 00000000000..780dec211de Binary files /dev/null and b/docs/assets/images/Terrain-AGL-Hold-diagram.png differ diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index f4ab6479a5f..bd33dd477f9 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -669,6 +669,21 @@ 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 + terrain/terrain_nav.h + terrain/terrain_nav.c + terrain/terrain_nav_hold_core.h + terrain/terrain_nav_hold_core.c + terrain/terrain_nav_hold.h + terrain/terrain_nav_hold.c ) list(APPEND COMMON_SRC ${DSDLC_GENERATED_SRC}) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 75ab075feac..0df04927cf4 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 @@ -470,6 +522,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, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"terrainAMSL", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, +#endif }; #define BLACKBOX_FIRST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_HEADER @@ -576,6 +632,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; @@ -1364,6 +1424,10 @@ static void writeSlowFrame(void) blackboxWriteUnsignedVB(slowHistory.escRPM); blackboxWriteSignedVB(slowHistory.escTemperature); #endif +#ifdef USE_TERRAIN + blackboxWriteSignedVB(slowHistory.terrainAGL); + blackboxWriteSignedVB(slowHistory.terrainAMSL); +#endif blackboxSlowFrameIterationTimer = 0; } @@ -1439,6 +1503,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 } /** @@ -2186,6 +2254,25 @@ 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; + return; + } + } + } +#endif + if (blackboxState >= BLACKBOX_FIRST_HEADER_SENDING_STATE && blackboxState <= BLACKBOX_LAST_HEADER_SENDING_STATE) { blackboxReplenishHeaderBudget(); } @@ -2316,11 +2403,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/common/log.h b/src/main/common/log.h index f46d8d918f1..34862a12824 100644 --- a/src/main/common/log.h +++ b/src/main/common/log.h @@ -31,6 +31,7 @@ typedef enum { LOG_TOPIC_VTX, // 9, mask = 512 LOG_TOPIC_OSD, // 10, mask = 1024 LOG_TOPIC_CAN, // 11, mask = 2048 + LOG_TOPIC_TERRAIN, // 12, mask = 4096 LOG_TOPIC_COUNT, } logTopic_e; diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index da1be28507d..203eee4ef36 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 @@ -133,7 +134,8 @@ #define PG_GEOZONES 1043 #define PG_GEOZONE_VERTICES 1044 #define PG_DRONECAN_CONFIG 1045 -#define PG_INAV_END PG_DRONECAN_CONFIG +#define PG_TERRAIN_NAV_CONFIG 1046 +#define PG_INAV_END PG_TERRAIN_NAV_CONFIG // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/drivers/sdcard/sdcard.c b/src/main/drivers/sdcard/sdcard.c index ff8cc4fa850..a5b088243dd 100644 --- a/src/main/drivers/sdcard/sdcard.c +++ b/src/main/drivers/sdcard/sdcard.c @@ -91,6 +91,8 @@ void sdcard_init(void) sdcardVTable = &sdcardSpiVTable; #elif defined(USE_SDCARD_SDIO) sdcardVTable = &sdcardSdioVTable; +#elif defined(USE_SDCARD_SITL) + sdcardVTable = &sdcardSitlVTable; #endif if (sdcardVTable) { diff --git a/src/main/drivers/sdcard/sdcard_impl.h b/src/main/drivers/sdcard/sdcard_impl.h index 3eba6095904..81839c28a64 100644 --- a/src/main/drivers/sdcard/sdcard_impl.h +++ b/src/main/drivers/sdcard/sdcard_impl.h @@ -112,3 +112,7 @@ extern sdcardVTable_t sdcardSpiVTable; #ifdef USE_SDCARD_SDIO extern sdcardVTable_t sdcardSdioVTable; #endif + +#ifdef USE_SDCARD_SITL +extern sdcardVTable_t sdcardSitlVTable; +#endif diff --git a/src/main/drivers/sdcard/sdcard_sitl.c b/src/main/drivers/sdcard/sdcard_sitl.c new file mode 100644 index 00000000000..98cb2743227 --- /dev/null +++ b/src/main/drivers/sdcard/sdcard_sitl.c @@ -0,0 +1,215 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#if defined(USE_SDCARD) && defined(USE_SDCARD_SITL) + +#include "common/utils.h" + +#include "drivers/sdcard/sdcard.h" +#include "drivers/sdcard/sdcard_impl.h" +#include "drivers/sdcard/sdcard_standard.h" +#include "drivers/sdcard/sdcard_sitl.h" + +#define SDCARD_SITL_PATH_MAX 260 + +static FILE *imageFile = NULL; +static char imagePath[SDCARD_SITL_PATH_MAX]; + +bool sdcardSitlSetPath(const char *path) +{ + if (path == NULL || strlen(path) == 0 || strlen(path) >= SDCARD_SITL_PATH_MAX) { + return false; + } + strcpy(imagePath, path); + return true; +} + +static void sdcardSitl_init(void) +{ + sdcard.state = SDCARD_STATE_NOT_PRESENT; + + if (imagePath[0] == '\0') { + fprintf(stderr, "[SDCARD] No image file specified (--sdcard=), no card simulated\n"); + return; + } + + imageFile = fopen(imagePath, "r+b"); + if (imageFile == NULL) { + fprintf(stderr, "[SDCARD] Cannot open image file %s, no card simulated\n", imagePath); + return; + } + + long imageSize = -1; + if (fseek(imageFile, 0, SEEK_END) == 0) { + imageSize = ftell(imageFile); + } + + if (imageSize < SDCARD_BLOCK_SIZE) { + fprintf(stderr, "[SDCARD] Image file %s is unusable, no card simulated\n", imagePath); + fclose(imageFile); + imageFile = NULL; + return; + } + + memset(&sdcard.metadata, 0, sizeof(sdcard.metadata)); + sdcard.metadata.numBlocks = imageSize / SDCARD_BLOCK_SIZE; + memcpy(sdcard.metadata.productName, "SITL", 4); + + sdcard.state = SDCARD_STATE_READY; + fprintf(stderr, "[SDCARD] Simulating card from %s (%u blocks)\n", imagePath, (unsigned)sdcard.metadata.numBlocks); +} + +static bool sdcardSitl_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) +{ + if (sdcard.state != SDCARD_STATE_READY) { + return false; + } + + sdcard.pendingOperation.buffer = buffer; + sdcard.pendingOperation.blockIndex = blockIndex; + sdcard.pendingOperation.callback = callback; + sdcard.pendingOperation.callbackData = callbackData; + + sdcard.state = SDCARD_STATE_READING; + + return true; +} + +static sdcardOperationStatus_e sdcardSitl_beginWriteBlocks(uint32_t blockIndex, uint32_t blockCount) +{ + UNUSED(blockIndex); + UNUSED(blockCount); + + // Writes are addressed per block, no setup is needed for the backing file + if (sdcard.state != SDCARD_STATE_READY) { + return SDCARD_OPERATION_BUSY; + } + + return SDCARD_OPERATION_SUCCESS; +} + +static sdcardOperationStatus_e sdcardSitl_writeBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) +{ + if (sdcard.state != SDCARD_STATE_READY) { + return SDCARD_OPERATION_BUSY; + } + + sdcard.pendingOperation.buffer = buffer; + sdcard.pendingOperation.blockIndex = blockIndex; + sdcard.pendingOperation.callback = callback; + sdcard.pendingOperation.callbackData = callbackData; + + sdcard.state = SDCARD_STATE_SENDING_WRITE; + + return SDCARD_OPERATION_IN_PROGRESS; +} + +static bool sdcardSitl_transferBlock(bool isWrite) +{ + if (imageFile == NULL || sdcard.pendingOperation.blockIndex >= sdcard.metadata.numBlocks) { + return false; + } + + if (fseek(imageFile, (long)sdcard.pendingOperation.blockIndex * SDCARD_BLOCK_SIZE, SEEK_SET) != 0) { + return false; + } + + if (isWrite) { + if (fwrite(sdcard.pendingOperation.buffer, 1, SDCARD_BLOCK_SIZE, imageFile) != SDCARD_BLOCK_SIZE) { + return false; + } + fflush(imageFile); + return true; + } + + return fread(sdcard.pendingOperation.buffer, 1, SDCARD_BLOCK_SIZE, imageFile) == SDCARD_BLOCK_SIZE; +} + +static bool sdcardSitl_poll(void) +{ + switch (sdcard.state) { + case SDCARD_STATE_READING: { + bool success = sdcardSitl_transferBlock(false); + + sdcard.state = SDCARD_STATE_READY; + + if (sdcard.pendingOperation.callback) { + sdcard.pendingOperation.callback( + SDCARD_BLOCK_OPERATION_READ, + sdcard.pendingOperation.blockIndex, + success ? sdcard.pendingOperation.buffer : NULL, + sdcard.pendingOperation.callbackData + ); + } + break; + } + case SDCARD_STATE_SENDING_WRITE: { + bool success = sdcardSitl_transferBlock(true); + + sdcard.state = SDCARD_STATE_READY; + + if (sdcard.pendingOperation.callback) { + sdcard.pendingOperation.callback( + SDCARD_BLOCK_OPERATION_WRITE, + sdcard.pendingOperation.blockIndex, + success ? sdcard.pendingOperation.buffer : NULL, + sdcard.pendingOperation.callbackData + ); + } + break; + } + default: + break; + } + + return sdcard.state == SDCARD_STATE_READY; +} + +static bool sdcardSitl_isFunctional(void) +{ + return sdcard.state != SDCARD_STATE_NOT_PRESENT; +} + +static bool sdcardSitl_isInitialized(void) +{ + return sdcard.state >= SDCARD_STATE_READY; +} + +static const sdcardMetadata_t* sdcardSitl_getMetadata(void) +{ + return &sdcard.metadata; +} + +sdcardVTable_t sdcardSitlVTable = { + .init = &sdcardSitl_init, + .readBlock = &sdcardSitl_readBlock, + .beginWriteBlocks = &sdcardSitl_beginWriteBlocks, + .writeBlock = &sdcardSitl_writeBlock, + .poll = &sdcardSitl_poll, + .isFunctional = &sdcardSitl_isFunctional, + .isInitialized = &sdcardSitl_isInitialized, + .getMetadata = &sdcardSitl_getMetadata, +}; + +#endif diff --git a/src/main/drivers/sdcard/sdcard_sitl.h b/src/main/drivers/sdcard/sdcard_sitl.h new file mode 100644 index 00000000000..7fdbef7f4e3 --- /dev/null +++ b/src/main/drivers/sdcard/sdcard_sitl.h @@ -0,0 +1,22 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#include + +bool sdcardSitlSetPath(const char *path); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 108e48e2012..2446edd76aa 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -154,6 +154,8 @@ #include "telemetry/telemetry.h" +#include "terrain/terrain.h" + #if defined(SITL_BUILD) #include "target/SITL/serial_proxy.h" #endif @@ -624,6 +626,23 @@ void init(void) } #endif +#ifdef USE_SDCARD + + bool sdcardNeeded = false; +#ifdef USE_BLACKBOX + sdcardNeeded = (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD); +#endif +#ifdef USE_TERRAIN + sdcardNeeded = sdcardNeeded || terrainConfig()->terrainEnabled; +#endif + if (sdcardNeeded) { + sdcardInsertionDetectInit(); + sdcard_init(); + afatfs_init(); + } +#endif // USE_SDCARD + + #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 @@ -648,20 +667,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_msp_box.c b/src/main/fc/fc_msp_box.c index 842b56563a0..ecac17bf1d7 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -41,6 +41,11 @@ #include "navigation/navigation.h" +#ifdef USE_TERRAIN +#include "terrain/terrain.h" +#include "terrain/terrain_nav_hold.h" +#endif + #include "telemetry/telemetry.h" #include "drivers/gimbal_common.h" @@ -110,6 +115,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXGIMBALCENTER, .boxName = "GIMBAL CENTER", .permanentId = 67 }, { .boxId = BOXGIMBALHTRK, .boxName = "GIMBAL HEADTRACKER", .permanentId = 68 }, { .boxId = BOXAUTOSPEED, .boxName = "AUTO SPEED", .permanentId = 69 }, + { .boxId = BOXTERRAINAGLHOLD, .boxName = "TERRAIN AGL HOLD", .permanentId = 70 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -250,6 +256,11 @@ void initActiveBoxIds(void) if (STATE(AIRPLANE) || platformTypeConfigured(PLATFORM_AIRPLANE)) { ADD_ACTIVE_BOX(BOXSOARING); ADD_ACTIVE_BOX(BOXAUTOSPEED); +#ifdef USE_TERRAIN + if (terrainConfig()->terrainEnabled) { + ADD_ACTIVE_BOX(BOXTERRAINAGLHOLD); + } +#endif } } @@ -440,6 +451,9 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)), BOXAUTOLEVEL); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXPLANWPMISSION)), BOXPLANWPMISSION); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSOARING)), BOXSOARING); +#ifdef USE_TERRAIN + CHECK_ACTIVE_BOX(IS_ENABLED(terrainNavHoldIsEngaged()), BOXTERRAINAGLHOLD); +#endif #ifdef USE_MULTI_MISSION CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)), BOXCHANGEMISSION); #endif diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 14adc09771c..53cfd909db9 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -97,6 +97,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) @@ -379,6 +382,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) @@ -785,6 +792,21 @@ 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 + #ifdef USE_DRONECAN [TASK_DRONECAN] = { .taskName = "DRONECAN", diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 48e4357a1a5..5fba051378d 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -86,6 +86,7 @@ typedef enum { BOXGIMBALCENTER = 58, BOXGIMBALHTRK = 59, BOXAUTOSPEED = 60, + BOXTERRAINAGLHOLD = 61, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 74ad48e8d58..f727b5b84c1 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4643,3 +4643,30 @@ groups: field: bitRateKbps table: dronecan_bitrate_table 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 + - name: PG_TERRAIN_NAV_CONFIG + type: terrainNavConfig_t + headers: ["terrain/terrain_nav_hold.h"] + condition: USE_TERRAIN + members: + - name: terrain_nav_min_agl + description: "TERRAIN AGL HOLD: minimum held height above ground [cm]. Engaging below it commands a gentle climb to it. The floor absorbs worst-case terrain map error on steep slopes plus canopy - the minimum is the lowest value where that error budget still clears" + default_value: 6000 + field: minAglCm + min: 5000 + max: 12000 + - name: terrain_nav_lookahead + description: "TERRAIN AGL HOLD: check terrain up to this distance ahead along the flight path and climb early for rising ground [m]. 0 disables the lookahead. The effective distance is also limited by the terrain block cache of the flight controller" + default_value: 1000 + field: lookaheadDistM + min: 0 + max: 2000 diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index d06fc2b8a76..cfa7da71ae8 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -44,7 +44,9 @@ #define AFATFS_SECTOR_SIZE 512 #define AFATFS_NUM_FATS 2 -#define AFATFS_MAX_OPEN_FILES 3 +// One handle is held long-term by the terrain module (kept-open .DAT tile file) in addition to the +// blackbox log file and transient directory/open operations +#define AFATFS_MAX_OPEN_FILES 4 #define AFATFS_DEFAULT_FILE_DATE FAT_MAKE_DATE(2015, 12, 01) #define AFATFS_DEFAULT_FILE_TIME FAT_MAKE_TIME(00, 00, 00) @@ -495,6 +497,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 +3668,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 35927944d31..a065675ac43 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -99,6 +99,10 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#ifdef USE_TERRAIN +#include "terrain/terrain_nav_hold.h" +#endif + #include "rx/rx.h" #include "rx/msp_override.h" @@ -119,6 +123,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 @@ -2479,19 +2487,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: @@ -2627,6 +2645,10 @@ static bool osdDrawSingleElement(uint8_t item) p = "LOTR"; else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) p = "HOLD"; +#ifdef USE_TERRAIN + else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE) && terrainNavHoldIsEngaged()) + p = "TERR"; +#endif else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) p = "CRUZ"; else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) @@ -5437,7 +5459,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) int32_t logNumber = blackboxGetLogNumber(); if (logNumber >= 0) { - tfp_sprintf(buff, ": %05ld ", logNumber); + tfp_sprintf(buff, ": %05ld ", (long)logNumber); } else { strcat(buff, ": INVALID"); } @@ -6118,6 +6140,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter const char *messages[8]; unsigned messageCount = 0; #define ADD_MSG(msg) do { if (messageCount < ARRAYLEN(messages)) messages[messageCount++] = (msg); } while(0) +#ifdef USE_TERRAIN + /* The terrain floor warnings are crash-avoidance class: they blink + * like the failsafe info text below. Remembering the stored pointer + * lets the blink decision match the exact message on display */ + const char *terrainUrgentMessage = NULL; +#endif const char *failsafeInfoMessage = NULL; const char *invertedInfoMessage = NULL; @@ -6248,6 +6276,51 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter case GEOZONE_MESSAGE_STATE_NONE: break; } +#endif +#ifdef USE_TERRAIN + /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL: the worst warning, and - + * while the hold is actually climbing to the minimum - the + * auto-climb info alternating beneath it, so the pilot always + * sees both the danger and the action being taken */ + switch (terrainNavHoldGetWarning()) { + case TERRAIN_NAV_HOLD_WARN_NOT_READY: + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_TERRAIN_NOT_READY)); + break; + case TERRAIN_NAV_HOLD_WARN_DATA_LOST: + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_TERRAIN_DATA_LOST)); + break; + case TERRAIN_NAV_HOLD_WARN_MAX_ALT: + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_TERRAIN_VS_MAX_ALT)); + break; + case TERRAIN_NAV_HOLD_WARN_PULL_UP: + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_TERRAIN_PULL_UP)); + if (messageCount) { + terrainUrgentMessage = messages[messageCount - 1]; + } + break; + case TERRAIN_NAV_HOLD_WARN_TURN_AWAY: + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_TERRAIN_TURN_AWAY)); + if (messageCount) { + terrainUrgentMessage = messages[messageCount - 1]; + } + break; + case TERRAIN_NAV_HOLD_WARN_NO_HEADING: + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_TERRAIN_NO_HEADING)); + break; + case TERRAIN_NAV_HOLD_WARN_AUTO_CLIMB: + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_TERRAIN_AUTO_CLIMB)); + break; + case TERRAIN_NAV_HOLD_WARN_TERRAIN_AHEAD: + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_TERRAIN_AHEAD)); + break; + case TERRAIN_NAV_HOLD_WARN_NONE: + break; + } + if (terrainNavHoldAutoClimbRunning() + && terrainNavHoldGetWarning() != TERRAIN_NAV_HOLD_WARN_AUTO_CLIMB + && terrainNavHoldGetWarning() != TERRAIN_NAV_HOLD_WARN_NONE) { + ADD_MSG(OSD_MESSAGE_STR(OSD_MSG_TERRAIN_AUTO_CLIMB)); + } #endif if (STATE(AIRPLANE)) { /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ #ifdef USE_FW_AUTOLAND @@ -6376,6 +6449,13 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter // a lost model, but might help avoiding a crash. // Blink to grab user attention. TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); +#ifdef USE_TERRAIN + } else if (terrainUrgentMessage && message == terrainUrgentMessage) { + // The terrain floor warnings are the same class: act NOW to + // avoid the terrain. The no-blink note below protects + // recovery info, not urgent warnings + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); +#endif } else if (message == invertedInfoMessage) { TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 21af2d7b290..63d8a3c0fa2 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -147,6 +147,17 @@ #define OSD_MSG_GEOZONE_ACTION "PERFORM ACTION IN %s %s" #endif +#if defined(USE_TERRAIN) +#define OSD_MSG_TERRAIN_NOT_READY "TERRAIN NOT READY" +#define OSD_MSG_TERRAIN_DATA_LOST "TERRAIN LOST - ALT FROZEN" +#define OSD_MSG_TERRAIN_VS_MAX_ALT "TERRAIN VS MAX ALT" +#define OSD_MSG_TERRAIN_PULL_UP "TERRAIN! PULL UP!" +#define OSD_MSG_TERRAIN_TURN_AWAY "TERRAIN! TURN AWAY!" +#define OSD_MSG_TERRAIN_NO_HEADING "TERRAIN LOOKAHEAD OFF" +#define OSD_MSG_TERRAIN_AUTO_CLIMB "TERRAIN AUTO CLIMB TO MIN" +#define OSD_MSG_TERRAIN_AHEAD "TERRAIN AHEAD!" +#endif + #ifdef USE_ADSB static const char* const ADSB_EMITTER_TYPE_STRINGS[] = { "NOINFO", // 0 - No information about the emitter type diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e112ce8a1b9..330057111f3 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -58,6 +58,10 @@ #include "navigation/navigation_private.h" #include "navigation/rth_trackback.h" +#ifdef USE_TERRAIN +#include "terrain/terrain_nav_hold.h" +#endif + #include "rx/rx.h" #include "sensors/sensors.h" @@ -4416,6 +4420,13 @@ void applyWaypointNavigationAndAltitudeHold(void) // Reset all navigation requests - NAV controllers will set them if necessary DISABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); +#ifdef USE_TERRAIN + // TERRAIN AGL HOLD gate - the single entry point for map terrain into the + // altitude target path. Runs every cycle (also disarmed) so it engages, + // freezes and disengages in the same place it commands + terrainNavCruiseHoldUpdate(); +#endif + // No navigation when disarmed if (!ARMING_FLAG(ARMED)) { // If we are disarmed, abort forced RTH or Emergency Landing diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index e86865d55e3..b1d5b8713aa 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -144,6 +144,11 @@ typedef enum { TASK_GEOZONE, #endif +#if defined (USE_TERRAIN) + TASK_TERRAIN, + TASK_TERRAIN_IO, +#endif + #ifdef USE_DRONECAN TASK_DRONECAN, #endif diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 2542a352bc1..201977813d0 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -52,6 +52,7 @@ #include "drivers/timer.h" #include "drivers/serial.h" #include "drivers/serial_tcp.h" +#include "drivers/sdcard/sdcard_sitl.h" #include "config/config_streamer.h" #include "build/version.h" @@ -237,6 +238,7 @@ void printCmdLineOptions(void) fprintf(stderr, "--parity=[Even|None|Odd] Serial receiver parity (default: None).\n"); fprintf(stderr, "--fcproxy Use inav/betaflight FC as a proxy for serial receiver.\n"); fprintf(stderr, "--tcpbaseport=[port] Base TCP port for UART sockets (default: 5760)\n"); + fprintf(stderr, "--sdcard=[path] Path to a FAT32 SD-card image file. If not specified, no SD card is simulated.\n"); fprintf(stderr, "--chanmap=[mapstring] Channel mapping. Maps INAVs motor and servo PWM outputs to the virtual receiver output in the simulator.\n"); fprintf(stderr, " The mapstring has the following format: M(otor)|S(servo)-,... All numbers must have two digits\n"); fprintf(stderr, " For example: Map motor 1 to virtal receiver output 1, servo 1 to output 2 and servo 2 to output 3:\n"); @@ -268,6 +270,7 @@ void parseArguments(int argc, char *argv[]) {"parity", required_argument, 0, '4'}, {"fcproxy", no_argument, 0, '5'}, {"tcpbaseport", required_argument, 0, '6'}, + {"sdcard", required_argument, 0, '7'}, {NULL, 0, NULL, 0} }; @@ -363,6 +366,11 @@ void parseArguments(int argc, char *argv[]) tcpBasePort = (uint16_t)basePort; break; } + case '7': + if (!sdcardSitlSetPath(optarg)) { + fprintf(stderr, "[SDCARD] Invalid image path, no SD card will be simulated\n."); + } + break; default: printCmdLineOptions(); diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index aefd6686e10..7052b29e978 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -70,6 +70,13 @@ #define USE_RANGEFINDER_FAKE #define USE_RX_SIM + +// SD card simulated from a host image file (see drivers/sdcard/sdcard_sitl.c), +// which also enables the terrain module for SITL +#define USE_SDCARD +#define USE_SDCARD_SITL +#define USE_TERRAIN +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 4 // mirror the small-RAM (F7/AT32) terrain cache tier of the new base (each entry ~1.1 KB packed) #define USE_DRONECAN #define DRONECAN_SITL_INTERFACE "vcan0" diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 5d262ad48f5..765eef6a10b 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -129,6 +129,28 @@ extern uint8_t __config_end; #endif +// Terrain keeps a sizeable grid cache in RAM, so restrict it to MCUs with enough of it. +// MCU_FLASH_SIZE is a poor proxy for RAM (e.g. STM32F405 has 1 MB flash but only ~192 KB +// RAM), so gate on the MCU family/subfamily instead. Enabled targets (>= ~320 KB RAM): +// STM32H7 (512 KB+), STM32F745/F746 (320 KB), STM32F765 (512 KB), AT32F43x (384 KB). +#if defined(USE_BARO) && defined(USE_SDCARD) && !defined(USE_TERRAIN) && ( \ + defined(STM32H7) || \ + defined(AT32F43x) || \ + defined(STM32F745xx) || \ + defined(STM32F746xx) || \ + defined(STM32F765xx)) +#define USE_TERRAIN + +// number of grid blocks held in the RAM cache; each entry is a gridCache_t +// wrapping one packed gridBlock_t (~1.1 KB), no longer the old 2048-byte block +#if (MCU_FLASH_SIZE > 1024) +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 8 +#else +#define TERRAIN_GRID_BLOCK_CACHE_SIZE 4 +#endif + +#endif + // CRSF sensor input on a dedicated UART #if defined(USE_SERIALRX_CRSF) #define USE_CRSF_SENSOR_INPUT diff --git a/src/main/terrain/terrain.c b/src/main/terrain/terrain.c new file mode 100644 index 00000000000..826faea6fd9 --- /dev/null +++ b/src/main/terrain/terrain.c @@ -0,0 +1,214 @@ +/* + * 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_NO_AMSL_DATA; + } + + gridInfo_t info; + calculateGridInfo(loc, &info); + + if (info.idx_x > TERRAIN_GRID_BLOCK_SIZE_X - 2) { + return TERRAIN_STATUS_NO_AMSL_DATA; + } + if (info.idx_y > TERRAIN_GRID_BLOCK_SIZE_Y - 2) { + return TERRAIN_STATUS_NO_AMSL_DATA; + } + + // find the grid + gridCache_t *cache = findGridCache(&info); + if(cache == NULL){ + return TERRAIN_STATUS_NO_AMSL_DATA; + } + 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_NO_AMSL_DATA; + } + + // hXY are the heights of the 4 surrounding grid points, reconstructed from + // the packed 10-bit offsets plus the block's heightBase + const int16_t h00 = grid->heightBase + TERRAIN_HEIGHT_OFFSET_RESOLUTION_M * getHeightOffsetByIndex(grid, info.idx_x+0, info.idx_y+0); + const int16_t h01 = grid->heightBase + TERRAIN_HEIGHT_OFFSET_RESOLUTION_M * getHeightOffsetByIndex(grid, info.idx_x+0, info.idx_y+1); + const int16_t h10 = grid->heightBase + TERRAIN_HEIGHT_OFFSET_RESOLUTION_M * getHeightOffsetByIndex(grid, info.idx_x+1, info.idx_y+0); + const int16_t h11 = grid->heightBase + TERRAIN_HEIGHT_OFFSET_RESOLUTION_M * getHeightOffsetByIndex(grid, 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 != TERRAIN_STATUS_NO_AMSL_DATA){ + 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 != TERRAIN_STATUS_NO_AMSL_DATA){ + 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_AMSL_DATA; + } + + return terrainHeight.lastUpdate + TERRAIN_NO_DATA_DELAY_MS < millis() ? TERRAIN_STATUS_NO_AMSL_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_AGL_DATA; + } + + return terrainHeight.lastUpdate + TERRAIN_NO_DATA_DELAY_MS < millis() ? TERRAIN_STATUS_NO_AGL_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..795bbb3a07e --- /dev/null +++ b/src/main/terrain/terrain.h @@ -0,0 +1,166 @@ +/* + * 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_TER_FILE_FORMAT_VERSION 50 +#define TERRAIN_SPACING 30 + +// Sentinel returned by the terrain getters when no valid height/distance is +// available. It must lie outside the range of any real altitude or distance: +// terrain can legitimately be negative (below sea level), so INT32_MIN is used +// to guarantee it never collides with valid data. +#define TERRAIN_STATUS_NO_AMSL_DATA (INT32_MIN) +#define TERRAIN_STATUS_NO_AGL_DATA (-1) + +// 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) +// heights are packed 10 bits each, so the whole 32x28 grid needs +// TERRAIN_GRID_BLOCK_SIZE_X * TERRAIN_GRID_BLOCK_SIZE_Y * 10 / 8 bytes. +// Integer math keeps this a valid array dimension (896 values -> 1120 bytes). +#define TERRAIN_GRID_BLOCK_DATA_SIZE ((TERRAIN_GRID_BLOCK_SIZE_X * TERRAIN_GRID_BLOCK_SIZE_Y * 10) / 8) +// each 10-bit height offset is stored at 2 m resolution, so a raw code of +// 0..1023 maps to 0..2046 m above heightBase +#define TERRAIN_HEIGHT_OFFSET_RESOLUTION_M 2 + +#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 + GRID_CACHE_READING = 3, // disk read in flight, block content is partially overwritten +}; + +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, stored as 10-bit offsets from + // heightBase, little-endian bit-packed (4 values / 5 bytes). Use + // getHeightOffsetByIndex() to decode a single value. + uint8_t heightOffset[TERRAIN_GRID_BLOCK_DATA_SIZE]; + int16_t heightBase; + + // 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; + + +/* + 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; + // lat/lon this slot is expected to hold, captured when entering + // GRID_CACHE_DISKWAIT. The disk read overwrites gridBlock (including its + // lat/lon), so a READING slot can only be identified by these fields + int32_t expectedLat; + int32_t expectedLon; +} 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..8d9d1d7781d --- /dev/null +++ b/src/main/terrain/terrain_io.c @@ -0,0 +1,575 @@ +/* + * 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_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 Acquires exclusive SD card access from blackbox for a single operation. + * + * Access is held only around individual SD operations (open, seek, sector reads) and released + * in between, so blackbox can keep logging while a grid block load is in progress. + * + * @return true if access is held and the operation may proceed, false to retry on the next task call. + */ +static bool acquireSdAccess(void) +{ + if(terrainIoState.sdAccessHeld){ + return true; + } + + //request access from blackbox, returns false while the request is still pending + if(requestToSdCardAccess() == false){ + if(!terrainIoState.sdAccessRequested){ + LOG_DEBUG(TERRAIN, "TERRAIN SD LOCK REQUESTED, PENDING"); + } + terrainIoState.sdAccessRequested = true; + return false; + } + + //for terrain SD operations the card must be in idle + if(!afatfs_isIdle()){ + LOG_DEBUG(TERRAIN, "TERRAIN SD LOCK RELEASED, FS NOT IDLE"); + releaseSdCardAccess(); + terrainIoState.sdAccessRequested = false; + return false; + } + + LOG_DEBUG(TERRAIN, "TERRAIN SD LOCK ACQUIRED"); + terrainIoState.sdAccessRequested = false; + terrainIoState.sdAccessHeld = true; + return true; +} + +/** + * @brief Releases SD card access back to blackbox, also cancels a pending request. + */ +static void releaseSdAccess(void) +{ + if(terrainIoState.sdAccessHeld || terrainIoState.sdAccessRequested){ + LOG_DEBUG(TERRAIN, "TERRAIN SD LOCK RELEASED (held=%d requested=%d)", terrainIoState.sdAccessHeld, terrainIoState.sdAccessRequested); + releaseSdCardAccess(); + } + terrainIoState.sdAccessHeld = false; + terrainIoState.sdAccessRequested = false; +} + +/** + * @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(TERRAIN, "TERRAIN HARD FAILURE, STATE MACHINE DISABLED"); + releaseSdAccess(); +} + +/** + * @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(TERRAIN, "TERRAIN RESET STATE -> IDLE"); + releaseSdAccess(); +} + +/** + * @brief Finishes a successful block read, keeping the .DAT file open for subsequent reads. + * + * Unlike resetStateMachine() the file handle stays open, so the next block from the same + * tile skips the expensive directory scan in afatfs_fopen. + */ +static void finishGridBlockRead(void) +{ + terrainIoState.status = TERRAIN_IO_IDLE; + terrainIoState.gridBlock = NULL; + terrainIoState.bytesRead = 0; + terrainIoState.readsZeroBytesCount = 0; + terrainIoState.lastReadActivityMs = millis(); + + LOG_DEBUG(TERRAIN, "TERRAIN READ FINISHED -> IDLE (file kept open)"); + releaseSdAccess(); +} + +/** + * @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; + } + + if(terrainIoState.datFile != NULL){ + LOG_DEBUG(TERRAIN, "TERRAIN CLEANUP -> CLOSE"); + terrainIoState.status = TERRAIN_IO_CLOSE; + } else { + LOG_DEBUG(TERRAIN, "TERRAIN CLEANUP, NO FILE, 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(TERRAIN, "TERRAIN OPEN CALLBACK, FILE NULL, RESET STATE"); + setGridStatus(terrainIoState.gridBlock, GRID_CACHE_INVALID); + increaseFileStatusErrorCount(terrainIoState.gridBlock->latDegrees, terrainIoState.gridBlock->lonDegrees); + resetStateMachine(); + return; + } + + terrainIoState.status = TERRAIN_IO_SEEK; + terrainIoState.datFile = file; + terrainIoState.openFileLatDegrees = terrainIoState.gridBlock->latDegrees; + terrainIoState.openFileLonDegrees = terrainIoState.gridBlock->lonDegrees; + terrainIoState.lastReadActivityMs = millis(); + + LOG_DEBUG(TERRAIN, "TERRAIN OPEN CALLBACK, FILE OK -> SEEK"); + + //the open (directory scan) is the longest SD operation, give the card back to blackbox before the seek starts + releaseSdAccess(); +} + +/** + * @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(TERRAIN, "TERRAIN CLOSE CALLBACK, FILE CLOSED"); + 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(TERRAIN, "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 + //a kept-open file handle is also invalid once the card went away + if(terrainIoState.status != TERRAIN_IO_IDLE || terrainIoState.datFile != NULL){ + 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){ + //cancel a pending access request so blackbox is not stalled waiting for us + releaseSdAccess(); + + //close the kept-open file when it was not used for a while, frees the file handle + if(terrainIoState.datFile != NULL && millis() - terrainIoState.lastReadActivityMs > TERRAIN_IO_KEEP_OPEN_IDLE_MS){ + LOG_DEBUG(TERRAIN, "TERRAIN IDLE TIMEOUT -> CLOSE"); + terrainIoState.status = TERRAIN_IO_CLOSE; + } + 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(TERRAIN, "TERRAIN TOO MANY OPEN ERRORS, BLOCK INVALID"); + setGridStatus(gridBlock, GRID_CACHE_INVALID); + return; + } + + if(terrainIoState.datFile != NULL){ + //the file for this tile is still open from the previous block, skip the expensive open and go straight to seek + if(terrainIoState.openFileLatDegrees == gridBlock->latDegrees && terrainIoState.openFileLonDegrees == gridBlock->lonDegrees){ + /////////////////////////////////////////////////////////////////// + //request access from blackbox, SD card must be in idle + if(!acquireSdAccess()){ + return; + } + //////////////////////////////////////////////////////////////// + + LOG_DEBUG(TERRAIN, "TERRAIN IDLE, FILE ALREADY OPEN -> SEEK"); + terrainIoState.gridBlock = gridBlock; + terrainIoState.bytesRead = 0; + terrainIoState.readsZeroBytesCount = 0; + terrainIoState.status = TERRAIN_IO_SEEK; + return; + } + + //block is from a different tile, close the old file first; the block stays pending and is picked up again next call + LOG_DEBUG(TERRAIN, "TERRAIN IDLE, DIFFERENT TILE -> CLOSE"); + terrainIoState.status = TERRAIN_IO_CLOSE; + return; + } + + /////////////////////////////////////////////////////////////////// + //request access from blackbox, SD card must be in idle + //access is held for the whole open (directory scan cannot be split), released again in terrainIoOpenedFileCallback + if(!acquireSdAccess()){ + return; + } + //////////////////////////////////////////////////////////////// + + //set grid block for process. open file -> seek -> read + LOG_DEBUG(TERRAIN, "TERRAIN IDLE -> CHANGE_DIR"); + 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()){ + LOG_DEBUG(TERRAIN, "TERRAIN CHANGE_DIR, ALREADY ROOT -> OPEN_FILE"); + terrainIoState.status = TERRAIN_IO_OPEN_FILE; + return; + } + + LOG_DEBUG(TERRAIN, "TERRAIN CHANGE_DIR, CHANGING TO ROOT"); + //change dir to ROOT (null) + if(!afatfs_chdir(NULL)){ + LOG_DEBUG(TERRAIN, "TERRAIN CHANGE_DIR ERROR"); + setGridStatus(terrainIoState.gridBlock, GRID_CACHE_INVALID); + resetStateMachine(); + return; + } + + LOG_DEBUG(TERRAIN, "TERRAIN CHANGE_DIR OK -> OPEN_FILE"); + terrainIoState.status = TERRAIN_IO_OPEN_FILE; + return; + } + /////////////////////////////////////////////////////////////////// + + /////////////////////////////////////////////////////////////////// + /////// OPEN FILE ///////////////////////////////////////////// + // wait to call callback terrainIoOpenedFileCallback + if(terrainIoState.status == TERRAIN_IO_OPEN_FILE){ + if(terrainIoState.gridBlock == NULL){ + LOG_DEBUG(TERRAIN, "TERRAIN OPEN_FILE, GRID BLOCK NULL, RESET STATE"); + setGridStatus(terrainIoState.gridBlock, GRID_CACHE_INVALID); + resetStateMachine(); + return; + } + + terrainIoState.status = TERRAIN_IO_OPEN_FILE_PENDING; + terrainIoState.openFileStartTimeMs = millis(); + + char filename[20]; + snprintf(filename, sizeof(filename), + "%c%02ld%c%03ld.TER", + terrainIoState.gridBlock->latDegrees < 0 ? 'S' : 'N', + labs(terrainIoState.gridBlock->latDegrees), + terrainIoState.gridBlock->lonDegrees < 0 ? 'W' : 'E', + labs(terrainIoState.gridBlock->lonDegrees)); + + LOG_DEBUG(TERRAIN, "TERRAIN OPEN_FILE %s -> OPEN_FILE_PENDING", filename); + + //of most of the time is callback terrainIoOpenedFileCallback called immediately, not in next cycle + if(!afatfs_fopen(filename, "r", terrainIoOpenedFileCallback)){ + LOG_DEBUG(TERRAIN, "TERRAIN OPEN_FILE ERROR"); + setGridStatus(terrainIoState.gridBlock, GRID_CACHE_INVALID); + 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(TERRAIN, "TERRAIN OPEN_FILE_PENDING TIMEOUT, RESET STATE"); + setGridStatus(terrainIoState.gridBlock, GRID_CACHE_INVALID); + 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){ + setGridStatus(terrainIoState.gridBlock, GRID_CACHE_INVALID); + cleanUp(); + return; + } + + /////////////////////////////////////////////////////////////////// + //access was released after the open, request it again for the seek + read + if(!acquireSdAccess()){ + return; + } + //////////////////////////////////////////////////////////////// + + LOG_DEBUG(TERRAIN, "TERRAIN SEEK TO POSITION %d,%d", (int)terrainIoState.gridBlock->grid_idx_x, (int)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 * TERRAIN_GRID_BLOCK_SIZE_SD_CARD; + + if(fileOffset64 > UINT32_MAX) { + setGridStatus(terrainIoState.gridBlock, GRID_CACHE_INVALID); + cleanUp(); + return; + } + + afatfsOperationStatus_e seekState = afatfs_fseek(terrainIoState.datFile, (int32_t)fileOffset64, AFATFS_SEEK_SET); + if(seekState != AFATFS_OPERATION_FAILURE){ + LOG_DEBUG(TERRAIN, "TERRAIN SEEK OK -> READ"); + //the read overwrites the destination block, stash the expected idx for the completion check + terrainIoState.expectedIdxX = terrainIoState.gridBlock->grid_idx_x; + terrainIoState.expectedIdxY = terrainIoState.gridBlock->grid_idx_y; + setGridStatus(terrainIoState.gridBlock, GRID_CACHE_READING); + 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(TERRAIN, "TERRAIN SEEK ERROR"); + setGridStatus(terrainIoState.gridBlock, GRID_CACHE_INVALID); + cleanUp(); + return; + } + ////////////////////////////////////////////////////////////////////// + + ///////////////////////////////////////////////////////////////////////// + /////// READING BLOCK DATA ///////////////////////////////////////////// + if(terrainIoState.status == TERRAIN_IO_READ){ + if(terrainIoState.datFile == NULL || terrainIoState.gridBlock == NULL){ + setGridStatus(terrainIoState.gridBlock, GRID_CACHE_INVALID); + cleanUp(); + return; + } + + /////////////////////////////////////////////////////////////////// + //access is released after every completed sector read, request it again for the next one + if(!acquireSdAccess()){ + return; + } + //////////////////////////////////////////////////////////////// + + uint32_t readNow = afatfs_fread(terrainIoState.datFile, (uint8_t*)terrainIoState.gridBlock + terrainIoState.bytesRead, sizeof(gridBlock_t) - terrainIoState.bytesRead); + terrainIoState.bytesRead += readNow; + + LOG_DEBUG(TERRAIN, "TERRAIN READING DATA %d/%d", (int)terrainIoState.bytesRead, (int)sizeof(gridBlock_t)); + if(terrainIoState.bytesRead == 0 && !(sdcard_isInserted() && sdcard_isFunctional() && afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY)){ + LOG_DEBUG(TERRAIN, "TERRAIN READ, SD CARD FAILURE"); + //the block is left half-overwritten by the partial read, drop it out of READING so the slot is reusable + setGridStatus(terrainIoState.gridBlock, GRID_CACHE_INVALID); + 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 + // a block needs ceil(sizeof(gridBlock_t) / 512) sector reads, asyncfatfs reads 512. so we accept 2 errors per sector for a single reading. + if(terrainIoState.readsZeroBytesCount > ((sizeof(gridBlock_t) + 511) / 512) * 2){ + LOG_DEBUG(TERRAIN, "TERRAIN READ, TOO MANY ZERO READS"); + setGridStatus(terrainIoState.gridBlock, GRID_CACHE_INVALID); + + //we have to increase error for file, for case if error for file reach threshold, and mark file as invalid + //use the open-file tile, the block's own degrees may already be overwritten by the partial read + increaseFileStatusErrorCount(terrainIoState.openFileLatDegrees, terrainIoState.openFileLonDegrees); + cleanUp(); + return; + } + + if(terrainIoState.bytesRead < sizeof(gridBlock_t)){ + //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)) { + LOG_DEBUG(TERRAIN, "TERRAIN READ, UNEXPECTED EOF"); + //if it happens we have to close file and increase error count for file + //use the open-file tile, the block's own degrees may already be overwritten by the partial read + increaseFileStatusErrorCount(terrainIoState.openFileLatDegrees, terrainIoState.openFileLonDegrees); + setGridStatus(terrainIoState.gridBlock, GRID_CACHE_INVALID); + cleanUp(); + return; + } + + //a sector was read, give the card back to blackbox so it can flush before the next sector read + if(readNow > 0){ + releaseSdAccess(); + } + + } else { + if( + terrainIoState.gridBlock->grid_idx_x != terrainIoState.expectedIdxX + || terrainIoState.gridBlock->grid_idx_y != terrainIoState.expectedIdxY + || terrainIoState.gridBlock->crc != getBlockCrc(terrainIoState.gridBlock) + || terrainIoState.gridBlock->version != TERRAIN_TER_FILE_FORMAT_VERSION + || terrainIoState.gridBlock->spacing != TERRAIN_SPACING + + ) { + LOG_DEBUG(TERRAIN, "TERRAIN READ, BLOCK VALIDATION FAILED"); + setGridStatus(terrainIoState.gridBlock, GRID_CACHE_INVALID); + cleanUp(); + return; + } + + setGridStatus(terrainIoState.gridBlock, GRID_CACHE_VALID); + + //keep the file open, next block from the same tile skips the expensive open + finishGridBlockRead(); + return; + } + } + ////////////////////////////////////////////////////////////////////// + + ///////////////////////////////////////////////////////////////////////// + /////// CLOSING BLOCK DATA ///////////////////////////////////////////// + // wait to call callback terrainIoClosedFileCallback + if(terrainIoState.status == TERRAIN_IO_CLOSE){ + if(terrainIoState.datFile == NULL) { + resetStateMachine(); + return; + } + LOG_DEBUG(TERRAIN, "TERRAIN CLOSE -> CLOSE_PENDING"); + terrainIoState.status = TERRAIN_IO_CLOSE_PENDING; + if(!afatfs_fclose(terrainIoState.datFile, terrainIoClosedFileCallback)){ + LOG_DEBUG(TERRAIN, "TERRAIN CLOSE ERROR, RESET STATE"); + 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..b529892f72e --- /dev/null +++ b/src/main/terrain/terrain_io.h @@ -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/. + */ + +#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 +#define TERRAIN_IO_KEEP_OPEN_IDLE_MS 60000 //close the kept-open .DAT file after this long without any block read + +#define TERRAIN_GRID_BLOCK_SIZE_SD_CARD 2048 + +/** + * @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; + afatfsFilePtr_t datFile; + terrainIoFileOpenStatus_t fileOpenStatus[TERRAIN_IO_MAX_FILE_OPEN_STATUS]; + uint32_t bytesRead; + uint32_t readsZeroBytesCount; + uint16_t expectedIdxX; //grid_idx of the block being read, stashed before the direct read overwrites the destination + uint16_t expectedIdxY; + timeMs_t openFileStartTimeMs; + int8_t openFileLatDegrees; //tile of the currently open datFile, valid only while datFile != NULL + int16_t openFileLonDegrees; + timeMs_t lastReadActivityMs; //last time a block was read from datFile, used for the keep-open idle timeout + bool sdAccessHeld; //we currently hold exclusive SD card access granted by blackbox + bool sdAccessRequested; //a request to blackbox is pending and must be cancelled if we stop needing it +} terrainIoState_t; + + + +void loadGridToCacheTask(timeUs_t currentTimeUs); +void terrainIoOpenedFileCallback(afatfsFilePtr_t file); +void terrainIoClosedFileCallback(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_nav.c b/src/main/terrain/terrain_nav.c new file mode 100644 index 00000000000..2a98f955f10 --- /dev/null +++ b/src/main/terrain/terrain_nav.c @@ -0,0 +1,190 @@ +/* + * 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 + +#include "platform.h" + +#ifdef USE_TERRAIN + +#include "terrain.h" +#include "terrain_utils.h" +#include "terrain_io.h" +#include "terrain_location.h" +#include "terrain_nav.h" + +#include "io/gps.h" +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +// The module's grid spacing is fixed at 30 m (terrain_utils.c); sampling at +// the same step never skips a grid cell along the path +#define TERRAIN_NAV_STEP_M 30.0f + +// Bound the work (and cache pressure) of a single lookahead call: 64 samples +// at 30 m cover ~1.9 km ahead, well beyond a practical climb-planning horizon +#define TERRAIN_NAV_LOOKAHEAD_MAX_SAMPLES 64 + +bool terrainNavIsHealthy(void) +{ + if (!terrainConfig()->terrainEnabled || isTerrainIoFailure()) { + return false; + } + + // terrainGetLastAMSL returns its no-data sentinel unless the home anchor + // is latched AND the last update is fresher than TERRAIN_NO_DATA_DELAY_MS + return terrainGetLastAMSL() != TERRAIN_STATUS_NO_AMSL_DATA; +} + +bool terrainNavGetAGLCm(int32_t *aglCm) +{ + if (!terrainNavIsHealthy()) { + return false; + } + + const int32_t agl = terrainGetLastDistanceCm(); + if (agl == TERRAIN_STATUS_NO_AGL_DATA) { + return false; + } + + *aglCm = agl; + return true; +} + +bool terrainNavGetHeightAtLocation(const gpsLocation_t *loc, float *heightM) +{ + if (!terrainConfig()->terrainEnabled || isTerrainIoFailure()) { + return false; + } + + gridInfo_t info; + calculateGridInfo(loc, &info); + + // the +1 neighbours used for interpolation must lie inside the same block + if (info.idx_x > TERRAIN_GRID_BLOCK_SIZE_X - 2 || info.idx_y > TERRAIN_GRID_BLOCK_SIZE_Y - 2) { + return false; + } + + gridCache_t *cache = findGridCache(&info); + if (cache == NULL) { + return false; + } + gridBlock_t *grid = &cache->gridBlock; + + // all four surrounding grid points must have been loaded from the 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 false; + } + + const int16_t h00 = grid->heightBase + TERRAIN_HEIGHT_OFFSET_RESOLUTION_M * getHeightOffsetByIndex(grid, info.idx_x, info.idx_y); + const int16_t h01 = grid->heightBase + TERRAIN_HEIGHT_OFFSET_RESOLUTION_M * getHeightOffsetByIndex(grid, info.idx_x, info.idx_y + 1); + const int16_t h10 = grid->heightBase + TERRAIN_HEIGHT_OFFSET_RESOLUTION_M * getHeightOffsetByIndex(grid, info.idx_x + 1, info.idx_y); + const int16_t h11 = grid->heightBase + TERRAIN_HEIGHT_OFFSET_RESOLUTION_M * getHeightOffsetByIndex(grid, info.idx_x + 1, info.idx_y + 1); + + 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; + + *heightM = (1.0f - info.frac_y) * avg1 + info.frac_y * avg2; + return true; +} + +bool terrainNavGetHomeTerrainHeight(float *heightM) +{ + if (!posControl.gpsOrigin.valid) { + return false; + } + + const gpsLocation_t homeLoc = { + .lat = posControl.gpsOrigin.lat, + .lon = posControl.gpsOrigin.lon, + .alt = 0, + }; + + return terrainNavGetHeightAtLocation(&homeLoc, heightM); +} + +bool terrainNavLookahead(float bearingDeg, float distanceM, float climbRatio, float escapeRatio, terrainNavLookaheadResult_t *result) +{ + if (result == NULL || !terrainNavIsHealthy()) { + return false; + } + + result->climbNeededM = 0.0f; + result->escapeDeficitM = 0.0f; + result->samplesTotal = 0; + result->samplesMissed = 0; + result->complete = true; + + gpsLocation_t pos = gpsSol.llh; + float baseHeightM; + if (!terrainNavGetHeightAtLocation(&pos, &baseHeightM)) { + return false; + } + + const float bearingRad = bearingDeg * DEG2RAD; + const float stepNorthM = cosf(bearingRad) * TERRAIN_NAV_STEP_M; + const float stepEastM = sinf(bearingRad) * TERRAIN_NAV_STEP_M; + const float climbPerStepM = climbRatio * TERRAIN_NAV_STEP_M; + const float escapePerStepM = escapeRatio * TERRAIN_NAV_STEP_M; + + float achievableClimbM = 0.0f; + float escapeClimbM = 0.0f; + + for (float travelledM = TERRAIN_NAV_STEP_M; travelledM <= distanceM; travelledM += TERRAIN_NAV_STEP_M) { + if (result->samplesTotal >= TERRAIN_NAV_LOOKAHEAD_MAX_SAMPLES) { + break; + } + + offsetLatlng(&pos, stepNorthM, stepEastM); + achievableClimbM += climbPerStepM; + escapeClimbM += escapePerStepM; + result->samplesTotal++; + + float heightM; + if (terrainNavGetHeightAtLocation(&pos, &heightM)) { + const float riseM = heightM - baseHeightM; + const float deficitM = riseM - achievableClimbM; + if (deficitM > result->climbNeededM) { + result->climbNeededM = deficitM; + } + const float escapeM = riseM - escapeClimbM; + if (escapeM > result->escapeDeficitM) { + result->escapeDeficitM = escapeM; + } + } else { + result->samplesMissed++; + } + } + + result->complete = (result->samplesMissed == 0); + return true; +} + +#endif diff --git a/src/main/terrain/terrain_nav.h b/src/main/terrain/terrain_nav.h new file mode 100644 index 00000000000..6d259e68cf1 --- /dev/null +++ b/src/main/terrain/terrain_nav.h @@ -0,0 +1,70 @@ +/* + * 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 "io/gps.h" + +/* + * Navigation-facing terrain API. + * + * Contract for every function here: success/failure is an explicit boolean, + * values are returned through out-parameters and are written ONLY on success. + * No caller ever sees a sentinel, a stale value or a substituted value. + */ + +typedef struct { + float climbNeededM; // extra climb needed now to clear terrain along the path (>= 0) + float escapeDeficitM; // worst height deficit vs the escapeRatio slope (>= 0) - the escape test's raw input + uint8_t samplesTotal; // points sampled along the path + uint8_t samplesMissed; // points that had no terrain data (loads were scheduled) + bool complete; // true when every sampled point had terrain data +} terrainNavLookaheadResult_t; + +// True only while terrain data is usable: module enabled, no IO failure +// latched, home anchor found and data fresher than TERRAIN_NO_DATA_DELAY_MS +bool terrainNavIsHealthy(void); + +// Height above ground at the current position, in cm +bool terrainNavGetAGLCm(int32_t *aglCm); + +// Terrain height (map datum, meters) at an arbitrary location. On a cache +// miss the block is scheduled for loading and the call fails - retry later. +bool terrainNavGetHeightAtLocation(const gpsLocation_t *loc, float *heightM); + +// Terrain height (map datum, meters) at the GPS origin (home) +bool terrainNavGetHomeTerrainHeight(float *heightM); + +// Walk the map from the current position along bearingDeg for distanceM, +// accumulating achievable climb (climbRatio = m of climb per m travelled), +// and report the worst height deficit found. A descending path (negative +// climbRatio) correctly increases the reported deficit. Sampled blocks not +// yet cached are scheduled for loading and counted in samplesMissed. +// One walk, two slopes: climbNeededM is the deficit vs climbRatio (the +// early-climb demand model), escapeDeficitM the deficit vs escapeRatio (the +// full-capability escape test) - same samples, no extra SD load +bool terrainNavLookahead(float bearingDeg, float distanceM, float climbRatio, float escapeRatio, terrainNavLookaheadResult_t *result); diff --git a/src/main/terrain/terrain_nav_hold.c b/src/main/terrain/terrain_nav_hold.c new file mode 100644 index 00000000000..147603f2af5 --- /dev/null +++ b/src/main/terrain/terrain_nav_hold.c @@ -0,0 +1,292 @@ +/* + * 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 "common/maths.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "drivers/time.h" + +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" + +#include "flight/imu.h" +#include "flight/pid.h" + +#include "io/gps.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#include "terrain/terrain_nav.h" +#include "terrain/terrain_nav_hold.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(terrainNavConfig_t, terrainNavConfig, PG_TERRAIN_NAV_CONFIG, 0); + +PG_RESET_TEMPLATE(terrainNavConfig_t, terrainNavConfig, + .minAglCm = 6000, // 60 m: worst-case map error in steep terrain + grid blind spots + canopy + .lookaheadDistM = 1000, +); + +// Terrain queries run at this interval; the block budget below is defined +// per query cycle (the cache starves when a cycle exceeds it - C-9) +#define TERRAIN_NAV_HOLD_QUERY_INTERVAL_MS 100 + +// Worst-case meters of straight flight per newly touched cache block: block +// stride is 720 x 840 m ground (24 x 28 points at 30 m), the worst bearing +// crosses a block boundary every 1/sqrt(1/720^2 + 1/840^2) = 546 m +#define TERRAIN_NAV_HOLD_M_PER_BLOCK_WORST 540 + +// Below this ground speed the course over ground is too noisy for a lookahead +#define TERRAIN_NAV_HOLD_MIN_LOOKAHEAD_SPEED_CM_S 300 + +// Time horizon of the lookahead scan: distance is additionally capped at +// groundspeed x this many seconds. Fixes both ends of the fixed-distance +// problem (LOG00010 analysis, docs/01 "Task 2"): slow flight stops climbing +// for peaks a minute away, and the escape test inherently reasons in time. +// At full nav_fw_auto_climb_rate the aircraft gains more height over this +// horizon than the largest demand ever seen in the logs +#define TERRAIN_NAV_HOLD_TIME_HORIZON_S 35 + +static terrainNavHoldState_t holdState; +static terrainNavHoldOutput_t holdOutput; + +// Latest terrain answers, refreshed once per query interval +static timeMs_t lastQueryTimeMs; +static bool queryAglValid; +static int32_t queryAglCm; +static bool queryLookaheadValid; +static float queryLookaheadClimbCm; +static float queryEscapeDeficitCm; + +// The hard-disengage conditions: launch, any landing, emergency landing, +// VTOL transition, non-altitude platforms, degraded GPS, rangefinder SURFACE +// mode. The cruise whitelist is the primary defense - this is the explicit +// override on top of it +static bool terrainNavHoldMustDisengage(void) +{ + if (navGetCurrentStateFlags() & (NAV_CTL_LAUNCH | NAV_CTL_LAND | NAV_CTL_EMERG | NAV_MIXERAT)) { + return true; + } + + if (FLIGHT_MODE(NAV_LAUNCH_MODE) || FLIGHT_MODE(NAV_FW_AUTOLAND)) { + return true; + } + + if (!STATE(ALTITUDE_CONTROL)) { + return true; + } + + if (!STATE(GPS_FIX)) { + return true; + } + +#ifdef USE_GPS_FIX_ESTIMATION + if (STATE(GPS_ESTIMATED_FIX)) { + return true; + } +#endif + + // Never stack with the rangefinder surface mode + if (posControl.flags.isTerrainFollowEnabled) { + return true; + } + + return false; +} + +static bool terrainNavHoldEligible(void) +{ + if (!ARMING_FLAG(ARMED) || !STATE(AIRPLANE)) { + return false; + } + + if (!IS_RC_MODE_ACTIVE(BOXTERRAINAGLHOLD)) { + return false; + } + + // The whitelist: 3D cruise only + if (posControl.navState != NAV_STATE_CRUISE_IN_PROGRESS && posControl.navState != NAV_STATE_CRUISE_ADJUSTING) { + return false; + } + + return !terrainNavHoldMustDisengage(); +} + +static void terrainNavHoldRunQueries(timeMs_t currentTimeMs) +{ + lastQueryTimeMs = currentTimeMs; + + queryAglValid = terrainNavGetAGLCm(&queryAglCm); + queryLookaheadValid = false; + + // No course to scan along: lookahead disabled by config, too slow for a + // usable course over ground, or the heading estimate itself is invalid + // (the cog would be stale - never scan a direction we cannot trust) + if (terrainNavConfig()->lookaheadDistM == 0 || gpsSol.groundSpeed < TERRAIN_NAV_HOLD_MIN_LOOKAHEAD_SPEED_CM_S || !isImuHeadingValid()) { + return; + } + + // Global block budget per query cycle: cache size minus 2 (C-9). The AGL + // query above uses the current block, so the lookahead may touch at most + // cacheSize - 3 new blocks - its distance is capped accordingly + const int32_t lookaheadBudgetM = (TERRAIN_GRID_BLOCK_CACHE_SIZE - 3) * TERRAIN_NAV_HOLD_M_PER_BLOCK_WORST; + float lookaheadDistM = MIN((int32_t)terrainNavConfig()->lookaheadDistM, lookaheadBudgetM); + + // Conservative achievable climb slope: half the configured climb rate at + // the current ground speed - underestimating it makes climbs start early + const float groundSpeedCmS = MAX(gpsSol.groundSpeed, 800); + const float climbRatio = 0.5f * navConfig()->fw.max_auto_climb_rate / groundSpeedCmS; + + // Time cap on the scan (the fixed-distance fix): never look further ahead + // than the aircraft flies in the time horizon + lookaheadDistM = MIN(lookaheadDistM, groundSpeedCmS * 0.01f * TERRAIN_NAV_HOLD_TIME_HORIZON_S); + + // The escape slope: the FULL configured climb rate at the current ground + // speed - a deficit against it means "even climbing at maximum from this + // moment, that point ahead arrives below the minimum" + const float escapeRatio = (float)navConfig()->fw.max_auto_climb_rate / groundSpeedCmS; + + const float bearingDeg = CENTIDEGREES_TO_DEGREES((float)posControl.actualState.cog); + + terrainNavLookaheadResult_t lookahead; + if (terrainNavLookahead(bearingDeg, lookaheadDistM, climbRatio, escapeRatio, &lookahead)) { + // A partial answer (samples missed at the far end) is still a valid + // lower bound; the missed blocks are already scheduled for loading + queryLookaheadValid = true; + queryLookaheadClimbCm = lookahead.climbNeededM * 100.0f; + queryEscapeDeficitCm = lookahead.escapeDeficitM * 100.0f; + } +} + +void terrainNavCruiseHoldUpdate(void) +{ + const timeMs_t currentTimeMs = millis(); + + terrainNavHoldInput_t in = { 0 }; + in.nowMs = currentTimeMs; + in.eligible = terrainNavHoldEligible(); + + if (!in.eligible) { + // Resets the core state; cruise and every other mode fly as stock + terrainNavHoldCoreUpdate(&holdState, &in, &holdOutput); + return; + } + + if (currentTimeMs - lastQueryTimeMs >= TERRAIN_NAV_HOLD_QUERY_INTERVAL_MS) { + terrainNavHoldRunQueries(currentTimeMs); + } + + in.stickAdjusting = posControl.flags.isAdjustingAltitude; + in.healthy = terrainNavIsHealthy(); + in.aglValid = queryAglValid; + in.aglCm = queryAglCm; + in.currentZCm = navGetCurrentActualPositionAndVelocity()->pos.z; + in.lookaheadValid = queryLookaheadValid; + in.lookaheadClimbCm = queryLookaheadClimbCm; + in.escapeDeficitValid = queryLookaheadValid; + in.escapeDeficitCm = queryEscapeDeficitCm; + in.lookaheadDegraded = terrainNavConfig()->lookaheadDistM != 0 && !isImuHeadingValid(); + in.minAglCm = terrainNavConfig()->minAglCm; + in.maxAltCm = navConfig()->general.max_altitude; + + // Handover blend inputs. The blend seed is the aircraft's real vertical + // speed, bounded by the same authority limit as every hold command. The + // stick's wish is read back from the funnel - the stock stick writer ran + // earlier this same cycle (fc_core.c ordering) - with the same manual + // clamp getDesiredClimbRate applies to it + in.actualClimbRateCmS = constrainf(navGetCurrentActualPositionAndVelocity()->vel.z, + -(float)navConfig()->fw.max_auto_climb_rate, + (float)navConfig()->fw.max_auto_climb_rate); + if (in.stickAdjusting && posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) { + in.stickWishValid = true; + in.stickWishCmS = constrainf(posControl.desiredState.climbRateDemand, + -(float)navConfig()->fw.max_manual_climb_rate, + (float)navConfig()->fw.max_manual_climb_rate); + // Near-full pull = the pilot's vertical channel is maxed (for the + // red-text reserve question; 90% leaves room for stick noise) + in.stickFullPull = in.stickWishCmS >= 0.9f * (float)navConfig()->fw.max_manual_climb_rate; + } + // Which side holds the unused climb reserve, from the two stock rates + in.pilotHasMoreClimb = navConfig()->fw.max_manual_climb_rate > navConfig()->fw.max_auto_climb_rate; + in.autoBeatsManual = navConfig()->fw.max_auto_climb_rate > navConfig()->fw.max_manual_climb_rate; + + terrainNavHoldCoreUpdate(&holdState, &in, &holdOutput); + + if (holdOutput.writeTarget) { + // The one gate into the altitude target path. The funnel enforces the + // slew limit (climb rate) and the nav_max_altitude clamp downstream + updateClimbRateToAltitudeController(navConfig()->fw.max_auto_climb_rate, holdOutput.targetZCm, ROC_TO_ALT_TARGET); + } else if (holdOutput.writeRate) { + // Handover blend: a signed climb rate expressed through the funnel's + // own target math (rate = response_factor * altitude_error / 100), + // so the command is smooth in both directions and the ceiling clamp + // still applies downstream. The rate itself is already bounded by + // max_auto_climb_rate through the blend inputs above + const float responseFactor = MAX(pidProfile()->fwAltControlResponseFactor, 1); + const float blendTargetZCm = navGetCurrentActualPositionAndVelocity()->pos.z + + holdOutput.rateCmS * 100.0f / responseFactor; + updateClimbRateToAltitudeController(navConfig()->fw.max_auto_climb_rate, blendTargetZCm, ROC_TO_ALT_TARGET); + } +} + +terrainNavHoldStatus_e terrainNavHoldGetStatus(void) +{ + return holdState.status; +} + +terrainNavHoldWarning_e terrainNavHoldGetWarning(void) +{ + return holdOutput.warning; +} + +bool terrainNavHoldAutoClimbRunning(void) +{ + return holdOutput.autoClimbRunning; +} + +bool terrainNavHoldIsEngaged(void) +{ + return holdState.status == TERRAIN_NAV_HOLD_ACTIVE || holdState.status == TERRAIN_NAV_HOLD_FROZEN; +} + +bool terrainNavHoldGetTargetAglCm(int32_t *targetAglCm) +{ + if (!terrainNavHoldIsEngaged()) { + return false; + } + + *targetAglCm = holdState.targetAglCm; + return true; +} + +#endif diff --git a/src/main/terrain/terrain_nav_hold.h b/src/main/terrain/terrain_nav_hold.h new file mode 100644 index 00000000000..65dc2783422 --- /dev/null +++ b/src/main/terrain/terrain_nav_hold.h @@ -0,0 +1,65 @@ +/* + * 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 "config/parameter_group.h" + +#include "terrain/terrain_nav_hold_core.h" + +/* + * TERRAIN AGL HOLD - cruise holds height above ground from SD terrain data. + * + * The single gate for map terrain into the altitude target path: one update + * per navigation cycle, active only in the cruise whitelist, opt-in by RC box, + * health-gated, all commands through the one altitude-target funnel + * (updateClimbRateToAltitudeController). Everything else in the firmware + * behaves bit-for-bit as stock when the box is off. + * + * Not to be confused with INAV's existing "terrain follow" naming, which is + * the rangefinder SURFACE mode. + */ + +typedef struct terrainNavConfig_s { + uint16_t minAglCm; // minimum held height above ground [cm] + uint16_t lookaheadDistM; // how far ahead rising terrain is checked [m], 0 = off +} terrainNavConfig_t; + +PG_DECLARE(terrainNavConfig_t, terrainNavConfig); + +// The gate. Called once per navigation cycle from applyWaypointNavigationAndAltitudeHold() +void terrainNavCruiseHoldUpdate(void); + +terrainNavHoldStatus_e terrainNavHoldGetStatus(void); +terrainNavHoldWarning_e terrainNavHoldGetWarning(void); +bool terrainNavHoldAutoClimbRunning(void); + +// True while the hold owns the altitude target (active or frozen) +bool terrainNavHoldIsEngaged(void); + +// The held height above ground; false unless engaged +bool terrainNavHoldGetTargetAglCm(int32_t *targetAglCm); diff --git a/src/main/terrain/terrain_nav_hold_core.c b/src/main/terrain/terrain_nav_hold_core.c new file mode 100644 index 00000000000..db755ec40bb --- /dev/null +++ b/src/main/terrain/terrain_nav_hold_core.c @@ -0,0 +1,490 @@ +/* + * 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 "terrain_nav_hold_core.h" + +void terrainNavHoldCoreReset(terrainNavHoldState_t *state) +{ + state->status = TERRAIN_NAV_HOLD_INACTIVE; + state->targetAglCm = 0; + state->usableSinceMs = 0; + state->lastUsableMs = 0; + state->reCapturePending = false; + state->minAglReached = false; + state->climbBestAglCm = 0; + state->pullUpActive = false; + state->pullUpRatchet = false; + state->pullUpCapFight = false; + state->uncoverCapFight = false; + state->stickWasAdjusting = false; + state->blendActive = false; + state->blendRateCmS = 0.0f; + state->blendStartMs = 0; + state->blendLastMs = 0; + state->stickQuietSinceMs = 0; + state->terrainAheadActive = false; + state->escapeBadSinceMs = 0; + state->escapeClearSinceMs = 0; + state->escapeDeepFail = false; +} + +// TERRAIN AHEAD - the predictive escape test. Fails when even the FULL +// configured climb rate, applied from this moment, arrives below the +// minimum AGL at some point along the course ahead ("at this speed, this +// slope will beat you"). Below the minimum, closeness alone is not a +// forward threat - AUTO CLIMB and PULL UP own that situation - so only the +// terrain-relative deficit counts there; above the minimum the altitude +// cushion legitimately buys escape headroom and stays in the sum. +// Persistence keeps single noisy samples silent; clearing needs the test +// passing AND the aircraft at/above the minimum, both sustained - normally +// the pilot turning away (the scan follows the course), slowing or +// climbing. A threat that returns simply re-fires the series. +// Without a trusted view ahead the alarm cannot exist - the reactive floor +// alarm stands alone and TERRAIN LOOKAHEAD OFF explains why +static void updateEscapeAlarm(terrainNavHoldState_t *state, const terrainNavHoldInput_t *in) +{ + if (!in->escapeDeficitValid) { + state->terrainAheadActive = false; + state->escapeBadSinceMs = 0; + state->escapeClearSinceMs = 0; + state->escapeDeepFail = false; + state->uncoverCapFight = false; + return; + } + + // Altitude cushion above the minimum offsets the deficit; below the + // minimum the same term would fire the alarm over flat ground, so + // there it is clamped out and only a real forward deficit remains + float cushionCm = in->minAglCm - in->aglCm; + if (cushionCm > 0.0f) { + cushionCm = 0.0f; + } + const float shortfallCm = in->escapeDeficitCm + cushionCm; + + // Depth of the failure, stashed for the floor alarm's prospective + // upgrade: only a shortfall this deep proves the vertical channel + // truly exhausted (a marginal map-step deficit must not birth TURN AWAY) + state->escapeDeepFail = (shortfallCm >= TERRAIN_NAV_HOLD_ESCAPE_DEEP_CM); + + // A red that ran against the ceiling clamp just cleared: a latched + // caution pointing at sky the aircraft has already escaped must not + // linger - if the escape test passes RIGHT NOW it drops immediately. + // Every other uncover keeps the sustained clear below (anti-flicker) + if (state->uncoverCapFight) { + state->uncoverCapFight = false; + if (state->terrainAheadActive && shortfallCm <= 0.0f) { + state->terrainAheadActive = false; + state->escapeBadSinceMs = 0; + state->escapeClearSinceMs = 0; + return; + } + } + + if (shortfallCm > 0.0f) { + state->escapeClearSinceMs = 0; + if (state->escapeBadSinceMs == 0) { + state->escapeBadSinceMs = (in->nowMs != 0) ? in->nowMs : 1; + } + if (in->nowMs - state->escapeBadSinceMs >= TERRAIN_NAV_HOLD_ESCAPE_PERSIST_MS) { + state->terrainAheadActive = true; + } + } else { + state->escapeBadSinceMs = 0; + if (in->aglCm >= in->minAglCm) { + if (state->escapeClearSinceMs == 0) { + state->escapeClearSinceMs = (in->nowMs != 0) ? in->nowMs : 1; + } + if (in->nowMs - state->escapeClearSinceMs >= TERRAIN_NAV_HOLD_ESCAPE_CLEAR_MS) { + state->terrainAheadActive = false; + } + } else { + // Passing but still below the minimum: not clear yet - the + // aircraft is not where it should be until the minimum is back + state->escapeClearSinceMs = 0; + } + } +} + +// Handover blend: one continuous writer across the whole stick episode. +// Seeded from the aircraft's real climb rate at the grab (zero step by +// construction), it chases the wish - the stick's clamped demand while +// deflected, zero after release (the stock release semantics) - and goes +// silent once close enough or past the lifetime bound. While it runs the +// caller commands out->rateCmS; the stock writer takes over seamlessly after +static void runBlend(terrainNavHoldState_t *state, const terrainNavHoldInput_t *in, terrainNavHoldOutput_t *out, float wishCmS) +{ + if (!state->blendActive) { + return; + } + + if (in->nowMs - state->blendStartMs >= TERRAIN_NAV_HOLD_BLEND_MAX_MS) { + state->blendActive = false; + return; + } + + // First-order chase, dt-based so the loop rate does not matter + float alpha = (float)(in->nowMs - state->blendLastMs) / TERRAIN_NAV_HOLD_BLEND_TAU_MS; + if (alpha > 1.0f) { + alpha = 1.0f; + } + state->blendLastMs = in->nowMs; + state->blendRateCmS += (wishCmS - state->blendRateCmS) * alpha; + + const float remaining = wishCmS - state->blendRateCmS; + if (remaining < TERRAIN_NAV_HOLD_BLEND_DONE_CM_S && remaining > -TERRAIN_NAV_HOLD_BLEND_DONE_CM_S) { + state->blendActive = false; + return; + } + + out->writeRate = true; + out->rateCmS = state->blendRateCmS; +} + +// The red text is the honest instruction: PULL UP while unused vertical +// reserve remains (release a push, deepen a pull, or - with manual rate +// above auto - pull at all); TURN AWAY once the vertical channel is +// exhausted and still losing, or clamped by the ceiling. The reserve +// question at the moment height is being LOST: +// - hands off: the hold already commands the full auto rate, so reserve +// exists only when a manual pull outclimbs it (manual > auto) +// - full pull: the pilot's channel is maxed, so reserve exists only when +// releasing hands the climb to a FASTER autopilot (auto > manual) +// - a push or partial pull always leaves reserve (release / pull deeper) +static bool noStickReserve(const terrainNavHoldInput_t *in) +{ + if (!in->stickAdjusting) { + return !in->pilotHasMoreClimb; + } + if (in->stickFullPull) { + return !in->autoBeatsManual; + } + return false; +} + +static terrainNavHoldWarning_e pullUpWarning(const terrainNavHoldState_t *state) +{ + return (state->pullUpRatchet || state->pullUpCapFight) + ? TERRAIN_NAV_HOLD_WARN_TURN_AWAY + : TERRAIN_NAV_HOLD_WARN_PULL_UP; +} + +// Floor alarm: the red must fire on real AGL, ceiling or not. Once the +// minimum has been reached, dropping below (min - margin) latches the alarm +// and only climbing back to the minimum clears it. During the initial +// automatic climb the aircraft is legitimately below the minimum, so there +// the alarm fires only when height above terrain is actually being LOST +// (terrain outclimbing the aircraft), not merely still low +static void updateFloorAlarm(terrainNavHoldState_t *state, const terrainNavHoldInput_t *in) +{ + if (in->aglCm >= in->minAglCm) { + state->minAglReached = true; + // The uncover moment: a red that ran against the ceiling clamp just + // cleared - the escape alarm may drop its stale caution this cycle + if (state->pullUpActive && state->pullUpCapFight) { + state->uncoverCapFight = true; + } + state->pullUpActive = false; + state->pullUpRatchet = false; + state->pullUpCapFight = false; + return; + } + + if (state->minAglReached) { + if (in->aglCm < in->minAglCm - TERRAIN_NAV_HOLD_PULLUP_HYST_CM) { + if (!state->pullUpActive) { + state->pullUpActive = true; + // The breach fight starts: track its best AGL to detect a + // losing climb (the field is otherwise idle in this phase) + state->climbBestAglCm = in->aglCm; + } + } + if (state->pullUpActive) { + if (in->aglCm > state->climbBestAglCm) { + state->climbBestAglCm = in->aglCm; + } else if (in->aglCm < state->climbBestAglCm - TERRAIN_NAV_HOLD_PULLUP_HYST_CM + && noStickReserve(in)) { + // Still losing height with nothing left to give vertically + state->pullUpRatchet = true; + } + } + } else { + if (in->aglCm > state->climbBestAglCm) { + state->climbBestAglCm = in->aglCm; + } else if (in->aglCm < state->climbBestAglCm - TERRAIN_NAV_HOLD_PULLUP_HYST_CM) { + // The red fires regardless of cause (a pilot push below the + // margin re-arms it too - sealed semantics); TURN AWAY only + // when no vertical reserve remains + state->pullUpActive = true; + if (noStickReserve(in)) { + state->pullUpRatchet = true; + } + } + } + + // Prospective upgrade, any red, any moment: a latched TERRAIN AHEAD has + // already proven that even the FULL climb rate loses the path ahead - + // releasing or pulling cannot save the margin, so the red must not + // promise it. Born-red-under-caution starts as TURN AWAY outright. + // Only a DEEP shortfall qualifies: a marginal deficit stays PULL UP + // (and escalates within a second if a real wall deepens it) + if (state->pullUpActive && state->terrainAheadActive && state->escapeDeepFail) { + state->pullUpRatchet = true; + } +} + +// Every capture (engagement or stick release) starts a fresh floor-alarm +// phase: capturing at/above the minimum arms the alarm immediately, below it +// the automatic climb to the minimum begins +static void startCapture(terrainNavHoldState_t *state, const terrainNavHoldInput_t *in) +{ + state->targetAglCm = (in->aglCm > in->minAglCm) ? in->aglCm : in->minAglCm; + state->reCapturePending = false; + state->minAglReached = false; + state->climbBestAglCm = in->aglCm; + // A fresh capture hands a below-minimum situation to the automatic climb + // (info message): the red alarm means a live threat only, and returns via + // the losing-ground ratchet, a pilot push below the margin, or a breached + // floor - never just for starting low + state->pullUpActive = false; + state->pullUpRatchet = false; + state->pullUpCapFight = false; + updateFloorAlarm(state, in); +} + +static void computeTarget(terrainNavHoldState_t *state, const terrainNavHoldInput_t *in, terrainNavHoldOutput_t *out) +{ + // Hold: climb or descend by exactly the difference between the height + // above ground we have and the one we are holding + float targetZCm = in->currentZCm + (state->targetAglCm - in->aglCm); + + // Lookahead: if terrain ahead demands more, climb early so the worst + // point along the path still clears the minimum AGL + if (in->lookaheadValid && in->lookaheadClimbCm > 0.0f) { + const float lookaheadZCm = in->currentZCm + (in->lookaheadClimbCm + in->minAglCm - in->aglCm); + if (lookaheadZCm > targetZCm) { + targetZCm = lookaheadZCm; + } + } + + // Warning ladder, least severe first - a later assignment overrides. + // The lookahead cannot scan ahead (heading estimate invalid) - the + // reactive hold keeps tracking, but the pilot loses the early-climb + // layer and must be told + if (in->lookaheadDegraded) { + out->warning = TERRAIN_NAV_HOLD_WARN_NO_HEADING; + } + // Engaged below the minimum: the hold itself is climbing to the floor - + // information, not an alarm, the pilot has nothing to do. The flag lets + // the OSD alternate this fact under a covering warning: the pilot must + // always see both the danger and the action already being taken + if (!state->minAglReached && in->aglCm < in->minAglCm) { + out->warning = TERRAIN_NAV_HOLD_WARN_AUTO_CLIMB; + out->autoClimbRunning = true; + } + // Ceiling arbitration: nav_max_altitude always wins - the funnel clamps + // the target for real, here we only detect the conflict and warn. Warns + // ahead of the pinch too, because the lookahead demand is in targetZCm + const bool capConflict = (in->maxAltCm > 0 && targetZCm > in->maxAltCm); + if (capConflict) { + out->warning = TERRAIN_NAV_HOLD_WARN_MAX_ALT; + } + // Predictive: the terrain ahead cannot be out-climbed at full rate - + // the pilot has time to act (turn, throttle, slow) and must use it + if (state->terrainAheadActive) { + out->warning = TERRAIN_NAV_HOLD_WARN_TERRAIN_AHEAD; + } + // The floor alarm outranks everything: too low NOW and not recovering. + // Latched against the ceiling clamp the red sticks to TURN AWAY for the + // whole episode - a demand dipping under the cap mid-fight must not + // flip the instruction on screen + if (state->pullUpActive) { + if (capConflict) { + state->pullUpCapFight = true; + } + out->warning = pullUpWarning(state); + } + + out->writeTarget = true; + out->targetZCm = targetZCm; +} + +static void engage(terrainNavHoldState_t *state, const terrainNavHoldInput_t *in, terrainNavHoldOutput_t *out) +{ + // Capture the current AGL: zero commanded movement at engagement. + // Sole exception: below the minimum - the target becomes the minimum and + // the slew-limited funnel turns that into a gentle climb + startCapture(state, in); + state->status = TERRAIN_NAV_HOLD_ACTIVE; + + if (in->stickAdjusting) { + state->reCapturePending = true; // stock stick behavior runs first + return; + } + + computeTarget(state, in, out); +} + +void terrainNavHoldCoreUpdate(terrainNavHoldState_t *state, const terrainNavHoldInput_t *in, terrainNavHoldOutput_t *out) +{ + out->writeTarget = false; + out->targetZCm = 0.0f; + out->writeRate = false; + out->rateCmS = 0.0f; + out->autoClimbRunning = false; + out->warning = TERRAIN_NAV_HOLD_WARN_NONE; + + if (!in->eligible) { + // Disengagement always clears the captured target - a fresh + // engagement captures a fresh AGL, a stale setpoint never survives + terrainNavHoldCoreReset(state); + out->status = state->status; + return; + } + + // Track data usability for the freeze grace period and resume hysteresis + const bool usable = in->healthy && in->aglValid; + if (usable) { + if (state->usableSinceMs == 0) { + state->usableSinceMs = (in->nowMs != 0) ? in->nowMs : 1; + } + state->lastUsableMs = in->nowMs; + } else { + state->usableSinceMs = 0; + } + const bool usableStable = usable && + (in->nowMs - state->usableSinceMs >= TERRAIN_NAV_HOLD_RESUME_HYSTERESIS_MS); + + switch (state->status) { + case TERRAIN_NAV_HOLD_INACTIVE: + case TERRAIN_NAV_HOLD_WAITING: + if (!usable) { + // Refuse engagement instead of engaging and degrading: + // cruise keeps flying exactly as stock, the pilot is told why + state->status = TERRAIN_NAV_HOLD_WAITING; + out->warning = TERRAIN_NAV_HOLD_WARN_NOT_READY; + break; + } + engage(state, in, out); + break; + + case TERRAIN_NAV_HOLD_ACTIVE: + if (!usable) { + if (in->nowMs - state->lastUsableMs > TERRAIN_NAV_HOLD_FREEZE_GRACE_MS) { + // Freeze: stop commanding, the last target holds. + // Never descend on dead data. A running handover blend + // and the predictive alarm die with the writes - health + // gates every output + state->status = TERRAIN_NAV_HOLD_FROZEN; + state->blendActive = false; + state->terrainAheadActive = false; + state->escapeBadSinceMs = 0; + state->escapeClearSinceMs = 0; + state->escapeDeepFail = false; + state->uncoverCapFight = false; + out->warning = TERRAIN_NAV_HOLD_WARN_DATA_LOST; + } + // Inside the grace period: a single missed read just pauses + // target updates for a cycle - no drama, no warning + break; + } + updateFloorAlarm(state, in); + updateEscapeAlarm(state, in); + if (in->stickAdjusting) { + // Pilot input wins: stock climb-rate control runs, and the + // AGL present at release becomes the new held target. The + // alarms stay live - they report the aircraft's real state, + // not the hold's commands + state->reCapturePending = true; + state->stickQuietSinceMs = 0; + if (!state->blendActive && !state->stickWasAdjusting) { + // The grab: seed the handover blend from the aircraft's + // real climb rate - zero command step by construction. + // (Not re-seeded when a blend already runs: a stick + // re-crossing the deadband keeps one continuous command) + state->blendActive = true; + state->blendRateCmS = in->actualClimbRateCmS; + state->blendStartMs = in->nowMs; + state->blendLastMs = in->nowMs; + } + runBlend(state, in, out, in->stickWishValid ? in->stickWishCmS : 0.0f); + if (state->terrainAheadActive) { + out->warning = TERRAIN_NAV_HOLD_WARN_TERRAIN_AHEAD; + } + if (state->pullUpActive) { + out->warning = pullUpWarning(state); + } + break; + } + if (state->reCapturePending) { + // Retake dwell: the stick must stay inside the deadband this + // long before the hold takes back - a stick hovering at the + // edge otherwise alternates two writers within milliseconds. + // The blend keeps running through the dwell, chasing zero + // (the stock release semantics), so the whole episode has + // one continuous writer + if (state->stickQuietSinceMs == 0) { + state->stickQuietSinceMs = (in->nowMs != 0) ? in->nowMs : 1; + } + if (in->nowMs - state->stickQuietSinceMs < TERRAIN_NAV_HOLD_RETAKE_DWELL_MS) { + runBlend(state, in, out, 0.0f); + if (state->terrainAheadActive) { + out->warning = TERRAIN_NAV_HOLD_WARN_TERRAIN_AHEAD; + } + if (state->pullUpActive) { + out->warning = pullUpWarning(state); + } + break; + } + // A release is a fresh capture - fresh floor-alarm phase too + state->blendActive = false; + startCapture(state, in); + } + computeTarget(state, in, out); + break; + + case TERRAIN_NAV_HOLD_FROZEN: + if (usableStable) { + // Resume the same held AGL, slew-limited by the funnel rate + state->status = TERRAIN_NAV_HOLD_ACTIVE; + updateFloorAlarm(state, in); + updateEscapeAlarm(state, in); + if (in->stickAdjusting) { + // Pilot is commanding at the moment of resume: yield + // immediately, the release becomes a fresh capture + state->reCapturePending = true; + if (state->pullUpActive) { + out->warning = pullUpWarning(state); + } + break; + } + computeTarget(state, in, out); + } else { + out->warning = TERRAIN_NAV_HOLD_WARN_DATA_LOST; + } + break; + } + + state->stickWasAdjusting = in->stickAdjusting; + out->status = state->status; +} diff --git a/src/main/terrain/terrain_nav_hold_core.h b/src/main/terrain/terrain_nav_hold_core.h new file mode 100644 index 00000000000..8f5cd5d196a --- /dev/null +++ b/src/main/terrain/terrain_nav_hold_core.h @@ -0,0 +1,167 @@ +/* + * 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 AGL HOLD decision core. + * + * Pure logic, no firmware dependencies: everything it needs comes in through + * terrainNavHoldInput_t and everything it decides goes out through + * terrainNavHoldOutput_t. It never touches the altitude controllers - the + * caller applies out->targetZCm through the one altitude-target funnel. + */ + +typedef enum { + TERRAIN_NAV_HOLD_INACTIVE = 0, // not eligible (switch off, not in cruise, disengage condition) + TERRAIN_NAV_HOLD_WAITING, // eligible but terrain data unusable - engagement refused + TERRAIN_NAV_HOLD_ACTIVE, // holding height above ground + TERRAIN_NAV_HOLD_FROZEN, // data lost mid-hold - altitude target frozen, never descend on dead data +} terrainNavHoldStatus_e; + +typedef enum { + TERRAIN_NAV_HOLD_WARN_NONE = 0, + TERRAIN_NAV_HOLD_WARN_NOT_READY, // switch is on but terrain data is not usable + TERRAIN_NAV_HOLD_WARN_DATA_LOST, // frozen: holding last altitude on stale map data + TERRAIN_NAV_HOLD_WARN_MAX_ALT, // terrain needs more altitude than nav_max_altitude allows + TERRAIN_NAV_HOLD_WARN_PULL_UP, // below the minimum AGL and not recovering - fires with or without a ceiling + TERRAIN_NAV_HOLD_WARN_NO_HEADING, // heading estimate invalid: lookahead off, reactive hold continues + TERRAIN_NAV_HOLD_WARN_AUTO_CLIMB, // engaged below the minimum AGL - automatic climb to the minimum in progress + TERRAIN_NAV_HOLD_WARN_TERRAIN_AHEAD, // escape test failing: terrain ahead cannot be out-climbed at full rate + TERRAIN_NAV_HOLD_WARN_TURN_AWAY, // floor alarm where pulling cannot help: the full-rate climb is losing, or the target is pinned at the ceiling +} terrainNavHoldWarning_e; + +typedef struct { + uint32_t nowMs; + bool eligible; // armed + cruise whitelist + switch on + no hard-disengage condition + bool stickAdjusting; // pilot is moving the pitch stick - stock behavior runs, re-capture on release + bool healthy; // terrain module healthy this cycle + bool aglValid; // AGL answer available (may lag one query cycle behind healthy) + int32_t aglCm; // current height above ground, valid only when aglValid + float currentZCm; // current altitude in the local frame (same frame as the target funnel) + float actualClimbRateCmS; // the aircraft's real vertical speed (seeds the handover blend) + bool stickWishValid; // the pilot's stick climb-rate wish is known this cycle + float stickWishCmS; // what the stick currently asks for, manual-clamped by the caller + bool stickFullPull; // the pitch stick is at (or near) full pull - the pilot's vertical channel is maxed + bool pilotHasMoreClimb; // nav_fw_manual_climb_rate > nav_fw_auto_climb_rate: a pull adds climb the hold cannot + bool autoBeatsManual; // nav_fw_auto_climb_rate > nav_fw_manual_climb_rate: releasing the stick adds climb a pull cannot + bool lookaheadValid; // lookahead answer available + float lookaheadClimbCm; // worst height deficit along the path ahead (>= 0) + bool escapeDeficitValid; // escape-test answer available (same walk as the lookahead) + float escapeDeficitCm; // worst deficit vs the FULL climb rate along the path (>= 0) + bool lookaheadDegraded; // lookahead wanted but unavailable for an abnormal reason (heading estimate invalid) - warn the pilot + int32_t minAglCm; // configured minimum AGL + int32_t maxAltCm; // nav_max_altitude in the same frame, 0 = no ceiling +} terrainNavHoldInput_t; + +typedef struct { + bool writeTarget; // when true the caller commands targetZCm through the funnel + float targetZCm; + bool writeRate; // when true the caller commands rateCmS instead (handover blend window) + float rateCmS; // signed climb rate; bounded by the caller's clamp on the blend inputs + bool autoClimbRunning; // the hold is climbing to the minimum right now (for the OSD to alternate the info under a covering warning) + terrainNavHoldStatus_e status; + terrainNavHoldWarning_e warning; +} terrainNavHoldOutput_t; + +typedef struct { + terrainNavHoldStatus_e status; + int32_t targetAglCm; // the held height above ground; captured at engagement, never persisted across disengagement + uint32_t usableSinceMs; // 0 = data not usable right now (for the re-engage hysteresis) + uint32_t lastUsableMs; // last time data was usable (for the freeze grace period) + bool reCapturePending; // stick released - capture a new target AGL on the next usable cycle + bool minAglReached; // AGL has reached the minimum since this capture - arms the floor alarm + int32_t climbBestAglCm; // best AGL of the current below-minimum fight (the auto-climb phase, and the breach episode after the minimum was reached) + bool pullUpActive; // floor alarm latched: on below (min - margin), off at/above the minimum + bool pullUpRatchet; // this red latched via the losing-auto-climb ratchet - pulling cannot help, the text says TURN AWAY + bool pullUpCapFight; // this red episode ran against the ceiling clamp - same TURN AWAY text, latched for the episode + bool uncoverCapFight; // one-shot: a cap-fight red just cleared at the minimum - the escape alarm may fast-clear this cycle + bool stickWasAdjusting; // previous-cycle stick state (grab-edge detection for the blend) + bool blendActive; // handover blend running (stick grab through the retake dwell) + float blendRateCmS; // the blend's current climb-rate command + uint32_t blendStartMs; // when this blend began (hard lifetime bound) + uint32_t blendLastMs; // last blend update (dt for the first-order chase) + uint32_t stickQuietSinceMs; // stick back inside the deadband since (retake dwell); 0 = not quiet + bool terrainAheadActive; // predictive escape alarm latched + uint32_t escapeBadSinceMs; // escape test failing since (persistence); 0 = not failing + uint32_t escapeClearSinceMs;// escape test passing with margin since (clear hysteresis); 0 = not counting + bool escapeDeepFail; // the current escape shortfall is at least the deep threshold - only such a failure births TURN AWAY prospectively +} terrainNavHoldState_t; + +// A momentary AGL gap (single cache miss) only pauses target updates; the hold +// freezes - with a pilot warning - when data stays unusable longer than this +#define TERRAIN_NAV_HOLD_FREEZE_GRACE_MS 500 + +// After a freeze, data must be continuously usable this long before the hold +// resumes (slew-limited); prevents flapping on marginal data +#define TERRAIN_NAV_HOLD_RESUME_HYSTERESIS_MS 3000 + +// Floor alarm margin: terrain naturally breathes around the minimum when +// riding the floor, so once the minimum has been reached PULL UP fires only +// below (min - margin) and clears at/above the minimum. The same margin +// decides "losing height" during the initial automatic climb +#define TERRAIN_NAV_HOLD_PULLUP_HYST_CM 500 + +// Handover blend: when the pilot grabs the stick during an active hold the +// climb-rate command must not step (a large hold demand collapsing into a +// small stick demand pitches the nose the wrong way). The blend seeds from +// the aircraft's real climb rate and chases the stick's wish with this time +// constant - most of the pilot's wish arrives within the first tau +#define TERRAIN_NAV_HOLD_BLEND_TAU_MS 300 + +// The blend never outlives this: a hard bound on the shared-authority window, +// after which the stock stick control flies alone +#define TERRAIN_NAV_HOLD_BLEND_MAX_MS 1000 + +// The blend ends early once this close to the stick's wish - close enough +// that the switch to the stock writer is seamless +#define TERRAIN_NAV_HOLD_BLEND_DONE_CM_S 25.0f + +// Retake dwell: after the stick returns inside the deadband it must stay +// there this long before the hold captures and resumes writing - a stick +// hovering at the deadband edge otherwise alternates two writers within +// milliseconds (the LOG00010 whiplash) +#define TERRAIN_NAV_HOLD_RETAKE_DWELL_MS 250 + +// TERRAIN AHEAD (predictive escape test): the failing condition must hold +// this long before the alarm fires - no flicker on single noisy samples +#define TERRAIN_NAV_HOLD_ESCAPE_PERSIST_MS 1000 + +// ...and clears only after the test passes with the aircraft at/above the +// minimum, both sustained this long (typically because the pilot turned, +// slowed or climbed); a returning threat simply re-fires the series +#define TERRAIN_NAV_HOLD_ESCAPE_CLEAR_MS 2000 + +// A red born under a latched TERRAIN AHEAD says TURN AWAY outright only when +// the current escape shortfall is at least this deep: a marginal map-step +// deficit self-resolves in seconds and stays PULL UP, while the deficit of a +// genuine wall grows so fast at cruise closure that it crosses this line +// within a second of the alarm +#define TERRAIN_NAV_HOLD_ESCAPE_DEEP_CM 1000.0f + +void terrainNavHoldCoreReset(terrainNavHoldState_t *state); +void terrainNavHoldCoreUpdate(terrainNavHoldState_t *state, const terrainNavHoldInput_t *in, terrainNavHoldOutput_t *out); diff --git a/src/main/terrain/terrain_utils.c b/src/main/terrain/terrain_utils.c new file mode 100644 index 00000000000..3d751bb956e --- /dev/null +++ b/src/main/terrain/terrain_utils.c @@ -0,0 +1,291 @@ +/* + * 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 "common/crc.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; +} + +/** + set the cache state of a grid block + */ +void setGridStatus(gridBlock_t *gridBlock, enum GridCacheState state) +{ + if (gridBlock == NULL) { + return; + } + + for (uint16_t i = 0; i < TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) { + if (&(cache[i].gridBlock) == gridBlock) { + if (state == GRID_CACHE_DISKWAIT) { + //block lat/lon are still valid here, stash them so the slot + //stays identifiable while the disk read overwrites the block + cache[i].expectedLat = gridBlock->lat; + cache[i].expectedLon = gridBlock->lon; + } + cache[i].state = state; + return; + } + } +} + +/** + mark a grid block as needing to be read from disk + */ +void markGridBlockNeedRead(gridBlock_t *gridBlock) +{ + setGridStatus(gridBlock, GRID_CACHE_DISKWAIT); +} + +/** + 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; returns NULL + while the block is not readable yet (DISKWAIT/READING or newly allocated) + */ +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 (cache[i].state == GRID_CACHE_READING) { + //block content is being overwritten by the disk read, match by the + //identity stashed at DISKWAIT time instead, otherwise a repeat query + //would allocate a duplicate slot for the same block + if (TERRAIN_LATLON_EQUAL(cache[i].expectedLat, info->gridLat) && + TERRAIN_LATLON_EQUAL(cache[i].expectedLon, info->gridLon)) { + cache[i].lastAccessMs = nowMs; + //not readable until the disk read finished + return NULL; + } + //never evict while the read is in flight + continue; + } + + //INVALID blocks never match by content, a failed direct read leaves partial file data in them + if (cache[i].state != GRID_CACHE_INVALID && + TERRAIN_LATLON_EQUAL(grid->lat, info->gridLat) && + TERRAIN_LATLON_EQUAL(grid->lon , info->gridLon) && + cache[i].gridBlock.spacing == grid_spacing) { + cache[i].lastAccessMs = nowMs; + //not readable until the disk read finished + if (cache[i].state == GRID_CACHE_DISKWAIT) { + return NULL; + } + 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; a DISKWAIT block is not readable, + // so the caller gets NULL until the disk read has finished + gridCache->expectedLat = info->gridLat; + gridCache->expectedLon = info->gridLon; + gridCache->state = GRID_CACHE_DISKWAIT; + + return NULL; +} + +/* + 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; +} + +/* + decode a single 10-bit height offset from the packed heightOffset[] array. + + Values are stored in row-major order (idx_x * TERRAIN_GRID_BLOCK_SIZE_Y + + idx_y), little-endian bit-packed, 10 bits each (4 values per 5 bytes). The + bit offset within a byte is therefore always 0, 2, 4 or 6, so a single value + never spans more than two bytes: read those two bytes little-endian, shift + down by the bit offset and mask off the low 10 bits. + */ +uint16_t getHeightOffsetByIndex(gridBlock_t *grid, uint8_t idx_x, uint8_t idx_y) +{ + uint32_t valueIndex = (uint32_t)idx_x * TERRAIN_GRID_BLOCK_SIZE_Y + idx_y; + uint32_t bitPos = valueIndex * 10; + uint32_t bytePos = bitPos / 8; + uint32_t bitOffset = bitPos % 8; + + uint16_t twoBytes = grid->heightOffset[bytePos] | ((uint16_t)grid->heightOffset[bytePos + 1] << 8); + + return (twoBytes >> bitOffset) & 0x3FF; +} + +uint16_t getBlockCrc(gridBlock_t *grid){ + // crc is taken over the whole block with the crc field zeroed + uint16_t saved_crc = grid->crc; + grid->crc = 0; + uint16_t ret = crc16_ccitt_update(0, grid, sizeof(*grid)); + grid->crc = saved_crc; + return ret; +} + + +#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..4ba115ec5ff --- /dev/null +++ b/src/main/terrain/terrain_utils.h @@ -0,0 +1,43 @@ +/* + * 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); +uint16_t getHeightOffsetByIndex(gridBlock_t *grid, uint8_t idx_x, uint8_t idx_y); +uint16_t getBlockCrc(gridBlock_t *grid); + +gridCache_t* findGridCache(gridInfo_t *info); +gridBlock_t* getGridBlockToRead(void); + +uint32_t eastBlocks(gridBlock_t *gridBlock); +void setGridStatus(gridBlock_t *gridBlock, enum GridCacheState state); +void markGridBlockNeedRead(gridBlock_t *gridBlock); \ No newline at end of file diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index a48c140ba54..98ab3799f1d 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -73,6 +73,9 @@ set_property(SOURCE gps_null_port_unittest.cc PROPERTY definitions GPS_NULL_PORT 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 "common/crc.c" "common/streambuf.c" "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) + set_property(SOURCE mavlink_unittest.cc PROPERTY depends "fc/fc_mavlink.c" "mavlink/mavlink_command.c" "mavlink/mavlink_guided.c" "mavlink/mavlink_mission.c" "mavlink/mavlink_modes.c" "mavlink/mavlink_ports.c" "mavlink/mavlink_routing.c" "mavlink/mavlink_runtime.c" "mavlink/mavlink_streams.c" "telemetry/mavlink.c" diff --git a/src/test/unit/terrain_unittest.cc b/src/test/unit/terrain_unittest.cc new file mode 100644 index 00000000000..5e3d63ab21b --- /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 uint32_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); + + ///////////////////////////////////////////////////////////////////// +}