Sorry, dass ich erst jetzt antworte, du scheinst in deinem code bei der Berechnung von Xh und Yh Pitch und Roll teilweise durcheinander gebracht zu haben.

So müsste es sein:
Code:
Xh = mag_X * cos_pitch + mag_Z * sin_pitch
Yh = (mag_X * sin_roll * sin_pitch + mag_Y * cos_roll) - mag_Z * sin_roll * cos_pitch