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};
+};