[rotorcraft] refactor guidance_h_ref and fix route_ref

If both argumens for atan2 are zero, theoretically the result is undefined. Regardless of what the c implementation actually defines in that case, better be specific about it.

Not really sure what the max_ref values should be in the case where the route_ref angle can't be computed (i.e. both values are zero). Set to max on both axes for now...

Also shouldn't the route_ref angle in the update_from_speed_sp also be computed from the speed_err instead of the speed_sp?

see also issue #521