wrap heading. bounding rotational speed. replace atan to atan2