Flying Quadcopters with Fantom

In this article we're going to write a program in the Fantom programming language to control the Parrot AR Drone quadcopter.

Fanny riding a Quadcopter

In particular, the program will aim to:

control the drone via keyboard input
print real-time telemetry data from the drone
display a real-time video feed from the drone's camera

This article assumes a familiarity with programming languages and that Fantom is already installed on your system.

The Parrot A.R. Drone is a popular low cost quadcopter. It is often the drone of choice for quadcopter enthusiasts due to it's open source client SDK written in C.

On the Fantom side, the program will use the Parrot Drone SDK by Alien-Factory. It is a pure Fantom implementation of the Parrot SDK that lets you pilot Parrot quadcopters remotely.

Controlling the Parrot AR Drone
Create Fantom Project
Print Telemetry Data
Keyboard Control
Video Stream
Putting It All Together
Finale
References

1. Controlling the Parrot AR Drone

The Parrot AR Drone has an on-board microprocessor that runs Linux Busy Box. It uses this to read sensors and send output to its 4 motors. It also contains Wifi hardware. When you turn the drone on, it sets itself up as a Wifi hot spot, to which your computer connects.

The drone and your computer then use standard TCP and UDP protocols to send and receive data. In particular, your computer sends configuration and movement commands, and the drone sends back video feeds and navigation data.

On our computer we're going to be running a Fantom program that uses the Parrot Drone SDK library to send flying commands to the drone. To make use of video streaming we need to use the popular FFMEG utility to convert raw video data from the drone into usable images. To use FFMEG, just ensure the executable is on your PATH or in the same directory as from where you start Fantom.

2. Create Fantom Project

The first thing is to create a simple project that opens a window to display text and receive keyboard input.

Fantom projects are compiled into a .pod file, much like how Java projects are compiled into .jar files. Only in Fantom you are encouraged to make .pod files self contained so they often contain documentation, source code, and any related resources.

Every Fantom project has a build.fan file. This is a Fantom script that's responsible for creating the .pod file. Conveniently, Fantom is bundled with a core library called build that contains a lot of utility classes and methods that do most of the pod building work for you. In particular, if you subclass the BuildPod class then all you need to do is configure it in the constructor!

Here's the build.fan we're gonna use for our JaxDrone project:

select all

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
    }
}

Our demo project will be compiled into a file called jaxDrone.pod and has a number of dependencies:

sys - the core Fantom library
gfx - contains useful constructs like Color & Font
fwt - Fantom Windowing Toolkit for creating Window applications; based on eclipse SWT.
afParrotSdk2 - a library from Alien-Factory (as denoted by the af prefix) that controls the Parrot AR Drone.

The pods sys, gfx, and fwt are core pods that come pre-bundled so any Fantom installation should already include them. afParrotSdk2 however, is a third party pod that we need to download and install. Running the following Fantom command should do just that.

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

As per the srcDirs we will put our source code in the fan/ directory, starting with a Main class:

select all

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
    }
}

It creates a window with a Label and prints some text when it is opened. We decorate the label so it looks like a console window, complete with green text on a black background in a monospace font.

To build our pod, run the build.fan Fantom script:

> 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]!

See Build in the Fantom Tools documentation for more details on building pods.

Then to run our project, run the newly built pod:

> fan jaxDrone

Screenshot of basic FWT Window

See Running Pods in the Fantom Tools documentation for more details on running pods.

3. Print Telemetry Data

Next comes the exciting bit - connecting to the drone itself! For this bit we'll introduce the ParrotSDK library for Fantom. The library is centred around the Drone class, so we'll add it as a using statement and instantiate an instance.

We'll update the onOpen() and onClose() events to connect and disconnect to/from the drone respectively.

After we've connected to the drone, we'll call clearEmergency() to ensure the drone is in a normal flying state. We'll also call flatTrim() so the drone can calibrate where horizontal is, needed for a stable and wobble free flight!

We'll also take this opportunity to set up a control loop which will be executed every 30 milliseconds or so. In this loop we'll be updating the screen with fresh telemetry data, sending movement commands, and processing video data.

select all

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 the first instance we'll just use the control loop to update the screen with telemetry data. In standard demo mode, the drone will send telemetry data every 60 milliseconds (about 15 times a seconds) so our 30 millisecond loop will be ample.

The Drone.navData field always contains the latest data from the drone, so our Screen class will just print data from that.

If we wanted to write an event driven display then we could utilise the Drone.onNavData event handler, but our loop keeps things simple.

select all

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))
    }
}

The printDroneInfo() method prints drone data out to a string buffer and sets it as the label text.

Some common emergency flags are printed at the bottom that alert you to problems, should you not be aware that that drone has just crash landed!

The alignTop() method just makes sure the text is aligned at the top of the label by padding it out with extra new lines. The other methods perform benign formatting.

Note that the Velocity and Orientation data updates even when the drone is not flying. So now is a good time to try out our program!

Before you build and run the pod again, turn the drone on (connect up its battery) and wait for it to power up. It should then start broadcasting an open WiFi hotspot calling something like ardrone2_v2.4.8. Connect to it as you would any other, then build and run the pod:

Screenshot of telemetry data

Now pick up your AR Drone and play with it, pretending it's a F16 fighter jet or a X-Wing starfighter, and you should see the telemetry data update on the screen!

4. Keyboard Control

To control the drone via keyboard we'll create a Controller class. It will hook into the Window's keyUp and keyDown events to maintain a list of keys that are currently pressed down. We'll monitor the WASD and cursor keys to perform the following:

W & S - tilt drone forward and backward
A & D - tilt drone left and right
Up & Down - move drone up and down
Left & Right - spin drone clockwise and anti-clockwise

We'll also use the following keys for special commands:

Enter - take off and land
Esc - emergency landing!

Because we're using the Enter key for both take off and landing, we need to first check what the drone is doing before we issue a command. Note that asking the drone to take off puts it in a stable hover state where it hovers at a height of about 1 meter above the ground. It can sometimes take up to 5 seconds for this stable hover to be achieved - at which point the drone sets the flying flag.

The emergency landing key is our back up should anything go wrong. Hitting the Esc keys sets the User Emergency flag which cuts power to the drones engines, ensuring it falls (ungracefully) out of the sky - which is sometimes better than watching it go sailing over the neighbours hedge!

When performing a special command, we'll clear the list of depressed keys so we don't confuse the drone by trying to make it so several things at once!

The Main class needs to be updated to create an instance of a new Controller class and call it during the control loop. And this is the Controller class:

select all

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. Video Stream

The cool part of controlling a drone is being able to see what it sees, so let's create a DroneCam class!

For this, we'll use the VideoStreamer class, which requires the FFMPEG utility to be on the PATH. Once we've configured the video and attached it to the live stream on the front camera, then the drone starts to send out raw video data (encoded H.264 frames). The VideoStreamer intercepts these video frames and uses FFMPEG to convert them to PNG images.

To display PNG images, we subclass the FWT Canvas class. The Canvas class, much like a HTML 5 canvas object, creates its content by painting. The only thing our CamCanvas class paints is the PNG image. Only we need to make sure we dispose of any previous images, otherwise it creates huge memory leaks!

select all

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)
    }
}

And now you can view a live video feed from the Drone!

Screenshot of Drone Cam

6. Putting It All Together

The final project should look like this:

jaxDrone/
 |-- fan/
 |    |-- Controller.fan
 |    |-- DroneCam.fan
 |    |-- Main.fan
 |    `-- Screen.fan
 `-- build.fan

For the sake of completion, here is final Main class that shows how to setup and call Screen, Controller, and DroneCam:

select all

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. Finale

This article has looked at the Fantom Programming Language and the Parrot SDK 2 for the AR Drone and we've covered quite a lot:

Set up simple Fantom project, complete with build script
Created a basic window application
Connected to the AR Drone via WiFi
Updated real time telemetry data to the window
Added keyboard controls for flying the drone
Shown real-time video feed from the Drone's camera

As to what happens next is up to you!

But me? Well, I'll be assigning some of the built-in stunt manoeuvres to function keys! Hmm, lets see... I think F1 for a backward flip, F2 for a psi dance, F3 for a wave...

Have fun!

References

The following versions were used in the making of this article:

Other resources:

Edits

12 July 2017 - Article re-mastered and re-published on Alien-Factory.
7 July 2017 - Article translated to German:
- Grundkurs Drohnensteuerung: So lernen Quadrocopter mit Fantom fliegen
5 July 2017 - Original article published on JAXenter:
- Drone controlling 101: Flying Quadcopters with Fantom

Written by Steve Eynon ( ) on 5th July 2017
Last edited on 12th July 2017
Tags: Quadcopter, Parrot Drone SDK 2

Flying Quadcopters with Fantom
12th July 2017

1. Controlling the Parrot AR Drone

2. Create Fantom Project

3. Print Telemetry Data

4. Keyboard Control

5. Video Stream

6. Putting It All Together

7. Finale

References

Edits

Discuss

Flying Quadcopters with Fantom12th July 2017

1. Controlling the Parrot AR Drone

2. Create Fantom Project

3. Print Telemetry Data

4. Keyboard Control

5. Video Stream

6. Putting It All Together

7. Finale

References

Edits

Discuss

Flying Quadcopters with Fantom
12th July 2017