Update Path Manager to newest algorithm

[iandareid]

The newest implementation of the path management algorithm has some improvements, we need to implement them, and along the way, fix the issue of always adding (0,0) as a waypoint. While we are looking over the path manager it should also be adjusted to handle double backing waypoints or very shallow waypoints. Also it should handle only 2 waypoints.

[iandareid]

The algorithm matches closely what the newest algorithm is. The names are different than in the book, so a big part of the upgrade will simply be refactoring names for consistency, and readability. The change will also likely introduce a new state to handle the last waypoint in the list, to either orbit it or initiate the cycle again.

[iandareid]

It looks like the looping doesn't quite work, in the case of waypoints in a straight line. This is also a problem when the waypoints double back on themselves. It appears that the center radius is sent to infinity, so the aircraft simply flies on forever. This is also an issue for shallow angles waypoints (sharp turns). The radius is sent far away, there should be a check to see if it is placed strangely.

[iandareid]

For the situation that a set of three waypoints create too acute an angle, we are opting to default to vanilla line following for the points.

The criteria for "too acute" is if the center of the orbit radius is past the closest waypoint to the waypoint, w.

Perhaps we should consider adding follow type between line and fillet to the path message. This would allow people to choose if they would rather have the aircraft follow the path in a fairly acute angle even if the fillet isn't 'too far'.

[iandareid]

This has been implemented along with a warning if an added point is within the minimum turning radius of the previous point. Essentially this warns that the behavior will not be what is expected.

What is yet to be done:

• Refactoring code to match book
• Cleaning up duplicate code
• Implementing final waypoint as a state
• Getting clear on what the transition state is for in the MAVsim version.

[iandareid]

Upon further study, the implementation of the final waypoint as a state is necessary. This is because the incrementing of the indexes happens else where than in the similar code in MAVsim. So this will be skipped.

[iandareid]

The refactoring has been complete, idx_a_, idx_b and idx_c because they are just as valid as any other notation and there is clear doucmentation.