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

feat(navigator): Geofence Aware RTL (#27145)

Browse Source 
Plan RTL paths that route around geofence boundaries — both inclusion
and exclusion zones — instead of flying straight through them and
breaching. The planner builds a visibility graph over the margin-inflated
geofence polygons and circles and runs Dijkstra to find the shortest
legal return path, falling back to a straight line to the destination
when no valid path exists.

Highlights:
- New reusable libraries: src/lib/dijkstra (generic shortest path) and
  src/lib/geofence (fixed-point geometry, polygon inflation, bitangent
  visibility); the RTL planner lives in navigator/RTLPlanner.
- Visibility graph keeps only bitangent edges and skips edges that poke
  into a forbidden region, greatly reducing edge-cost computation.
- Corner splitting is limited to sharp convex corners.
- Geometry validation: reject self-intersecting polygons and vertices
  outside the fixed-point range; centimeter fixed-point scaling keeps
  orientation tests exact and avoids drift at large distances.
- Failure handling: a Status enum replaces silent bool returns, and
  failures (unbuildable fence, planner capacity overflow, dataman load
  errors, NaN waypoints, destinations that breach the fence) are
  surfaced to the operator via MAVLink warnings/criticals; the planner
  falls back to a straight-line RTL.
- Destination updates are centralized in RTL::setRtlTypeAndDestination;
  the path is replanned only when the destination or geometry changes.
- VTOLs always use the fixed-wing margin (FW loiter radius).
- kMaxNodes is exposed as a Kconfig option (default 100); the feature is
  disabled on flash-constrained boards (FMUv4 and older, various F4/F7).
- Tests: unit tests for the Dijkstra lib, geofence geometry utils, and
  the avoidance planner, plus a MAVSDK SITL test flying an RTL through a
  geofence.
- Documentation: new "Geofence Awareness" section in the RTL docs.

Co-authored-by: Balduin <balduin@auterion.com>
This commit is contained in:
Roman Bapst
2026-07-09 15:08:27 +03:00
committed by GitHub
parent d448e3f5b0
commit 40e61d2429
47 changed files with 4147 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -46,10 +46,10 @@ PX4 provides four alternative approaches for finding an unobstructed path to a s
At high level these are:
- [Home/rally point return](#home_return) (`RTL_TYPE=0`): Ascend to safe altitude and return via a direct path to the closest rally point or home location.
- [Mission landing/rally point return](#mission-landing-rally-point-return-type-rtl-type-1) (`RTL_TYPE=1`): Ascend to a safe altitude, fly direct to the closest destination _other than home_: rally point or start of mission landing.
- [Home/rally point return](#rtl_type_0) (`RTL_TYPE=0`): Ascend to safe altitude and return via a direct path to the closest rally point or home location.
- [Mission landing/rally point return](#rtl_type_1) (`RTL_TYPE=1`): Ascend to a safe altitude, fly direct to the closest destination _other than home_: rally point or start of mission landing.
If no mission landing or rally points are defined, return home via direct path.
- [Mission path return](#mission-path-return-type-rtl-type-2) (`RTL_TYPE=2`): Use mission path and fast-continue to mission landing (if defined).
- [Mission path return](#rtl_type_2) (`RTL_TYPE=2`): Use mission path and fast-continue to mission landing (if defined).
If no mission _landing_ defined, fast-reverse mission to home.
If no _mission_ defined, return direct to home (rally points are ignored).
- [Closest safe destination return](#closest-safe-destination-return-type-rtl-type-3) (`RTL_TYPE=3`): Ascend to a safe altitude and return via direct path to closest destination: home, start of mission landing pattern, or rally point.
@@ -57,16 +57,14 @@ At high level these are:
More detailed explanations for each of the types are provided in the following sections.
<a id="home_return"></a>
### Home/Rally Point Return Type (RTL_TYPE=0)
### Home/Rally Point Return Type (RTL_TYPE=0) {#rtl_type_0}
This is the default return type for a [multicopter](../flight_modes_mc/return.md) (see topic for more information).
In this return type the vehicle:
- Ascends to a safe [minimum return altitude](#minimum-return-altitude) (above any expected obstacles).
- Flies via direct path to the home position or a rally point (whichever is closest)
- Flies to the home position or a rally point (whichever is closest), preferring a [geofence-aware](#geofence_awareness) horizontal path over a direct path where possible.
- On [arrival](#loiter-landing-at-destination) descends to "descent altitude" and waits for a configurable time.
This time may be used to deploy landing gear.
- Lands or waits (this depends on landing parameters),
@@ -77,7 +75,7 @@ In this return type the vehicle:
If no rally points are defined, this is the same as a _Return to Launch_ (RTL)/_Return to Home_ (RTH).
:::
### Mission Landing/Rally Point Return Type (RTL_TYPE=1)
### Mission Landing/Rally Point Return Type (RTL_TYPE=1) {#rtl_type_1}
This is the default return type for a [fixed-wing](../flight_modes_fw/return.md) or [VTOL](../flight_modes_vtol/return.md) vehicle (see topics for more information).
@@ -85,7 +83,7 @@ In this return type the vehicle:
- Ascends to a safe [minimum return altitude](#minimum-return-altitude) (above any expected obstacles) if needed.
The vehicle maintains its initial altitude if that is higher than the minimum return altitude.
- Flies via direct constant-altitude path to a rally point or the start of a [mission landing pattern](#mission-landing-pattern) (whichever is closest).
- Flies at constant-altitude to a rally point or the start of a [mission landing pattern](#mission-landing-pattern) (whichever is closest), preferring a [geofence-aware](#geofence_awareness) horizontal path over a direct path where possible.
If no mission landing or rally points are defined the vehicle instead returns home via a direct path.
- 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).
@@ -100,7 +98,7 @@ In this return type the vehicle:
Fixed wing vehicles commonly also set [MIS_TKO_LAND_REQ](#MIS_TKO_LAND_REQ) to _require_ a mission landing pattern.
:::
### Mission Path Return Type (RTL_TYPE=2)
### Mission Path Return Type (RTL_TYPE=2) {#rtl_type_2}
This return type uses the mission (if defined) to provide a safe return _path_, and the [mission landing pattern](#mission-landing-pattern) (if defined) to provide landing behaviour.
If there is a mission but no mission landing pattern, the mission is flown _in reverse_.
@@ -110,6 +108,10 @@ Rally points, if any, are ignored.
The behaviour is fairly complex because it depends on the flight mode, and whether a mission and mission landing are defined.
:::
::: warning
This return type does have [geofence awareness](#geofence_awareness) (at any stage).
:::
Mission _with_ landing pattern:
- **Mission mode:**
@@ -145,7 +147,7 @@ For `RTL_TYPE=4`, PX4 currently chooses between continuing to a mission landing
This is only an approximation of the flown path length, because the number if mission items is not indicative of the distance remaining and non-position items are also counted.
:::
### Closest Safe Destination Return Type (RTL_TYPE=3)
### Closest Safe Destination Return Type (RTL_TYPE=3) {#rtl_type_3}
In this return type the vehicle:
@@ -156,12 +158,62 @@ In this return type the vehicle:
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.
## Geofence Awareness {#geofence_awareness}
For most of the return types (including the default home/rally point return type) the return path is chosen to avoid breaching any geofence.
Planning is purely horizontal: the altitude profile is unaffected, and only the lateral path is adjusted to avoid the fence.
If no geofence is set, the vehicle flies a direct path to the destination.
While the return mode is inactive, the autopilot constantly recalculates a [shortest horizontal return path](#shortest-path-calculation) that does not enter any exclusion zones and does not exit any inclusion zones.
If the return mode is triggered while the vehicle is violating any geofence, then the vehicle will first fly directly to the most recent recorded location at which it was not violating the geofence.
If no such point exists, or if the autopilot fails to plan a feasible path (e.g. the destination is located in an exclusion zone), then the vehicle falls back to flying directly to the destination.
::: info
The estimated time for return is based on the current shortest horizontal path to the destination and may change if the geofence is updated.
:::
::: warning
Geofence awareness currently supports a maximum of 99 polygon vertices in total (circles count as 8 vertices each).
If this limit is exceeded, the autopilot falls back to a direct path as described above.
:::
::: warning
There is no absolute guarantee that the vehicle will not breach a geofence on the return path.
Things like path tracking error, wind and other disturbances may cause temporary violation of the geofence.
It is therefore very important to consider this possibility and especially to review the geofence breach action (e.g. [GF_ACTION](../advanced_config/parameter_reference.md#GF_ACTION)).
:::
### RTL-types with Geofence-Awareness
The following table shows which return types currently support geofence awareness:
| Return Type (RTL_TYPE) | Geofence Awareness |
|------------------------|--------------------|
| 0 (home/rally point) | Yes |
| 1 (mission landing) | Yes |
| 2 (mission path) | No |
| 3 (closest safe dest.) | Yes |
| 4 (mission path) | No |
| 5 (rally point only) | Yes |
### Shortest-Path Calculation
For the construction of the shortest path between the starting location and the destination, the autopilot uses the vertices of the geofence polygons as intermediate waypoints.
In order to avoid the path being too close to the polygon boundaries, the autopilot constructs a corresponding set of polygons, which are either enlarged (for exclusion zones) or shrunk (for inclusion zones).
The margin in both images below is 10m.
The figures below show an exclusion zone and an inclusion zone.
![Exclusion Zone](../../assets/flight_modes/rtl_geofence_exclusion.jpg)
![Inclusion Zone](../../assets/flight_modes/rtl_geofence_inclusion.jpg)
## Minimum Return Altitude
For most [return types](#return_types) a vehicle will ascend to a _minimum safe altitude_ before returning (unless already above that altitude), in order to avoid any obstacles between it and the destination.
::: info
The exception is when executing a [mission path return](#mission-path-return-type-rtl-type-2) from _within a mission_.
The exception is when executing a [mission path return](#rtl_type_2) from _within a mission_.
In this case the vehicle follows mission waypoints, which we assume are planned to avoid any obstacles.
:::
@@ -206,7 +258,7 @@ A mission landing pattern is a landing pattern defined as part of a mission plan
This 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_LAND](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_LAND) (or [MAV_CMD_NAV_VTOL_LAND](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_VTOL_LAND) for a VTOL Vehicle).
Landing patterns defined in missions are the safest way to automatically land a _fixed-wing_ vehicle on PX4.
For this reason fixed-wing vehicles are configured to use [Mission landing/really point return](#mission-landing-rally-point-return-type-rtl-type-1) by default.
For this reason fixed-wing vehicles are configured to use [Mission landing/really point return](#rtl_type_1) by default.
## Parameters

7
docs/en/flight_modes_fw/return.md
View File

@@ -4,10 +4,10 @@
The _Return_ flight mode is used to _fly a vehicle to safety_ on an unobstructed path to a safe destination, where it can land.
Fixed-wing vehicles use the [Mission Landing/Rally Point](../flight_modes/return.md#mission-landing-rally-point-return-type-rtl-type-1) return type by default, and are expected to always have a mission with a landing pattern.
Fixed-wing vehicles use the [Mission Landing/Rally Point](../flight_modes/return.md#rtl_type_1) return type by default, and are expected to always have a mission with a landing pattern.
With this configuration, return mode causes the vehicle to ascend to a minimum safe altitude above obstructions (if needed), fly to the start of the landing pattern defined in the mission plan, and then follow it to land.
Fixed-wing supports the [other PX4 return types](../flight_modes/return.md#return-types-rtl-type), including home/rally point return, mission path and closest safe destination.
Fixed-wing supports the [other PX4 return types](../flight_modes/return.md#return_types), including home/rally point return, mission path and closest safe destination.
The default type is recommended.
::: info
@@ -33,7 +33,8 @@ In this return type the vehicle:
- Ascends to a safe minimum return altitude defined by [RTL_RETURN_ALT](#RTL_RETURN_ALT) (safely above any expected obstacles).
The vehicle maintains its initial altitude if that is higher than the minimum return altitude.
Note that return altitude cannot be configured using the "cone" parameter in fixed-wing vehicles.
- Flies via direct constant-altitude path to the destination, which will be the closest of the start of a _mission landing pattern_ and any rally point, or the home location if no mission landing pattern or rally points are defined.
- Flies via a constant-altitude path to the destination, which will be the closest of the start of a _mission landing pattern_ and any rally point, or the home location if no mission landing pattern or rally points are defined.
The path is chosen to be the shortest horizontal [geofence-aware path](../flight_modes/return.md#geofence_awareness).
- 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 descend to the descent altitude, and then loiter or land (depending on landing parameters).

13
docs/en/flight_modes_mc/return.md
View File

@@ -4,11 +4,11 @@
The _Return_ flight mode is used to _fly a vehicle to safety_ on an unobstructed path to a safe destination, where it can land.
Multicopters use a [home/rally point return type](../flight_modes/return.md#home-rally-point-return-type-rtl-type-0) by default.
In this return type vehicles ascend to a safe altitude above obstructions if needed, fly directly to the closest safe landing point (a rally point or the home position), descend to the "descent altitude", wait briefly, and then land.
Multicopters use a [home/rally point return type](../flight_modes/return.md#home_return) by default.
In this return type vehicles ascend to a safe altitude above obstructions if needed, fly to the closest safe landing point (a rally point or the home position) via the shortest horizontal [geofence-aware path](../flight_modes/return.md#geofence_awareness), descend to the "descent altitude", wait briefly, and then land.
The return altitude, descent altitude, and landing delay are normally set to conservative "safe" values, but can be changed if needed.
Multicopter supports the [other PX4 return types](../flight_modes/return.md#return-types-rtl-type), including mission landing, mission path and closest safe destination.
Multicopter supports the [other PX4 return types](../flight_modes/return.md#return_types), including mission landing, mission path and closest safe destination.
The default type is recommended.
::: info
@@ -28,12 +28,13 @@ The default type is recommended.
## Technical Summary
Multicopters use the [home/rally point return type](../flight_modes/return.md#home-rally-point-return-type-rtl-type-0) by default ([RTL_TYPE=0](../advanced_config/parameter_reference.md#RTL_TYPE)).
Multicopters use the [home/rally point return type](../flight_modes/return.md#home_return) by default. ([RTL_TYPE=0](../advanced_config/parameter_reference.md#RTL_TYPE)).
For this return type the copter:
- Ascends to the [minimum return altitude](#minimum-return-altitude) (safely above any expected obstacles).
The vehicle maintains its initial altitude if that is higher than the minimum return altitude.
- Flies via a direct constant-altitude path to the safe landing point, which will be the nearest of any rally points and the home position.
- Flies via a constant-altitude path to the safe landing point, which will be the nearest of any rally points and the home position.
The path is chosen to be the shortest horizontal [geofence-aware path](../flight_modes/return.md#geofence_awareness).
- On arrival at its destination, it rapidly descends to the "descent altitude" ([RTL_DESCEND_ALT](#RTL_DESCEND_ALT)).
- It waits for a configurable time ([RTL_LAND_DELAY](#RTL_LAND_DELAY)), which may be used for deploying landing gear.
- Then lands.
@@ -52,7 +53,7 @@ The cone affects the minimum return altitude if return mode is triggered within
Inside the cone, the vehicle returns at an altitude calculated from the cone geometry, up to `RTL_RETURN_ALT`.
After reaching the destination, it descends to `RTL_DESCEND_ALT` (if above that altitude) before landing or loitering.
For more information on this return type see [Home/Rally Point Return Type (RTL_TYPE=0)](../flight_modes/return.md#home-rally-point-return-type-rtl-type-0)
For more information on this return type see [Home/Rally Point Return Type (RTL_TYPE=0)](../flight_modes/return.md#home_return)
## Parameters

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

@@ -4,7 +4,7 @@
The _Return_ flight mode is used to _fly a vehicle to safety_ on an unobstructed path to a safe destination, where it may either wait (hover or circle) or land.
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.
VTOL vehicles use the [Mission Landing/Rally Point](../flight_modes/return.md#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 to that destination and land.
@@ -12,7 +12,7 @@ If the destination is a rally point or the home location, the vehicle will fly t
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.
VTOL supports the [other PX4 return types](../flight_modes/return.md#return-types-rtl-type), including home/rally point return, mission path and closest safe destination.
VTOL supports the [other PX4 return types](../flight_modes/return.md#return_types), including home/rally point return, mission path and closest safe destination.
The default type is recommended.
::: info
@@ -32,7 +32,7 @@ The default type is recommended.
## Technical Summary
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, and return using the flying mode (MC or FW) it was using at the point when return mode was triggered.
VTOL vehicles use the [Mission Landing/Rally Point](../flight_modes/return.md#rtl_type_1) return type by default, and return using the flying mode (MC or FW) it was using at the point when return mode was triggered.
### Fixed-wing Mode (FW) Return
@@ -41,7 +41,8 @@ If returning as a fixed-wing, the vehicle:
- Ascends to a safe minimum return altitude defined by [RTL_RETURN_ALT](#RTL_RETURN_ALT) (safely above any expected obstacles).
The vehicle maintains its initial altitude if that is higher than the minimum return altitude.
<!-- Note that return altitude cannot be configured using the "cone" parameter in fixed-wing vehicles. -->
- Flies via direct constant-altitude path to the destination, which will be the closest of the start of a _mission landing pattern_ and any rally point, or the home location if no mission landing pattern or rally points are defined.
- Flies via a constant-altitude path to the destination, which will be the closest of the start of a _mission landing pattern_ and any rally point, or the home location if no mission landing pattern or rally points are defined.
The path is chosen to be the shortest horizontal [geofence-aware path](../flight_modes/return.md#geofence_awareness).
- If the destination is a mission landing pattern it will follow the pattern to land.
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).