If WP A and B were the same or ground speed was exactly zero, then the
previous code would produce a nan output. Protection against these
two cases has been added.
If WP A and B are equal, we track directly to the target waypoint
1) Explicit control of tracking loop period and damping which removes previous
variation in period with speed and fixed damping ratio
2) Explicit control of track capture angle (now set to 45 degrees by default)
3) Removal of restriction on loiter radius being greater than L1 distance
The circle(loiter) control is a L1 and PD hybrid utilising L1 for waypoint capture and PD control for circle tracking.
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
Pair-Programmed-With: Andrew Tridgell <tridge@samba.org>