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 <beniamino.pozzan@auterion.com>

* 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 <beniamino.pozzan@auterion.com>

* 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 <beniamino.pozzan@auterion.com>

* docs(sim_sih): document terrain elevation from a GeoTIFF DEM

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Beniamino Pozzan <beniamino.pozzan@auterion.com>

* 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 <hamishwillee@gmail.com>

* fix: cast to double for MAC builds

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@auterion.com>

* chore: implement TerrainBase interface

Assisted-by: Claude:claude-opus-5-5

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@auterion.com>

* chore: improve isolation of terrain logic from sih core

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@auterion.com>

* 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 <beniamino.pozzan@auterion.com>
Signed-off-by: Hamish Willee <hamishwillee@gmail.com>
Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
authored and GitHub committed 2026-11-12 16:49:11 +11:00
1 parent 5a533b77d1
commit cb00217286
13 files changed
+1462 -32

No files matched your search

@@ -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
+7
View File
@@ -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
+1
View File
@@ -140,6 +140,7 @@ if [[ $INSTALL_SIM == "--sim-tools" ]]; then
exiftool
glog
graphviz
libgeotiff # SIH terrain elevation (GeoTIFF DEM), pulls libtiff and proj
pixi
)
+3
View File
@@ -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+)
+43
View File
@@ -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}
<Badge type="tip" text="main (PX4 v2.0)" />
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.
@@ -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
+94 -27
View File
@@ -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<VehicleType>(constrain(_sih_vtype.get(),
static_cast<int32_t>(VehicleType::First),
static_cast<int32_t>(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<VehicleType>(constrain(_sih_vtype.get(),
static_cast<int32_t>(VehicleType::First),
static_cast<int32_t>(VehicleType::Last)));
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
lockstep_loop();
@@ -408,13 +411,19 @@ void Sih::parameters_updated()
if (!_lpos_ref.isInitialized()
|| (fabsf(static_cast<float>(_lpos_ref.getProjectionReferenceLat()) - _sih_lat0.get()) > FLT_EPSILON)
|| (fabsf(static_cast<float>(_lpos_ref.getProjectionReferenceLon()) - _sih_lon0.get()) > FLT_EPSILON)
|| (fabsf(_lpos_ref_alt - _sih_h0.get()) > FLT_EPSILON)) {
_lpos_ref.initReference(static_cast<double>(_sih_lat0.get()), static_cast<double>(_sih_lon0.get()));
_lpos_ref_alt = _sih_h0.get();
|| (fabsf(_terrain.defaultAltitude() - _sih_h0.get()) > FLT_EPSILON)) {
const double lat0 = static_cast<double>(_sih_lat0.get());
const double lon0 = static_cast<double>(_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<double>(_sih_lat0.get()));
_lla.setLongitudeDeg(static_cast<double>(_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<VehicleType>(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, "<file>", "Terrain elevation GeoTIFF (DEM), SITL only", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
+13 -4
View File
@@ -92,6 +92,8 @@
#include <sys/time.h>
#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
@@ -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
@@ -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 <lib/geo/geo.h>
#include <px4_platform_common/log.h>
#include <geotiff.h>
#include <geovalues.h>
#include <xtiffio.h>
#include <proj.h>
#include <algorithm>
#include <cfloat>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>
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 <in> <out.tif>";
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<float>(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<float>(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<float>(v); };
}
} else if (format == SAMPLEFORMAT_INT) {
if (bits == 8) {
read = [](const uint8_t *p, size_t i) { return static_cast<float>(static_cast<int8_t>(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<float>(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<float>(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<float>(v); };
}
}
return read != nullptr;
}
} // namespace
void TerrainDem::registerTiffTags()
{
static const TIFFFieldInfo field_info[] = {
{kTiffTagGdalNodata, -1, -1, TIFF_ASCII, FIELD_CUSTOM, 1, 0, const_cast<char *>("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<int>(cx), width - 2);
const int row = std::min(static_cast<int>(cy), height - 2);
const float tx = static_cast<float>(cx - col);
const float ty = static_cast<float>(cy - row);
const size_t i = static_cast<size_t>(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<size_t>(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<size_t>(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<size_t>(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<uint8_t> 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<size_t>(r) * tile_width + c));
}
}
}
}
} else {
std::vector<uint8_t> 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<float>(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<double> lon;
std::vector<double> lat;
for (int i = 0; i <= kEdgePoints; i++) {
const double t = static_cast<double>(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<double>(INFINITY);
double lon_max = -static_cast<double>(INFINITY);
double lat_min = static_cast<double>(INFINITY);
double lat_max = -static_cast<double>(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<int>((lon_max - lon_min) / dst.dx) + 1;
dst.height = static_cast<int>((lat_max - lat_min) / -dst.dy) + 1;
if (dst.width < 2 || dst.height < 2 || static_cast<size_t>(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<size_t>(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<double> x(dst.width);
std::vector<double> 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<size_t>(row) * dst.width + col] = value;
}
}
proj_destroy(wgs84_to_src);
proj_context_destroy(ctx);
_grid = std::move(dst);
_cell_size_m = static_cast<float>(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");
}
}
@@ -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 <string>
#include <vector>
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<float> 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;
};
@@ -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 <gtest/gtest.h>
#include "TerrainDem.hpp"
#include <geotiff.h>
#include <geovalues.h>
#include <xtiffio.h>
#include <cmath>
#include <cstdarg>
#include <cstdio>
#include <cstring>
#include <functional>
#include <string>
#include <vector>
// 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<double(uint32_t col, uint32_t row)>;
void writeSample(uint8_t *buffer, size_t index, const TiffSpec &spec, double value)
{
if (spec.format == SAMPLEFORMAT_IEEEFP) {
const float v = static_cast<float>(value);
memcpy(buffer + 4 * index, &v, 4);
} else {
const int16_t v = static_cast<int16_t>(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<uint8_t> 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<uint8_t> 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));
}
@@ -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 <float.h>
#include <math.h>
#include <matrix/matrix/math.hpp>
#include <px4_platform_common/log.h>
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};
};