3D kompass. Rechnung

[Spacecam]

    sensor = SensorADXL345(1, 0x53)
    mx,  my,  mz,  = sensor.read_data_beschl()
    #print mx, my, mz
    pitchAccelXh = math.atan2(my / 1024, (mz / 1024))
    rollAccelYh = math.atan2(mx / 1024, (mz / 1024))
 
    cos_roll= math.cos(pitchAccelXh);
    sin_roll = math.sin(pitchAccelXh);
    cos_pitch = math.cos(rollAccelYh);
    sin_pitch = math.sin(rollAccelYh);
  
    mag_X = x; 
    mag_Y = y;  
    mag_Z = z;  
  

    Yh = mag_Y * cos_roll - mag_Z * sin_roll;
    Xh = mag_X * cos_pitch + mag_Y * sin_roll * sin_pitch + mag_Z * cos_roll * sin_pitch;
    ang = 180+round(math.atan2(x, y)*(180/math.pi),0)
    realHeading = 180+round(math.atan2(Xh, Yh)*(180/math.pi),0)
    print "alt",ang,"neu",realHeading
    time.sleep(1)

jetzt geht es solange der Kompass grade ist.
In diesen fall stimmen alter und neuer wert überein (is ja normal) sobalt ich den Kompass + accelerometer kippe ist der alte wert fast besser als der neu ausgerechnete^^

jemand eine Idee woran das liegen könnte?

[Torrentula]

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:

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