Plane : 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.