UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-27 00:48:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(navigator): rtl compute wind angle to select best land approach based on rally point location instead of home location (#27004)

Browse Source
This commit is contained in:
Jonas Perolini
2026-06-04 05:55:31 +02:00
committed by GitHub
parent af2e16cd7e
commit 50c57be15f
12 changed files with 1369 additions and 164 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
docs/en/flight_modes/return.md
View File

@@ -92,7 +92,11 @@ In this return type the vehicle:
- If the destination is a mission landing pattern it will follow the pattern to land.
- If the destination is a rally point or home it will [land or wait](#loiter-landing-at-destination) at descent altitude (depending on landing parameters).
By default an MC or VTOL in MC mode will land, and a fixed-wing vehicle circles at the descent altitude.
A VTOL in FW mode aligns its heading to the destination point, transitions to MC mode, and then lands.
A VTOL in FW mode can first fly a _VTOL approach loiter_ associated with that destination.
A _VTOL approach loiter_ is a [MAV_CMD_NAV_LOITER_TO_ALT](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_LOITER_TO_ALT) item that the vehicle uses to descend to the approach altitude.
For upload details, see [VTOL Fixed-wing Mode Return](../flight_modes_vtol/return.md#fixed-wing-mode-fw-return).
If several approach loiters are defined for the destination, PX4 chooses the most wind-aligned one. It then flies from the approach loiter to the destination point, transitions to MC mode at the destination, and lands.
::: info
Fixed wing vehicles commonly also set [MIS_TKO_LAND_REQ](#MIS_TKO_LAND_REQ) to _require_ a mission landing pattern.
@@ -198,7 +202,8 @@ The default landing configuration is vehicle dependent:
- Fixed-wing vehicles use a return mode with a [mission landing pattern](#mission-landing-pattern), as this enables automated landing.
If not using a mission landing, the default configuration is to loiter indefinitely, so the user can take over and manually land.
- VTOLs in MC mode fly and land exactly as a multicopter.
- VTOLS in FW mode head towards the landing point, transition to MC mode, and then land on the destination.
- VTOLs in FW mode head towards the landing point, transition to MC mode, and then land on the destination.
If a VTOL approach loiter is defined for a rally point or home location, the vehicle uses that loiter to reach the approach altitude, then flies to the destination before back-transitioning.
## Mission Landing Pattern
@@ -220,6 +225,7 @@ The RTL parameters are listed in [Parameter Reference > Return Mode](../advanced
| <a id="RTL_LAND_DELAY"></a>[RTL_LAND_DELAY](../advanced_config/parameter_reference.md#RTL_LAND_DELAY) | Time to wait at `RTL_DESCEND_ALT` before landing (default: 0.5s) -by default this period is short so that the vehicle will simply slow and then land immediately. If set to -1 the system will loiter at `RTL_DESCEND_ALT` rather than landing. The delay is provided to allow you to configure time for landing gear to be deployed (triggered automatically). |
| <a id="RTL_MIN_DIST"></a>[RTL_MIN_DIST](../advanced_config/parameter_reference.md#RTL_MIN_DIST) | Minimum horizontal distance from home position to trigger ascent to the return altitude specified by the "cone". If the vehicle is horizontally closer than this distance to home, it will return at its current altitude or `RTL_DESCEND_ALT` (whichever is higher) instead of first ascending to RTL_RETURN_ALT. |
| <a id="RTL_CONE_ANG"></a>[RTL_CONE_ANG](../advanced_config/parameter_reference.md#RTL_CONE_ANG) | Half-angle of the cone that defines the vehicle RTL return altitude. Values (in degrees): 0, 25, 45, 65, 80, 90. Note that 0 is "no cone" (always return at `RTL_RETURN_ALT` or higher), while 90 indicates that the vehicle must return at the current altitude or `RTL_DESCEND_ALT` (whichever is higher). |
| <a id="RTL_APPR_FORCE"></a>[RTL_APPR_FORCE](../advanced_config/parameter_reference.md#RTL_APPR_FORCE) | [VTOL FW only] If set, home or rally-point RTL destinations are only considered when a valid VTOL approach loiter is defined for that landing location. Mission landing patterns are unaffected. |
| <a id="COM_RC_OVERRIDE"></a>[COM_RC_OVERRIDE](../advanced_config/parameter_reference.md#COM_RC_OVERRIDE) | Controls whether stick movement on a multicopter (or VTOL in MC mode) causes a mode change to [Position mode](../flight_modes_mc/position.md) (except when vehicle is handling a critical battery failsafe). This can be separately enabled for auto modes and for offboard mode, and is enabled in auto modes by default. |
| <a id="COM_RC_STICK_OV"></a>[COM_RC_STICK_OV](../advanced_config/parameter_reference.md#COM_RC_STICK_OV) | The amount of stick movement that causes a transition to [Position mode](../flight_modes_mc/position.md) (if [COM_RC_OVERRIDE](#COM_RC_OVERRIDE) is enabled). |
| <a id="RTL_LOITER_RAD"></a>[RTL_LOITER_RAD](../advanced_config/parameter_reference.md#RTL_LOITER_RAD) | [Fixed-wing Only] The radius of the loiter circle (at [RTL_LAND_DELAY](#RTL_LAND_DELAY)). |

43
docs/en/flight_modes_vtol/return.md
View File

@@ -7,7 +7,7 @@ The _Return_ flight mode is used to _fly a vehicle to safety_ on an unobstructed
VTOL vehicles use the [Mission Landing/Rally Point](../flight_modes/return.md#mission-landing-rally-point-return-type-rtl-type-1) return type by default.
In this return type a vehicle ascends to a minimum safe altitude above obstructions (if needed), and then flies directly to a rally point or the start of a mission landing point (whichever is nearest), or the home position if neither rally points or mission landing pattern is defined.
If the destination is a mission landing pattern, the vehicle will then follow the pattern to land.
If the destination is a rally point or the home location, the vehicle will fly back to the home position and land.
If the destination is a rally point or the home location, the vehicle will fly to that destination and land.
The vehicle will return using the flying mode (MC or FW) it was using at the point when return mode was triggered.
Generally it will follow the same return mode behaviour of the corresponding vehicle type, but will always transition to MC mode (if needed) before landing.
@@ -47,13 +47,47 @@ If returning as a fixed-wing, the vehicle:
A mission landing pattern for a VTOL vehicle consists of a [MAV_CMD_DO_LAND_START](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_LAND_START), one or more position waypoints, and a [MAV_CMD_NAV_VTOL_LAND](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_VTOL_LAND).
- If the destination is a rally point or home it will:
- Loiter/spiral down to [RTL_DESCEND_ALT](#RTL_DESCEND_ALT).
- Fly to the selected [VTOL approach loiter](#vtol-rally-point-approach-loiters) associated with that landing location (if any are defined) and use it to descend to the approach altitude.
If several approach loiters are defined for that location, PX4 chooses the one that best matches the estimated wind at the landing point.
See [VTOL Rally Point Approach Loiter](#vtol-rally-point-approach-loiters) below for information on how to define approach loiters (using [MAV_CMD_NAV_LOITER_TO_ALT](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_LOITER_TO_ALT) items in your _rally plan_).
- Loiter/spiral down to the approach altitude, or to [RTL_DESCEND_ALT](#RTL_DESCEND_ALT) above the destination if no approach altitude is defined.
- Circle for a short time, as defined by [RTL_LAND_DELAY](#RTL_LAND_DELAY).
- Yaw towards the destination (centre of loiter).
- Transition to MC mode and land.
- Fly from the approach loiter to the return destination (the rally point or home location).
- Transition to MC mode at the destination and land.
Note that [NAV_FORCE_VT](../advanced_config/parameter_reference.md#NAV_FORCE_VT) is ignored: the vehicle will always land as a multicopter for these destinations.
#### VTOL Rally Point Approach Loiters
VTOL _rally point approach loiters_ are [MAV_CMD_NAV_LOITER_TO_ALT](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_LOITER_TO_ALT) items associated with a particular rally point ([MAV_CMD_NAV_RALLY_POINT](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_RALLY_POINT)) in a [Rally plan](../flying/plan_safety_points.md).
They define options for where the VTOL can descend to the rally point approach altitude, and how it will approach the rally point.
Several approach loiters can be defined for a rally point, and PX4 will choose the one that best matches the estimated wind at the landing point.
::: tip
The `MAV_CMD_NAV_LOITER_TO_ALT` items that define approach loiters are associated with a Rally/safety point, and are hence part of the rally point plan when used in this way — _not_ the mission plan.
This behaviour is not defined in the MAVLink rally point plan specification.
:::
When uploading VTOL approach loiters through MAVLink, [upload them as rally/safe-point](https://mavlink.io/en/services/mission.html#mission_types) mission items (`MAV_MISSION_TYPE_RALLY`).
The `MAV_CMD_NAV_RALLY_POINT` item must come first, followed by one or more `MAV_CMD_NAV_LOITER_TO_ALT` items that define the approach loiters for that rally point.
The next `MAV_CMD_NAV_RALLY_POINT` starts a new landing-location block.
For each `MAV_CMD_NAV_LOITER_TO_ALT` item, `x/y/z` define the loiter center and approach altitude, and `param2` defines the loiter radius used by RTL.
If `param2` is unset or zero, PX4 falls back to [RTL_LOITER_RAD](#RTL_LOITER_RAD).
For example, a rally upload with one rally point and two possible approach loiters would use:
| Sequence | Command | Purpose | Key fields |
| -------- | ------------------------------------------------------------------------------------------------- | --------------------------- | ---------------------------------------------------------------------- |
| 0 | [MAV_CMD_NAV_RALLY_POINT](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_RALLY_POINT) | Landing location | `x/y/z`: rally latitude, longitude, altitude |
| 1 | [MAV_CMD_NAV_LOITER_TO_ALT](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_LOITER_TO_ALT) | First VTOL approach loiter | `x/y/z`: loiter latitude, longitude, altitude; `param2`: loiter radius |
| 2 | [MAV_CMD_NAV_LOITER_TO_ALT](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_LOITER_TO_ALT) | Second VTOL approach loiter | `x/y/z`: loiter latitude, longitude, altitude; `param2`: loiter radius |
Note that the approach loiter is not the back-transition point.
If the selected approach loiter is far from the rally point or home location, the vehicle remains in fixed-wing mode after the loiter, flies to the destination at the approach altitude, and only then back-transitions for landing.
## Multicopter Mode (MC) Return
If returning as a multicopter:
@@ -71,6 +105,7 @@ The others are relevant if the destination is a rally point or the home location
| Parameter | Description |
| ----------------------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| <a id="RTL_TYPE"></a>[RTL_TYPE](../advanced_config/parameter_reference.md#RTL_TYPE) | Return type. |
| <a id="RTL_APPR_FORCE"></a>[RTL_APPR_FORCE](../advanced_config/parameter_reference.md#RTL_APPR_FORCE) | [VTOL FW only] If set, PX4 only considers home or rally-point RTL destinations when a valid VTOL approach loiter is defined for that landing location. Mission landing patterns are unaffected. |
| <a id="RTL_RETURN_ALT"></a>[RTL_RETURN_ALT](../advanced_config/parameter_reference.md#RTL_RETURN_ALT) | Return altitude in meters (default: 60m)If already above this value the vehicle will return at its current altitude. |
| <a id="RTL_CONE_ANG"></a>[RTL_CONE_ANG](../advanced_config/parameter_reference.md#RTL_CONE_ANG) | Half-angle of the cone that defines the vehicle RTL return altitude. Values (in degrees): 0, 25, 45, 65, 80, 90. Note that 0 is "no cone" (always return at `RTL_RETURN_ALT` or higher), while 90 indicates that the vehicle must return at the current altitude or `RTL_DESCEND_ALT` (whichever is higher). |
| <a id="RTL_DESCEND_ALT"></a>[RTL_DESCEND_ALT](../advanced_config/parameter_reference.md#RTL_DESCEND_ALT) | Minimum return altitude and altitude at which the vehicle will slow or stop its initial descent from a higher return altitude (default: 30m) |

2
docs/en/releases/1.18.md
View File

@@ -120,7 +120,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
### VTOL
- TBD
- [Return mode VTOL Rally Point Approach Loiters](../flight_modes_vtol/return.md#vtol-rally-point-approach-loiters): VTOLs returning in fixed-wing mode can now use the approach loiter associated with the selected home/rally landing location (instead of home) to choose the most wind-aligned valid approach (if several are defined). ([PX4-Autopilot#27004: fix(navigator): rtl compute wind angle to select best land approach based on rally point location instead of home location](https://github.com/PX4/PX4-Autopilot/pull/27004))
### Fixed-wing

2
docs/en/releases/main.md
View File

@@ -34,6 +34,8 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
## Other changes
- Fast mission Return modes ([RTL_TYPE](../advanced_config/parameter_reference.md#RTL_TYPE) = 2 and 4) now skip `DO_JUMP` commands (loops) while following the mission path. ([PX4-Autopilot#26993: fix(navigator): goToNextPositionItem skip loops when required](https://github.com/PX4/PX4-Autopilot/pull/26993))
### Hardware Support
- TBD

299
src/modules/navigator/rtl.cpp
View File

@@ -53,6 +53,7 @@ using matrix::wrap_pi;
static constexpr float MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES{10.0f}; // [m] We don't consider safe points valid if the distance from the current home to the safe point is smaller than this distance
static constexpr float MIN_DIST_THRESHOLD = 2.f;
static constexpr double NULL_ISLAND_THRESHOLD_DEG{1e-12};
RTL::RTL(Navigator *navigator) :
NavigatorMode(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL),
@@ -378,18 +379,12 @@ void RTL::setRtlTypeAndDestination()
} else {
new_rtl_type = RtlType::RTL_DIRECT;
landing_loiter = selectLandingApproach(destination);
land_approaches_s rtl_land_approaches{readVtolLandApproaches(destination)};
// set loiter position to destination initially, overwrite for VTOL if land approaches exist
if (!landing_loiter.isValid()) {
landing_loiter.lat = destination.lat;
landing_loiter.lon = destination.lon;
landing_loiter.height_m = NAN;
if (_vehicle_status_sub.get().is_vtol
&& (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)
&& rtl_land_approaches.isAnyApproachValid()) {
landing_loiter = chooseBestLandingApproach(rtl_land_approaches);
}
}
}
@@ -424,17 +419,15 @@ PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_poin
const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol
&& (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
PositionYawSetpoint safe_point{(double)NAN, (double)NAN, NAN, NAN};
PositionYawSetpoint safe_point{static_cast<double>(NAN), static_cast<double>(NAN), NAN, NAN};
if (_safe_points_updated) {
_one_rally_point_has_land_approach = false;
for (int current_seq = 0; current_seq < _dataman_cache_safepoint.size(); ++current_seq) {
for (int current_seq = 0; current_seq < _stats.num_items; ++current_seq) {
mission_item_s mission_safe_point;
const bool success = _dataman_cache_safepoint.loadWait(static_cast<dm_item_t>(_stats.dataman_id), current_seq,
reinterpret_cast<uint8_t *>(&mission_safe_point),
sizeof(mission_item_s), 500_ms);
const bool success = loadSafePointItemWait(current_seq, mission_safe_point, 500_ms);
if (!success) {
PX4_ERR("dm_read failed");
@@ -446,17 +439,21 @@ PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_poin
continue;
}
PositionYawSetpoint safepoint_position{};
if (!extractValidSafePointPosition(mission_safe_point, _home_pos_sub.get().alt, safepoint_position)) {
continue;
}
// Ignore safepoints which are too close to the homepoint (only if home is an option to return to)
const bool far_from_home = get_distance_to_next_waypoint(_home_pos_sub.get().lat, _home_pos_sub.get().lon,
mission_safe_point.lat, mission_safe_point.lon) > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES;
safepoint_position.lat, safepoint_position.lon) > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES;
if (far_from_home || (_param_rtl_type.get() == 5)) {
const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)};
const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
safepoint_position.lat, safepoint_position.lon)};
PositionYawSetpoint safepoint_position;
setSafepointAsDestination(safepoint_position, mission_safe_point);
const bool current_safe_point_has_approaches{hasVtolLandApproach(safepoint_position)};
const bool current_safe_point_has_approaches{hasVtolLandApproachesAtSafePointIndex(current_seq, _home_pos_sub.get().alt)};
_one_rally_point_has_land_approach |= current_safe_point_has_approaches;
@@ -473,6 +470,12 @@ PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_poin
return safe_point;
}
bool RTL::loadSafePointItemWait(int seq, mission_item_s &item, hrt_abstime timeout) const
{
return _dataman_cache_safepoint.loadWait(static_cast<dm_item_t>(_stats.dataman_id), seq,
reinterpret_cast<uint8_t *>(&item), sizeof(mission_item_s), timeout);
}
void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &destination, uint8_t &safe_point_index)
{
const bool vtol_in_rw_mode = _vehicle_status_sub.get().is_vtol
@@ -484,7 +487,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
float min_dist = FLT_MAX;
if (_param_rtl_type.get() != 5) {
_home_has_land_approach = hasVtolLandApproach(destination);
_home_has_land_approach = hasVtolLandApproachesNearLocation(destination, _home_pos_sub.get().alt);
const bool prioritize_safe_points_over_home = ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode);
const bool required_approach_missing_for_home = (vtol_in_fw_mode && (_param_rtl_appr_force.get() == 1) && !_home_has_land_approach);
@@ -552,31 +555,42 @@ void RTL::setLandPosAsDestination(PositionYawSetpoint &rtl_position, mission_ite
rtl_position.lon = land_mission_item.lon;
}
void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mission_item_s &mission_safe_point) const
bool RTL::extractValidSafePointPosition(const mission_item_s &safe_point_item, float home_altitude_amsl,
PositionYawSetpoint &position) const
{
// There is a safe point closer than home/mission landing
// TODO: handle all possible mission_safe_point.frame cases
switch (mission_safe_point.frame) {
case 0: // MAV_FRAME_GLOBAL
case 5: // MAV_FRAME_GLOBAL_INT
rtl_position.lat = mission_safe_point.lat;
rtl_position.lon = mission_safe_point.lon;
rtl_position.alt = mission_safe_point.altitude; // alt of safe point is relative to MSL
if (safe_point_item.nav_cmd != NAV_CMD_RALLY_POINT) {
return false;
}
// TODO: handle all possible safe_point_item.frame cases
switch (safe_point_item.frame) {
case NAV_FRAME_GLOBAL:
case NAV_FRAME_GLOBAL_INT:
position.alt = safe_point_item.altitude;
break;
case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT
case 6: // MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
rtl_position.lat = mission_safe_point.lat;
rtl_position.lon = mission_safe_point.lon;
rtl_position.alt = mission_safe_point.altitude + _home_pos_sub.get().alt; // alt of safe point is rel to home
case NAV_FRAME_GLOBAL_RELATIVE_ALT:
case NAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
if (!PX4_ISFINITE(home_altitude_amsl)) {
return false;
}
position.alt = safe_point_item.altitude + home_altitude_amsl; // alt of safe point is rel to home
break;
default:
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME\t");
events::send<uint8_t>(events::ID("rtl_unsupported_mav_frame"), events::Log::Error, "RTL: unsupported MAV_FRAME ({1})",
mission_safe_point.frame);
break;
return false;
}
position.lat = safe_point_item.lat;
position.lon = safe_point_item.lon;
position.yaw = NAN;
// basic checks protect against invalid data in case of missing init or cache read errors
return PX4_ISFINITE(position.lat) && PX4_ISFINITE(position.lon) && PX4_ISFINITE(position.alt)
&& !((fabs(position.lat) < NULL_ISLAND_THRESHOLD_DEG) &&
(fabs(position.lon) < NULL_ISLAND_THRESHOLD_DEG))
&& (fabs(position.lat) <= 90.0) && (fabs(position.lon) <= 180.0);
}
float RTL::computeReturnAltitude(const PositionYawSetpoint &rtl_position) const
@@ -693,13 +707,154 @@ bool RTL::reverseIsFurther() const
}
bool RTL::hasVtolLandApproach(const PositionYawSetpoint &rtl_position) const
bool RTL::findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, float home_altitude_amsl,
int &safe_point_index, mission_item_s &safe_point_item) const
{
return readVtolLandApproaches(rtl_position).isAnyApproachValid();
if (!_safe_points_updated) {
return false;
}
loiter_point_s RTL::chooseBestLandingApproach(const land_approaches_s &vtol_land_approaches)
// The first nearby valid rally point defines the block that belongs to this destination.
for (int current_seq = 0; current_seq < _stats.num_items; ++current_seq) {
mission_item_s mission_item{};
const bool success = loadSafePointItemWait(current_seq, mission_item, 500_ms);
if (!success) {
PX4_ERR("dm_read failed");
break;
}
if (mission_item.nav_cmd != NAV_CMD_RALLY_POINT) {
continue;
}
PositionYawSetpoint safe_point_position{};
// Skip invalid rally points so later valid rally points are considered
if (!extractValidSafePointPosition(mission_item, home_altitude_amsl, safe_point_position)) {
continue;
}
const float dist_to_safepoint = get_distance_to_next_waypoint(safe_point_position.lat, safe_point_position.lon,
rtl_position.lat, rtl_position.lon);
if (dist_to_safepoint < MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) {
safe_point_index = current_seq;
safe_point_item = mission_item;
return true;
}
}
return false;
}
bool RTL::scanVtolLandApproachBlock(int safe_point_index, float home_altitude_amsl,
land_approaches_s *result) const
{
uint8_t valid_approach_counter = 0;
for (int current_seq = safe_point_index + 1; current_seq < _stats.num_items; ++current_seq) {
mission_item_s mission_item{};
const bool success = loadSafePointItemWait(current_seq, mission_item, 500_ms);
if (!success) {
PX4_ERR("dm_read failed");
break;
}
if (mission_item.nav_cmd == NAV_CMD_RALLY_POINT) {
// A rally point starts the next block, break
break;
}
if (mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT
|| valid_approach_counter >= land_approaches_s::num_approaches_max) {
continue;
}
const loiter_point_s approach = makeVtolLandApproachPoint(mission_item, home_altitude_amsl);
if (approach.isValid()) {
if (result) {
result->approaches[valid_approach_counter] = approach;
valid_approach_counter++;
} else {
return true; // Early exit for the check-only
}
}
}
return result ? (valid_approach_counter > 0) : false;
}
loiter_point_s RTL::makeVtolLandApproachPoint(const mission_item_s &mission_item, float home_altitude_amsl) const
{
loiter_point_s approach{};
approach.lat = mission_item.lat;
approach.lon = mission_item.lon;
approach.height_m = MissionBlock::get_absolute_altitude_for_item(mission_item, home_altitude_amsl);
approach.loiter_radius_m = mission_item.loiter_radius;
return approach;
}
land_approaches_s RTL::getVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position,
float home_altitude_amsl) const
{
int safe_point_index = -1;
mission_item_s safe_point_item{};
if (!findAssociatedSafePointIndex(rtl_position, home_altitude_amsl, safe_point_index, safe_point_item)) {
return {};
}
land_approaches_s vtol_land_approaches{};
vtol_land_approaches.land_location_lat_lon = matrix::Vector2d(safe_point_item.lat, safe_point_item.lon);
scanVtolLandApproachBlock(safe_point_index, home_altitude_amsl, &vtol_land_approaches);
return vtol_land_approaches;
}
bool RTL::hasVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position, float home_altitude_amsl) const
{
int safe_point_index = -1;
mission_item_s safe_point_item{};
return findAssociatedSafePointIndex(rtl_position, home_altitude_amsl, safe_point_index, safe_point_item)
&& hasVtolLandApproachesAtSafePointIndex(safe_point_index, home_altitude_amsl);
}
bool RTL::hasVtolLandApproachesAtSafePointIndex(int safe_point_index, float home_altitude_amsl) const
{
return scanVtolLandApproachBlock(safe_point_index, home_altitude_amsl, nullptr);
}
loiter_point_s RTL::selectLandingApproach(const PositionYawSetpoint &destination) const
{
loiter_point_s landing_approach{};
// Only a VTOL currently flying as fixed wing needs a wind-selected approach loiter.
if (!_vehicle_status_sub.get().is_vtol
|| (_vehicle_status_sub.get().vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
return landing_approach;
}
const land_approaches_s vtol_land_approaches = getVtolLandApproachesNearLocation(destination, _home_pos_sub.get().alt);
if (vtol_land_approaches.isAnyApproachValid()) {
landing_approach = chooseBestLandingApproach(vtol_land_approaches);
}
return landing_approach;
}
loiter_point_s RTL::chooseBestLandingApproach(const land_approaches_s &vtol_land_approaches) const
{
if (!vtol_land_approaches.land_location_lat_lon.isAllFinite()) {
return loiter_point_s();
}
const float wind_direction = atan2f(_wind_sub.get().windspeed_east, _wind_sub.get().windspeed_north);
int8_t min_index = -1;
float wind_angle_prev = INFINITY;
@@ -707,8 +862,9 @@ loiter_point_s RTL::chooseBestLandingApproach(const land_approaches_s &vtol_land
for (int i = 0; i < vtol_land_approaches.num_approaches_max; i++) {
if (vtol_land_approaches.approaches[i].isValid()) {
const float wind_angle = wrap_pi(get_bearing_to_next_waypoint(_home_pos_sub.get().lat,
_home_pos_sub.get().lon, vtol_land_approaches.approaches[i].lat,
// The approach circles are defined around the land location.
const float wind_angle = wrap_pi(get_bearing_to_next_waypoint(vtol_land_approaches.land_location_lat_lon(0),
vtol_land_approaches.land_location_lat_lon(1), vtol_land_approaches.approaches[i].lat,
vtol_land_approaches.approaches[i].lon) - wind_direction);
if (fabsf(wind_angle) < wind_angle_prev) {
@@ -727,60 +883,3 @@ loiter_point_s RTL::chooseBestLandingApproach(const land_approaches_s &vtol_land
return loiter_point_s();
}
}
land_approaches_s RTL::readVtolLandApproaches(PositionYawSetpoint rtl_position) const
{
// go through all mission items in the rally point storage. If we find a mission item of type NAV_CMD_RALLY_POINT
// which is within MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES of our current home position then treat ALL following mission items of type NAV_CMD_LOITER_TO_ALT which come
// BEFORE the next mission item of type NAV_CMD_RALLY_POINT as land approaches for the home position
land_approaches_s vtol_land_approaches{};
if (!_safe_points_updated) {
return vtol_land_approaches;
}
bool foundHomeLandApproaches = false;
uint8_t sector_counter = 0;
for (int current_seq = 0; current_seq < _stats.num_items; ++current_seq) {
mission_item_s mission_item{};
bool success_mission_item = _dataman_cache_safepoint.loadWait(static_cast<dm_item_t>(_stats.dataman_id), current_seq,
reinterpret_cast<uint8_t *>(&mission_item),
sizeof(mission_item_s));
if (!success_mission_item) {
PX4_ERR("dm_read failed");
break;
}
if (mission_item.nav_cmd == NAV_CMD_RALLY_POINT) {
if (foundHomeLandApproaches) {
break;
}
const float dist_to_safepoint = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon, rtl_position.lat,
rtl_position.lon);
if (dist_to_safepoint < MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) {
foundHomeLandApproaches = true;
vtol_land_approaches.land_location_lat_lon = matrix::Vector2d(mission_item.lat, mission_item.lon);
}
sector_counter = 0;
}
if (foundHomeLandApproaches && mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
vtol_land_approaches.approaches[sector_counter].lat = mission_item.lat;
vtol_land_approaches.approaches[sector_counter].lon = mission_item.lon;
vtol_land_approaches.approaches[sector_counter].height_m = MissionBlock::get_absolute_altitude_for_item(mission_item,
_home_pos_sub.get().alt);
vtol_land_approaches.approaches[sector_counter].loiter_radius_m = mission_item.loiter_radius;
sector_counter++;
}
}
return vtol_land_approaches;
}

84
src/modules/navigator/rtl.h
View File

@@ -62,6 +62,7 @@
#include <uORB/topics/rtl_time_estimate.h>
class Navigator;
class RTLTestPeer;
class RTL : public NavigatorMode, public ModuleParams
{
@@ -91,12 +92,21 @@ public:
bool isLanding();
private:
friend class RTLTestPeer;
enum class DestinationType {
DESTINATION_TYPE_HOME,
DESTINATION_TYPE_MISSION_LAND,
DESTINATION_TYPE_SAFE_POINT
};
protected:
/**
* @brief Load one safe-point mission item from the safe-point dataman cache.
*/
virtual bool loadSafePointItemWait(int seq, mission_item_s &item, hrt_abstime timeout) const;
private:
/**
@@ -145,11 +155,12 @@ private:
void setLandPosAsDestination(PositionYawSetpoint &rtl_position, mission_item_s &land_mission_item) const;
/**
* @brief Set the safepoint as destination.
* @brief Parse a rally-point mission item into a PositionYawSetpoint, validating frame, coordinates and finiteness.
*
* @param mission_safe_point is the mission safe point/rally point to set as destination.
* @return true if the item is a valid rally point with sane coordinates, false otherwise.
*/
void setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mission_item_s &mission_safe_point) const;
bool extractValidSafePointPosition(const mission_item_s &safe_point_item, float home_altitude_amsl,
PositionYawSetpoint &position) const;
/**
* @brief calculate return altitude from return altitude parameter, current altitude and cone angle
@@ -173,31 +184,64 @@ private:
void parameters_update();
/**
* @brief read VTOL land approaches
*
* @param[in] rtl_position landing position of the rtl
* @brief Read the landing-approach block associated with the first valid rally point near rtl_position.
*
* A block starts at the associated rally point and contains the consecutive NAV_CMD_LOITER_TO_ALT
* items that follow it. The next rally point starts a new block.
* Invalid rally points are skipped so a later nearby valid rally point can still be considered.
*/
land_approaches_s readVtolLandApproaches(PositionYawSetpoint rtl_position) const;
land_approaches_s getVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position,
float home_altitude_amsl) const;
/**
* @brief Has VTOL land approach
*
* @param[in] rtl_position landing position of the rtl
*
* @return true if home land approaches are defined for home position
* @return false otherwise
* @brief Return whether a valid associated block near rtl_position has at least one valid approach.
*/
bool hasVtolLandApproach(const PositionYawSetpoint &rtl_position) const;
bool hasVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position, float home_altitude_amsl) const;
/**
* @brief Choose best landing approach
*
* Choose best landing approach for home considering wind
*
* @return loiter_point_s best landing approach
* @brief Return whether the block after safe_point_index contains at least one valid approach.
*/
loiter_point_s chooseBestLandingApproach(const land_approaches_s &vtol_land_approaches);
bool hasVtolLandApproachesAtSafePointIndex(int safe_point_index, float home_altitude_amsl) const;
/**
* @brief Choose the most wind-aligned approach in a landing-approach block.
*
* Bearings are evaluated from the block's land location.
*/
loiter_point_s chooseBestLandingApproach(const land_approaches_s &vtol_land_approaches) const;
/**
* @brief Return the wind-selected VTOL approach for destination, or an invalid loiter if none exists.
*/
loiter_point_s selectLandingApproach(const PositionYawSetpoint &destination) const;
/**
* @brief Find the first rally point whose block should be associated with rtl_position.
*
* Invalid rally points are skipped so nearby valid fallbacks can still be associated.
* On success, safe_point_index and safe_point_item are populated with the rally point that starts the block.
*/
bool findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, float home_altitude_amsl,
int &safe_point_index, mission_item_s &safe_point_item) const;
/**
* @brief Scan one landing-approach block after a rally point.
*
* A block is the consecutive NAV_CMD_LOITER_TO_ALT items after safe_point_index.
* Scanning stops at the next rally point because it starts a different safe-point block.
*
* If result is non-null, all valid approaches are collected into it.
* If result is null, returns true on the first valid approach (early exit).
*
* @return true if at least one valid approach was found.
*/
bool scanVtolLandApproachBlock(int safe_point_index, float home_altitude_amsl,
land_approaches_s *result) const;
/**
* @brief Convert one loiter mission item into a landing-approach entry.
*/
loiter_point_s makeVtolLandApproachPoint(const mission_item_s &mission_item, float home_altitude_amsl) const;
enum class DatamanState {
UpdateRequestWait,

4
src/modules/navigator/rtl_params.yaml
View File

@@ -150,6 +150,8 @@ parameters:
RTL_APPR_FORCE:
description:
short: RTL force approach landing
long: Only consider RTL point, if it has an approach defined.
long: |-
Only consider home/rally RTL points that have a VTOL approach loiter.
A VTOL approach loiter is a MAV_CMD_NAV_LOITER_TO_ALT item with finite position and altitude.
type: boolean
default: 0

2
src/modules/navigator/safe_point_land.hpp
View File

@@ -69,6 +69,8 @@ struct land_approaches_s {
land_approaches_s()
{
resetAllApproaches();
land_location_lat_lon(0) = static_cast<double>(NAN);
land_location_lat_lon(1) = static_cast<double>(NAN);
}
void resetAllApproaches()

1
src/modules/navigator/test/CMakeLists.txt
View File

@@ -34,3 +34,4 @@
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/..)
px4_add_functional_gtest(SRC test_mission_base.cpp LINKLIBS modules__navigator)
px4_add_functional_gtest(SRC test_RTL.cpp LINKLIBS modules__navigator)

98
src/modules/navigator/test/support/vector_mission_item_store.h Normal file
View File

@@ -0,0 +1,98 @@
/****************************************************************************
*
* 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 vector_mission_item_store.h
*
* @author Jonas Perolini <jonspero@me.com>
*
*/
#pragma once
#include <algorithm>
#include <cstddef>
#include <cstdint>
#include <initializer_list>
#include <vector>
#include "navigation.h"
namespace navigator_test
{
class VectorMissionItemStore
{
public:
void setItems(const std::vector<mission_item_s> &items)
{
_items = items;
clearLoadFailures();
}
void setLoadFailureIndices(std::initializer_list<int32_t> indices)
{
_load_failure_indices.assign(indices.begin(), indices.end());
}
void clearLoadFailures()
{
_load_failure_indices.clear();
}
bool loadItem(int32_t index, mission_item_s &mission_item) const
{
if (std::find(_load_failure_indices.begin(), _load_failure_indices.end(), index)
!= _load_failure_indices.end()) {
return false;
}
if (index < 0 || index >= static_cast<int32_t>(_items.size())) {
return false;
}
mission_item = _items[static_cast<std::size_t>(index)];
return true;
}
std::size_t itemCount() const
{
return _items.size();
}
private:
std::vector<mission_item_s> _items;
std::vector<int32_t> _load_failure_indices;
};
} // namespace navigator_test

937
src/modules/navigator/test/test_RTL.cpp Normal file
View File

@@ -0,0 +1,937 @@
/****************************************************************************
*
* 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 test_RTL.cpp
*
* @author Jonas Perolini <jonspero@me.com>
*
*/
#include <gtest/gtest.h>
#include <cmath>
#include <string>
#include <vector>
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <parameters/param.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/wind.h>
#include "navigator.h"
#include "rtl.h"
#include "support/vector_mission_item_store.h"
extern "C" int dataman_main(int argc, char *argv[]);
namespace
{
constexpr double kBaseLat = 47.397742;
constexpr double kBaseLon = 8.545594;
constexpr float kAlt = 500.f;
constexpr float kApproachRadius = 50.f;
/**
* @brief Starts dataman for this file.
*/
class TestDatamanRuntime
{
public:
TestDatamanRuntime()
{
char name[] = "dataman";
char start[] = "start";
char ram[] = "-r";
char *argv[] = {name, start, ram};
dataman_main(3, argv);
}
~TestDatamanRuntime()
{
char name[] = "dataman";
char stop[] = "stop";
char *argv[] = {name, stop};
dataman_main(2, argv);
}
};
/**
* @brief Returns the shared dataman runtime.
*/
TestDatamanRuntime &testDatamanRuntime()
{
static TestDatamanRuntime runtime{};
return runtime;
}
mission_item_s makeSafePointItem(double lat, double lon, float altitude, NAV_FRAME frame,
NAV_CMD nav_cmd = NAV_CMD_RALLY_POINT)
{
mission_item_s item{};
item.nav_cmd = nav_cmd;
item.frame = frame;
item.lat = lat;
item.lon = lon;
item.altitude = altitude;
return item;
}
mission_item_s makeLandApproachItem(double lat, double lon, float altitude, float loiter_radius_m,
NAV_FRAME frame = NAV_FRAME_GLOBAL)
{
mission_item_s item{};
item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
item.frame = frame;
item.lat = lat;
item.lon = lon;
item.altitude = altitude;
item.altitude_is_relative = (frame == NAV_FRAME_GLOBAL_RELATIVE_ALT)
|| (frame == NAV_FRAME_GLOBAL_RELATIVE_ALT_INT);
item.loiter_radius = loiter_radius_m;
return item;
}
PositionYawSetpoint makePositionYawSetpointFromOffset(double base_lat, double base_lon, float north_m, float east_m,
float alt)
{
MapProjection ref{base_lat, base_lon};
PositionYawSetpoint position{};
ref.reproject(north_m, east_m, position.lat, position.lon);
position.alt = alt;
position.yaw = NAN;
return position;
}
loiter_point_s makeLoiterPoint(const PositionYawSetpoint &position, float loiter_radius_m = kApproachRadius)
{
loiter_point_s loiter_point{};
loiter_point.lat = position.lat;
loiter_point.lon = position.lon;
loiter_point.height_m = position.alt;
loiter_point.loiter_radius_m = loiter_radius_m;
return loiter_point;
}
uint8_t countValidApproaches(const land_approaches_s &vtol_land_approaches)
{
uint8_t count = 0;
for (uint8_t i = 0; i < land_approaches_s::num_approaches_max; ++i) {
if (vtol_land_approaches.approaches[i].isValid()) {
++count;
}
}
return count;
}
void expectLoiterPointNear(const loiter_point_s &actual, const PositionYawSetpoint &expected,
float loiter_radius_m = kApproachRadius)
{
ASSERT_TRUE(actual.isValid());
EXPECT_NEAR(actual.lat, expected.lat, 1e-9);
EXPECT_NEAR(actual.lon, expected.lon, 1e-9);
EXPECT_NEAR(actual.height_m, expected.alt, 0.01f);
EXPECT_NEAR(actual.loiter_radius_m, loiter_radius_m, 0.01f);
}
struct ExtractValidSafePointPositionCase {
const char *test_name;
mission_item_s item;
float home_altitude_amsl;
bool expected_valid;
double expected_lat;
double expected_lon;
float expected_alt;
};
struct ApproachGeometry {
PositionYawSetpoint land;
PositionYawSetpoint north;
PositionYawSetpoint south;
};
struct VehicleStateCase {
const char *test_name;
bool is_vtol;
uint8_t vehicle_type;
bool expect_valid;
};
struct ReadFailureCase {
const char *test_name;
int32_t failure_index;
bool expected_found;
uint8_t expected_count;
};
} // namespace
/**
* @brief RTL test peer with vector-backed safe points.
*/
class RTLTestPeer : public RTL
{
public:
explicit RTLTestPeer(Navigator *navigator) : RTL(navigator) {}
void resetSafePointStatsForTest()
{
_safe_point_store.setItems({});
_stats = {};
_safe_points_updated = false;
}
void setSafePointsForTest(const std::vector<mission_item_s> &items)
{
_safe_point_store.setItems(items);
_stats = {};
_stats.num_items = static_cast<uint16_t>(_safe_point_store.itemCount());
_safe_points_updated = true;
}
void setLoadFailureIndices(std::initializer_list<int32_t> indices)
{
_safe_point_store.setLoadFailureIndices(indices);
}
protected:
bool loadSafePointItemWait(int seq, mission_item_s &item, hrt_abstime timeout) const override
{
(void)timeout;
return _safe_point_store.loadItem(seq, item);
}
public:
bool extractValidSafePointPositionForTest(const mission_item_s &safe_point_item, float home_altitude_amsl,
PositionYawSetpoint &position) const
{
return extractValidSafePointPosition(safe_point_item, home_altitude_amsl, position);
}
loiter_point_s chooseBestLandingApproachForTest(const land_approaches_s &vtol_land_approaches)
{
_wind_sub.update();
return chooseBestLandingApproach(vtol_land_approaches);
}
loiter_point_s selectLandingApproachForTest(const PositionYawSetpoint &destination)
{
_home_pos_sub.update();
_vehicle_status_sub.update();
_wind_sub.update();
return selectLandingApproach(destination);
}
bool scanVtolLandApproachBlockForTest(int safe_point_index, float home_altitude_amsl,
land_approaches_s *result) const
{
return scanVtolLandApproachBlock(safe_point_index, home_altitude_amsl, result);
}
bool findAssociatedSafePointIndexForTest(const PositionYawSetpoint &rtl_position, float home_altitude_amsl,
int &safe_point_index, mission_item_s &safe_point_item) const
{
return findAssociatedSafePointIndex(rtl_position, home_altitude_amsl, safe_point_index, safe_point_item);
}
loiter_point_s makeVtolLandApproachPointForTest(const mission_item_s &mission_item, float home_altitude_amsl) const
{
return makeVtolLandApproachPoint(mission_item, home_altitude_amsl);
}
private:
navigator_test::VectorMissionItemStore _safe_point_store{};
};
/**
* @brief Shared fixture for RTL helper tests.
*/
class RTLTest : public ::testing::Test
{
protected:
static void SetUpTestSuite()
{
(void)testDatamanRuntime();
}
Navigator _navigator{};
RTLTestPeer _rtl{&_navigator};
void SetUp() override
{
param_control_autosave(false);
param_reset_all();
_rtl.resetSafePointStatsForTest();
publishHomePosition(makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 0.f, kAlt));
publishVehicleStatus(false, vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
publishWind(0.f, 0.f);
}
void TearDown() override
{
_rtl.resetSafePointStatsForTest();
if (_home_pub != nullptr) {
orb_unadvertise(_home_pub);
_home_pub = nullptr;
}
if (_vehicle_status_pub != nullptr) {
orb_unadvertise(_vehicle_status_pub);
_vehicle_status_pub = nullptr;
}
if (_wind_pub != nullptr) {
orb_unadvertise(_wind_pub);
_wind_pub = nullptr;
}
param_control_autosave(true);
}
void publishHomePosition(const PositionYawSetpoint &position)
{
home_position_s home{};
home.timestamp = hrt_absolute_time();
home.lat = position.lat;
home.lon = position.lon;
home.alt = position.alt;
home.valid_hpos = true;
home.valid_alt = true;
if (_home_pub == nullptr) {
_home_pub = orb_advertise(ORB_ID(home_position), &home);
} else {
orb_publish(ORB_ID(home_position), _home_pub, &home);
}
}
void publishVehicleStatus(bool is_vtol, uint8_t vehicle_type)
{
vehicle_status_s status{};
status.timestamp = hrt_absolute_time();
status.is_vtol = is_vtol;
status.vehicle_type = vehicle_type;
if (_vehicle_status_pub == nullptr) {
_vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
} else {
orb_publish(ORB_ID(vehicle_status), _vehicle_status_pub, &status);
}
}
void publishWind(float windspeed_north, float windspeed_east)
{
wind_s wind{};
wind.timestamp = hrt_absolute_time();
wind.windspeed_north = windspeed_north;
wind.windspeed_east = windspeed_east;
if (_wind_pub == nullptr) {
_wind_pub = orb_advertise(ORB_ID(wind), &wind);
} else {
orb_publish(ORB_ID(wind), _wind_pub, &wind);
}
}
ApproachGeometry makeApproachGeometry() const
{
return ApproachGeometry{
makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 100.f, 100.f, kAlt),
makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 150.f, 100.f, kAlt + 20.f),
makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 50.f, 100.f, kAlt + 20.f),
};
}
orb_advert_t _home_pub{nullptr};
orb_advert_t _vehicle_status_pub{nullptr};
orb_advert_t _wind_pub{nullptr};
};
// WHY: No land point means no usable approach bearing.
// WHAT: The chooser should return an invalid loiter.
TEST_F(RTLTest, ChooseBestLandingApproachRequiresLandLocation)
{
// GIVEN: A valid loiter and no land point.
publishWind(1.f, 0.f);
const PositionYawSetpoint north_approach = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 50.f, 0.f, kAlt + 20.f);
land_approaches_s vtol_land_approaches{};
vtol_land_approaches.approaches[0] = makeLoiterPoint(north_approach);
// WHEN: The chooser runs.
const loiter_point_s selected_approach = _rtl.chooseBestLandingApproachForTest(vtol_land_approaches);
// THEN: It returns no approach.
EXPECT_FALSE(selected_approach.isValid());
}
// WHY: Approach bearing is measured from the land point.
// WHAT: Home should not affect the choice.
TEST_F(RTLTest, ChooseBestLandingApproachUsesLandLocationAsBearingOrigin)
{
// GIVEN: Two approaches on opposite sides of the land point and a 60 degree wind.
publishWind(1.f, std::sqrt(3.0f));
const ApproachGeometry geometry = makeApproachGeometry();
land_approaches_s vtol_land_approaches{};
vtol_land_approaches.land_location_lat_lon(0) = geometry.land.lat;
vtol_land_approaches.land_location_lat_lon(1) = geometry.land.lon;
vtol_land_approaches.approaches[0] = makeLoiterPoint(geometry.north);
vtol_land_approaches.approaches[1] = makeLoiterPoint(geometry.south);
// WHEN: The chooser evaluates the block.
const loiter_point_s selected_approach = _rtl.chooseBestLandingApproachForTest(vtol_land_approaches);
// THEN: The north approach is selected.
expectLoiterPointNear(selected_approach, geometry.north);
}
/**
* @brief Vehicle-state cases for selectLandingApproach().
*/
class SelectLandingApproachVehicleStateTest :
public RTLTest,
public ::testing::WithParamInterface<VehicleStateCase>
{
};
// WHY: Only VTOL in FW mode should use an approach loiter.
// WHAT: All other vehicle states should reject it.
TEST_P(SelectLandingApproachVehicleStateTest, SelectLandingApproachHonorsVehicleState)
{
const VehicleStateCase &test_case = GetParam();
// GIVEN: One valid approach block, a 60 degree wind, and one vehicle state.
const ApproachGeometry geometry = makeApproachGeometry();
_rtl.setSafePointsForTest({
makeSafePointItem(geometry.land.lat, geometry.land.lon, geometry.land.alt, NAV_FRAME_GLOBAL),
makeLandApproachItem(geometry.north.lat, geometry.north.lon, geometry.north.alt, kApproachRadius),
makeLandApproachItem(geometry.south.lat, geometry.south.lon, geometry.south.alt, kApproachRadius),
});
publishWind(1.f, std::sqrt(3.0f));
publishVehicleStatus(test_case.is_vtol, test_case.vehicle_type);
// WHEN: selectLandingApproach evaluates the destination.
const loiter_point_s selected_approach = _rtl.selectLandingApproachForTest(geometry.land);
// THEN: Only VTOL FW gets the selected approach.
if (test_case.expect_valid) {
expectLoiterPointNear(selected_approach, geometry.north);
} else {
EXPECT_FALSE(selected_approach.isValid());
}
}
INSTANTIATE_TEST_SUITE_P(
RTL,
SelectLandingApproachVehicleStateTest,
::testing::Values(
VehicleStateCase{"VtolRotaryWing", true, vehicle_status_s::VEHICLE_TYPE_ROTARY_WING, false},
VehicleStateCase{"NonVtolFixedWing", false, vehicle_status_s::VEHICLE_TYPE_FIXED_WING, false},
VehicleStateCase{"VtolFixedWing", true, vehicle_status_s::VEHICLE_TYPE_FIXED_WING, true}),
[](const ::testing::TestParamInfo<SelectLandingApproachVehicleStateTest::ParamType> &param_info)
{
return std::string(param_info.param.test_name);
});
// WHY: Each rally point owns the loiters that follow it.
// WHAT: Scanning should stop at the next rally point.
TEST_F(RTLTest, ScanVtolLandApproachBlockStopsAtNextRallyPoint)
{
// GIVEN: One block with two loiters, then a new rally point.
const PositionYawSetpoint land_1 = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 0.f, kAlt);
const PositionYawSetpoint loiter_1 = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 50.f, 0.f, kAlt + 20.f);
const PositionYawSetpoint loiter_2 = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 50.f, kAlt + 30.f);
const PositionYawSetpoint land_2 = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 200.f, 0.f, kAlt);
const PositionYawSetpoint loiter_3 = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 250.f, 0.f, kAlt + 40.f);
_rtl.setSafePointsForTest({
makeSafePointItem(land_1.lat, land_1.lon, land_1.alt, NAV_FRAME_GLOBAL),
makeLandApproachItem(loiter_1.lat, loiter_1.lon, loiter_1.alt, kApproachRadius),
makeLandApproachItem(loiter_2.lat, loiter_2.lon, loiter_2.alt, kApproachRadius),
makeSafePointItem(land_2.lat, land_2.lon, land_2.alt, NAV_FRAME_GLOBAL),
makeLandApproachItem(loiter_3.lat, loiter_3.lon, loiter_3.alt, kApproachRadius),
});
land_approaches_s scanned_block{};
// WHEN: The first block is scanned.
const bool found_approach = _rtl.scanVtolLandApproachBlockForTest(0, kAlt, &scanned_block);
// THEN: Only the first two loiters are returned.
ASSERT_TRUE(found_approach);
EXPECT_EQ(countValidApproaches(scanned_block), 2);
expectLoiterPointNear(scanned_block.approaches[0], loiter_1);
expectLoiterPointNear(scanned_block.approaches[1], loiter_2);
EXPECT_FALSE(scanned_block.approaches[2].isValid());
}
// WHY: The result array has a fixed size.
// WHAT: Extra loiters should be ignored once it is full.
TEST_F(RTLTest, ScanVtolLandApproachBlockCapsApproachCount)
{
// GIVEN: More valid loiters than the block can hold.
const PositionYawSetpoint land = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 0.f, kAlt);
std::vector<mission_item_s> mission_items{
makeSafePointItem(land.lat, land.lon, land.alt, NAV_FRAME_GLOBAL),
};
PositionYawSetpoint last_included{};
for (uint8_t i = 0; i < land_approaches_s::num_approaches_max + 2; ++i) {
const PositionYawSetpoint approach = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 25.f * (i + 1), 0.f,
kAlt + 10.f + i);
if (i == land_approaches_s::num_approaches_max - 1) {
last_included = approach;
}
mission_items.push_back(makeLandApproachItem(approach.lat, approach.lon, approach.alt, kApproachRadius));
}
_rtl.setSafePointsForTest(mission_items);
land_approaches_s scanned_block{};
// WHEN: The block is scanned.
const bool found_approach = _rtl.scanVtolLandApproachBlockForTest(0, kAlt, &scanned_block);
// THEN: The result stops at the hard limit.
ASSERT_TRUE(found_approach);
EXPECT_EQ(countValidApproaches(scanned_block), land_approaches_s::num_approaches_max);
expectLoiterPointNear(scanned_block.approaches[0], makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 25.f, 0.f, kAlt + 10.f));
expectLoiterPointNear(scanned_block.approaches[land_approaches_s::num_approaches_max - 1], last_included);
}
// WHY: A rally point can own an empty block.
// WHAT: Another rally point right after it should keep the block empty.
TEST_F(RTLTest, ScanVtolLandApproachBlockHandlesEmptyBlockBeforeNextRallyPoint)
{
// GIVEN: A rally point followed immediately by another rally point.
const PositionYawSetpoint land_1 = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 0.f, kAlt);
const PositionYawSetpoint land_2 = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 100.f, 0.f, kAlt);
const PositionYawSetpoint loiter = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 150.f, 0.f, kAlt + 20.f);
_rtl.setSafePointsForTest({
makeSafePointItem(land_1.lat, land_1.lon, land_1.alt, NAV_FRAME_GLOBAL),
makeSafePointItem(land_2.lat, land_2.lon, land_2.alt, NAV_FRAME_GLOBAL),
makeLandApproachItem(loiter.lat, loiter.lon, loiter.alt, kApproachRadius),
});
land_approaches_s scanned_block{};
// WHEN: The first block is scanned.
const bool found_approach = _rtl.scanVtolLandApproachBlockForTest(0, kAlt, &scanned_block);
// THEN: It stays empty.
EXPECT_FALSE(found_approach);
EXPECT_EQ(countValidApproaches(scanned_block), 0);
}
// WHY: End-of-mission is the other empty-block case.
// WHAT: A final rally point should also return zero approaches.
TEST_F(RTLTest, ScanVtolLandApproachBlockHandlesEmptyBlockAtMissionEnd)
{
// GIVEN: A mission that ends with a rally point.
const PositionYawSetpoint land = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 0.f, kAlt);
_rtl.setSafePointsForTest({
makeSafePointItem(land.lat, land.lon, land.alt, NAV_FRAME_GLOBAL),
});
land_approaches_s scanned_block{};
// WHEN: Its block is scanned.
const bool found_approach = _rtl.scanVtolLandApproachBlockForTest(0, kAlt, &scanned_block);
// THEN: It is empty.
EXPECT_FALSE(found_approach);
EXPECT_EQ(countValidApproaches(scanned_block), 0);
}
/**
* @brief Read-failure cases for scanVtolLandApproachBlock().
*/
class ScanVtolLandApproachBlockReadFailureTest :
public RTLTest,
public ::testing::WithParamInterface<ReadFailureCase>
{
};
// WHY: Read failures should not leave partial results behind.
// WHAT: Scanning should stop cleanly on a broken item.
TEST_P(ScanVtolLandApproachBlockReadFailureTest, ScanVtolLandApproachBlockHandlesReadFailures)
{
const ReadFailureCase &test_case = GetParam();
// GIVEN: One rally point with two loiters.
const PositionYawSetpoint land = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 0.f, kAlt);
const PositionYawSetpoint loiter_1 = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 50.f, 0.f, kAlt + 20.f);
const PositionYawSetpoint loiter_2 = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 100.f, 0.f, kAlt + 30.f);
const std::vector<mission_item_s> mission_items{
makeSafePointItem(land.lat, land.lon, land.alt, NAV_FRAME_GLOBAL),
makeLandApproachItem(loiter_1.lat, loiter_1.lon, loiter_1.alt, kApproachRadius),
makeLandApproachItem(loiter_2.lat, loiter_2.lon, loiter_2.alt, kApproachRadius),
};
_rtl.setSafePointsForTest(mission_items);
_rtl.setLoadFailureIndices({test_case.failure_index});
land_approaches_s scanned_block{};
// WHEN: The block scan hits a read failure.
const bool found_approach = _rtl.scanVtolLandApproachBlockForTest(0, kAlt, &scanned_block);
// THEN: The scan stops without inventing extra approaches.
EXPECT_EQ(found_approach, test_case.expected_found);
EXPECT_EQ(countValidApproaches(scanned_block), test_case.expected_count);
if (test_case.expected_count > 0) {
expectLoiterPointNear(scanned_block.approaches[0], loiter_1);
}
EXPECT_FALSE(scanned_block.approaches[1].isValid());
}
INSTANTIATE_TEST_SUITE_P(
RTL,
ScanVtolLandApproachBlockReadFailureTest,
::testing::Values(
ReadFailureCase{"FirstLoiter", 1, false, 0},
ReadFailureCase{"SecondLoiter", 2, true, 1}),
[](const ::testing::TestParamInfo<ScanVtolLandApproachBlockReadFailureTest::ParamType> &param_info)
{
return std::string(param_info.param.test_name);
});
// WHY: Association is distance-limited.
// WHAT: A rally point outside the 10 m window should be skipped.
TEST_F(RTLTest, FindAssociatedSafePointIndexRejectsFarSafePoints)
{
// GIVEN: One rally point outside the threshold and one inside.
const PositionYawSetpoint rtl_destination = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 0.f, kAlt);
const PositionYawSetpoint outside_safe_point = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 10.25f, 0.f, kAlt);
const PositionYawSetpoint inside_safe_point = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 9.75f, kAlt);
_rtl.setSafePointsForTest({
makeSafePointItem(outside_safe_point.lat, outside_safe_point.lon, outside_safe_point.alt, NAV_FRAME_GLOBAL),
makeSafePointItem(inside_safe_point.lat, inside_safe_point.lon, inside_safe_point.alt, NAV_FRAME_GLOBAL),
});
int safe_point_index = -1;
mission_item_s safe_point_item{};
// WHEN: The association lookup runs.
const bool found_safe_point = _rtl.findAssociatedSafePointIndexForTest(rtl_destination, kAlt, safe_point_index,
safe_point_item);
// THEN: The nearby rally point is selected.
ASSERT_TRUE(found_safe_point);
EXPECT_EQ(safe_point_index, 1);
EXPECT_NEAR(safe_point_item.lat, inside_safe_point.lat, 1e-9);
EXPECT_NEAR(safe_point_item.lon, inside_safe_point.lon, 1e-9);
EXPECT_NEAR(safe_point_item.altitude, inside_safe_point.alt, 0.01f);
}
// WHY: The first valid nearby rally point owns the block.
// WHAT: A later match should not replace it.
TEST_F(RTLTest, FindAssociatedSafePointIndexReturnsFirstMatch)
{
// GIVEN: Two nearby valid rally points.
const PositionYawSetpoint rtl_destination = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 0.f, kAlt);
const PositionYawSetpoint first_safe_point = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 4.f, 0.f, kAlt);
const PositionYawSetpoint second_safe_point = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 5.f, kAlt);
_rtl.setSafePointsForTest({
makeSafePointItem(first_safe_point.lat, first_safe_point.lon, first_safe_point.alt, NAV_FRAME_GLOBAL),
makeSafePointItem(second_safe_point.lat, second_safe_point.lon, second_safe_point.alt, NAV_FRAME_GLOBAL),
});
int safe_point_index = -1;
mission_item_s safe_point_item{};
// WHEN: The association lookup runs.
const bool found_safe_point = _rtl.findAssociatedSafePointIndexForTest(rtl_destination, kAlt, safe_point_index,
safe_point_item);
// THEN: The first match is returned.
ASSERT_TRUE(found_safe_point);
EXPECT_EQ(safe_point_index, 0);
EXPECT_NEAR(safe_point_item.lat, first_safe_point.lat, 1e-9);
EXPECT_NEAR(safe_point_item.lon, first_safe_point.lon, 1e-9);
EXPECT_NEAR(safe_point_item.altitude, first_safe_point.alt, 0.01f);
}
// WHY: Association reads can fail too.
// WHAT: A failed load should stop the search and return no match.
TEST_F(RTLTest, FindAssociatedSafePointIndexHandlesReadFailure)
{
// GIVEN: A matching rally point hidden behind a failed read.
const PositionYawSetpoint rtl_destination = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 0.f, kAlt);
const PositionYawSetpoint skipped_safe_point = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 4.f, 0.f, kAlt);
const PositionYawSetpoint later_safe_point = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 5.f, kAlt);
_rtl.setSafePointsForTest({
makeSafePointItem(skipped_safe_point.lat, skipped_safe_point.lon, skipped_safe_point.alt, NAV_FRAME_GLOBAL),
makeSafePointItem(later_safe_point.lat, later_safe_point.lon, later_safe_point.alt, NAV_FRAME_GLOBAL),
});
_rtl.setLoadFailureIndices({0});
int safe_point_index = -1;
mission_item_s safe_point_item{};
// WHEN: The association lookup hits the failed read.
const bool found_safe_point = _rtl.findAssociatedSafePointIndexForTest(rtl_destination, kAlt, safe_point_index,
safe_point_item);
// THEN: The search stops and reports no match.
EXPECT_FALSE(found_safe_point);
EXPECT_EQ(safe_point_index, -1);
}
// WHY: An invalid rally point should not block the next one.
// WHAT: Association should skip bad entries and keep scanning.
TEST_F(RTLTest, FindAssociatedSafePointIndexSkipsInvalidRallyPoints)
{
// GIVEN: One invalid nearby rally point followed by a valid nearby rally point.
const PositionYawSetpoint rtl_destination = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 0.f, kAlt);
const PositionYawSetpoint valid_safe_point = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 5.f, kAlt);
_rtl.setSafePointsForTest({
makeSafePointItem(91.0, kBaseLon, kAlt, NAV_FRAME_GLOBAL),
makeSafePointItem(valid_safe_point.lat, valid_safe_point.lon, valid_safe_point.alt, NAV_FRAME_GLOBAL),
});
int safe_point_index = -1;
mission_item_s safe_point_item{};
// WHEN: The association lookup runs.
const bool found_safe_point = _rtl.findAssociatedSafePointIndexForTest(rtl_destination, kAlt, safe_point_index,
safe_point_item);
// THEN: The valid rally point is returned.
ASSERT_TRUE(found_safe_point);
EXPECT_EQ(safe_point_index, 1);
EXPECT_NEAR(safe_point_item.lat, valid_safe_point.lat, 1e-9);
EXPECT_NEAR(safe_point_item.lon, valid_safe_point.lon, 1e-9);
EXPECT_NEAR(safe_point_item.altitude, valid_safe_point.alt, 0.01f);
}
/**
* @brief Input cases for extractValidSafePointPosition().
*/
class ExtractValidSafePointPositionTest :
public RTLTest,
public ::testing::WithParamInterface<ExtractValidSafePointPositionCase>
{
};
// WHY: Safe-point parsing should fail fast on bad input.
// WHAT: Valid frames pass; bad commands, frames and coordinates do not.
TEST_P(ExtractValidSafePointPositionTest, ExtractValidSafePointPositionValidatesInput)
{
const ExtractValidSafePointPositionCase &test_case = GetParam();
PositionYawSetpoint extracted_position{};
// GIVEN: One safe-point item.
// WHEN: The parser runs.
const bool is_valid = _rtl.extractValidSafePointPositionForTest(test_case.item, test_case.home_altitude_amsl,
extracted_position);
// THEN: Only valid items are accepted.
EXPECT_EQ(is_valid, test_case.expected_valid);
if (test_case.expected_valid) {
EXPECT_NEAR(extracted_position.lat, test_case.expected_lat, 1e-9);
EXPECT_NEAR(extracted_position.lon, test_case.expected_lon, 1e-9);
EXPECT_NEAR(extracted_position.alt, test_case.expected_alt, 0.01f);
}
}
INSTANTIATE_TEST_SUITE_P(
RTL,
ExtractValidSafePointPositionTest,
::testing::Values(
ExtractValidSafePointPositionCase{
"GlobalAbsoluteRallyPoint",
makeSafePointItem(kBaseLat, kBaseLon, 510.f, NAV_FRAME_GLOBAL),
kAlt,
true,
kBaseLat,
kBaseLon,
510.f,
},
ExtractValidSafePointPositionCase{
"GlobalIntRallyPoint",
makeSafePointItem(kBaseLat, kBaseLon, 510.f, NAV_FRAME_GLOBAL_INT),
kAlt,
true,
kBaseLat,
kBaseLon,
510.f,
},
ExtractValidSafePointPositionCase{
"GlobalRelativeRallyPoint",
makeSafePointItem(kBaseLat, kBaseLon, 25.f, NAV_FRAME_GLOBAL_RELATIVE_ALT),
kAlt,
true,
kBaseLat,
kBaseLon,
kAlt + 25.f,
},
ExtractValidSafePointPositionCase{
"GlobalRelativeIntRallyPoint",
makeSafePointItem(kBaseLat, kBaseLon, 25.f, NAV_FRAME_GLOBAL_RELATIVE_ALT_INT),
kAlt,
true,
kBaseLat,
kBaseLon,
kAlt + 25.f,
},
ExtractValidSafePointPositionCase{
"RelativeRallyPointWithoutHomeAltitude",
makeSafePointItem(kBaseLat, kBaseLon, 25.f, NAV_FRAME_GLOBAL_RELATIVE_ALT),
NAN,
false,
NAN,
NAN,
NAN,
},
ExtractValidSafePointPositionCase{
"UnsupportedFrame",
makeSafePointItem(kBaseLat, kBaseLon, 510.f, NAV_FRAME_MISSION),
kAlt,
false,
NAN,
NAN,
NAN,
},
ExtractValidSafePointPositionCase{
"NonRallyCommand",
makeSafePointItem(kBaseLat, kBaseLon, 510.f, NAV_FRAME_GLOBAL, NAV_CMD_WAYPOINT),
kAlt,
false,
NAN,
NAN,
NAN,
},
ExtractValidSafePointPositionCase{
"NanLatitude",
makeSafePointItem(NAN, kBaseLon, 510.f, NAV_FRAME_GLOBAL),
kAlt,
false,
NAN,
NAN,
NAN,
},
ExtractValidSafePointPositionCase{
"NullIsland",
makeSafePointItem(0.0, 0.0, 510.f, NAV_FRAME_GLOBAL),
kAlt,
false,
NAN,
NAN,
NAN,
},
ExtractValidSafePointPositionCase{
"LatitudeOutOfRange",
makeSafePointItem(91.0, kBaseLon, 510.f, NAV_FRAME_GLOBAL),
kAlt,
false,
NAN,
NAN,
NAN,
},
ExtractValidSafePointPositionCase{
"LongitudeOutOfRange",
makeSafePointItem(kBaseLat, 181.0, 510.f, NAV_FRAME_GLOBAL),
kAlt,
false,
NAN,
NAN,
NAN,
}),
[](const ::testing::TestParamInfo<ExtractValidSafePointPositionTest::ParamType> &param_info)
{
return std::string(param_info.param.test_name);
});
// WHY: Approach altitude can be absolute or relative.
// WHAT: Relative altitude should add home altitude; absolute altitude should not.
TEST_F(RTLTest, MakeVtolLandApproachPointConvertsRelativeAndAbsoluteAltitude)
{
// GIVEN: Absolute and relative loiter items in both MAVLink frame variants.
const PositionYawSetpoint absolute_position = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 40.f, 0.f, 530.f);
const PositionYawSetpoint relative_position = makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 40.f, NAN);
const mission_item_s absolute_item = makeLandApproachItem(absolute_position.lat, absolute_position.lon,
absolute_position.alt, kApproachRadius);
const mission_item_s absolute_int_item = makeLandApproachItem(absolute_position.lat, absolute_position.lon,
absolute_position.alt, kApproachRadius, NAV_FRAME_GLOBAL_INT);
const mission_item_s relative_item = makeLandApproachItem(relative_position.lat, relative_position.lon, 25.f,
kApproachRadius, NAV_FRAME_GLOBAL_RELATIVE_ALT);
const mission_item_s relative_int_item = makeLandApproachItem(relative_position.lat, relative_position.lon, 25.f,
kApproachRadius, NAV_FRAME_GLOBAL_RELATIVE_ALT_INT);
// WHEN: The mission items are converted.
const loiter_point_s absolute_point = _rtl.makeVtolLandApproachPointForTest(absolute_item, kAlt);
const loiter_point_s absolute_int_point = _rtl.makeVtolLandApproachPointForTest(absolute_int_item, kAlt);
const loiter_point_s relative_point = _rtl.makeVtolLandApproachPointForTest(relative_item, kAlt);
const loiter_point_s relative_int_point = _rtl.makeVtolLandApproachPointForTest(relative_int_item, kAlt);
// THEN: The AMSL altitude is resolved correctly.
ASSERT_TRUE(absolute_point.isValid());
ASSERT_TRUE(absolute_int_point.isValid());
ASSERT_TRUE(relative_point.isValid());
ASSERT_TRUE(relative_int_point.isValid());
EXPECT_NEAR(absolute_point.height_m, 530.f, 0.01f);
EXPECT_NEAR(absolute_int_point.height_m, 530.f, 0.01f);
EXPECT_NEAR(relative_point.height_m, kAlt + 25.f, 0.01f);
EXPECT_NEAR(relative_int_point.height_m, kAlt + 25.f, 0.01f);
EXPECT_NEAR(absolute_point.loiter_radius_m, kApproachRadius, 0.01f);
EXPECT_NEAR(absolute_int_point.loiter_radius_m, kApproachRadius, 0.01f);
EXPECT_NEAR(relative_point.loiter_radius_m, kApproachRadius, 0.01f);
EXPECT_NEAR(relative_int_point.loiter_radius_m, kApproachRadius, 0.01f);
}

45
src/modules/navigator/test/test_mission_base.cpp
View File

@@ -52,6 +52,7 @@
#include <gtest/gtest.h>
#include "mission_base.h"
#include "support/vector_mission_item_store.h"
#include <lib/parameters/param.h>
#include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
@@ -105,36 +106,24 @@ public:
bool loadMissionItemFromCache(int32_t index, mission_item_s &mission_item) override
{
for (int32_t failed_index : _load_failure_indices) {
if (failed_index == index) {
return false;
}
}
if (index < 0 || index >= static_cast<int32_t>(_items.size())) {
return false;
}
mission_item = _items[static_cast<size_t>(index)];
return true;
return _mission_store.loadItem(index, mission_item);
}
void loadTestMission(const std::vector<mission_item_s> &items)
{
_items = items;
_mission.count = static_cast<int32_t>(items.size());
_mission_store.setItems(items);
_mission.count = static_cast<int32_t>(_mission_store.itemCount());
_mission.current_seq = 0;
clearLoadFailures();
}
void setLoadFailureIndices(std::initializer_list<int32_t> indices)
{
_load_failure_indices.assign(indices.begin(), indices.end());
_mission_store.setLoadFailureIndices(indices);
}
void clearLoadFailures()
{
_load_failure_indices.clear();
_mission_store.clearLoadFailures();
}
void setCurrentSequence(int32_t current_seq)
@@ -157,8 +146,7 @@ public:
using MissionBase::MissionTraversalType;
private:
std::vector<mission_item_s> _items;
std::vector<int32_t> _load_failure_indices;
navigator_test::VectorMissionItemStore _mission_store{};
};
class IgnoreDoJumpMissionBaseTestPeer : public MissionBaseTestPeer
@@ -202,7 +190,8 @@ static mission_item_s makeVtolTransitionItem(int transition_mode)
return item;
}
class MissionBaseTraversalTest : public ::testing::Test
template<typename TestPeer>
class MissionBaseTraversalTestBase : public ::testing::Test
{
protected:
static void SetUpTestSuite()
@@ -212,21 +201,11 @@ protected:
static void TearDownTestSuite() {}
MissionBaseTestPeer mission_base{};
TestPeer mission_base{};
};
class IgnoreDoJumpMissionBaseTraversalTest : public ::testing::Test
{
protected:
static void SetUpTestSuite()
{
(void)navigatorDatamanRuntime();
}
static void TearDownTestSuite() {}
IgnoreDoJumpMissionBaseTestPeer mission_base{};
};
class MissionBaseTraversalTest : public MissionBaseTraversalTestBase<MissionBaseTestPeer> {};
class IgnoreDoJumpMissionBaseTraversalTest : public MissionBaseTraversalTestBase<IgnoreDoJumpMissionBaseTestPeer> {};
// WHY: getNonJumpItem is used to find the next mission item.
// WHAT: A non-DO_JUMP item is returned unchanged.