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 :D 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 :D
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

[ichbinsisyphos]

Na gut, dann positive Drehrichtung um die Vertikale (Uhrzeigersinn):

positiver Nick- wird zu positivem Rollwinkel. Anders ausgedrückt eine Neigung nach vorne wird zu einer nach links.


y+ -> x+
y- -> x-
x+ -> y-
x- -> y+

y wird zu x bei Vorzeichenerhalt und x wechselt zu y jedes Mal das Vorzeichen. Wichtig wär, zu verstehen, was dazwischen abgeht, aber dazu fehlt mir im Moment die Vorstellungskraft.

[Che Guevara]

Hi,

ich habs jetzt mal aufgeschrieben (so wie du):


Rotation im UZS:
Nick --> Roll Roll --> Nick
+ + + -
- - - +

Rotation gegen UZS:
Nick --> Roll Roll --> Nick
+ - + +
- + - -

Jetzt muss ich noch versuchen, das ganze irgendwie zeitsparend in den Code zu integrieren!
Danke schonmal für die Hilfe ;)

Gruß
Chris

EDIT:
Allerdings hab ich jetzt noch keinen Plan, wie ich das programmieren soll... Ich war eigentlich auch der Meinung, dass das ohne große Fallunterscheidungen gehen sollte.. Aber da hab ich mich wohl getäuscht!
Vorschläge bzgl. der Umsetzung sind weiterhin willkommen :D

[ichbinsisyphos]

k = Gyroz / 1600000

xtemp = (y-x)*k + x
y = -(y+x)*k + y
x = xtemp


Mir ist übrigens grad erst aufgefallen, dass (x,y) -> (-y, x) eine Drehung eines Vektors um 90° beschreibt. Heißt für den Fall besonders, dass das für 90° stimmt, dazwischen nicht, die Winkelfunktionen sind natürlich nicht linear im Winkel.

Außerdem frag ich mich, ob es nicht genauso gut oder besser wäre, den Komplementärfilter so einzustellen, dass er neue Werte schneller annimmt. Was genau erhoffst du dir von der Vorgehensweise? Verpfuscht du dir damit nicht die Trägheitsnavigation?


edit: ok, vergessen wir den anderen Ansatz, das sind keine Winkel ...

[Che Guevara]

Hi,

danke für den Code, werde ich dann später gleich noch ausprobieren! :D
Es muss / soll ja auch nicht 100%ig genau stimmen, das geht schon alleine wegen dem Gyro-Drift nicht ;) Aber eine Annäherung reicht auch für den Fall.
Nunja, der Komp. Filter ist momentan so eingestellt, dass Störungen des ACCs ABSOLUT keinen Einfluss auf den Winkel haben, das ist für mich sehr wichtig, da ich einen Kompass verbaut habe der einen absolut-stimmigen Winkel braucht ;) Trotzdem ist der Filter ja nicht träge, er arbeitet voll und ganz zu meiner Zufriedenheit.

Wie läufts eig. mit deinem Projekt? Hast du den schon angefangen?

Gruß
Chris

[ichbinsisyphos]

Wie läufts eig. mit deinem Projekt? Hast du den schon angefangen?Planung ist schon weit fortgeschritten, an der Finanzierung scheiterts noch ;)

[Che Guevara]

Ja, das ist meist ein Problem (ich spreche da aus Erfahrung) :D
Als ich meinen ersten Kopter gebaut habe, so vor 3 Jahren, hab ich mir die EMAX CF2822 für nicht mal 10€ / Stk. geholt. Diese haben ordentlich Schub und wenn man über die Schwachstellen Bescheid weiß, sinds zum Anfangen auch definitiv keine schlechten Motoren.
Als Regler hab ich damals die Turnigy-Plush verwendet.

Gruß
Chris

[ichbinsisyphos]

Das stimmt übrigens immer noch nicht. Die Formeln vertragen keine negativen gyroz.
Gegen den Uhrzeigersinn wechseln die Vorzeichen anders.

UZS (k>=0) GUZS (k<0)

x = x - (x-y)*k x = x + (x+y)*k
y = y - (x+y)*k y = y - (x-y)*k

Aber Garantie will ich dafür auch keine übernehmen.

[Che Guevara]

Hi,

es hat zwar einige Zeit gedauert, bis ich mich mit dem Gedanken angefreundet hatte, aber ich berechne jetzt die Winkel durch eine Drehmatrix:


Y = Gyro_xangle
X = Gyro_yangle

A = Gyroz
A = A / 8192 '10000

If A < -90 Then A = -90
If A >= 90 Then A = 90

A = Deg2rad(a)

Sina = Sin(a)
Cosa = Cos(a)

Xtemp = Cosa * X
Xtemp2 = Sina * Y
Xtemp = Xtemp - Xtemp2

Ytemp = Sina * X
Ytemp2 = Cosa * Y
Ytemp = Ytemp + Ytemp2

Gyro_xangle = Ytemp
Gyro_yangle = Xtemp

Leider wollte ich diese Lösung nicht haben, weil die Sin & Cos Funktion eben sehr zeitintensiv sind... Deswegen werde ich mir wohl eine LookUp-Tabelle erstellen, damits etwas schneller geht ;)

Gruß
Chris