Orbit approach fixes

[MaEtUgR]

Describe problem solved by this pull request
Credits got to @RomanBapst for finding all of these issues and helping me to fix them:

1. sending a new orbit command during approach lead to finishing the old approach and jumping to the new circle
2. changing the radius during approach lead to approaching to the old radius and jumping to the new one
3. the way setpoint output was not wrapped and could exceed the range
   it showed in the log but still executed fine since sin() and cos() work properly also outside of [-pi, pi]

Describe your solution

1. reset the approach when a new orbit is commanded
2. reset the approach when the radius is changed
3. wrap the yaw setpoint before it's sent out

Test data / coverage
I SITL tested.

[MaEtUgR]

@RomanBapst "Code looks good to me. Tested, works as expected."

Can we proceed? I'm not allowed anymore to merge in this state.

[dayjaby]

If this gets merged I will create another rebase on #13426

[MaEtUgR]

@dayjaby Thank you. Sorry for taking so long on it, I literally just reminded myself to bring the pr in. I can rebase and check if you want.

[MaEtUgR]

I rebased on master after merging #13426.