Hi,

also 5ms funktionieren zwar, aber ich würde versuchen, es auf 2ms oder gar 1ms runterzuschrauben. Die integration der Sensoren läuft dadurch besser und das Flugbild wird insgesamt noch etwas smoother, wobei er jetzt auch schon sehr gut fliegt!
Ich hab in meinen Koptern nen MPU6000 verbaut, der hat SPI und ist dementsprechend wesentlich schneller in der Auswertung, für 7 Register brauche ich gerade mal ein paar µs (hab den Wert nicht im Kopf, denke so ca 30).
Zum einen fällt auf, dass du keine Rotationsmatrix programmiert hast (oder sie nicht gezeigt hast). Zum anderen kommen mir die Komp.Filter Faktoren etwas falsch vor und errinnern mich an ein Paper, das immer mal wieder aufgegriffen wird
Ich hab bei meinem Filter (1kHz) einen Wert von 0.00028 für den Acc.
Die Werte auf 1g begrenzen würde ich nicht, da können u.U. sehr blöde Sachen rauskommen! Außerdem würde ich den Sensor auf +-4g stellen.
Wenn, dann würde ich den Komplementärfilter überspringen, wenn z.b. die Gesamtbeschleunigung mehr als 0.5g von 1g abweicht.

Gruß
Chris

EDIT:
Achja, hast du die ACC Sensoren auch kalibriert? Dein
Code:
   'Trimm Wert verrechnen
   Winkel_X_T = Winkel_X + Winkel_X_Trimm
   Winkel_Y_T = Winkel_Y + Winkel_Y_Trimm
sieht nicht danach aus
Du musst den Sensor 6x drehen und jeweils die max Werte bestimmen, daraus dann Offset & Range berechnen und bei jedem Auslesen dann mit diesen Werten verrechnen.