You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
Copy file name to clipboardExpand all lines: docs/en/concept/mission_route_planning.md
+47-15Lines changed: 47 additions & 15 deletions
Display the source diff
Display the rich diff
Original file line number
Diff line number
Diff line change
@@ -1,17 +1,19 @@
1
1
# Mission Route Planning Infrastructure
2
2
3
-
PX4 includes mission-route planning infrastructure in Navigator that provides a non-blocking full-route cache for quick access to the whole uploaded mission, and a planner that projects the vehicle and safe-point positions onto the mission path.
4
-
The goal is to minimize how far the vehicle deviates from the planned route during contingencies, for example:
3
+
PX4 includes mission-route planning infrastructure in Navigator for intelligently joining, following, and completing a planned route.
4
+
This infrastructure might be used for:
5
+
-**Smart mission rejoin**: Finding the best branch-in point to rejoin a mission (for example after moving to a GoTo point).
6
+
-**Route-following Return**: Choosing the best [safe point](../flying/plan_safety_points.md) exit point when using the mission route as a [Return mode](../flight_modes/return.md): branching off close to the selected point instead of cutting straight across terrain.
5
7
6
-
-**Smart mission join**: after a GoTo or a manual reposition, project the vehicle onto every route segment and rejoin the mission at the best branch-in point.
7
-
-**Route-following Return**: a [Return mode](../flight_modes/return.md)that follows the mission route and only branches off once it is close to the selected [safe point](../flying/plan_safety_points.md), instead of cutting straight across terrain.
8
+
The infrastructure is included in the Navigator, and provides a non-blocking full-route cache for quick access to the whole uploaded mission.
9
+
It also includes a planner that projects the vehicle and safe-point positions perpendicularly onto each mission segment, then selects the best candidate using that projection (along with additional criteria such as the last segment flown).
8
10
9
11
The planner computes geometry and scoring only.
10
12
It does not publish setpoints, change the mission index, or decide when a flight mode should use the result.
11
13
12
14
::: info
13
-
This page documents developer infrastructure.
14
-
By itself it does not add any user-selectable behavior.
15
+
This page documents the algorithms and features (such as mission cache) used for smart route planning.
16
+
At time of writing this infrastructure is not yet used.
15
17
:::
16
18
17
19
## Point Projection
@@ -23,6 +25,32 @@ The planner draws a perpendicular projection from the point onto every route seg
23
25
A projection is a valid candidate only if its crosstrack distance is within a search margin of the closest candidate's crosstrack distance.
24
26
The consuming feature supplies that margin.
25
27
28
+
::: warning
29
+
At time of writing, no geofence check is applied to the projection candidates.
30
+
:::
31
+
32
+
::: details Click here for more detail on how projections land on segments and corners
33
+
34
+
A projection is normally perpendicular to the point on a segment, but it is only kept if it is a _local minimum_ of the distance to the route.
35
+
This rule decides what happens at a waypoint, where two segments meet:
36
+
37
+
- An interior projection, one that lands part-way along a segment, is always kept.
38
+
- A projection that lands exactly on a waypoint (a corner) is kept only when the previous segment and the current segment both project onto that same shared corner.
39
+
This happens when two consecutive segments form a V-shape and the point is closest to the apex:
40
+
41
+
```text
42
+
A \ / C
43
+
\ / P projects onto corner B from both segments.
44
+
\ / Both projections land on B, so the shared
45
+
B \/ corner is kept as a single candidate.
46
+
47
+
^
48
+
P
49
+
```
50
+
51
+
This is why some projections in the images below land on a waypoint instead of perpendicular to a segment.
52
+
:::
53
+
26
54
The example below uses a large margin.
27
55
The mission route is drawn in white and the rally-point (`R`) projections in green.
28
56
Every rally point has three projections because the margin is large.
@@ -52,25 +80,29 @@ The vehicle is projected onto the route as described in [Point Projection](#poin
52
80
The best candidate is chosen with a priority system:
53
81
54
82
- Priority 1: a candidate is selected immediately if it lies on the segment the vehicle is currently expected to be flying (the last targeted waypoint index, or the last flown `DO_JUMP` segment if the caller supplied it).
55
-
- Priority 2: if no candidate matches Priority 1, the planner scores each candidate by `A + B` and keeps the lowest:
56
-
- A (crosstrack distance): from the vehicle to the projection on the route.
57
-
- B (distance to last flown segment): from the projection to whichever end of the last-flown segment is closer.
83
+
- Priority 2: if no candidate matches Priority 1, the planner scores each candidate by summing the following distances, and keeps the lowest:
84
+
- Crosstrack distance: from the vehicle to the projection on the route.
85
+
- Distance along the route to the last-flown segment: from the projection, following the mission path to whichever end of the last-flown segment is closer.
86
+
This selects the candidate that gets the vehicle back to where it was last flying if available, or the best match for a close in-sequence segment if it is not.
87
+
58
88
59
89
**Example 1:**
60
90
61
-
The vehicle is flying an A-B-A mission, where the outbound and inbound legs are close to one another at the start.
91
+
The vehicle is flying a mission, where the outbound and inbound legs are close to one another at the start.
62
92
While on the outbound leg, the vehicles deviates east to a GoTo location (e.g. due to incoming traffic).
63
93
That GoTo location is closer to the inbound leg, but both legs are close enough that the projection step returns two valid candidates: one on the outbound leg and one on the inbound leg.
64
94
The outbound segment is the one the vehicle is currently expected to fly, so it wins immediately on Priority 1.
65
-
Even without that rule it would still win on Priority 2, because its distance to the last-flown segment is much smaller.
95
+
Even without that rule it would still win on Priority 2, which measures distance _along the route_ back to the last-flown segment.
96
+
The vehicle was last flying on the outbound leg, so the outbound projection is only a short way along the route from it.
97
+
Reaching that same segment from the inbound projection would mean flying almost the entire mission back, so the inbound candidate scores far higher and loses.
66
98
The vehicle rejoins the mission and continues toward B.
67
99
68
-

100
+

69
101
70
102
**Example 2:**
71
103
72
104
The vehicle is flying north and deviates to a GoTo location that has two valid branch-in candidates.
73
-
Neither projection lies on the current segment, so each candidate is scored by `A + B`: crosstrack distance plus the along-route distance to the last-flown segment.
105
+
Neither projection lies on the current segment, so each candidate is scored by summing its crosstrack distance and the distance along the route to the last-flown segment.
74
106
Take the candidate projected onto the east segment.
75
107
Its score is the crosstrack distance (166.95 m) plus the distance along the route back to the last-flown segment (the red lines).
76
108
The closer end of the last-flown segment is used, and the along-route distance is the sum of the straight lines between waypoints.
@@ -101,9 +133,9 @@ Each candidate branch-off is scored by total path cost, and the lowest wins. The
101
133
102
134
- Along-route distance: along the route geometry from the vehicle projection to the safe-point projection (branch-off point), using straight lines between waypoints.
103
135
- Branch-off leg: the straight-line distance from the safe-point projection to the safe point (the off-route leg flown after leaving the route).
104
-
- U-turn penalty: for fixed-wing and VTOL-in-FW, an extra distance penalty is added when the path would require an immediate U-turn, so forward-flowing paths are preferred. The caller supplies the penalty value (set it to 0 to disable).
136
+
- U-turn penalty: for fixed-wing and VTOL-in-FW, an extra distance penalty is added when the path would require an immediate U-turn, so forward-flowing paths are preferred. The U-turn is detected by comparing the vehicle's current velocity with the direction it would have to fly along the route toward the goal: if the two are more than 90° apart, the vehicle would have to turn around, and the penalty is applied. The caller supplies the penalty value (set it to 0 to disable). Multirotors and hovering VTOL are exempt, because they can turn on the spot.
105
137
106
-
Safe points are filtered while the planner builds its batch:
138
+
As the planner gathers the safe points to consider, it filters out the ones it cannot use:
107
139
108
140
- Safe points are read from the dataman store through the provider.
109
141
- Invalid coordinates, unsupported frames, or filtered safe points are skipped.
0 commit comments