[rotorcraft][stabilization] calculate correct yaw from guidance psi setpoint

Instead of using the psi setpoint angle to rotate around the body z-axis,
calculate the real angle needed to align the projection of the body x-axis onto the horizontal plane
with the psi setpoint.
Only makes a few degrees of difference at high roll/pitch angles, but should be correc now.
still needs to be optimized...