Tschack die Drohne!

Grundkurs Drohnensteuerung: So lernen Quadrocopter mit Fantom fliegen

Steve Eynon

Der Traum vom Fliegen – welcher Mensch hatte ihn noch nicht? Da ein Pilotenschein allerdings sehr teuer ist und die Evolution uns nach wie vor keine Flügel spendiert hat, kann man versuchen, sich mit einem Quadrocopter Ausgleich zu verschaffen. In diesem Artikel zeigt Steve Eynon, Technical Consultant bei SkyFoundry, wie man mit der Programmiersprache Fantom ein Programm zur Kontrolle des Quadrocopter „Parrot AR Drone“ schreibt.

Der Vorteil des Quadrocopters Parrot AR Drone ist das quelloffene Client SDK, das die Drohne gerade bei Entwicklern so beliebt macht. Gemeinsam werden wir ein Programm zur Steuerung des Parrot AR Drone Quadrocopters erstellen. Das Programm werden wir in der Programmiersprache Fantom verfassen und die folgenden Dinge sollen damit möglich werden:

  • Steuerung der Drohne mit der Tastatur des Computers
  • Ausgabe von Telemetriedaten der Drohe in Echtzeit
  • Darstellung eines Videostreams in Echtzeit mithilfe der Drohnenkamera

Der vorliegende Artikel setzt ein grundlegendes Wissen um Programmiersprachen voraus und die gezeigten Beispiele funktionieren ausschließlich in Verbindung mit einer bereits erfolgten Installation von Fantom auf dem jeweiligen System. Der Parrot AR Drone ist ein preisgünstiger und weit verbreiteter Quadrocopter, der bei Drohnenliebhabern besonders wegen des oben erwähnten Client SDKs beliebt ist. Dieses ist in C geschrieben.

Bezüglich Fantom wird das Programm auf dem Parrot Drone SDK von Alien-Factory basieren, was praktisch eine pure Fantom-Implementierung des Parrot SDKs ist und es ermöglicht, die Drohnen von Parrot fernzusteuern.

1. Steuerung des Quadrocopter „Parrot AR Drone“

Der Parrot AR Drone hat einen On-Board-Mikroprozessor, auf dem die Linux Busy Box läuft. Verwendet wird diese Kombination, um Sensoren abzulesen und (Steuerungs-)Befehle an die vier Motoren zu senden. Die Drohne hat außerdem W-LAN-Hardware an Bord: Aktiviert man sie, richtet sie sich selbst als W-LAN-Hotspot ein, mit dem sich der eigene Computer verbinden lässt.

Daten zwischen dem Quadrocopter und dem Computer werden via TCP- und UDP-Protokoll ausgetauscht. Vom Computer gehen dabei in erster Linie die Konfiguration und Informationen zur Steuerung aus, die Drohne schickt dagegen Video- und Navigationsdaten zurück.

Auf unserem Computer werden wir eine Fantom-Anwendung ausführen, die das Parrot Drone SDK nutzt, um Steuerungsbefehle an die Drohne zu senden. Das Videostreaming lässt sich über das weit verbreitete Hilfsprogramm FFMPEG verwenden, das die reinen Videodaten in nutzbare Bilder umzuwandeln. Wichtig: Das FFMEG-Executable muss in eurem PATH oder im gleichen Verzeichnis liegen, von dem aus ihr auch Fantom startet.

Lesen Sie auch: JVM-Sprache Fantom: pragmatisch und plattformübergreifend

2. Das Fantom-Projekt

Das erste, was nun getan werden muss, ist die Erschaffung eines einfachen Projektes, mit dem sich ein Fenster zur Anzeige von Text und dem Empfangen von Tastatureingaben öffnet. Fantom-Projekte werden zu einer *.pod-Datei kompiliert, was äquivalent zur Kompilierung eines Java-Projektes zu einer *.jar-Datei abläuft. Es wird empfohlen, *.pod-Dateien self-contained zu erstellen, sodass sie die Dokumentation, den Quelltext und alle zugehörigen Ressourcen enthalten.

Jedes Fantom-Projekt beinhaltet zudem eine build.fan-Datei. Dabei handelt es sich um ein Fantom-Script, das für die Erstellung der *.pod-Datei verantwortlich ist. Angenehmerweise verfügt Fantom über eine Core Library, die viele Utility-Klassen und –Methoden enthält, die einem die meiste Arbeit für das Pod Building abnimmt. Es reicht, um genauer zu sein, die BuildPod-Klasse im Constructor zu konfigurieren, wenn man eine Subklasse erstellt.

So sieht die build.fan-Datei aus, die wir für unser JaxDrone-Projekt nutzen wollen:

syntax: fantom
 
using build
 
class Build : BuildPod {
    new make() {
        podName = "jaxDrone"
        summary = "Controller for the Parrot AR Drone"
        version = Version("1.0")
 
        depends = [
            "sys          1.0.69 - 1.0",
            "gfx          1.0.69 - 1.0",
            "fwt          1.0.69 - 1.0",
            "afParrotSdk2 0.0.8  - 0.1",
        ]
 
        srcDirs = [`fan/`]
        resDirs = [,]
 
        docApi = true
        docSrc = true
    }
}

Unser Demo-Projekt wird dann zu einer Datei mit dem Namen jaxDrone.pod kompiliert, die eine Reihe von Abhängigkeiten hat:

  • sys – die Core Library von Fantom
  • gfx – beinhaltet nützliche Konstrukte wie Color & Font
  • fwt – das Fantom Windowing Toolkit für die Erstellung von Window-Anwendungen; basiert auf dem Eclipse SWT
  • afParrotSdk2 – eine Bibliothek von Alien-Factory (wie das Präfix anzeigt), die den Quadrocopter Parrot AR Drone steuert

Die Pods sys, gfx und fwt sind Kern-Pods, die bereits standardmäßig erhältlich sind und so eigentlich in jeder Fantom-Installation enthalten sein sollten. afParrotSdk2 ist allerdings ein Pod von einem Drittanbieter, den wir erst herunterladen und installieren müssen. Das folgende Fantom-Kommando sollte das erledigen:

fanr install -r http://eggbox.fantomfactory.org/fanr/afParrotSdk2

Unseren Source Code legen wir im fan/-Verzeichnis ab, genau wie die srcDirs; starten tun wir mit der Main-Klasse:

syntax: fantom
 
using fwt::Desktop
using fwt::Label
using fwt::Window
using gfx::Color
using gfx::Font
using gfx::Size
 
class Main {
    Void main() {
        Window {
            it.title = "Fantom AR Drone Controller"
            it.size  = Size(440, 340)
 
            label := Label {
                it.font = Desktop.sysFontMonospace.toSize(10)
                it.bg   = Color(0x202020)   // dark grey
                it.fg   = Color(0x31E722)   // bright green
            }
 
            it.add(screen)
            it.onOpen.add |->| {
                label.text = "Let's fly!"
            }
            it.onClose.add |->| {
                label.text = "Goodbye!"
            }
        }.open
    }
}

Diese erstellt ein Fenster mit einem Label und gibt ein wenig Text aus, wenn es geöffnet wird. Wir bearbeiten das Label so, dass es wie ein Konsolenfenster aussieht – komplett mit schwarzem Hintergrund, grüner Schrift und einer nichtproportionalen Schriftart. Um den Pod zu erstellen, muss man nun das Fantom-Script build.fan ausführen:

> fan build.fan
 
compile [jaxDrone]
  Compile [jaxDrone]
    FindSourceFiles [1 files]
    WritePod [file:/C:/Apps/fantom-1.0.69/lib/fan/jaxDrone.pod]
BUILD SUCCESS [127ms]!

Weitere Informationen zum Erstellen von Pods gibt es im Eintrag Build in der Fantom-Tools-Dokumentation. Geschafft? Dann wird es nun Zeit, den neu erstellten Pod und damit unser Projekt zu starten:

< fan jaxDrone

Wer zum Ausführen von Pods noch Fragen hat, findet in der Fantom-Tools-Dokumentation dazu ebenfalls etwas (Menüpunkt Running Pods).

3. Telemetriedaten ausgeben

Jetzt wird es spannend – wir verbinden uns mit der Drohne! Für dieses Kunststück nutzen wir die ParrotSDK Library für Fantom. Die Bibliothek ist auf die Drone-Klasse fokussiert, wir fügen sie also als eine using-Anweisung hinzu und instanziieren eine Instanz. Dann müssen wir die Events onOpen() und onClose() anpassen, um uns mit der Drohne zu verbinden oder die Verbindung zu unterbrechen.

Haben wir eine Verbindung zur Drohne hergestellt, rufen wir clearEmergency() auf, um sicherzustellen, dass der Quadrocopter sich im Status „Normalflug“ befindet. Außerdem rufen wir flatTrim() auf, damit die Drohne die Horizontale richtig kalibrieren kann, was für einen stabilen Flug notwendig ist. Die Gelegenheit nutzen wir, um auch einen Control Loop zu erstellen, der etwa alle 30 Millisekunden ausgeführt wird. In diesem Loop werden auf dem Bildschirm neue Telemetriedaten angezeigt, Steuerungsbefehle übermittelt und Videodaten verarbeitet.

syntax: fantom
 
using afParrotSdk2::Drone
using fwt::Desktop
using fwt::Label
using fwt::Window
using gfx::Color
using gfx::Font
using gfx::Size
 
class Main {
    Drone?      drone
    Label?      label
    Screen?     screen
 
    Void main() {
        label = Label {
            it.font = Desktop.sysFontMonospace.toSize(10)
            it.bg   = Color(0x202020)   // dark grey
            it.fg   = Color(0x31E722)   // bright green
        }
 
        Window {
            it.title = "Fantom AR Drone Controller"
            it.size  = Size(440, 340)
            it.add(label)
            it.onOpen.add  |->| { connect()    }
            it.onClose.add |->| { disconnect() }
        }.open
    }
 
    Void connect() {
        drone  = Drone()
        screen = Screen(drone, label)
 
        drone.connect
        drone.clearEmergency
        drone.flatTrim
 
        controlLoop()
    }
 
    Void disconnect() {
        drone.disconnect()
    }
 
    Void controlLoop() {
        screen.printDroneInfo()
 
        Desktop.callLater(30ms) |->| { controlLoop() }
    }
}

In der ersten Instanz nutzen wir den Control Loop lediglich, um die Telemetriedaten auf dem Bildschirm zu aktualisieren. Im Standardmodus würde die Drohne alle 60 Millisekunden (also etwa 15 Mal pro Sekunde) neue Telemetriedaten senden. Ein 30-Millisekunden-Loop sollte daher ausreichend sein. Das Datenfeld Drone.navData enthält immer die neuesten Daten des Quadrocopters, also wird die Screen-Klasse einfach die Daten aus diesem Datenfeld ausgeben. Wollten wir eine Event-getriebene Darstellung schreiben, könnten wir den Event Handler Drone.onNavData nutzen. Aber unser Loop sorgt dafür, dass die Dinge einfach bleiben.

syntax: fantom
 
using afParrotSdk2::Drone
using fwt::Key
using fwt::Label
 
class Screen {
    private const Int   screenWidth     := 52   // in characters
    private const Int   screenHeight    := 20   // in characters
 
    private Label       label
    private Drone       drone
 
    new make(Drone drone, Label label) {
        this.drone      = drone
        this.label      = label
    }
 
    Void printDroneInfo() {
        buf := StrBuf(screenWidth * screenHeight)
        buf.add(logo("Connected to ${drone.config.droneName}"))
 
        navData := drone.navData
        data    := navData.demoData
        flags   := navData.flags
 
        buf.addChar('\n')
        buf.addChar('\n')
        buf.add("Flight Status: ${data.flightState.name.toDisplayName}\n")
        buf.add("Battery Level: ${data.batteryPercentage}%\n")
        buf.add("Altitude     : " + data.altitude.toLocale("0.00") + " m\n")
        buf.add("Orientation  : X:${num(data.phi      )}   Y:${num(data.theta    )}   Z:${num(data.psi      )}\n")
        buf.add("Velocity     : X:${num(data.velocityX)}   Y:${num(data.velocityY)}   Z:${num(data.velocityZ)}\n")
        buf.addChar('\n')
 
        // show some common flags / problems
        if (flags.flying)           buf.add(centre("--- Flying ---"))
        if (flags.emergencyLanding) buf.add(centre("*** EMERGENCY ***"))
        if (flags.batteryTooLow)    buf.add(centre("*** BATTERY LOW ***"))
        if (flags.anglesOutOufRange)buf.add(centre("*** TOO MUCH TILT ***"))
 
        label.text = alignTop(buf.toStr)
    }
 
    private Str logo(Str text) {
        padding := " " * (screenWidth - 10 - text.size)
        return
            "  _____
              /X | X\\                       A.R. Drone Controller
             |__\\#/__|                           by Alien-Factory
             |  /#\\  |
              \\X_|_X/ $padding $text"
    }
 
    private Str num(Float num) {
        num.toLocale("0.00").justr(7)
    }
 
    private Str centre(Str txt) {
        " " * ((screenWidth - txt.size) / 2) + txt
    }
 
    private Str alignTop(Str txt) {
        txt + ("\n" * (screenHeight - txt.numNewlines))
    }
}

Die Methode printDroneInfo() gibt Drohnendaten an einen String Buffer aus und setzt diese dann als Label-Text. Manche Emergency Flags werden unterhalb der Daten angezeigt, für den Fall, dass man sich über den gerade erfolgten Absturz der Drohne nicht bewusst ist. Die Methode alignTop() sorgt lediglich dafür, dass der Text an der Oberseite des Labels ausgerichtet ist, die anderen Methoden sind für kleinere Formatierungen.

Wie man sehen kann, aktualisieren sich die Daten von „Velocity“ und „Orientation“ auch, wenn die Drohne nicht fliegt – eine gute Gelegenheit, um unser Programm auszuprobieren. Bevor wir den Pod erneut erstellen und ausführen, startet euren Quadrocopter und wartet, bis er hochgefahren ist; er sollte kurz darauf einen offenen W-LAN-Hotspot bereitstellen, der einen Namen wie ardrone2_v2.4.8 trägt. Verbindet euch mit ihm und erstellt den Pod, dann startet diesen.

4. Tastatursteuerung

Um die Drohne via Tastatur bedienen zu können, müssen wir zunächst eine Controller-Klasse erstellen. Diese wird sich in die Events keyUp und keyDown von Window einklinken und eine Liste von Tasten anlegen und aktuell halten, die aktuell gedrückt werden. Die WASD- und Pfeiltasten werden so eingestellt, dass sie die folgenden Funktionen ausführen:

  • W & S – Vorwärts- und Rückwärtsbewegung
  • A & D – Seitwärtsbewegung
  • UP & DOWN – Bewegung nach oben bzw. unten
  • LEFT & RIGHT – Drehen der Drohne mit oder entgegen dem Uhrzeigersinn

Die Tasten ENTER und ESC bekommen Spezialkommandos:

  • ENTER – Starten / Landung der Drohne
  • ESC – Notlandung

Da wir die ENTER-Taste sowohl für das Starten als auch das Landen verwenden, müssen wir immer im Blick haben, was der Quadrocopter tut, bevor wir das Kommando nutzen. Wird der Befehl zum Start gegeben, wird die Drohne in etwa einem Meter über dem Boden in einen stabilen Schwebezustand versetzt. Bis dieser Zustand erreicht ist, kann es mitunter fünf Sekunden dauern – die Drohne gibt dann eine entsprechende Flag aus.

Die Taste für die Notlandung ist unsere Versicherung, falls irgendetwas schief läuft. Betätigt man die ESC-Taste, wird die User Emergency Flag aufgerufen: Die Stromzufuhr zum Motor wird gekappt, sodass die Drohne (nicht sehr grazil) vom Himmel fällt. Das ist allerdings in manchen Fällen besser, als sie irgendwo hineinknallen zu sehen.

Beim Ausführen eines Spezialkommandos wird die Liste gedrückter Tasten sofort geleert, um den Quadrocopter nicht mit widersprüchlichen Befehlen zu verwirren, die scheinbar gleichzeitig gegeben werden. Die Main-Klasse muss aktualisiert werden, um eine Instanz der neuen Controller-Klasse anzulegen und während des Control Loops aufzurufen. Dies hier hier ist die Controller-Klasse:

syntax: fantom
 
using afParrotSdk2::Drone
using afParrotSdk2::FlightState
using fwt::Event
using fwt::Key
using fwt::Window
 
class Controller {
    private Drone   drone
    private Key[]   keys    := Key[,]
 
    new make(Drone drone, Window window) {
        this.drone  = drone
        window.onKeyUp.add   |Event e| { keys.add(e.key) }
        window.onKeyDown.add |Event e| { keys.remove(e.key) }
    }
 
    Void controlDrone() {
        if (keys.contains(Key.esc)) {
            keys.clear()
            drone.setUserEmergency()
        }
 
        if (keys.contains(Key.enter)) {
            keys.clear()
            if (drone.flightState == FlightState.def || drone.flightState == FlightState.landed) {
                drone.clearEmergency
                drone.takeOff(false)
            } else {
                drone.land(false)
            }
        }
 
        roll  := 0f
        pitch := 0f
        vert  := 0f
        yaw   := 0f
 
        if (keys.contains(Key.a))       roll  = -1f
        if (keys.contains(Key.d))       roll  =  1f
        if (keys.contains(Key.w))       pitch = -1f
        if (keys.contains(Key.s))       pitch =  1f
        if (keys.contains(Key.down))    vert  = -1f
        if (keys.contains(Key.up))      vert  =  1f
        if (keys.contains(Key.left))    yaw   = -1f
        if (keys.contains(Key.right))   yaw   =  1f
 
        drone.move(roll, pitch, vert, yaw)
    }
}

5. Videostream

Das wirklich Coole am Steuern einer Drohne ist es doch, das zu sehen, was sie auch sieht, oder? Eben! Lasst uns also eine DroneCam-Klasse erstellen. Dafür nutzen wir die VideoStreamer-Klasse, die voraussetzt, dass das FFMPEG Utility auf dem PATH ist. Sobald wir das Video eingerichtet und es mit dem Livestream der Frontkamera verbunden haben, sendet die Drohne reine Videodaten (H.264-Frames). Der VideoStreamer empfängt diese Videoframes und nutzt FFMPEG, um sie in PNG-Bilder zu konvertieren.

Um die so generierten PNG-Bilder anzuzeigen, erstellen wir eine Subklasse der Klasse FWT Canvas. Die Canvas-Klasse erstellt ihren Inhalt durch zeichnen, einem HTML-5-Canvas-Objekt nicht unähnlich. Das einzige, das unsere CamCanvas-Klasse zeichnet, ist allerdings das PNG-Bild. Wir müssen allerdings darauf achten, sämtliche vorherigen Bilder zu löschen, da sonst massive Speicherlecks evoziert werden.

syntax: fantom
 
using afParrotSdk2::Drone
using afParrotSdk2::VideoCamera
using afParrotSdk2::VideoResolution
using afParrotSdk2::VideoStreamer
using fwt::Canvas
using fwt::Desktop
using fwt::Window
using gfx::Graphics
using gfx::Image
using gfx::Size
 
class DroneCam {
    Drone           drone
    Window          window
    VideoStreamer   streamer    := VideoStreamer.toPngImages
    CamCanvas       canvas      := CamCanvas()
 
    new make(Drone drone, Window window) {
        this.drone  = drone
        this.window = window
    }
 
    Void open() {
        drone.config.session("jaxDemo").with {
            videoCamera     = VideoCamera.horizontal
            videoResolution = VideoResolution._360p
        }
        streamer.attachToLiveStream(drone)
 
        // open a new window, attaching it as a child of the existing window
        // open() blocks until window is closed, so call it in its own thread
        Desktop.callAsync |->| {
            Window(window) {
                it.title = "AR Drone Cam"
                it.size  = Size(640, 360)
                it.add(canvas)
            }.open
        }
    }
 
    Void updateVideoStream() {
        canvas.onPngImage(streamer.pngImage)
    }
}
 
class CamCanvas : Canvas {
    Image? pngImage
 
    Void onPngImage(Buf? pngBuf) {
        if (pngBuf == null) return
 
        // you get a MASSIVE memory leak if you don't call this!
        pngImage?.dispose
 
        // note this creates is an in-memory file, not a real file
        pngImage = Image(pngBuf.toFile(`droneCam.png`))
        this.repaint
    }
 
    override Void onPaint(Graphics g) {
        if (pngImage != null)
            g.drawImage(pngImage, 0, 0)
    }
}

Und schon können wir uns den Livestream der Drohne ansehen:

6. Alles zusammenfügen

Der Vollständigkeit halber möchte ich an dieser Stelle die finale Main-Klasse zeigen, die abbildet, wie man Screen, Controller und DroneCam aufruft:

syntax: fantom
 
using afParrotSdk2::Drone
using fwt::Desktop
using fwt::Label
using fwt::Window
using gfx::Color
using gfx::Font
using gfx::Size
 
using gfx::Image
 
class Main {
    Drone?      drone
    Label?      label
    Screen?     screen
    Controller? controller
    DroneCam?   droneCam
 
    Void main() {
        label = Label {
            it.font = Desktop.sysFontMonospace.toSize(10)
            it.bg   = Color(0x202020)   // dark grey
            it.fg   = Color(0x31E722)   // bright green
        }
 
        Window {
            it.title    = "Fantom AR Drone Controller"
            it.size     = Size(440, 340)
            it.add(label)
            it.onOpen.add  |->| { connect()    }
            it.onClose.add |->| { disconnect() }
        }.open
    }
 
    Void connect() {
        drone       = Drone()
        screen      = Screen(drone, label)
        controller  = Controller(drone, label.window)
        droneCam    = DroneCam(drone, label.window)
 
        drone.connect
        drone.clearEmergency
        drone.flatTrim
        droneCam.open
 
        controlLoop()
    }
 
    Void disconnect() {
        drone.disconnect()
    }
 
    Void controlLoop() {
        screen.printDroneInfo()
        controller.controlDrone()
        droneCam.updateVideoStream()
 
        Desktop.callLater(30ms) |->| { controlLoop() }
    }
}

7. Fazit

In diesem Artikel haben wir uns mit der Programmiersprache Fantom und dem Parrot SDK 2 für den Quadrocopter AR Drone auseinandergesetzt und dabei so einiges geschaft: Wir haben ein einfaches Fantom-Projekt inklusive Build-Script aufgesetzt und eine grundlegende Window-Anwendung erstellt. Dann haben wir uns via W-LAN mit dem Quadrocopter verbunden und Telemetriedaten in Echtzeit an das Kommandofenster übertragen. Schließlich haben wir die Steuerung der Drohne via Tastatur und den Livestream von der Videokamera eingerichtet.

Was ihr als nächstes macht, ist natürlich eure Entscheidung. Ich werde jedenfalls einige Stunt-Manöver erstellen und den Funktionstasten zuweisen. Vielleicht F1 für einen Rückwärtslooping, F2 für den Psi-Dance und vielleicht F3 für eine Wellenbewegung…

Viel Spaß!

Links & Literatur

In diesem Artikel werden die folgenden Tools verwendet:

Weitere nützliche Links:

Verwandte Themen:

Geschrieben von
Steve Eynon
Steve Eynon
Steve Eynon arbeitet als technischer Berater bei SkyFoundry. Er hat bereits bei verschiedenen Unternehmen als Senior Lead Developer gearbeitet, unter anderem bei AOL, BP, Roche, Sky & Virgin Media. Durch seine Arbeit dort hat er einschlägige Erfahrungen im Bereich Softwarearchitektur und Design-Lösungen für Unternehmen gesammelt.
Kommentare

Schreibe einen Kommentar

Deine E-Mail-Adresse wird nicht veröffentlicht.