Quadrocopter

[Bluesmash]

Hallo Chris

Vielen Dank für deine Antworten.

Momentan bin ich bei ca. 3,8ms für einen Durchlauf. Eventuell kann ich dies noch ein wenig optimieren... Potential wäre auf jedenfall noch da
Das kommt dann aber erst wenn ich mit allem anderen zufrieden bin...

Ich weiss ja nicht einmal was eine Rotationsmatrix ist...
Dann glaube ich nicht, dass ich diese programmiert habe Kannst du mir da weiterhelfen?

Ja die Kompl. Faktoren habe ich irgendwo so übernommen, und da es auf der Wippe gut funktionierte sah ich keinen Grund diese zu ändern... aber 0.00028 ist ja extrem klein... wie hast du diesen ermittelt?
Hmmm... ein kleinerer Faktor hätte auch einen kleineren Einfluss auf die kurze Querbeschleunigung, ich denke das hilft mir weiter.

Also du meinst, wenn ACC_X+ACC_Y+ACC_Z > 1,5g oder < 0,5g nur noch den Gyro-Winkel integrieren und den ACC-Winkel ignorieren?
Das hört sich gut an!

Der Code Teil ist nur für die Trimmung...
Beim Start bestimme ich den Offset des Gyros (Mittelwert aus 500 Messungen) sollte man den ACC auch kalibrieren?

gruss Bluesmash

[Che Guevara]

Hi,

ich bastel seit ein paar Jahren (glaub 2011 oder 2012) an meinem Kopter, hab angefangen mit Mega328P, dann AtXMega, jetzt STM32F4.
Solange du "nur" normal fliegen willst, also ohne Kompass, GPS & Höhe, solltest du LOCKER auf 1ms kommen, weniger ist mehr
Wichtig ist, dass du Divisionen vermeidest und auch floats, solange du keine FPU hast (was dein Prozessor nicht hat).
Divisionen lassen sich am besten mittels Shift ersetzen, da muss man dann ggf einwenig die Variablen skalieren, dann läufts wesentlich schneller.
Eine Rotationsmatrix (oder auch Drehmatrix) ist eine Matrix, mit der man einen Vektor drehen kann. In deinem Fall musst du die Komplementärfilter Winkel um einen bestimmten Winkel (aus GyroZ bestimmt) drehen. Schau mal bei Wikipedia, da ist das gut erklärt. Da deine Resourcen aber beschränkt sind und der Kopter sowieso nicht allzu schnell sich drehen wird, reicht da auch die Kleinwinkelnäherung.
Gedankenexperiment:
Du nickst 20° nach vorne (und bleibst da), yaw'st dann um 90° nach links (GUZ). Wie stehst du dann?

Ermittelt hab ich den Wert durch ausprobieren, ist über die Jahre so gewachsen Hab aber auch einen Kalmanfilter programmiert, der gibt sehr sehr ähnliche Ergebnisse aus, also denke ich mal, dass es ein sehr guter Wert ist, im Flug isses auch super!
Du musst die Länge des Vektor bestimmen, also

TotalForce = sqrt(AccX*AccX + AccY*AccY + AccZ*AccZ)
Diff1G = 1 - TotalForce
Diff1G = abs(Diff1G)

'Hier Winkel integrieren

If Diff1G < 0.5 Then 'nur anwenden, wenn Gesamtbeschleunigung nahe 1g
   'Komplementärfilter
End If

Den Acc musst du nur einmalig kalibrieren, den Gyro bei jedem Start, das machst du schon gold-richtig.

Gruß
Chris

[Bluesmash]

Hallo Chris

Zu deinem Gedankenexperiment:
Dann schaut der Copter nach links mit 20° Nick...?? Aber das möchte ich ja?
Wird die Drehmatrix verwendet, dass die Front des Copters immer in die selbe Richtung zeigt? Egal wie ich ihn um Yaw drehe?
Wenn ich z.B. den Knüppel nach vorne drücke, dass er immer von mir weg Fliegt? Egal wie er in der Yaw-Achse steht?

Ich habe mal den Komplementärfilter mit 0.002 versucht. Das funktioniert aber gar nicht...
Solange ich den Copter nur um eine Achse kippe und dann wieder Horzontal auf den Tisch stelle stimmen die Winkel.
Wenn ich jetzt aber den Copter "rühre" z.B. 30° Nick danach 30° Roll, -30° Nick, -30° Roll und Ihn wieder auf den Tisch stelle, steht der Nick Winkel z.B. auf 10° und Kompensiert sich ganz langsam wieder auf 0°
Das kommt von der Gyro integration wenn gleichzeitig um alle Achsen gekippt wird stimmen die Winkel überhaupt nicht mehr, ist das eigentlich normal? oder habe ich hier noch einen Fehler?


Gruss Bluesmash

[Che Guevara]

Hi,

nein, da denkst du falsch
Der Kopter steht dann mit Nick bei 0° und Roll bei 20° nach rechts.
Überleg mal:
Wenn du den Kopter, wenn dieser nicht waagerecht ist, um die Yaw Achse drehst, verändert sich dessen Winkel (wie beim Gedankenexperiment). Der Acc bekommt diese Veränderung mit, die Gyros aber nicht, weil sie nur die Drehung um die Yaw Achse registrieren. Folge: Es dauert eine ganze Weile, bis sich die Komplementärfilter Winkel wieder an die Acc Winkel angleichen.
Du musst also die Gyro-Drehungen (welche im Körpereigenen Koordinatensystem "stattfinden") in das gleiche Bezugssystem wie die Acc Winkel bringen, welche im Erdkoordinatensystem arbeiten.
Hier hab ich mal einen Code für dich, kann sein, dass du manche Vorzeichen wechseln musst:

GyroZSin = Sin(GyroZ*0.01745) 'Drehung um Yaw Achse von Deg zu Rad konvertieren
GyroZCos = Cos(GyroZ*0.01745)

'Drehmatrix
KompFilterXTmp = (GyroZCos * KompFilterX) - (GyroZSin * KompFilterY)
KompFilterYTmp = (GyroZSin * KompFilterX) + (GyroZCos * KompFilterY)

'Werte zurückschreiben
KompFilterX = KompFilterXTmp
KompFilterY = KompFilterYTmp

Die Variablen KompFilterXYTmp brauchst du, damit nach der ersten Zeile der Drehmatrix die Variable KompFilterX noch gleich bleibt, weil der Wert noch in der zweiten Zeile gebraucht wird.
Die grundsätzliche Formel stimmt, es kann aber sein, je nachdem, in welche Richtung deine Winkel ausgelegt sind (also ob Nick positiv vorne oder hinten, Roll positiv vorne oder hinten, Yaw positiv UZ oder GUZ).
Dein oben beschriebenes Verhalten, der Kopter steuere immer von dir weg (wie zb. MK Carefree), wird damit nicht erreicht!
Um das zu realisieren braucht man zwar auch eine Rotationsmatrix, aber auch einen Kompasssensor, welchen du glaube ich nicht verbaut hast?

Ich denke, dein beschriebenes Verhalten (rühren) hängt mit der fehlenden Rotationsmatrix zusammen. Probier das mal aus und berichte dann.

Gruß
Chris

EDIT:
Achja, deine Homepage funktioniert irgendwie nicht?

[Bluesmash]

Vielen Dank für deine Hilfe!

Irgendwie macht es bei mir nicht "klick" ;(
Wenn ich doch um die Yaw-Achse drehe, ist der einzige Sensor der eine Änderung feststellt, der Gyro von der Yaw-Achse?
Der Copter behält ja seinen Winkel (einfach um die Yaw-Achse gedreht) und somit bleiben doch die Werte der ACC gleich, oder?

Ah ja danke, ich war schon seit jahren nicht mehr aktiv hier, ich muss mir noch nen neuen Platz für meine Homepage suchen...

Gruss Bluesmash

[Che Guevara]

Hi,

ja, wenn die Platine / der Kopter / Sensor waagrecht ist, ändert sich nur die Yaw-Achse.
Wenn er aber schief steht und du dann um Yaw drehst, verändern sich die Acc-Werte, weil er dann anders im Raum steht!
Is jetzt schwierig zu erklären, evtl probierst dus nochmal mit dem Gedankenexperiment, eigentlich sollte es da klar werden.

Gruß
Chris

[Bluesmash]

Ahhh.... endlich, jetzt hats klick gemacht
Ich hatte das die ganze zeit falsch im kopf... jetzt macht alles einen Sinn
Ich muss mir das jetzt mal aufzeichnen damit ich deinen Code verstehe...

Eine Frage habe ich noch:
GyroZ, ist das die Winkeländerung seit der letzten Berechnung?

Gruss Bluesmash