AP_L1_Control: Remove potential nan errors

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
AP_AHRS: use vectors for ground vector complimentary filter
PX4: fixed handling of nostart option