What is the orientation of your xyz axes at the home/normal position.
Those atan2() expressions should be much simpler like atan2(x, z) and atan2(y, z) - assuming z-axis is vertical.
What is the orientation of your xyz axes at the home/normal position.
Those atan2() expressions should be much simpler like atan2(x, z) and atan2(y, z) - assuming z-axis is vertical.