From cb002172860dc7cde8f68cf7dcd5cdbf990942f7 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Thu, 12 Nov 2026 05:49:11 +0000 Subject: [PATCH] feat(sih): add optional support for non-flat terrain for MC, FW and VTOL (#29619) * feat(simulator_sih): add a GeoTIFF terrain elevation model for SITL SIH only simulates flat ground. This adds the loader for a terrain elevation model (DEM) in a GeoTIFF file, the base for non-flat terrain in SITL. The DEM is read with libtiff and libgeotiff. A DEM in a projected CRS is resampled once at load time with PROJ onto a WGS84 lat/lon grid, so a lookup in the simulation loop is a plain bilinear interpolation. It is built only for px4_sitl and only when the three libraries are found, so hardware builds and SITL without them are unchanged. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Beniamino Pozzan * feat(simulator_sih): use the terrain DEM for ground contact and the rangefinder With PX4_SIH_TERRAIN set to a GeoTIFF DEM, SIH in SITL places the vehicle on the terrain at start, lands it on the terrain, ray-casts the downward distance sensor against it, and publishes it as the groundtruth terrain_alt. Outside the DEM the ground stays flat at SIH_LOC_H0. Terrain is only used for multicopters, VTOLs and fixed-wing; other vehicle types keep flat ground. The DEM is loaded in task_spawn() so a bad file fails the start command, and because the SIH task stack is too small for libtiff and PROJ. The groundtruth terrain_alt now holds the WGS84 terrain altitude, as the message documents, instead of the height above the origin. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Beniamino Pozzan * build(setup): install the SIH terrain dependencies with the simulation tools Install libtiff, libgeotiff and PROJ so SITL builds SIH with terrain elevation (DEM) support. They are optional: without them SIH builds as before. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Beniamino Pozzan * docs(sim_sih): document terrain elevation from a GeoTIFF DEM Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Beniamino Pozzan * docs(sim_sih): fix and clarify the terrain elevation section Address review findings on the new terrain section: list the sample types the loader actually reads, note that the prebuilt px4 package and px4io/px4-sitl container are built without terrain support, and say when SIH_LOC_H0 still sets the start altitude. Also add a version badge, link GeoTIFF, EPSG and GDAL, mention the dist_bottom groundtruth field, and correct the GLO-30 coverage. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Hamish Willee * fix: cast to double for MAC builds Signed-off-by: Beniamino Pozzan * chore: implement TerrainBase interface Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Beniamino Pozzan * chore: improve isolation of terrain logic from sih core Signed-off-by: Beniamino Pozzan * refactor(simulator_sih): move DEM handling into the terrain classes SIH should only use the terrain, not manage it. TerrainFlat becomes the base class, since flat ground is what every terrain model builds on, and TerrainDem falls back to it until a DEM is loaded. Terrain.hpp picks the model at compile time, so SIH holds the terrain by value with no #ifdef, new/delete or pointer handover, and NuttX builds only get the flat model. Tracking whether the vehicle is inside the DEM, and the matching warnings and status, move from SIH into TerrainDem. The DEM is still loaded in task_spawn() because the SIH task stack is too small for libtiff and PROJ, so the terrain is a static member that the instance uses directly instead of moving it. A terrain file with an unsupported vehicle type now fails the start like a bad file instead of silently using flat ground. --------- Signed-off-by: Beniamino Pozzan Signed-off-by: Hamish Willee Co-authored-by: Hamish Willee Co-authored-by: Matthias Grob --- .../px4fmu_common/init.d-posix/px4-rc.sihsim | 8 +- Tools/setup/arch.sh | 7 + Tools/setup/macos.sh | 1 + Tools/setup/ubuntu.sh | 3 + docs/en/sim_sih/index.md | 43 ++ .../simulation/simulator_sih/CMakeLists.txt | 27 + src/modules/simulation/simulator_sih/sih.cpp | 121 +++- src/modules/simulation/simulator_sih/sih.hpp | 17 +- .../simulator_sih/terrain/Terrain.hpp | 47 ++ .../simulator_sih/terrain/TerrainDem.cpp | 598 ++++++++++++++++++ .../simulator_sih/terrain/TerrainDem.hpp | 116 ++++ .../simulator_sih/terrain/TerrainDemTest.cpp | 394 ++++++++++++ .../simulator_sih/terrain/TerrainFlat.hpp | 112 ++++ 13 files changed, 1462 insertions(+), 32 deletions(-) create mode 100644 src/modules/simulation/simulator_sih/terrain/Terrain.hpp create mode 100644 src/modules/simulation/simulator_sih/terrain/TerrainDem.cpp create mode 100644 src/modules/simulation/simulator_sih/terrain/TerrainDem.hpp create mode 100644 src/modules/simulation/simulator_sih/terrain/TerrainDemTest.cpp create mode 100644 src/modules/simulation/simulator_sih/terrain/TerrainFlat.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim b/ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim index b6fd1dedcfa..d802fc376b9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim @@ -20,7 +20,13 @@ if [ -n "${PX4_SIM_GPS_USED}" ]; then param set SIM_GPS_USED ${PX4_SIM_GPS_USED} fi -if simulator_sih start; then +sih_args="" +if [ -n "${PX4_SIH_TERRAIN}" ]; then + # terrain elevation GeoTIFF (DEM) + sih_args="-t ${PX4_SIH_TERRAIN}" +fi + +if simulator_sih start ${sih_args}; then if param compare -s SENS_EN_BAROSIM 1 then diff --git a/Tools/setup/arch.sh b/Tools/setup/arch.sh index 972761e31d7..d72be01f45d 100755 --- a/Tools/setup/arch.sh +++ b/Tools/setup/arch.sh @@ -96,6 +96,13 @@ if [[ $INSTALL_SIM == "true" ]]; then echo echo "Installing PX4 simulation dependencies" + # SIH terrain elevation (GeoTIFF DEM) support + sudo pacman -S --noconfirm --needed \ + libgeotiff \ + libtiff \ + proj \ + ; + # Gazebo setup if [[ $INSTALL_GAZEBO == "true" ]]; then echo diff --git a/Tools/setup/macos.sh b/Tools/setup/macos.sh index 2cf37f79c39..092415a3f89 100755 --- a/Tools/setup/macos.sh +++ b/Tools/setup/macos.sh @@ -140,6 +140,7 @@ if [[ $INSTALL_SIM == "--sim-tools" ]]; then exiftool glog graphviz + libgeotiff # SIH terrain elevation (GeoTIFF DEM), pulls libtiff and proj pixi ) diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index c947aff2b25..a657f650e21 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -216,6 +216,9 @@ if [[ $INSTALL_SIM == "true" ]]; then # General simulation dependencies sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ bc \ + libgeotiff-dev \ + libproj-dev \ + libtiff-dev \ ; # Gazebo Harmonic installation (Ubuntu 22.04+) diff --git a/docs/en/sim_sih/index.md b/docs/en/sim_sih/index.md index 48a3aeca50c..5bd45febfda 100644 --- a/docs/en/sim_sih/index.md +++ b/docs/en/sim_sih/index.md @@ -163,6 +163,49 @@ Run it from the PX4 shell, or the [MAVLink shell](../debug/mavlink_shell.md), af `orient` is only built for SITL: sensor calibration is rejected when SIH runs on flight controller hardware. +#### Terrain Elevation (DEM) {#terrain} + + + +By default SIH simulates flat ground at [`SIH_LOC_H0`](../advanced_config/parameter_reference.md#SIH_LOC_H0). +SIH can instead load the terrain elevation from a digital elevation model (DEM) [GeoTIFF](https://www.ogc.org/standards/geotiff/) file, set with the `PX4_SIH_TERRAIN` environment variable: + +```sh +PX4_SIH_TERRAIN=/path/to/dem.tif make px4_sitl_sih sihsim_quadx +``` + +The terrain is then used for: + +- Ground contact: the vehicle starts, lands and rests on the terrain. + It is placed at the terrain elevation below [`SIH_LOC_LAT0`](../advanced_config/parameter_reference.md#SIH_LOC_LAT0)/[`SIH_LOC_LON0`](../advanced_config/parameter_reference.md#SIH_LOC_LON0) (`PX4_HOME_LAT`/`PX4_HOME_LON`). + `SIH_LOC_H0` (`PX4_HOME_ALT`) sets the start altitude only when that point is outside the DEM. +- The downward facing distance sensor, which measures the range to the terrain along the vehicle body z axis (up to [`SIH_DISTSNSR_MAX`](../advanced_config/parameter_reference.md#SIH_DISTSNSR_MAX)). +- The `terrain_alt` field of the `vehicle_global_position_groundtruth` topic. +- The `dist_bottom` field of the `vehicle_local_position_groundtruth` topic. + +Outside the area covered by the DEM, or over cells without data, the ground is flat at `SIH_LOC_H0` and a warning is printed. +Terrain is supported for multicopters, VTOLs and fixed-wing vehicles; with any other vehicle type (such as Ackermann rovers), setting `PX4_SIH_TERRAIN` stops SIH from starting with an error. +`simulator_sih status` shows the loaded DEM and whether the vehicle is inside it. + +The DEM must be a single-band GeoTIFF whose coordinate reference system has an [EPSG code](https://epsg.io/), either geographic (such as WGS84, `EPSG:4326`) or projected (such as UTM or the Swiss `EPSG:2056`). +8, 16 and 32-bit integer and 32 and 64-bit floating point samples are supported with any TIFF compression, and the [_GDAL_](https://gdal.org/) nodata value is respected. +Elevations are used as they are, as altitude above mean sea level. +Free sources include [Copernicus GLO-30](https://registry.opendata.aws/copernicus-dem/) (near-global, 30 m, including buildings and vegetation) and national elevation models such as [swissALTI3D](https://www.swisstopo.admin.ch/en/height-model-swissalti3d) (2 m). +Crop large files to the area you fly in, for example with _GDAL_: + +```sh +gdal_translate -projwin 8.48 47.44 8.61 47.35 Copernicus_DSM_COG_10_N47_00_E008_00_DEM.tif zurich.tif +``` + +Files in other formats, with other sample types or without an EPSG code can be converted with `gdalwarp -t_srs EPSG:4326 -ot Float32 in.tif out.tif`. + +::: info +Terrain support is optional and only available in SITL, not when [running SIH on flight controller hardware](#sih-on-flight-controller-hardware). +The [prebuilt](../simulation/px4_sitl_prebuilt_packages.md) `px4` package and `px4io/px4-sitl` container are built without it, and SIH reports an error and does not start if `PX4_SIH_TERRAIN` is set. +When building from source, the [setup scripts](../dev_setup/dev_env.md) install the required libraries with the simulation tools. +The CMake output shows `SIH terrain (DEM) support enabled` when it is available. +::: + ### ROS 2 Integration SIH works with ROS 2 via the [uXRCE-DDS](../middleware/uxrce_dds.md) client, which auto-starts in SITL mode. diff --git a/src/modules/simulation/simulator_sih/CMakeLists.txt b/src/modules/simulation/simulator_sih/CMakeLists.txt index 6532bf8b30a..1aa35a77194 100644 --- a/src/modules/simulation/simulator_sih/CMakeLists.txt +++ b/src/modules/simulation/simulator_sih/CMakeLists.txt @@ -49,6 +49,33 @@ px4_add_module( geo ) +# optional terrain from a GeoTIFF DEM, SITL only (requires libtiff, libgeotiff and PROJ) +if(${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL} STREQUAL "px4_sitl") + find_package(TIFF QUIET) + find_package(PROJ CONFIG QUIET) + find_path(GEOTIFF_INCLUDE_DIR geotiff.h PATH_SUFFIXES geotiff libgeotiff) + find_library(GEOTIFF_LIBRARY NAMES geotiff geotiff_i) + + if(TIFF_FOUND AND PROJ_FOUND AND GEOTIFF_INCLUDE_DIR AND GEOTIFF_LIBRARY) + message(STATUS "SIH terrain (DEM) support enabled") + + px4_add_library(sih_terrain terrain/TerrainDem.cpp) + target_include_directories(sih_terrain PRIVATE ${GEOTIFF_INCLUDE_DIR}) + target_link_libraries(sih_terrain PRIVATE ${GEOTIFF_LIBRARY} TIFF::TIFF PROJ::proj geo) + + target_link_libraries(modules__simulation__simulator_sih PRIVATE sih_terrain) + target_compile_definitions(modules__simulation__simulator_sih PRIVATE SIH_TERRAIN_DEM) + + px4_add_unit_gtest(SRC terrain/TerrainDemTest.cpp + INCLUDES ${GEOTIFF_INCLUDE_DIR} + LINKLIBS sih_terrain geo ${GEOTIFF_LIBRARY} TIFF::TIFF + ) + + else() + message(STATUS "SIH terrain (DEM) support disabled: libtiff, libgeotiff or PROJ not found") + endif() +endif() + if(PX4_PLATFORM MATCHES "posix") # create targets for sihsim set(models diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index e007fea4f28..4fe56eeef13 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -96,6 +96,7 @@ Sih::~Sih() { perf_free(_loop_perf); perf_free(_loop_interval_perf); + _terrain = Terrain{}; // free the DEM } void Sih::run() @@ -103,6 +104,11 @@ void Sih::run() _px4_accel.set_temperature(T1_C); _px4_gyro.set_temperature(T1_C); + // the vehicle type is needed by parameters_updated() to place the vehicle on the terrain + _vehicle = static_cast(constrain(_sih_vtype.get(), + static_cast(VehicleType::First), + static_cast(VehicleType::Last))); + parameters_updated(); if (_sih_optical_flow_en.get()) { @@ -119,9 +125,6 @@ void Sih::run() _dist_snsr_time = task_start; _optical_flow_time = task_start; _ranging_beacon_time = task_start; - _vehicle = static_cast(constrain(_sih_vtype.get(), - static_cast(VehicleType::First), - static_cast(VehicleType::Last))); #if defined(ENABLE_LOCKSTEP_SCHEDULER) lockstep_loop(); @@ -408,13 +411,19 @@ void Sih::parameters_updated() if (!_lpos_ref.isInitialized() || (fabsf(static_cast(_lpos_ref.getProjectionReferenceLat()) - _sih_lat0.get()) > FLT_EPSILON) || (fabsf(static_cast(_lpos_ref.getProjectionReferenceLon()) - _sih_lon0.get()) > FLT_EPSILON) - || (fabsf(_lpos_ref_alt - _sih_h0.get()) > FLT_EPSILON)) { - _lpos_ref.initReference(static_cast(_sih_lat0.get()), static_cast(_sih_lon0.get())); - _lpos_ref_alt = _sih_h0.get(); + || (fabsf(_terrain.defaultAltitude() - _sih_h0.get()) > FLT_EPSILON)) { + const double lat0 = static_cast(_sih_lat0.get()); + const double lon0 = static_cast(_sih_lon0.get()); + _lpos_ref.initReference(lat0, lon0); + + // start on the ground: flat at SIH_LOC_H0, or on the terrain model + _terrain.setDefaultAltitude(_sih_h0.get()); + _lpos_ref_alt = _terrain.vehicleGroundAltitude(lat0, lon0); + _ground_alt = _lpos_ref_alt; // Reset earth position, velocity and attitude - _lla.setLatitudeDeg(static_cast(_sih_lat0.get())); - _lla.setLongitudeDeg(static_cast(_sih_lon0.get())); + _lla.setLatitudeDeg(lat0); + _lla.setLongitudeDeg(lon0); _lla.setAltitude(_lpos_ref_alt); _p_E = _lla.toEcef(); @@ -755,7 +764,7 @@ void Sih::equations_of_motion(const float dt) const float force_down = Vector3f(_R_N2E.transpose() * sum_of_forces_E)(2); Vector3f ground_force_E; - if ((_lla.altitude() - _lpos_ref_alt) < 0.f && force_down > 0.f) { + if (_lla.altitude() < _ground_alt && force_down > 0.f) { if (_vehicle == VehicleType::Quadcopter || _vehicle == VehicleType::Hexacopter || _vehicle == VehicleType::TailsitterVTOL @@ -815,6 +824,23 @@ void Sih::equations_of_motion(const float dt) ecefToNed(); + // ground below the new position, for the next contact check and the sensors + _ground_alt = _terrain.vehicleGroundAltitude(_lla.latitude_deg(), _lla.longitude_deg()); + + // the ground force only stops the vertical motion, keep the vehicle from sinking into rising terrain + static constexpr float MAX_PENETRATION = 0.05f; // [m] + + if (_lla.altitude() < _ground_alt - MAX_PENETRATION) { + _lla.setAltitude(_ground_alt - MAX_PENETRATION); + _p_E = _lla.toEcef(); + + if (_v_N(2) > 0.f) { + _v_N(2) = 0.f; + _v_E = _R_N2E * _v_N; + _v_apparent_N = _v_N + _v_wind_N; + } + } + _lpos_ref.project(_lla.latitude_deg(), _lla.longitude_deg(), _lpos(0), _lpos(1)); _lpos(2) = -(_lla.altitude() - _lpos_ref_alt); } @@ -933,6 +959,11 @@ void Sih::orient_step(const hrt_abstime &now, const float dt) } #endif // __PX4_POSIX +float Sih::ground_distance(float max_distance) const +{ + return _terrain.rayDistance(_lla.latitude_deg(), _lla.longitude_deg(), _lla.altitude(), _q.dcm_z(), max_distance); +} + void Sih::ecefToNed() { _lla = LatLonAlt::fromEcef(_p_E); @@ -1009,21 +1040,6 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us) _airspeed_pub.publish(airspeed); } -float Sih::ground_distance() const -{ - // the ground is assumed flat at the altitude of the simulation reference (SIH_LOC_H0) - const float cos_tilt = _q.dcm_z()(2); - - if (cos_tilt < FLT_EPSILON) { - // the body down axis points away from the ground - return NAN; - } - - // the fake ground lets the vehicle settle marginally below the reference altitude, clamp it - // so that a landed vehicle reads zero instead of dropping out of range - return math::max(-_lpos(2), 0.f) / cos_tilt; -} - void Sih::send_dist_snsr(const hrt_abstime &time_now_us) { if (_distance_sensor_blocked) { @@ -1041,7 +1057,7 @@ void Sih::send_dist_snsr(const hrt_abstime &time_now_us) } else { // the quality is decided on the true distance rather than on the noisy reading, so // that a measurement sitting at a range limit does not flicker in and out of validity - const float true_distance = ground_distance(); + const float true_distance = ground_distance(_distance_snsr_max); if (!PX4_ISFINITE(true_distance) || true_distance > _distance_snsr_max) { // nothing within reach, the sensor reports its saturated raw reading @@ -1080,7 +1096,7 @@ void Sih::accumulate_optical_flow(const float dt) // value is produced by a right handed rotation of the sensor about the body axis, which // gives the translation over the ground the opposite sign (see the flow observation model // in the EKF). - const float distance = ground_distance(); + const float distance = ground_distance(_sih_optical_flow_h_max.get()); if (!PX4_ISFINITE(distance) || distance > _sih_optical_flow_h_max.get()) { // too high above the ground, or not looking at it: no image to track @@ -1260,6 +1276,8 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us) local_position.heading_good_for_control = true; local_position.unaided_heading = NAN; + local_position.dist_bottom = _lla.altitude() - _ground_alt; + local_position.timestamp = hrt_absolute_time(); _local_position_ground_truth_pub.publish(local_position); } @@ -1272,7 +1290,7 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us) global_position.lon = _lla.longitude_deg(); global_position.alt = _lla.altitude(); global_position.alt_ellipsoid = global_position.alt; - global_position.terrain_alt = -_lpos(2); + global_position.terrain_alt = _ground_alt; global_position.timestamp = hrt_absolute_time(); _global_position_ground_truth_pub.publish(global_position); } @@ -1313,6 +1331,9 @@ int Sih::print_status() PX4_INFO("Rover Ackermann"); } + _terrain.printStatus(); + + PX4_INFO("ground altitude: %.2f m", (double)_ground_alt); PX4_INFO("vehicle landed: %d", _grounded); #if defined(__PX4_POSIX) @@ -1353,8 +1374,53 @@ int Sih::run_trampoline(int argc, char *argv[]) }, argc, argv); } +bool Sih::load_terrain(int argc, char *argv[]) +{ + _terrain = Terrain{}; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + const char *terrain_path = nullptr; + + while ((ch = px4_getopt(argc, argv, "t:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 't': + terrain_path = myoptarg; + break; + + default: + print_usage("unrecognized flag"); + return false; + } + } + + if (!terrain_path) { + return true; + } + + int32_t vehicle_type = 0; + param_get(param_find("SIH_VEHICLE_TYPE"), &vehicle_type); + const VehicleType vehicle = static_cast(vehicle_type); + + if (vehicle != VehicleType::Quadcopter + && vehicle != VehicleType::Hexacopter + && vehicle != VehicleType::FixedWing + && vehicle != VehicleType::TailsitterVTOL + && vehicle != VehicleType::StandardVTOL) { + PX4_ERR("terrain is not supported for SIH_VEHICLE_TYPE %" PRId32, vehicle_type); + return false; + } + + return _terrain.load(terrain_path); +} + int Sih::task_spawn(int argc, char *argv[]) { + if (!load_terrain(argc, argv)) { + return -1; + } + desc.task_id = px4_task_spawn_cmd("sih", SCHED_DEFAULT, SCHED_PRIORITY_MAX, @@ -1472,6 +1538,7 @@ $ simulator_sih orient off PRINT_MODULE_USAGE_ARG("down|up|front|back|left|right|all|off", "Side facing down, every side in turn, or level", false); PRINT_MODULE_USAGE_ARG("rotate", "Rotate about the vertical", true); #endif // __PX4_POSIX + PRINT_MODULE_USAGE_PARAM_STRING('t', nullptr, "", "Terrain elevation GeoTIFF (DEM), SITL only", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index ca7a2e87a18..76593eaf629 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -92,6 +92,8 @@ #include #endif +#include "terrain/Terrain.hpp" + using namespace time_literals; class Sih : public ModuleBase, public ModuleParams @@ -220,10 +222,6 @@ private: void send_optical_flow(const hrt_abstime &time_now_us); void send_ranging_beacon(const hrt_abstime &time_now_us); - // distance from the vehicle to the ground measured along the body down axis, NAN if the - // body down axis does not point at the ground anymore - float ground_distance() const; - void publish_ground_truth(const hrt_abstime &time_now_us); void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust_for_prowash); void generate_ts_aerodynamics(); @@ -253,6 +251,16 @@ private: float _orient_yaw{0.f}; // heading kept while posing #endif // __PX4_POSIX + // distance from the vehicle to the terrain measured along the body down axis, NAN if the + // body down axis does not point at the ground or nothing is hit within max_distance + float ground_distance(float max_distance) const; + + // terrain from the start arguments: the -t DEM, flat without it + // false (and print the reason) on error or if the vehicle type does not support terrain + static bool load_terrain(int argc, char *argv[]); + + static inline Terrain _terrain{}; // loaded by task_spawn(), the SIH task stack is too small for the terrain libraries + #if defined(ENABLE_LOCKSTEP_SCHEDULER) void lockstep_loop(); int _lockstep_component{-1}; @@ -355,6 +363,7 @@ private: // parameters MapProjection _lpos_ref{}; float _lpos_ref_alt; + float _ground_alt{0.f}; // ground altitude below the vehicle [m AMSL] float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _T_TAU, _F_T_MAX, _F_Q_MAX; matrix::Matrix3f _I; // vehicle inertia matrix matrix::Matrix3f _Im1; // inverse of the inertia matrix diff --git a/src/modules/simulation/simulator_sih/terrain/Terrain.hpp b/src/modules/simulation/simulator_sih/terrain/Terrain.hpp new file mode 100644 index 00000000000..68dc1e0b73a --- /dev/null +++ b/src/modules/simulation/simulator_sih/terrain/Terrain.hpp @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Terrain.hpp + * Terrain model used by SIH: the GeoTIFF DEM when PX4 is built with its libraries, flat otherwise. + */ + +#pragma once + +#if defined(SIH_TERRAIN_DEM) +#include "TerrainDem.hpp" +using Terrain = TerrainDem; +#else +#include "TerrainFlat.hpp" +using Terrain = TerrainFlat; +#endif diff --git a/src/modules/simulation/simulator_sih/terrain/TerrainDem.cpp b/src/modules/simulation/simulator_sih/terrain/TerrainDem.cpp new file mode 100644 index 00000000000..9d0dd7b1763 --- /dev/null +++ b/src/modules/simulation/simulator_sih/terrain/TerrainDem.cpp @@ -0,0 +1,598 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "TerrainDem.hpp" + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace +{ + +constexpr double kMetersPerDegree = 6371000.0 * M_PI / 180.0; +constexpr size_t kMaxCells = 200'000'000; // ~800 MB of floats + +constexpr const char *kConvertHint = + "convert with: gdalwarp -t_srs EPSG:4326 -ot Float32 -co COMPRESS=LZW "; + +TIFFExtendProc parent_tag_extender = nullptr; +const TIFFFieldInfo *custom_field_info = nullptr; // set by TerrainDem::registerTiffTags() +int custom_field_count = 0; + +void tagExtender(TIFF *tif) +{ + TIFFMergeFieldInfo(tif, custom_field_info, custom_field_count); + + if (parent_tag_extender) { + parent_tag_extender(tif); + } +} + +// RAII wrapper so that every early return closes the file +struct GeoTiffFile { + TIFF *tif{nullptr}; + GTIF *gtif{nullptr}; + + ~GeoTiffFile() + { + if (gtif) { + GTIFFree(gtif); + } + + if (tif) { + XTIFFClose(tif); + } + } +}; + +// read one sample of a raster row/tile buffer as float +bool sampleReader(uint16_t bits, uint16_t format, float ( *&read)(const uint8_t *, size_t)) +{ + read = nullptr; + + if (format == SAMPLEFORMAT_UINT) { + if (bits == 8) { + read = [](const uint8_t *p, size_t i) { return static_cast(p[i]); }; + + } else if (bits == 16) { + read = [](const uint8_t *p, size_t i) { uint16_t v; memcpy(&v, p + 2 * i, 2); return static_cast(v); }; + + } else if (bits == 32) { + read = [](const uint8_t *p, size_t i) { uint32_t v; memcpy(&v, p + 4 * i, 4); return static_cast(v); }; + } + + } else if (format == SAMPLEFORMAT_INT) { + if (bits == 8) { + read = [](const uint8_t *p, size_t i) { return static_cast(static_cast(p[i])); }; + + } else if (bits == 16) { + read = [](const uint8_t *p, size_t i) { int16_t v; memcpy(&v, p + 2 * i, 2); return static_cast(v); }; + + } else if (bits == 32) { + read = [](const uint8_t *p, size_t i) { int32_t v; memcpy(&v, p + 4 * i, 4); return static_cast(v); }; + } + + } else if (format == SAMPLEFORMAT_IEEEFP) { + if (bits == 32) { + read = [](const uint8_t *p, size_t i) { float v; memcpy(&v, p + 4 * i, 4); return v; }; + + } else if (bits == 64) { + read = [](const uint8_t *p, size_t i) { double v; memcpy(&v, p + 8 * i, 8); return static_cast(v); }; + } + } + + return read != nullptr; +} + +} // namespace + +void TerrainDem::registerTiffTags() +{ + static const TIFFFieldInfo field_info[] = { + {kTiffTagGdalNodata, -1, -1, TIFF_ASCII, FIELD_CUSTOM, 1, 0, const_cast("GDALNoDataValue")}, + }; + + static bool registered = false; + + if (!registered) { + registered = true; + custom_field_info = field_info; + custom_field_count = sizeof(field_info) / sizeof(field_info[0]); + parent_tag_extender = TIFFSetTagExtender(tagExtender); + } +} + +bool TerrainDem::Grid::sample(double x, double y, float &value) const +{ + const double fx = (x - x0) / dx; + const double fy = (y - y0) / dy; + + // accept up to half a cell outside the outermost cell centres (also rejects NAN) + if (!(fx >= -0.5 && fy >= -0.5 && fx <= width - 0.5 && fy <= height - 0.5)) { + return false; + } + + const double cx = fmin(fmax(fx, 0.), width - 1.); + const double cy = fmin(fmax(fy, 0.), height - 1.); + const int col = std::min(static_cast(cx), width - 2); + const int row = std::min(static_cast(cy), height - 2); + const float tx = static_cast(cx - col); + const float ty = static_cast(cy - row); + + const size_t i = static_cast(row) * width + col; + const float v00 = data[i]; + const float v01 = data[i + 1]; + const float v10 = data[i + width]; + const float v11 = data[i + width + 1]; + + if (std::isnan(v00) || std::isnan(v01) || std::isnan(v10) || std::isnan(v11)) { + return false; + } + + value = (1.f - ty) * ((1.f - tx) * v00 + tx * v01) + ty * ((1.f - tx) * v10 + tx * v11); + return true; +} + +bool TerrainDem::load(const char *path) +{ + registerTiffTags(); + + GeoTiffFile file; + file.tif = XTIFFOpen(path, "r"); + + if (!file.tif) { + PX4_ERR("terrain: cannot open %s", path); + return false; + } + + file.gtif = GTIFNew(file.tif); + + if (!file.gtif) { + PX4_ERR("terrain: %s has no GeoTIFF keys, %s", path, kConvertHint); + return false; + } + + TIFF *tif = file.tif; + GTIF *gtif = file.gtif; + + // raster layout + uint32_t width = 0; + uint32_t height = 0; + uint16_t samples_per_pixel = 1; + uint16_t bits_per_sample = 0; + uint16_t sample_format = SAMPLEFORMAT_UINT; + TIFFGetField(tif, TIFFTAG_IMAGEWIDTH, &width); + TIFFGetField(tif, TIFFTAG_IMAGELENGTH, &height); + TIFFGetFieldDefaulted(tif, TIFFTAG_SAMPLESPERPIXEL, &samples_per_pixel); + TIFFGetFieldDefaulted(tif, TIFFTAG_BITSPERSAMPLE, &bits_per_sample); + TIFFGetFieldDefaulted(tif, TIFFTAG_SAMPLEFORMAT, &sample_format); + + if (samples_per_pixel != 1) { + PX4_ERR("terrain: %s has %u bands, a single elevation band is required", path, samples_per_pixel); + return false; + } + + if (width < 2 || height < 2 || static_cast(width) * height > kMaxCells) { + PX4_ERR("terrain: %s is %ux%u cells, it must be between 2x2 and %zu cells (crop or downsample it)", + path, width, height, kMaxCells); + return false; + } + + float (*read_sample)(const uint8_t *, size_t) = nullptr; + + if (!sampleReader(bits_per_sample, sample_format, read_sample)) { + PX4_ERR("terrain: %s has unsupported samples (%u bit, format %u), %s", path, bits_per_sample, sample_format, + kConvertHint); + return false; + } + + // georeferencing: pixel corner (0, 0) in model coordinates and pixel size + Grid src{}; + uint16_t count = 0; + double *values = nullptr; + + if (TIFFGetField(tif, TIFFTAG_GEOTRANSMATRIX, &count, &values) && count >= 16) { + if (fabs(values[1]) > 0. || fabs(values[4]) > 0.) { + PX4_ERR("terrain: %s is rotated, %s", path, kConvertHint); + return false; + } + + src.dx = values[0]; + src.dy = values[5]; + src.x0 = values[3]; + src.y0 = values[7]; + + } else { + double *tiepoint = nullptr; + uint16_t tiepoint_count = 0; + + if (!TIFFGetField(tif, TIFFTAG_GEOPIXELSCALE, &count, &values) || count < 2 + || !TIFFGetField(tif, TIFFTAG_GEOTIEPOINTS, &tiepoint_count, &tiepoint) || tiepoint_count < 6) { + PX4_ERR("terrain: %s has no pixel scale and tiepoint, %s", path, kConvertHint); + return false; + } + + src.dx = values[0]; + src.dy = -values[1]; + src.x0 = tiepoint[3] - tiepoint[0] * src.dx; + src.y0 = tiepoint[4] - tiepoint[1] * src.dy; + } + + unsigned short raster_type = RasterPixelIsArea; + GTIFKeyGet(gtif, GTRasterTypeGeoKey, &raster_type, 0, 1); + + if (raster_type != RasterPixelIsPoint) { + // the model coordinates refer to the pixel corner, move them to the pixel centre + src.x0 += 0.5 * src.dx; + src.y0 += 0.5 * src.dy; + } + + if (!(src.dx > 0.) || !(src.dy < 0.)) { + PX4_ERR("terrain: %s must be north-up with increasing columns towards east, %s", path, kConvertHint); + return false; + } + + // coordinate reference system + unsigned short model_type = 0; + unsigned short epsg = 0; + GTIFKeyGet(gtif, GTModelTypeGeoKey, &model_type, 0, 1); + + if (model_type == ModelTypeGeographic) { + GTIFKeyGet(gtif, GeographicTypeGeoKey, &epsg, 0, 1); + + unsigned short angular_units = Angular_Degree; + GTIFKeyGet(gtif, GeogAngularUnitsGeoKey, &angular_units, 0, 1); + + if (angular_units != Angular_Degree) { + PX4_ERR("terrain: %s geographic units are not degrees, %s", path, kConvertHint); + return false; + } + + } else if (model_type == ModelTypeProjected) { + GTIFKeyGet(gtif, ProjectedCSTypeGeoKey, &epsg, 0, 1); + } + + if (epsg == 0 || epsg == KvUserDefined) { + PX4_ERR("terrain: %s has no EPSG coded CRS, %s", path, kConvertHint); + return false; + } + + // nodata + bool has_nodata = false; + float nodata = 0.f; + char *nodata_str = nullptr; + + if (TIFFGetField(tif, kTiffTagGdalNodata, &nodata_str) && nodata_str) { + char *end = nullptr; + nodata = strtof(nodata_str, &end); + has_nodata = (end != nodata_str) && !std::isnan(nodata); + } + + // samples + src.width = width; + src.height = height; + src.data.resize(static_cast(width) * height); + + const auto store = [&](uint32_t col, uint32_t row, float value) { + if (has_nodata && fabsf(value - nodata) <= FLT_EPSILON * fmaxf(1.f, fabsf(nodata))) { + value = NAN; + } + + src.data[static_cast(row) * width + col] = value; + }; + + if (TIFFIsTiled(tif)) { + uint32_t tile_width = 0; + uint32_t tile_height = 0; + TIFFGetField(tif, TIFFTAG_TILEWIDTH, &tile_width); + TIFFGetField(tif, TIFFTAG_TILELENGTH, &tile_height); + std::vector buffer(TIFFTileSize(tif)); + + for (uint32_t tile_row = 0; tile_row < height; tile_row += tile_height) { + for (uint32_t tile_col = 0; tile_col < width; tile_col += tile_width) { + if (TIFFReadTile(tif, buffer.data(), tile_col, tile_row, 0, 0) < 0) { + PX4_ERR("terrain: %s: failed to read tile at %u,%u", path, tile_col, tile_row); + return false; + } + + for (uint32_t r = 0; r < tile_height && tile_row + r < height; r++) { + for (uint32_t c = 0; c < tile_width && tile_col + c < width; c++) { + store(tile_col + c, tile_row + r, read_sample(buffer.data(), static_cast(r) * tile_width + c)); + } + } + } + } + + } else { + std::vector buffer(TIFFScanlineSize(tif)); + + for (uint32_t row = 0; row < height; row++) { + if (TIFFReadScanline(tif, buffer.data(), row, 0) < 0) { + PX4_ERR("terrain: %s: failed to read row %u", path, row); + return false; + } + + for (uint32_t col = 0; col < width; col++) { + store(col, row, read_sample(buffer.data(), col)); + } + } + } + + _epsg = epsg; + _path = path; + + if (epsg == 4326) { + _grid = std::move(src); + const double lat_center = _grid.y0 + 0.5 * (_grid.height - 1) * _grid.dy; + _cell_size_m = static_cast(kMetersPerDegree * fmin(-_grid.dy, _grid.dx * cos(lat_center * M_PI / 180.))); + + } else if (!resampleToWgs84(src)) { + _grid = Grid{}; + return false; + } + + printStatus(); + + return true; +} + +bool TerrainDem::resampleToWgs84(const Grid &src) +{ + char crs[32]; + snprintf(crs, sizeof(crs), "EPSG:%d", _epsg); + + PJ_CONTEXT *ctx = proj_context_create(); + PJ *wgs84_to_src = nullptr; + + { + PJ *transform = proj_create_crs_to_crs(ctx, "EPSG:4326", crs, nullptr); + + if (transform) { + // lon/lat and easting/northing axis order, whatever the CRS definitions say + wgs84_to_src = proj_normalize_for_visualization(ctx, transform); + proj_destroy(transform); + } + } + + if (!wgs84_to_src) { + PX4_ERR("terrain: PROJ cannot transform WGS84 to %s (%s), %s", crs, + proj_context_errno_string(ctx, proj_context_errno(ctx)), kConvertHint); + proj_context_destroy(ctx); + return false; + } + + // WGS84 bounding box of the source raster, from points along its edges + constexpr int kEdgePoints = 32; + std::vector lon; + std::vector lat; + + for (int i = 0; i <= kEdgePoints; i++) { + const double t = static_cast(i) / kEdgePoints; + const double col = t * (src.width - 1); + const double row = t * (src.height - 1); + const double edge_cols[4] = {col, col, 0., src.width - 1.}; + const double edge_rows[4] = {0., src.height - 1., row, row}; + + for (int edge = 0; edge < 4; edge++) { + lon.push_back(src.x0 + edge_cols[edge] * src.dx); + lat.push_back(src.y0 + edge_rows[edge] * src.dy); + } + } + + // the centre pixel and its neighbours, to keep the source resolution + const double center_col = 0.5 * (src.width - 1); + const double center_row = 0.5 * (src.height - 1); + const size_t center = lon.size(); + lon.insert(lon.end(), {src.x0 + center_col * src.dx, src.x0 + (center_col + 1.) * src.dx, src.x0 + center_col * src.dx}); + lat.insert(lat.end(), {src.y0 + center_row * src.dy, src.y0 + center_row * src.dy, src.y0 + (center_row + 1.) * src.dy}); + + proj_trans_generic(wgs84_to_src, PJ_INV, lon.data(), sizeof(double), lon.size(), lat.data(), sizeof(double), lat.size(), + nullptr, 0, 0, nullptr, 0, 0); + + double lon_min = static_cast(INFINITY); + double lon_max = -static_cast(INFINITY); + double lat_min = static_cast(INFINITY); + double lat_max = -static_cast(INFINITY); + + for (size_t i = 0; i < lon.size(); i++) { + if (!std::isfinite(lon[i]) || !std::isfinite(lat[i])) { + PX4_ERR("terrain: %s extent cannot be transformed to WGS84", _path.c_str()); + proj_destroy(wgs84_to_src); + proj_context_destroy(ctx); + return false; + } + + lon_min = fmin(lon_min, lon[i]); + lon_max = fmax(lon_max, lon[i]); + lat_min = fmin(lat_min, lat[i]); + lat_max = fmax(lat_max, lat[i]); + } + + const double cos_lat = cos(lat[center] * M_PI / 180.); + const double step_col_m = kMetersPerDegree * hypot(lat[center + 1] - lat[center], (lon[center + 1] - lon[center]) * cos_lat); + const double step_row_m = kMetersPerDegree * hypot(lat[center + 2] - lat[center], (lon[center + 2] - lon[center]) * cos_lat); + const double cell_m = fmin(step_col_m, step_row_m); + + Grid dst{}; + dst.dx = cell_m / (kMetersPerDegree * cos_lat); + dst.dy = -cell_m / kMetersPerDegree; + dst.x0 = lon_min; + dst.y0 = lat_max; + dst.width = static_cast((lon_max - lon_min) / dst.dx) + 1; + dst.height = static_cast((lat_max - lat_min) / -dst.dy) + 1; + + if (dst.width < 2 || dst.height < 2 || static_cast(dst.width) * dst.height > kMaxCells) { + PX4_ERR("terrain: %s resampled to WGS84 is %dx%d cells (crop or downsample it)", _path.c_str(), dst.width, dst.height); + proj_destroy(wgs84_to_src); + proj_context_destroy(ctx); + return false; + } + + dst.data.resize(static_cast(dst.width) * dst.height); + + // transform one row of WGS84 cell centres at a time into the source CRS and sample the source there + std::vector x(dst.width); + std::vector y(dst.width); + + for (int row = 0; row < dst.height; row++) { + for (int col = 0; col < dst.width; col++) { + x[col] = dst.x0 + col * dst.dx; + y[col] = dst.y0 + row * dst.dy; + } + + proj_trans_generic(wgs84_to_src, PJ_FWD, x.data(), sizeof(double), x.size(), y.data(), sizeof(double), y.size(), + nullptr, 0, 0, nullptr, 0, 0); + + for (int col = 0; col < dst.width; col++) { + float value; + + if (!src.sample(x[col], y[col], value)) { + value = NAN; + } + + dst.data[static_cast(row) * dst.width + col] = value; + } + } + + proj_destroy(wgs84_to_src); + proj_context_destroy(ctx); + + _grid = std::move(dst); + _cell_size_m = static_cast(cell_m); + return true; +} + +bool TerrainDem::altitude(double lat_deg, double lon_deg, float &alt) const +{ + if (!loaded()) { + return TerrainFlat::altitude(lat_deg, lon_deg, alt); + } + + if (_grid.sample(lon_deg, lat_deg, alt)) { + return true; + } + + alt = _default_alt; + return false; +} + +float TerrainDem::vehicleGroundAltitude(double lat_deg, double lon_deg) +{ + float alt; + const bool inside = altitude(lat_deg, lon_deg, alt); + + if (!inside && !_vehicle_outside) { + PX4_WARN("terrain: vehicle outside the DEM, using flat ground at %.1f m", (double)_default_alt); + + } else if (inside && _vehicle_outside) { + PX4_INFO("terrain: vehicle back inside the DEM"); + } + + _vehicle_outside = !inside; + return alt; +} + +float TerrainDem::rayDistance(double lat_deg, double lon_deg, float alt, const matrix::Vector3f &dir_ned, + float max_distance) const +{ + if (!loaded()) { + return TerrainFlat::rayDistance(lat_deg, lon_deg, alt, dir_ned, max_distance); + } + + if (dir_ned(2) <= 0.f) { + return NAN; + } + + float ground_alt; + altitude(lat_deg, lon_deg, ground_alt); + float prev_height = alt - ground_alt; // height above the terrain along the ray + + if (prev_height <= 0.f) { + return 0.f; + } + + MapProjection ref{lat_deg, lon_deg}; + + // sample each DEM cell at least twice, the bilinear terrain is then followed within centimetres + static constexpr float STEPS_PER_CELL = 2.f; + static constexpr float MAX_STEP = 1.f; // [m] + const float step = std::min(_cell_size_m / STEPS_PER_CELL, MAX_STEP); + float prev_distance = 0.f; + + for (float distance = step; distance < max_distance + step; distance += step) { + const matrix::Vector3f point = dir_ned * distance; + double lat; + double lon; + ref.reproject(point(0), point(1), lat, lon); + altitude(lat, lon, ground_alt); + const float height = (alt - point(2)) - ground_alt; + + if (height <= 0.f) { + // interpolate the crossing between the last two samples + return prev_distance + (distance - prev_distance) * prev_height / (prev_height - height); + } + + prev_distance = distance; + prev_height = height; + } + + return NAN; +} + +void TerrainDem::printStatus() const +{ + if (!loaded()) { + TerrainFlat::printStatus(); + return; + } + + PX4_INFO("terrain: %s", path()); + PX4_INFO("terrain: EPSG:%d, %dx%d cells of %.1f m", epsg(), width(), height(), (double)_cell_size_m); + PX4_INFO("terrain: lat [%.5f, %.5f] lon [%.5f, %.5f]", latMin(), latMax(), lonMin(), lonMax()); + + if (_vehicle_outside) { + PX4_INFO("terrain: vehicle outside the DEM"); + } +} diff --git a/src/modules/simulation/simulator_sih/terrain/TerrainDem.hpp b/src/modules/simulation/simulator_sih/terrain/TerrainDem.hpp new file mode 100644 index 00000000000..eb15399f3f9 --- /dev/null +++ b/src/modules/simulation/simulator_sih/terrain/TerrainDem.hpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file TerrainDem.hpp + * Terrain elevation model loaded from a GeoTIFF DEM, for SIH in SITL. + * + * The raster is read with libtiff/libgeotiff. A DEM in any CRS with an EPSG code is + * resampled once at load time with PROJ onto a regular WGS84 lat/lon grid, so that + * altitude() is a cheap bilinear lookup in the simulation loop. Until a DEM is loaded the terrain is flat. + */ + +#pragma once + +#include "TerrainFlat.hpp" + +#include +#include + +class TerrainDem : public TerrainFlat +{ +public: + TerrainDem() = default; + + /** load a single-band GeoTIFF DEM, check loaded() for the result (the reason of a failure is printed) */ + explicit TerrainDem(const char *path) { load(path); } + + /** load a single-band GeoTIFF DEM, @return false (the reason is printed) on error */ + bool load(const char *path) override; + + bool loaded() const { return !_grid.data.empty(); } + + /** DEM elevation interpolated bilinearly, the default altitude outside the DEM or next to a nodata cell */ + bool altitude(double lat_deg, double lon_deg, float &alt) const override; + + /** the DEM elevation, warns when the vehicle leaves the DEM */ + float vehicleGroundAltitude(double lat_deg, double lon_deg) override; + + /** march along the ray, sampling each DEM cell at least twice */ + float rayDistance(double lat_deg, double lon_deg, float alt, const matrix::Vector3f &dir_ned, + float max_distance) const override; + + void printStatus() const override; + +private: + friend class TerrainDemTest; // unit test, writes GeoTIFF files and checks the parsed metadata + + /** smallest grid spacing [m] */ + float cellSizeM() const { return _cell_size_m; } + + const char *path() const { return _path.c_str(); } + int epsg() const { return _epsg; } + int width() const { return _grid.width; } + int height() const { return _grid.height; } + + double latMin() const { return _grid.y0 + (_grid.height - 1) * _grid.dy; } + double latMax() const { return _grid.y0; } + double lonMin() const { return _grid.x0; } + double lonMax() const { return _grid.x0 + (_grid.width - 1) * _grid.dx; } + + /** register the GDAL_NODATA TIFF tag with libtiff */ + static void registerTiffTags(); + + static constexpr unsigned kTiffTagGdalNodata = 42113; + + // regular raster, value of cell (col, row) at x = x0 + col * dx, y = y0 + row * dy + struct Grid { + std::vector data; // row-major, NAN for nodata + int width{0}; + int height{0}; + double x0{0.}; + double y0{0.}; + double dx{0.}; + double dy{0.}; + + bool sample(double x, double y, float &value) const; + }; + + bool resampleToWgs84(const Grid &src); + + Grid _grid{}; // x: longitude [deg], y: latitude [deg], dy < 0 (row 0 is the northern edge) + float _cell_size_m{0.f}; + bool _vehicle_outside{false}; + int _epsg{0}; + std::string _path; +}; diff --git a/src/modules/simulation/simulator_sih/terrain/TerrainDemTest.cpp b/src/modules/simulation/simulator_sih/terrain/TerrainDemTest.cpp new file mode 100644 index 00000000000..8f91272ab91 --- /dev/null +++ b/src/modules/simulation/simulator_sih/terrain/TerrainDemTest.cpp @@ -0,0 +1,394 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#define MODULE_NAME "TerrainDemTest" + +#include + +#include "TerrainDem.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +// TerrainDem logs with PX4_INFO/PX4_WARN/PX4_ERR, satisfying them from px4_platform drags uORB and +// the POSIX daemon into this host unit test +__BEGIN_DECLS + +void px4_log_info(const char *moduleName, const char *fmt, ...) +{ + va_list args; + va_start(args, fmt); + vfprintf(stdout, fmt, args); + va_end(args); + fputc('\n', stdout); +} + +void px4_log_warn(const char *moduleName, const char *fmt, ...) +{ + va_list args; + va_start(args, fmt); + vfprintf(stderr, fmt, args); + va_end(args); + fputc('\n', stderr); +} + +void px4_log_error(const char *moduleName, const char *fmt, ...) +{ + va_list args; + va_start(args, fmt); + vfprintf(stderr, fmt, args); + va_end(args); + fputc('\n', stderr); +} + +__END_DECLS + +// friend of TerrainDem, gives the tests access to its private metadata +class TerrainDemTest : public testing::Test +{ +public: + static void registerTiffTags() { TerrainDem::registerTiffTags(); } + static constexpr unsigned kTiffTagGdalNodata = TerrainDem::kTiffTagGdalNodata; + + static int epsg(const TerrainDem &dem) { return dem.epsg(); } + static int width(const TerrainDem &dem) { return dem.width(); } + static int height(const TerrainDem &dem) { return dem.height(); } + static float cellSizeM(const TerrainDem &dem) { return dem.cellSizeM(); } +}; + +namespace +{ + +struct TiffSpec { + uint32_t width{3}; + uint32_t height{3}; + unsigned short model_type{ModelTypeGeographic}; + unsigned short epsg{4326}; + double corner_x{8.0}; // model coordinates of the corner of pixel (0, 0) + double corner_y{47.0}; + double scale_x{0.01}; + double scale_y{0.01}; + bool pixel_is_point{false}; + uint16_t bits{32}; + uint16_t format{SAMPLEFORMAT_IEEEFP}; + uint16_t compression{COMPRESSION_NONE}; + uint32_t tile{0}; // 0: strips + uint16_t bands{1}; + const char *nodata{nullptr}; +}; + +using ValueFn = std::function; + +void writeSample(uint8_t *buffer, size_t index, const TiffSpec &spec, double value) +{ + if (spec.format == SAMPLEFORMAT_IEEEFP) { + const float v = static_cast(value); + memcpy(buffer + 4 * index, &v, 4); + + } else { + const int16_t v = static_cast(lround(value)); + memcpy(buffer + 2 * index, &v, 2); + } +} + +std::string writeTiff(const std::string &name, const TiffSpec &spec, const ValueFn &value) +{ + TerrainDemTest::registerTiffTags(); + + const std::string path = testing::TempDir() + name; + TIFF *tif = XTIFFOpen(path.c_str(), "w"); + + if (!tif) { + ADD_FAILURE() << "cannot create " << path; + return path; + } + + TIFFSetField(tif, TIFFTAG_IMAGEWIDTH, spec.width); + TIFFSetField(tif, TIFFTAG_IMAGELENGTH, spec.height); + TIFFSetField(tif, TIFFTAG_SAMPLESPERPIXEL, spec.bands); + TIFFSetField(tif, TIFFTAG_BITSPERSAMPLE, spec.bits); + TIFFSetField(tif, TIFFTAG_SAMPLEFORMAT, spec.format); + TIFFSetField(tif, TIFFTAG_PLANARCONFIG, PLANARCONFIG_CONTIG); + TIFFSetField(tif, TIFFTAG_PHOTOMETRIC, PHOTOMETRIC_MINISBLACK); + TIFFSetField(tif, TIFFTAG_COMPRESSION, spec.compression); + + if (spec.tile) { + TIFFSetField(tif, TIFFTAG_TILEWIDTH, spec.tile); + TIFFSetField(tif, TIFFTAG_TILELENGTH, spec.tile); + + } else { + TIFFSetField(tif, TIFFTAG_ROWSPERSTRIP, 1); + } + + if (spec.nodata) { + TIFFSetField(tif, TerrainDemTest::kTiffTagGdalNodata, spec.nodata); + } + + double tiepoint[6] = {0., 0., 0., spec.corner_x, spec.corner_y, 0.}; + double scale[3] = {spec.scale_x, spec.scale_y, 0.}; + TIFFSetField(tif, TIFFTAG_GEOTIEPOINTS, 6, tiepoint); + TIFFSetField(tif, TIFFTAG_GEOPIXELSCALE, 3, scale); + + GTIF *gtif = GTIFNew(tif); + GTIFKeySet(gtif, GTModelTypeGeoKey, TYPE_SHORT, 1, spec.model_type); + GTIFKeySet(gtif, GTRasterTypeGeoKey, TYPE_SHORT, 1, spec.pixel_is_point ? RasterPixelIsPoint : RasterPixelIsArea); + GTIFKeySet(gtif, spec.model_type == ModelTypeGeographic ? GeographicTypeGeoKey : ProjectedCSTypeGeoKey, + TYPE_SHORT, 1, spec.epsg); + GTIFWriteKeys(gtif); + GTIFFree(gtif); + + const size_t sample_size = spec.bits / 8; + + if (spec.tile) { + std::vector buffer(spec.tile * spec.tile * spec.bands * sample_size); + + for (uint32_t tile_row = 0; tile_row < spec.height; tile_row += spec.tile) { + for (uint32_t tile_col = 0; tile_col < spec.width; tile_col += spec.tile) { + std::fill(buffer.begin(), buffer.end(), 0); + + for (uint32_t r = 0; r < spec.tile && tile_row + r < spec.height; r++) { + for (uint32_t c = 0; c < spec.tile && tile_col + c < spec.width; c++) { + for (uint16_t b = 0; b < spec.bands; b++) { + writeSample(buffer.data(), (r * spec.tile + c) * spec.bands + b, spec, value(tile_col + c, tile_row + r)); + } + } + } + + TIFFWriteTile(tif, buffer.data(), tile_col, tile_row, 0, 0); + } + } + + } else { + std::vector buffer(spec.width * spec.bands * sample_size); + + for (uint32_t row = 0; row < spec.height; row++) { + for (uint32_t col = 0; col < spec.width; col++) { + for (uint16_t b = 0; b < spec.bands; b++) { + writeSample(buffer.data(), col * spec.bands + b, spec, value(col, row)); + } + } + + TIFFWriteScanline(tif, buffer.data(), row, 0); + } + } + + XTIFFClose(tif); + return path; +} + +// WGS84 position of the centre of pixel (col, row) of a geographic PixelIsArea raster +double pixelLon(const TiffSpec &spec, double col) { return spec.corner_x + (col + 0.5) * spec.scale_x; } +double pixelLat(const TiffSpec &spec, double row) { return spec.corner_y - (row + 0.5) * spec.scale_y; } + +double ramp(uint32_t col, uint32_t row) { return 400. + 10. * col + 100. * row; } + +} // namespace + +TEST_F(TerrainDemTest, MissingFile) +{ + TerrainDem dem("/nonexistent/dem.tif"); + EXPECT_FALSE(dem.loaded()); +} + +TEST_F(TerrainDemTest, GeographicFloat32) +{ + const TiffSpec spec{}; + TerrainDem dem(writeTiff("float32.tif", spec, ramp).c_str()); + ASSERT_TRUE(dem.loaded()); + EXPECT_EQ(epsg(dem), 4326); + EXPECT_EQ(width(dem), 3); + EXPECT_EQ(height(dem), 3); + + float alt = 0.f; + + // cell centres + for (uint32_t row = 0; row < spec.height; row++) { + for (uint32_t col = 0; col < spec.width; col++) { + ASSERT_TRUE(dem.altitude(pixelLat(spec, row), pixelLon(spec, col), alt)); + EXPECT_FLOAT_EQ(alt, ramp(col, row)); + } + } + + // bilinear between cell centres + ASSERT_TRUE(dem.altitude(pixelLat(spec, 0.5), pixelLon(spec, 1.25), alt)); + EXPECT_NEAR(alt, 400. + 12.5 + 50., 1e-3); + + // within the outer half cell the edge value is used + ASSERT_TRUE(dem.altitude(spec.corner_y - 0.001, spec.corner_x + 0.001, alt)); + EXPECT_FLOAT_EQ(alt, ramp(0, 0)); + + // outside + EXPECT_FALSE(dem.altitude(spec.corner_y + 0.001, pixelLon(spec, 1), alt)); + EXPECT_FALSE(dem.altitude(pixelLat(spec, 1), spec.corner_x + 0.031, alt)); + EXPECT_FALSE(dem.altitude(NAN, NAN, alt)); + + EXPECT_NEAR(cellSizeM(dem), 0.01 * 6371000. * M_PI / 180. * cos(46.985 * M_PI / 180.), 1.); +} + +TEST_F(TerrainDemTest, VehicleGroundAltitude) +{ + const TiffSpec spec{}; + TerrainDem dem; + dem.setDefaultAltitude(123.f); + + // flat until a DEM is loaded + EXPECT_FLOAT_EQ(dem.vehicleGroundAltitude(pixelLat(spec, 1), pixelLon(spec, 1)), 123.f); + + ASSERT_TRUE(dem.load(writeTiff("vehicle.tif", spec, ramp).c_str())); + EXPECT_FLOAT_EQ(dem.vehicleGroundAltitude(pixelLat(spec, 1), pixelLon(spec, 1)), ramp(1, 1)); + + // outside the DEM the ground is flat at the default altitude, and back on the DEM when reentering + EXPECT_FLOAT_EQ(dem.vehicleGroundAltitude(spec.corner_y + 0.001, pixelLon(spec, 1)), 123.f); + EXPECT_FLOAT_EQ(dem.vehicleGroundAltitude(pixelLat(spec, 2), pixelLon(spec, 0)), ramp(0, 2)); +} + +TEST_F(TerrainDemTest, Int16LzwTiled) +{ + TiffSpec spec{}; + spec.width = 21; // partial tiles + spec.height = 19; + spec.bits = 16; + spec.format = SAMPLEFORMAT_INT; + spec.compression = COMPRESSION_LZW; + spec.tile = 16; + + TerrainDem dem(writeTiff("int16_lzw_tiled.tif", spec, ramp).c_str()); + ASSERT_TRUE(dem.loaded()); + + float alt = 0.f; + const uint32_t points[][2] = {{0, 0}, {15, 15}, {16, 16}, {20, 18}, {17, 3}}; + + for (const auto &p : points) { + ASSERT_TRUE(dem.altitude(pixelLat(spec, p[1]), pixelLon(spec, p[0]), alt)); + EXPECT_FLOAT_EQ(alt, ramp(p[0], p[1])); + } + + ASSERT_TRUE(dem.altitude(pixelLat(spec, 15.5), pixelLon(spec, 15.5), alt)); + EXPECT_NEAR(alt, 400. + 155. + 1550., 1e-2); +} + +TEST_F(TerrainDemTest, PixelIsPoint) +{ + TiffSpec spec{}; + spec.pixel_is_point = true; + + TerrainDem dem(writeTiff("pixel_is_point.tif", spec, ramp).c_str()); + ASSERT_TRUE(dem.loaded()); + + // the tiepoint is the centre of pixel (0, 0) + float alt = 0.f; + ASSERT_TRUE(dem.altitude(spec.corner_y, spec.corner_x, alt)); + EXPECT_FLOAT_EQ(alt, ramp(0, 0)); + ASSERT_TRUE(dem.altitude(spec.corner_y - 2. * spec.scale_y, spec.corner_x + 2. * spec.scale_x, alt)); + EXPECT_FLOAT_EQ(alt, ramp(2, 2)); +} + +TEST_F(TerrainDemTest, Nodata) +{ + TiffSpec spec{}; + spec.width = 4; + spec.nodata = "-9999"; + + TerrainDem dem(writeTiff("nodata.tif", spec, [](uint32_t col, uint32_t row) { + return (col == 3 && row == 2) ? -9999. : ramp(col, row); + }).c_str()); + ASSERT_TRUE(dem.loaded()); + + float alt = 0.f; + EXPECT_TRUE(dem.altitude(pixelLat(spec, 0.5), pixelLon(spec, 0.5), alt)); + EXPECT_TRUE(dem.altitude(pixelLat(spec, 1), pixelLon(spec, 2), alt)); + // interpolating with the nodata cell + EXPECT_FALSE(dem.altitude(pixelLat(spec, 1.5), pixelLon(spec, 2.5), alt)); + EXPECT_FALSE(dem.altitude(pixelLat(spec, 2), pixelLon(spec, 3), alt)); +} + +TEST_F(TerrainDemTest, RejectsMultipleBands) +{ + TiffSpec spec{}; + spec.bands = 3; + + TerrainDem dem(writeTiff("rgb.tif", spec, ramp).c_str()); + EXPECT_FALSE(dem.loaded()); +} + +TEST_F(TerrainDemTest, RejectsUserDefinedCrs) +{ + TiffSpec spec{}; + spec.epsg = KvUserDefined; + + TerrainDem dem(writeTiff("user_defined.tif", spec, ramp).c_str()); + EXPECT_FALSE(dem.loaded()); +} + +TEST_F(TerrainDemTest, ProjectedUtm) +{ + // 1 km x 1 km at 10 m in UTM zone 32N (EPSG:32632) around Zurich, a plane rising towards north-east + TiffSpec spec{}; + spec.width = 100; + spec.height = 100; + spec.model_type = ModelTypeProjected; + spec.epsg = 32632; + spec.corner_x = 465000.; + spec.corner_y = 5250000.; + spec.scale_x = 10.; + spec.scale_y = 10.; + + const auto plane = [](double easting, double northing) { + return 500. + 0.1 * (easting - 465000.) + 0.05 * (northing - 5249000.); + }; + + TerrainDem dem(writeTiff("utm.tif", spec, [&](uint32_t col, uint32_t row) { + return plane(spec.corner_x + (col + 0.5) * spec.scale_x, spec.corner_y - (row + 0.5) * spec.scale_y); + }).c_str()); + ASSERT_TRUE(dem.loaded()); + + EXPECT_EQ(epsg(dem), 32632); + EXPECT_NEAR(cellSizeM(dem), 10.f, 0.5f); + + // UTM 32N E 465500 N 5249500 (centre of the raster) is 47.3980425 N, 8.5427984 E (cs2cs EPSG:32632 EPSG:4326) + float alt = 0.f; + ASSERT_TRUE(dem.altitude(47.3980425, 8.5427984, alt)); + EXPECT_NEAR(alt, plane(465500., 5249500.), 0.5); + + // outside the raster + EXPECT_FALSE(dem.altitude(47.40, 8.70, alt)); +} diff --git a/src/modules/simulation/simulator_sih/terrain/TerrainFlat.hpp b/src/modules/simulation/simulator_sih/terrain/TerrainFlat.hpp new file mode 100644 index 00000000000..fc0820481b4 --- /dev/null +++ b/src/modules/simulation/simulator_sih/terrain/TerrainFlat.hpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file TerrainFlat.hpp + * Flat terrain at the default altitude, the base of every SIH terrain model. + */ + +#pragma once + +#include +#include + +#include +#include + +class TerrainFlat +{ +public: + virtual ~TerrainFlat() = default; + + /** load terrain data from a file, flat terrain has none, @return false (the reason is printed) */ + virtual bool load(const char *path) + { + PX4_ERR("terrain: cannot load %s, PX4 was built without terrain DEM support", path); + return false; + } + + /** flat ground altitude [m AMSL], also used where the terrain model has no data */ + void setDefaultAltitude(float alt) { _default_alt = alt; } + float defaultAltitude() const { return _default_alt; } + + /** + * Terrain altitude [m AMSL] at a WGS84 position. + * @return false outside the terrain data, alt is then the default altitude + */ + virtual bool altitude(double lat_deg, double lon_deg, float &alt) const + { + alt = _default_alt; + return true; + } + + /** + * Terrain altitude [m AMSL] below the vehicle at a WGS84 position, unlike altitude() it + * keeps track of the vehicle leaving and reentering the terrain data. + */ + virtual float vehicleGroundAltitude(double lat_deg, double lon_deg) + { + return _default_alt; + } + + /** + * Distance [m] from a WGS84 position along a direction to the terrain. + * @param alt altitude of the start point [m AMSL] + * @param dir_ned unit vector in the local NED frame + * @param max_distance search range [m], a result above it may be returned + * @return NAN if the ray does not hit the terrain within max_distance + */ + virtual float rayDistance(double lat_deg, double lon_deg, float alt, const matrix::Vector3f &dir_ned, + float max_distance) const + { + const float cos_tilt = dir_ned(2); + + if (cos_tilt < FLT_EPSILON) { + // pointing away from the ground + return NAN; + } + + // the fake ground lets the vehicle settle marginally below the ground altitude, clamp it + // so that a landed vehicle reads zero instead of dropping out of range + const float height = alt - _default_alt; + return (height > 0.f ? height : 0.f) / cos_tilt; + } + + virtual void printStatus() const + { + PX4_INFO("terrain: flat at %.2f m", (double)_default_alt); + } + +protected: + float _default_alt{0.f}; +};