Adjust Yaw rotation speed and smothness in wp missions with user controlled heading

And I did do some flight test to examine ways to minimize, and the results are posted here: