[Alan Anderson]

It sounds like you might get what you want if you just limit the maximum turn rate. Compute Kp * angle and clip it at 0.6, or 0.4, or whatever works for you.

If you are having problems with the turn command not being enough to get the heading all the way to the target, you could add some Integral to the equation of control. That's what it's for.