search

ArduPilot / ardupilot

ArduPlane, ArduCopter, ArduRover, ArduSub source
http://ardupilot.org/
GNU General Public License v3.0
10.84k stars 17.3k forks source link

Plane: Implement SmartRTL #2380

Open magicrub opened 9 years ago

magicrub commented 9 years ago

Add "breadcrumbs" to the waypoints param. A simple boolean will work. Not all waypoints need to be flagged as a breadcrumb, just the ones that will give the shortest, or safest, return path. This is useful when navigating around hills or no fly zones (ie. airport) and an RTL is triggered and a direct return-to-home path is not desired.

When RTL_AUTOLAND == 3, an RTL event will cause the waypoint trigger mechanism to execute backwards (decrement to zero) and to skip all non-breadcrumb waypoints. When a NAV_TAKEOFF or index = 0 is executed then we jump to DO_LAND_START and perform an auto-land.

AndKe commented 9 years ago

While the idea is great - it will not solve more complex missions where going back may require much more time/range than acceptable.

Please view my simplified mission plan that covers rivers and hydro-power related installations - and is to be flown mostly at 200mAGL while the airspace will be a D-area up to 4500ft. 

QGC WPL 110
0   1   0   16  0   5   0   0   69.7863078535199293 20.9600257873535156 0   1
1   0   3   16  0   5   0   0   69.7701685849457789 21.0161590576171875 200 1
2   0   3   16  0   5   0   0   69.7273873773582977 21.2139129638671875 200 1
3   0   3   16  0   5   0   0   69.6838043134417688 21.2331390380859375 200 1
4   0   3   16  0   5   0   0   69.6556540002226257 21.302490234375 200 1
5   0   3   16  0   5   0   0   69.6150327876682269 21.2969970703125    200 1
6   0   3   16  0   5   0   0   69.591341136690076  21.3045501708984375 200 1
7   0   3   16  0   5   0   0   69.5601908520236947 21.320343017578125  200 1
8   0   3   16  0   5   0   0   69.5563538268032033 21.542816162109375  200 1
9   0   3   16  0   5   0   0   69.6030706003336945 21.501617431640625  200 1
10  0   3   16  0   5   0   0   69.6224459702857956 21.5641021728515625 200 1
11  0   3   16  0   5   0   0   69.6284224631071424 21.5496826171875    200 1
12  0   3   16  0   5   0   0   69.6241195574781671 21.533203125    200 1
13  0   3   16  0   5   0   0   69.6675866566755673 21.551055908203125  200 1
14  0   3   16  0   5   0   0   69.6890485494462695 21.578521728515625  200 1
15  0   3   16  0   5   0   0   69.7004860189441757 21.5737152099609375 200 1
16  0   3   16  0   5   0   0   69.7409441283639779 21.6162872314453125 200 1
17  0   3   16  0   5   0   0   69.7490264758031913 21.649932861328125  200 1
18  0   3   16  0   5   0   0   69.8104936259925921 21.60736083984375   200 1
19  0   3   16  0   5   0   0   69.8284963665843321 21.4501190185546875 200 1
20  0   3   16  0   5   0   0   69.8436446360485093 21.42059326171875   200 1
21  0   3   16  0   5   0   0   69.8654011734524403 21.4267730712890625 200 1
22  0   3   16  0   5   0   0   69.8531067673118429 21.4013671875   200 1
23  0   3   16  0   5   0   0   69.823997124189674  21.43707275390625   200 1
24  0   3   16  0   5   0   0   69.7955586310950338 21.4020538330078125 200 1
25  0   3   16  0   5   0   0   69.7846470280158826 21.3306427001953125 200 1
26  0   3   16  0   5   0   0   69.7563929826385873 21.2921905517578125 200 1
27  0   3   16  0   5   0   0   69.7352370815084868 21.2235260009765625 200 1
28  0   3   16  0   5   0   0   69.7834606440347329 20.9845733642578125 200 1
29  0   3   20  0   5   0   0   69.7898663250795011 20.9893798828125    200 1
30  0   3   16  0   5   0   0   69.7430838739579428 21.644439697265625  200 1
31  0   3   16  0   5   0   0   69.7575808884536031 21.4075469970703125 200 1
32  0   3   16  0   5   0   0   69.7872568379650176 21.2070465087890625 200 1
33  0   3   20  0   5   0   0   69.7884430084903613 21.19537353515625   200 1
34  0   3   16  0   5   0   0   69.8393852875752827 21.402740478515625  200 1
35  0   3   16  0   5   0   0   69.862564641217574  21.367034912109375  200 1
36  0   3   16  0   5   0   0   69.849322425782276  21.2633514404296875 200 1
37  0   3   16  0   5   0   0   69.8398585911116072 21.1795806884765625 200 1
38  0   3   16  0   5   0   0   69.7972185976148012 21.1287689208984375 200 1
39  0   3   20  0   5   0   0   69.7967443348015308 21.1054229736328125 200 1

Ignore the altitudes. Imagine a suggested variable of RTL_PATH = 0 by default.

Of course, this was only two examples, insufficient for this mission - but you get my point, the advantage is:

EShamaev commented 9 years ago

Better would be to setup a separate return plan and go the closes point of this plan if RTL is executed

EShamaev commented 9 years ago

I can do this after I finish new auto land code

magicrub commented 9 years ago

Here is @AndKe 's mission: mtnmissionexample

magicrub commented 9 years ago

@EShamaev , what you just described is already implemented as In RTL_AUTOLAND=1or2 and mission cmd DO_LAND_START where multiple DO_LAND_STARTS are used in a single mission. http://planner.ardupilot.com/wiki/common-planning-a-mission-with-waypoints-and-events/common-mavlink-mission-command-messages-mav_cmd/#mav_cmd_do_land_start

The problem with is, as @AndKe described in his example, is a direct line to a given return is not helpful when trying to avoid mountains and airports.

And in @AndKe if you get an RTL event between points 7,8,9,10,(ect) then you're crashing into the mountain so your jump method doesn't make much sense to me.

AndKe commented 9 years ago

@magicrub ; no , as I wrote: " this was only two examples, insufficient for this mission" The point is not having a complete solution for a mission of this size, the point was to visualize the challenges with big areas and a desire to avoid climbing over mountains, or spending too much time, or even exceeding fuel capacity by backtracking in case of error.

My approach may not be the best, but it is the only one here that offers multiple back-tracks based on where in a mission we are.
The breadcrumbs are still a great idea, very useful for simpler missions and survey-patterns. - This is for more complex missions.

magicrub commented 9 years ago

Sounds like no single solution will solve this?

AndKe commented 9 years ago

no, unless you think my approach is better, no - then both solutions are best in their own ways. I actually like your idea, for a single mission up a valley, to follow edge of a reservoir it is great. - solving the problem fine with a minimum of work, leaves a clean mission plan. For a bigger, more complex mission, it is not good enough. But who says all should be satisfied in one solution. The reason I posted it here, is that maybe, you thought my plan would cover everyones needs. My backtrack plan could be auto-generated along "forward" path in mission planner, also, it's function could be visualized to simply see at a glance which path would be taken back - from where. (different colors, thin lines)

robustini commented 9 years ago

My similar request: https://github.com/diydrones/ardupilot/issues/1815

EShamaev commented 9 years ago

My point was not so obvious. What I meant is to have several absolutely separate flight plans that are selected by nearest (as a tangent line to route, not only to the point) when RTL is commanded. image It will give some overhead during mission programming but in difficult conditions, it may be worth it.

Going further, the RTL graph maybe setup and any appropriate algorithm for finding short route may be used.

AndKe commented 9 years ago

@EShamaev Maybe you are onto something better there, (when you mention "finding short route") - what if we only drew "allowed-shortcuts" between which AP were allowed to use to automtically take shortest route home, - provided that the lines crossed, or the emergency waypoints were max 50m from an actual WP (path?) - like telling AP "fly any legs as you wish, for shortest route home - you may also fly the red shortcuts is any crosses, or are within 50m from nearest path"

EShamaev commented 9 years ago

This is exactly what I propose. Make a "graph" of safe routes. And use any algo for shortest route such as Dijkstra's algorithm. The AP will return to the closes edge of graph and continue on the safe route back to home. Also it will be the shortest one.

JohnArneBirkeland commented 9 years ago

All this is pretty much solved when we get support for topographic maps. No need for breadcrumbs/predefined safe paths if the autopilot can do actual terrain + margin height calculations and select the optimal path based on this.

EShamaev commented 9 years ago

I don't believe there will be computational power on board to allow this at such extent. Maybe with help of companion computer but it's not the general case that people use. Also the coding difficulty will be way higher than methods discussed here. However the topographic support is good. Can you provide rough terms of such feature to be implemented and useful in real applications?

JamesHarrison commented 9 years ago

@JohnArneBirkeland @EShamaev More to the point re topographic support, that covers you just fine in the case of missions with no man-made structures potentially impacting the flight path, but reality is rarely so forgiving. RTL should be straight-line at set height unless more data is provided by users before takeoff, imho - support complex flight scenarios without making vehicle behaviour unexpected or complex. What form that input takes and how complex it gets in terms of routing, that's an interesting question. A simple list of linked lists of waypoints might be sufficient - each chain ends at home, vehicle goes to its closest RTL waypoint and then follows the route home.

magicrub commented 9 years ago

Just so we're on the same page, does everyone know how the existing mission cmd DO_LAND_START and param RTL_AUTOLAND=1or2 functions?

Upon an RTL mode change, the entire waypoint cmd list is scanned for the nearest geographically located DO_LAND_START command (it contains a lat/lon). Once the nearest one is found, it will jump the wyapoint index to that command and execute the next command from there. We never actually navigate directly to the DO_LAND_START point, it's just a marker in the waypoint list.

AndKe commented 9 years ago

@JohnArneBirkeland @EShamaev a direct RTL is ok for small missions. Computing power is not really a problem, nor for pixhawk, or Raspberry Pi+ NAVIO. Regulations, however: Look at my map, if GCS link fails, how can I cannot "assume" the whole area is precached, nor does such data contain high voltage lines across mountains or fjords, nor does it contain other data. The area during such flight would be a restricted area, we cannot allow the AP to get creative regarding it's path home, in the unlucky event of GCS communication failure. It's simply not a legal solution, we need to maintain flight within the restricted zone, be able to predict where and when the UAV should be back, and if not , have a strong clue about where to search for it. Terrain following based on low resolution altitude data that may or may not be cached, is not an option.

AndKe commented 9 years ago

@magicrub - are you saying a shrt/safe way home can be selected for any spot in a mission like mine ?

JamesHarrison commented 9 years ago

@magicrub Right, so by having a few DO_LAND_START points followed by waypoints to home, the multiple-predefined-routes-to-home RTL as described above can already be programmed?

@AndKe Absolutely agree on needing to be able to define RTL behaviour in a deterministic manner. Letting the AP figure it out itself isn't ever going to be viable as an option; there's too much data it needs to know about.

magicrub commented 9 years ago

@AndKe Yes, but what I'm trying to avoid is a mission with 5 missions in it. Also, each of those missions must be hand configured making mission planning much harder. My initial proposal may not be optimum but will happen with little-to-no human effort and without an overly confusing mission to plan. In fact, I suppose both mechanisms could be used at the same time.

magicrub commented 9 years ago

There's also Rally points that may play into this solution but I don't understand those so perhaps someone could enlighten us?

davidbuzz commented 9 years ago

At the moment, when a aircraft RTLs, it flies to the nearest Rally Point ( plane then loiters, and copter then lands). I suggest that it might be easy to implement this solution by allowing any Rally Point to instruct the aircraft to fly to another Rally Point.

magicrub commented 9 years ago

That's a reasonably good idea On Jun 8, 2015 7:00 PM, "Buzz" notifications@github.com wrote:

at the moment, when a aircraft RTLs, it flies to teh nearest Rally Point ( plane then loiters, and copter then lands). I suggest that it might be easy to implement this solution by allowing any Rally Point to instruct the aircraft to fly to another Rally Point.

— Reply to this email directly or view it on GitHub https://github.com/diydrones/ardupilot/issues/2380#issuecomment-110196708 .

SkyHawkDP commented 9 years ago

I had been wondering why Rally Points hadn't been considered.

Aside: there's a trade off between getting to a safe location quickly and knowing where your vehicle ended up. Perhaps the Rally Point solution suggested by davidbuzz might be the most reasonable, especially if that leads to reestablishing communications as quickly as possible.

SkyHawkDP commented 9 years ago

There's another important RTL requirement for FPV. It's probably a lot to ask that it be covered by the same algorithm. Perhaps the prime objective there would be to recover communications... (although not the only one). Perhaps the ideas for the previous discussion should be closed before taking this on, but hopefully we can pick it up later.

AndKe commented 9 years ago

Rally points, as currently, in a scenario like this just ditching points.
I don't mind planning "5 missions in it" for an occasional mission with very expensive UAV/payload. Just would like to have an option to do so,

EShamaev commented 9 years ago

Anyway, please decide if my idea of separate graph of safe RTL is worth implementing. If it is I will be able to do it.

@AndKe Regarding: Computing power is not really a problem, nor for pixhawk, or Raspberry Pi+ NAVIO.

Speaking of pixhawk route planning with terrain data is actually a problem with computing power. Using companion board solves this, but this kind of solution is beyond my abilities and time.

magicrub commented 9 years ago

@EShamaev what do you mean "separate path"? How is it different than the existing alternate RTL path solutions: DO_LAND_START and Rally points? Where do you store the mission? How does it get planned? How much [human] effort is required to plan it?

WickedShell commented 9 years ago

I exclusively fly rally points :) Where we takeoff from is usually a poor place for the aircraft to loiter/land at, (IE the edge of a field by a road), and we will typically move and land at a point 4+ km's from our takeoff location, which means that returning to home in the event of RTL will really cause the AV to fly further away from the operator. That said being able to use multiple RTL points to path plan would be a nice/interesting feature.

Something I'm not sure about, maybe someone could tell me, how does RTL_AUTOLAND and rally points interact? IE it would be interesting to use a rally point to dictate what DO_LAND_START (or other rally point) to jump to.

magicrub commented 9 years ago

I don't think they interact. Here's my previous post about how DO_LAND_START works with RTL_AUTOLAND:

Just so we're on the same page, does everyone know how the existing mission cmd DO_LAND_START and param RTL_AUTOLAND=1or2 functions?

Upon an RTL mode change, the entire waypoint cmd list is scanned for the nearest geographically located DO_LAND_START command (it contains a lat/lon). Once the nearest one is found, it will jump the wyapoint index to that command and execute the next command from there. We never actually navigate directly to the DO_LAND_START point, it's just a marker in the waypoint list.

AndKe commented 9 years ago

seeing these questions, and understanding that not everybody can read source to verify behaviour - I think we could use a flow-diagram with different mission items and errors in the documentation to make the outcome clear in different circumstances.

magicrub commented 9 years ago

possibly depends on https://github.com/diydrones/ardupilot/issues/2391

magicrub commented 9 years ago

@rmackay9 I heard you may want to work on this soon. Let's tag team!

amilcarlucas commented 7 years ago

AFAIK implemented by https://github.com/ArduPilot/ardupilot/pull/6792

WickedShell commented 6 years ago

This remains valid for plane to hook SmartRTL as well, updating the title to be appropriate.

olaldiko commented 1 year ago

Hello is there any ETA for when SafeRTL like the one implemented for copters in #6792 will be available for planes? We do need it for our use case, as direct RTL with Terrain following is not suficcient for us, as the mountains are too high to climb for the plane(Himalayas)

Ryanf55 commented 8 months ago

Pending the approval of a funding proposal for an aircraft, I am hoping to have something usable in time for the 2024 developer conference in September 2024. If funding falls through, then it could be a SITL solution and then perhaps I can re-apply for funding. Recent research and resultant open-source algorithms have shown a path forward. Due to the computation complexity of running a 3D path planner over terrain at the scales that fixed-wing vehicles operate at, it will require a companion computer until the STM32 or similar hardware can catch up in CPU+RAM. Right now we have a very prototype hack working in SITL over MAVLink.