Das Programm

Gleich schon mal vorneweg: Mittlerweile gibt es den Code vom Quadcopter auch auf github. Wer Interesse hat, kann sich ihn also dort anschauen. Verbesserungsvorschläge sind natürlich erwünscht. Am besten schreibt ihr uns dazu eine E-Mail oder nutzt einfach die Möglichkeit bei github selbst Kommentare zu schreiben. Aber hier erst mal der Link: github/Quadcopter

Nachfolgend der alte Text, der an dieser Stelle bislang stand.

Der Programmcode für unseren Quadcopter wurde von Null an komplett selbst geschrieben. Hier und da flossen Ideen aus anderen Codes ein, wurden dann aber teilweise auch wieder schnell durch was eigenes ersetzt, sodass man jetzt sagen kann, dass keine einzige Zeile von anderen bekannten Projekten oder OpenSource-Bibliotheken kopiert oder geklaut wurde.

Auf dieser Seite wird die Grundstruktur des Quadcopter-Quellcodes nahe gebracht und erklärt wie die einzelnen Programmteile miteinander arbeiten.

main.cpp

Am Dateinamen erkennt man schon, dass wir hier in C++ programmieren. In der Hauptdatei existieren allerdings keine Klassen, hier werden sie nur benutzt. Eigentlich gibt es hier nur eine Funktion, nämlich int main() . In ihr werden zunächst alle Module in folgender Reihenfolge initialisiert:

  1. CPU auf 32 MHz einstellen.
  2. Display initialisieren um nachfolgend Debug-Informationen ausgeben zu können.
  3. Die 4-Kanal PWM mit 100 Hz initialisieren.
  4. Beschleunigungssensor initialisieren.
  5. Gyrosensor initialisieren.
  6. Timer gesteuerten Interrupt mit 100 Hz starten.
  7. Variablen definieren und initialisieren.
  8. PID-Regler und Filter initialisieren.

Ab hier beginnt die Hauptschleife, die nie wieder beendet wird. Innerhalb der Schleife wird mittels Bedingungsüberprüfung darauf gewartet, dass ein atomic Integer von der Interrupt-Routine auf 1 gesetzt wird. Dann werden direkt alle Sensordaten ausgelesen, gefiltert, durch den PID-Regler gejagt und diese Werte dann direkt in den Motormischer übertragen. Dann werden die 4 PWM-Kanäle aktualisiert, damit die Motoren ihre neue Geschwindigkeit anfahren.

Bis jetzt gibt es noch keine Abfrage der Fernsteuerung, deswegen simulieren wir Gas und Stopp mit einem einfachen Taster. Weiterhin wird eine Status-LED immer vor dem Auslesen der Sensoren an und nach der Aktualisierung der PWM-Kanäle wieder aus geschaltet. So kann man mittels Oszilloskop ermitteln wie viel Spielraum zum Rechnen noch bleibt, denn Fließkommaoperationen auf dem ATXMega128A1 sind teuer. Bisher nutzen wir ca. 75% der Ressourcen, die uns die CPU geben kann. Allerdings wird es später kein Debugging-Display mehr geben und Tests ohne Display haben ergeben, dass wir dann auf fast 50% CPU-Nutzung fallen, was außerordentlich gut ist, da kaum noch neue kompexe Berechnungen hinzu kommen werden.

SPI.cpp und SPI.h

Informationen dazu findet man in diesem Artikel.

CFilter.cpp und CFilter.h

In diesen Dateien ist die Klasse CFilter zu finden, was die Abkürzung für Complementary Filter ist. Dieser Filter mischt die Werte, die aus dem Gyro und dem Beschleunigungssensor kommen. Der Filter ist unter anderem deswegen notwendig, da der Gyro Winkel pro Zeiteinheit und der Beschleunigungssensor absolute Winkelangaben ausspuckt. Der Filter integriert also die Gyrowerte über die Zeit, vermischt sie in einem gewissen Verhältnis mit dem des Beschleunigungssensors und gibt den sauberen absoluten Winkel schließlich zurück.

Es gibt Komplementärfilter erster und zweiter Ordnung. Wir nutzen den der ersten Ordnung, da wir damit die besseren Ergebnisse erzielt hatten. Aber noch sind ja nicht alle Parameter optimiert worden, und so weiß man nie, ob man den Filter zweiter Ordnung nicht doch noch gebrauchen kann.

Weitere Informationen und Codebeispiele gibt es auch hier zu finden: Kalman filter vs Complementary filter.

PID.cpp und PID.h

Hier versteckt sich die sehr schlanke Klasse PID , die mit der richtigen Parametrisierung unseren Quadcopter immer schön in der Horizontalen halten soll.

Der Konstruktor bekommt vier float -Werte, nämlich den proportionalen, den integralen und den differentiellen Anteil für den Regler und zusätzlich noch das feste Zeitintervall, in dem er immer wieder aufgerufen wird. Da die Sensoren 100 mal in der Sekunde ausgelesen werden, ergibt sich hier dementsprechend ein Zeitintervall von 0,01 Sekunden. Weiterhin werden im Konstruktor schon ein paar Werte einmalig vor berechnet, damit beim späteren Aufruf der Regelmethode möglichst viel Rechenzeit gespart werden kann. Dies bringt ein erhöhten Speicherbedarf mit sich, der sich aber mit 7 · 4 Bytes pro PID-Regler im Rahmen hält.

Weitere Informationen über PID-Regler findet man auch in Wikipedia unter PID-Regler.

Display.cpp und Display.h

Die Klasse Display entsprang aus einem anderen Projekt, dem Sinusoszillator. Dort existierte die Ansteuerung des Displays allerdings noch nicht als Klasse, sondern in reinem C-Code. Außerdem war der Code noch nicht für die ATXMega-Reihe bereit und musste fast komplett aktualisiert werden.

Der Konstruktor der Klasse erhält die Adresse zum Port, an dem das Display angeschlossen ist und stellt die Datenrichtung aller nötigen Bits auf Ausgang ein. Danach reicht es die Methode void Display::init() aufzurufen um die Datenübertragung auf 4 Bit in parallel einzustellen, das Blinken des Cursors zu unterbinden, den Zeichenspeicher zu löschen und das Display zu aktivieren.

Danach gibt es die Möglichkeit mit void Display::setCursorPos(uint8_t row, uint8_t column) die Position des Cursors zu bestimmen und mit verschiedenen write -Aufrufen und passender Parameter einzelne Zeichen und Zahlen bis zu ganzen Strings zu schreiben. Eine passende void Display::clear() -Methode gibt es natürlich auch noch.

myMath.cpp und myMath.h

Hier implementiere ich unter anderem meine eigenen trigonometrischen Funktionen als Ersatz für die bekannten Funktionen  float atan(float x) und  float atan2(float y, float x)  aus der Standard Library math.h . Hat man erst mal eine hinreichend genaue Implementierung für den Arkustangens, ist die Implementierung für die Version mit zwei Parametern kein Problem mehr.

Deswegen hier der erste zusammenhängende Code-Ausschnitt in diesem Artikel:

 ADXL345.cpp und ADXL345.h

Die Klasse ADXL345 ist dazu da den gleichnamigen Beschleunigungssensor auszulesen, seine Daten zu verarbeiten, zwischen zu speichern und umzurechnen.

In unserem Fall wird der Beschleunigungssensor für den Bereich ±2g initialisiert. Anschließend werden 200 Werte ausgelesen und gemittelt um ein mögliches Offset ausschließen zu können. Die damit ermittelten Werte werden anschließend bei jeder Messung subtrahiert.

Dann ist es möglich eine Messung anzustoßen und danach die Sensorwerte direkt in m/s² abzufragen oder sie über die letzten n Werte mitteln zu lassen. In unseren bisherigen Tests mitteln wir die Werte über die letzten 8 Sensordaten.

Neben dem Auslesen der Beschleunigung auf x-, y- und z-Achse, ist es auch möglich den Rotationswinkel dieses Vektors auf allen drei Achsen auszulesen. Dafür wird dann der oben schon erwähnte Arkustangens genutzt. Außerdem ist der Rotationswinkel das bevorzugte Maß um später im Zusammenklang mit dem Gyro die korrekte aktuelle Lage des Quadcopters zu berechnen.

L3G4200D.cpp und L3G4200D.h

In der Klasse L3G4200D  wird unser Gyro angesteuert, ausgelesen und ausgewertet. Wie auch der Beschleunigungssensor zuvor wird er zunächst initialisiert und dann ist es möglich Daten zu akquirieren, die dann auf Bogenmaß pro Sekunde umgerechnet werden und mittels einzelner Methoden ausgelesen werden können.

Auch hier gibt es die Möglichkeit das Ergebnis über die letzten n Werte zu mitteln. In unserem Fall mitteln wir über die letzten 3 Werte, was sich aber immer wieder mal ändert um etwas neues auszuprobieren.

Mehr Besonderheiten gibt es in dieser Klasse nicht.

timer.cpp und timer.h

Hier passiert eigentlich gar nichts. Im Header wird lediglich die Variable extern uint32_t milliSeconds;  deklariert und in der CPP-Datei mit uint32_t milliSeconds = 0; definiert.

Die Variable milliSeconds  wird ständig von dem oben schon erwähnten mit 100 Hz getaktetem Interrupt um den Wert 10 erhöht, womit es im ganzen Programm möglich ist die absolute Zeit seit dem Start des Programms abzufragen.

XMEGA_helper.h

Hier gibt es keine Klassen zu finden. Diese Headerdatei entstand schon ganz zu Beginn und definiert ein paar einfache Funktionen, die die verschiedenen PWM- und Interruptroutinen möglichst dynamisch starten können.

Ebenso findet man hier folgende Funktion:

Wie die Kommentare schon vermuten lassen, wird nach Aufruf der Funktion der interne Oszillator des ATXMega genutzt um ihn auf 32 MHz zu takten. Es sind auch höhere Taktraten bis zu 48 MHz möglich, aber angeblich soll dies zu möglichen Stabilitätsproblemen führen. Ausprobiert haben wir das allerdings noch nicht.

Ende

Zum Schluss lässt sich noch sagen, dass vor allem Dank der Objektorientierung alles sehr überschaubar geblieben ist. Von zu viel Speicherverbrauch oder Geschwindigkeitsverlust kann hier ebenfalls keine Rede sein, obwohl das viele Leute aus bekannten AVR-Foren und -Chats felsenfest behaupten. Klar ist jedenfalls, dass nach der Kompilierung nichts mehr von OOP zu sehen ist.

4 Gedanken zu „Das Programm“

  1. hallo,

    ich nutze quadrocopter, um in meiner schule mit mechatronischen themen und programmierung zu spielen. (webseite -> projektarbeiten).

    meine frage wäre : haben sie für die steuerung des 4copters ein uml-diagramm (klassendiagramm) gezeichnet ?

    wenn ja, wäre ich sehr interessiert daran, das mal sehen zu können. ich hänge hier ein wenig an der vereinbarkeit von klassendiagrammen und den zeitlichen abläufen in der coptersteuerung (z.b regelroutine und ppm-interrupt).

    mfg, reiner doll

    1. Hallo Reiner,

      ein UML-Diagramm gibt es für unseren Quadcopter nicht. die genaue Programmstruktur war in keinster Weise von Anfang an geplant. Es wurden nach und nach Klassen geschrieben, die die einzelnen Sensoren auslesen, die Motoren ansteuern und das Debug-Display ansteuern können und dann in der main-Funktion miteinander „verbunden“. Weitere Klassen wie die Filter und PID-Regler kamen dann nach und nach dazu.
      Außerdem gibt es noch ein paar ganz normale statische Funktionen außerhalb jedweder Klassen. Das kommt auch daher, dass es nicht so einfach ist auf einem Mikrocontroller mit Klassen zu arbeiten, wenn das vom Hersteller (Atmel) zur Verfügung gestellte Framework selbst nicht mit Klassen arbeitet, sondern reiner C-Code ist. Das liegt unter anderem wohl auch an der Hardware selbst, die ja nicht sonderlich dynamisch ist. Es macht fast keinen Sinn eine Klasse zu programmieren, die eh nur einmal instantiiert wird, weil sie nur zur Ansteuerung einer einzigen Hardware-Komponente nötig ist.
      Das ist auch der Grund, warum einfache Timer- und PWM-Initialisierungen bei mir meistens in reinem C geschrieben werden und nicht etwa in Klassen ausgelagert werden.

      Um dann noch mal auf die Regelroutinen zurück zu kommen. Die werden bei mir einfach nur regelmäßig in der main aufgerufen, die wiederum von einem Interrupt mit 100 Hz über eine volatile Variable getriggert wird.

      Ich würde mir auch wünschen, wenn man die AT(X)Mega-Reihe von Atmel objektorientiert programmieren könnte. Aber da sowas nicht von Haus aus zur Verfügung gestellt wird, wäre die Alternative nur sich selbst ein Framework dafür zu basteln. Ein interessanter Ansatzpunkt, gerade für Interrupts, stellt auch dieser schöne Artikel dar: http://www.mikrocontroller.net/articles/AVR_Interrupt_Routinen_mit_C%2B%2B

Schreibe einen Kommentar

Deine E-Mail-Adresse wird nicht veröffentlicht. Erforderliche Felder sind mit * markiert.

*