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

Also smooth RTL's initial climb stage by projecting stopping point

We do not want people modifying the COMPASS_MOTCT manually

Perhaps easier to understand if we use the "nav" and "conditional"
command labels

individual do_* commands are responsible for setting roll-pitch, yaw and
throttle modes.  Removing this redundant setting of the controllers
removes the short period (10th second max) where loiter and waypoint
controller's desired roll and pitch were used even before the
controllers have been run once.

This stops run-up in target position and nav controller I terms ahead of
throttle being raised

needed for new nuttx structure

Nose was only pointing home if your initial altitude was above the
RTL_ALT

this will remove lots of warnings when building with arduino

bug-fix to resolve 1/10th second twitch when loiter or waypoint
controller are first engaged

Bug fix to allow cli's compass motor compensation to be run from the
mission planner's terminal window

thanks to the suggestion from Kevin

the calculate_heading() based on roll/pitch is not needed anywhere

Thanks to piersh for this improvement. See

https://github.com/diydrones/ardupilot/commit/b5f0635455957d6df65c7653e3077a6fa0737ea4#commitcomment-3171806