Rotation um Hochachse (Yaw) auf Nick & Roll übertragen

[Che Guevara]

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:

   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

[HeXPloreR]

Hallo,
was heißt es funktioniert immer nur in eine richtung?

Ich würde erstmal prüfen lassen welcher wert größer ist, und davon dann den (kleineren) Anderen abziehen. Oder wenigstens sicher stellen das ich nicht "in Minus" rechne, wenn es nicht passieren soll.

Leider sieht man auch nicht welcher Variablen-Typ jeweils angelegt wurde.

Ob die Rechnung überhaupt dafür passt kann ich leider nicht beurteilen.

[Che Guevara]

Hi,

es funktioniert nur, wenn ich zuerst Nicke und Roll waagrecht ist...
Hm ich hab schon alles mögliche probiert, aber ich werd deinen Tipp trotzdem mal versuchen!
Gyro_xangle & Gyro_yangle & Angle_diff sind als Single deklariert, Gyroz ist ein Long.

Gruß
Chris

[Che Guevara]

Hi,

also ich habs probiert, aber auch das funktioniert nicht...
Das Problem ist einfach, dass es nur für bestimmte Drehungen / Konstellationen funktioniert, aber eben nicht für alle. Wenn ihr noch nen Tipp habt, immer her damit
Ich werd mich jetzt mal mit nem dicken Block Papier auf den Schreibtisch setzen und alles mal aufschreiben...

Gruß
Chris

[ichbinsisyphos]

Wenn ich das richtig sehe, willst du vorausrechnen, dass bei Drehung um die Vertikale die x- und y-Neigungswinkel ineinander übergehen. Und zwar umso schneller/mehr je schneller die Winkelgeschwindigkeit um die Vertikale.

Wie wärs dann mit sowas?

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

Mit Gyroz = 1600000 tauschen xangle und yangle die Werte.
Das müsste so abgestimmt sein, dass bei gyroz = 1600000 von einer Messung zur nächsten 90° Drehung ausgehen.

Bin mir nicht sicher, ob das Sinn macht, aber Vorzeichenwechsel müsste man auch berücksichtigen.

[Che Guevara]

Hi,

ja genau so will ich das machen
Mit Addition / Subtraktion hab ichs auch schon probiert, aber dann stimmts wieder nicht...
Durch die Division durch 1600000 erreiche ich, dass der Übergang genau richtig proportioniert ist.

Ich hab jetzt mal aufgeschrieben, welche Vorzeichen bei welcher Drehung / Winkel vorliegen:
Nicken nach vorne ist positiv, Rollen nach links ist positiv und Yawen im UZS ist positiv.
Jetzt bräuchte ich eine Art Wertetabelle, damit ich die Zusammenhänge rausfinden kann, allerdings ist mir noch nicht eingefallen, wie ich das gut leserlich aufschreiben kann.

Gruß
Chris