added bvlos_plan module - #1718
Open
tridge wants to merge 3 commits into
Open
Conversation
Planning a BVLOS mission means knowing that an RTL from anywhere along it returns through airspace that was surveyed and approved. With RTL_AUTOLAND=4 an ArduPlane RTL calls AP_Mission::jump_to_closest_mission_leg(), which makes the end waypoint of the closest leg after a DO_RETURN_PATH_START current, and the aircraft then flies direct to it with no crosstrack. That cut across can leave the mission corridor entirely; ArduPilot puts no distance limit on it and does not test it against a fence. Return Path Check samples the mission every 50m and, for each point, works out what ArduPilot would pick and how far the resulting line gets from the mission path. The allowed distance is the turn radius the vehicle can achieve at its cruise airspeed and bank limit at that altitude, so a return that stays inside it is one the aircraft could have flown as part of the mission. Failing stretches are reported with the leg they are on and highlighted on the map in their own layer, which Clear Highlight removes without disturbing the mission. The return path walk follows AP_Mission: DO_JUMP and DO_JUMP_TAG are followed with a 64 loop cap and a finite jump counter zeroed once its loop completes, the search shares a 1000 command budget and a candidate that exhausts it is rejected rather than used truncated, zero length legs are skipped without advancing prev_loc, the first vertex of a path is measured as a point rather than a leg, ties keep the earliest leg within a path and the last DO_RETURN_PATH_START across paths, the walk stops at a landing or DO_LAND_START inclusive, and because set_current_cmd() runs advance_current_nav_cmd() the target resolves forward to the next navigation command, following a DO_JUMP to reach it. A point whose return path reaches no navigation command is an error, not a pass: set_current_cmd() fails there, so an RTL follows nothing. The corridor itself is the navigation commands only. Commands that merely carry a location, such as DO_SET_HOME, DO_SET_ROI and DO_LAND_START, are never flown to and would invent corridor that does not exist. Geometry runs in a local metric frame using ArduPilot's own scaling so distances match get_distance_NED_alt_frame, projected once for the whole mission rather than per operation, which keeps a 110km mission under a few seconds. Leg selection is 3D as in ArduPilot while the corridor test is horizontal, as it is about lateral containment. Altitudes come from the terrain module where the frame needs it, and terrain that cannot be resolved is reported rather than quietly assumed. mission_model.py has no dependency on MAVProxy state so it can be driven offline. The check is sensitive to the airspeed and bank limit, as it should be, so cruise_airspeed and roll_limit settings can override the vehicle parameters for planning without a vehicle connected. return_path_width instead gives a fixed distance either side of the path, for when the corridor is set by the approval rather than by what the aircraft can fly. Not loaded by default: "module load bvlos_plan".
Knowing which stretches of a mission have no safe return is most of the work, but fixing them by hand means drawing a reversed offset track waypoint by waypoint. This does it: for each failing stretch, a DO_RETURN_PATH_START and a run of waypoints back along the mission the way the aircraft came, offset to one side so the reversal is a turn it can fly rather than a course reversal on the spot, ending in a DO_JUMP onto the nearest point of the existing return path. The offset is the turn radius over that stretch, taking the largest so it is enough everywhere along it. Deliberately not the return_path_width, which is a check tolerance and may be much wider than anything the aircraft can fly. return_path_sep overrides it, which is worth using: offsetting by exactly the turn radius leaves the result only a metre or so inside the limit, where a slightly larger separation gives real margin. The new items go after the landing, so they change nothing about the mission as flown, and ArduPilot considers every DO_RETURN_PATH_START and follows DO_JUMP while walking a return path, so the appended block is found and leads home. Because this writes to the mission, it checks the result before committing to it: the candidate is built, the mission is checked with it, and it is kept only if fewer points fail and no new errors appear, so a fix cannot leave a mission worse than it found it. It refuses a mission whose terrain is unavailable, where the new legs would be placed from guessed altitudes, and one that does not end at a landing, where appended navigation items would be flown. The mission is changed in MAVProxy only, and says so, leaving "wp save" or "wp list" to the operator.
The editor only refreshed on wp module activity, so a mission changed by another module was shown stale until the next download. Watch the loader's change stamp as well. That stamp moves for every item received during a download, and idle_task runs often enough that this would rebuild the whole table once per waypoint, so ignore the loader while it holds fewer items than are expected and refresh only on a finished mission.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
This is the start of commands to assist in BVLOS planning. First two commands are: