Fantom comes in handy when you want to control a drone

Drone controlling 101: Flying Quadcopters with Fantom

Steve Eynon

In this article, Steve Eynon, Technical Consultant at SkyFoundry writes a program in Fantom to control the Parrot AR Drone quadcopter, a beloved type of drone thanks to its open source client SDK.

In this article, we’re going to write a program in the Fantom programming language to control the Parrot AR Drone 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 that you are familiar with programming languages and that Fantom has already been 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 its 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.

1.Controlling the Parrot AR Drome

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 four motors. It also contains Wifi hardware. When you turn the drone on, it sets itself up as a WiFi hotspot, 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.

SEE ALSO: Fantom: Designed as a “better Java” [Pirates of the JVM]

2. Fantom project

The first thing you need to do 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. 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:

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

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:

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

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

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

3. Print telemetry data

Next comes the exciting part – connecting to the drone itself! For this bit, we’ll introduce the ParrotSDK library for Fantom. The library is centered 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.

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 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 utilize the Drone.onNavData event handler, but our loop keeps things simple.

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

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

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

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

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

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

6. Putting it all together

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

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

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 for me? Well, I’ll be assigning some of the built-in stunt maneuvers to function keys! Hmm, let’s see… I think F1 for a backward flip, F2 for a psi dance, F3 for a wave…

Have fun!

 

Author

Steve Eynon

Steve Eynon works as a Technical Consultant at SkyFoundry.

Having worked as senior lead developer for numerous household names such as AOL, BP, Roche, Sky, & Virgin Media, Steve has become more involved in architecture and design solutions for businesses.


Comments
comments powered by Disqus