Hi,

ich bin momentan wieder etwas am programmieren und möchte nun den Level-Mode meiner Kopter (also ACC-gestützte Winkelerfassung durch Komplementärfilter) "verbessern" bzw. korigieren.
Angenommen die Platine (also die Sensoren) sind um 45° nach vorne geneigt (Nick) und die Roll-Achse ist waagrecht (0°). Wenn ich jetzt die Platine um 90° um die Hochachse drehe, ist die Platine um 45° in Roll-Richtung geneigt und um 0° in Nick-Richtung. Ich glaube, das fällt auch unter die Kategorie "Gimbal Lock"?!
Die Accelerometer-Sensoren erfassen dies zwar, aber da es ja ein Komplementärfilter ist, dauert es entsprechend lange, bis die Werte wieder stimmen (so ca. 1-2Sek.).
Jetzt dachte ich mir, ich nehme die Differenz der beiden Winkel (Nick & Roll), multipliziere diese mit dem Gyro-Wert der Hochachse und subtrahiere die Werte dann von den Winkeln... Tja, aber irgendwie funktionierts nur immer in eine Richtung...
Hier mal der Code:
Code:
   Angle_diff = Gyro_xangle - Gyro_yangle

   Angle_diff = Angle_diff * Gyroz
   Angle_diff = Angle_diff / 1600000

   Gyro_xangle = Gyro_xangle - Angle_diff
   Gyro_yangle = Gyro_yangle - Angle_diff
Gyro_xangle ist der Roll-Winkel, berechnet aus dem Gyro-Integral & ACC-Winkel, Gyro_yangle ist der Nick-Winkel.
Gyroz ist die Drehrate um die Hochachse.

Ich bin mir sicher, dass es so ähnlich funktioniert denn wenn ich den Sensor erst nach vorne neige (Nick) und dann von oben gesehen gegen den Uhrzeigersinn drehe (um 90°), dann stimmt alles! Aber bei einer anderen Reihenfolge stimmts nicht mehr...
Kann mir jemand evtl. einen Tipp geben, was ich falsch mache?

Vielen Dank & Gruß
Chris