Copter: RTL to rally point with Dijkstra's enabled should consider shortest flight path

[rmackay9]

When Copter enter's RTL mode it checks whether it should return home or fly to a Rally point. That decision is based upon the horizontal distance from the vehicle to home and the rally points. Instead of using the direct distance it should use the length of the flight path to the point which, when using Dijkstra's, could be quite difference

Below is an illustration of a vehicle flying a mission to waypoint 3 after which an RTL command is executed. At this point the vehicle will attempt to fly to rally point 1 but Rally point 2 or home would be closer

[Ryanf55]

Can you share the fence file for this?

[rmackay9]

@Ryanf55,

Sure, here is a zip file with the rally, fence and mission
rally-dijkstras-fence-rally-mission.zip

BTW I've created a branch here with some mostly working code

Here's a video of it in action
https://www.youtube.com/watch?v=nH_pnZ0FbP0