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