That’s all, folks…*

Featured

I think I’ve done all that can be done with my Raspberry Pi, Python piDrones.  Code is updated on GitHub as a result.  Here’s the vimeo links to the proof-of-the-pudding as it were:

The hardest by far though was the simplest in concept: a stable autonomous hover beyond a few seconds; each of the cool functions listed above probably took a few weeks on average; in contrast, the one-minute-plus hover took years.

There’s lots more videos on Vimeo linked to the blog via pidrone.io/video.

I’ve achieved my personal target and then some: taking a Raspberry Pi and Python to create a piDrone, starting from absolutely ignorance, and doing it my way without using other’s code, ideas or suggestions.

What else could be done?  My only idea is long distance / time flights requiring:

  1. GPS fused with existing distances sensors
  2. Compass fused with existing orientation sensors
  3. Long range wireless connectivity
  4. Nuclear-fusion batteries.

Lack of #4 renders 1-3 pointless.

Also pointless sadly is Penelope; Hermione, my big 3B, is the queen of autonomous control and Zoe, my Pi0W, the queen of human control.  Two’s company, three’s a crowd. The only thing unique I can do with ‘P’ is to get her RPi 3B+ and Stretch O/S completed, and my motivation is lacking; that makes Penelope the queen of spare parts 😥

Time for me to find another hobby to hold back my terminal-boredom-syndrome.  On my bike, me thinks.

So long, and thanks for all the fish!


* …nearly.  I’m doing some refinement for Zoe, primarily so I can take new to the Cotswold Raspberry Jams and anything new and exciting the RPi releases next.

Human. I am human.

From the Mavic:

From my iPhone:

The only mysterious behaviour was the unexpected drift after the end of deliberate yaw; it’s not the RC nor me; best guess, the sensors are seeing the yaw as lateral drift somehow.


P.S Zoe is now retired as she’s done all she can within the constraint of her CPU speed.  She’ll be back if and when a faster Pi0W is announced.

P.P.S. Penelope is boxed up also, partly because there’s nothing she can do that Hermione hasn’t done already, and partly because Garmin are still faffing around launching their new LiDAR.  If that appears, there is some value in bringing her back to life, both because she’s a (faster) B3+ and she’s running Stretch – up to now, everyone is stuck on Jessie from February 2017  – the last point I2C worked with the Garmin.

P.P.P.S. The title is from the film “Arrival” which is very good if that’s the sort of film you like!

Hmm, needs more work…

This isn’t as bad as it looks!  The take-off is automatic to a safe altitude which unfortunately took Zoe out of frame; once the auto takeoff completed, I rc’d her back into frame, and then did what I described in the video.

Causes of the problems?

  • primarily, the nut-behind-the-wheel – I need a lot more person-practise!
  • secondly, Zoe is at her performance limit hence her instability.
  • third and finally, once she left the high-colour-contrast mat, she didn’t stand a chance!

What this does show is the code is good, and I next plan to do the same outdoors with Hermione once the wind drops and the temperature rises.

Neither Zoe nor furniture were damage in this video!

What the piDrone saw…

What the piDrone say

What the piDrone say

Here’s the piDrone’s stats of what the RC sent it, and it’s spot on – I just used the RC to take-off, hover, and land.  However, Zoe misbehaved as she always does, and it was chaotic from my point-of-view.  I really need to sort this out with her, but for the moment I have enough confidence now to run the RC past Hermione the next time the amazing-weather and my free-time come together again.  Hopefully a video anon.


A quick run this morning over a colourful picnic mat showed Zoe works beautifully using an autonomous hover, yet with the RC, was all over the place even during the automatic take-off phase; there’s either a crass bug or Zoe can’t cope with the extra input from the RC.  More digging before I try this on Hermione.

The nut behind the wheel…

and a flat battery, but the concept was proven:

Once the stable hover at one meter has attained, the RC takes over: look for the yaw, the forward lateral movement, the height gain, and the awfully unstable landing due to the battery running too low.

As a first live trial it’s brilliant; next step: charge the batteries, and get the human trained to use the RC better!

Passive RC + piDrone passed

RC control

RC control

Here’s what’s shown here:

  • Until the RC and piDrone connect, the piDrone does nothing; this is not on the graph.
  • When the piDrone sends a message to RC saying it’s ready, the flight control starts
  • Initially, it does nothing until the RC messages the piDrone a special command to take-off; the take off is automatic, the joysticks has not control this once striggered.
  • Once the 3s automated takeoff completes (about 16 – 19 seconds in the graph), the RC has full control again, and you can see me tweaking the real joystick actions up to the 46 second point.
  • At that point, another special message from the RC to piDrone sets it to descend; again once descending, the joysticks has no influence until landed.
  • In this case, I didn’t use the RC to trigger the take-off again; instead I then got the RC to trigger a stop to the flight at about 60s.

Next is to try it live, ideally on Zoe as the smallest, lighted, and strongest…

RC completed

OK, the RC code is complete, currently talking to a test receiver i.e. not the piDrone.  Next step is to merge the receiver code with Penelope’s.

#!/usr/bin/python

from __future__ import division
from smbus2 import SMBusWrapper, i2c_msg
import math
import socket
import struct
import sys
import select
import time

def client():

    ################################################################################################
    #                              MAKE CONNECTION TO SERVER
    ################################################################################################

    poll = select.poll()

    go_go_go = False

    pack_format = "=ffffb?"
    pack_size = struct.calcsize(pack_format)

    unpack_format = "=?"
    unpack_size = struct.calcsize(unpack_format)

    client = socket.socket()
    host = socket.gethostname()
    port = 31415

    while True:
        try:
            client.connect((host, port))
        except:
            time.sleep(0.1)
        else:
            break
        continue

    client_fd = client.fileno()
    poll.register(client_fd, select.POLLIN | select.POLLPRI)

    ################################################################################################
    #                                  SET UP THE JOYSTICKS FSM
    ################################################################################################

    PASSIVE = 1
    TAKEOFF = 2
    FLYING = 4
    LANDING = 8
    POWEROFF = 16

    passive_time = time.time()

    status_quo = [0.0, 0.0, 0.0, 0.0]

    state = PASSIVE
    beep = False

    #----------------------------------------------------------------------------------------------
    # Acquire contact with the piDrone; only continue below once acquired.
    #----------------------------------------------------------------------------------------------

    with SMBusWrapper(1) as bus:

#       bus.write_byte_data(0x40, 0x76, 2)
#       bus.write_byte_data(0x41, 0x76, 2)

        while state != POWEROFF:

            ########################################################################################
            #                        HANDLE MESSAGES FROM THE SERVER
            ########################################################################################

            results = poll.poll(500) # milliseconds

            #---------------------------------------------------------------------------------------
            # Check whether there's I/O from RC
            #---------------------------------------------------------------------------------------
            for fd, event in results:
                assert fd == client_fd, "WTF HAPPENED HERE"

                #-----------------------------------------------------------------------------------
                # Has piDrone told piRC to start?
                #-----------------------------------------------------------------------------------
                raw = client.recv(unpack_size)
                assert (len(raw) == unpack_size), "Invalid data"

                #-----------------------------------------------------------------------------------
                # React on the action
                #-----------------------------------------------------------------------------------
                formatted = struct.unpack(unpack_format, raw)

                if formatted[0]: # is True
                    go_go_go = True
                else:
                    go_go_go = False
                    state = POWEROFF

            if not go_go_go:
                continue

            ########################################################################################
            #                          HANDLE MESSAGES FROM THE JOYSTICKS
            ########################################################################################

            #---------------------------------------------------------------------------------------
            # UD = Up / Down - upwards is positive
            # YR = Yaw Rate - anticlockwise is positive
            # LR = Left / Right - leftwards is positive
            # FB = Forwards / Backwards - forwards is positive
            #---------------------------------------------------------------------------------------

            msg = i2c_msg.read(0x40, 2)
            bus.i2c_rdwr(msg)
            data = list(msg)

            assert (len(data) == 2), "Joystick 0 data len: %d" % len(data)

            if (data[0] > 127):
                UD = data[0] - 256
            else:
                UD = data[0]

            if (data[1] > 127):
                YR = data[1] - 256
            else:
                YR = data[1]

            msg = i2c_msg.read(0x41, 2)
            bus.i2c_rdwr(msg)
            data = list(msg)

            assert (len(data) == 2), "Joystick 1 data len: %d" % len(data)

            if (data[0] > 127):
                LR = data[0] - 256
            else:
                LR = data[0]

            if (data[1] > 127):
                FB = -(data[1] - 256)
            else:
                FB = -data[1]

            #=======================================================================================
            # FSM INPUT, STATES, OUTPUT
            #=======================================================================================
            beep = False

            #---------------------------------------------------------------------------------------
            # Special cases for takeoff and landing.  Only between these states do we tell the
            # piDrone what to do.
            #---------------------------------------------------------------------------------------
            if abs(UD) < 20 and YR > 20 and abs(FB) < 20 and LR < -20:
                if state == PASSIVE:
                    #-------------------------------------------------------------------------------
                    # Take-off - we send fixed takeoff param regardless of joystick for next 3 seconds.
                    #-------------------------------------------------------------------------------
                    print "takeoff-takeoff-takeoff"
                    state = TAKEOFF
                    beep = True
                    takeoff_time = time.time()

            if abs(UD) < 20 and YR < -20 and abs(FB) < 20 and LR > 20:
                #-----------------------------------------------------------------------------------
                # Send a shut-down to the piDrone, and shut our selves down once we get a confirmation
                # from the piDrone.
                #-----------------------------------------------------------------------------------
                if state == FLYING:
                    print "landing-landing-landing"
                    state = LANDING
                    beep = True
                    landing_time = time.time()
                elif state == PASSIVE:
                    print "poweroff-poweroff-poweroff"
                    state = POWEROFF
                    beep = True

            #=======================================================================================
            # FSM INPUTS
            #=======================================================================================
            if state == TAKEOFF:
                if time.time() - takeoff_time < 3.0: # seconds
                    UD = 0.33
                    YR = 0.0
                    LR = 0.0
                    FB = 0.0
                else:
                    UD, YR, FB, LR = status_quo
                    state = FLYING
                    beep = True

            elif state == FLYING:
                    #-------------------------------------------------------------------------------
                    # Joysticks are +/- 80, convert these to +/- 1m/s.  The exception is the yaw rate
                    # where +/-80 maps to +/- 90 degrees (pi/2) per second
                    #-------------------------------------------------------------------------------
                    UD /= 80
                    YR /= (80 * 2 / math.pi)
                    FB /= 80
                    LR /= 80

            elif state == LANDING:
                if time.time() - landing_time < 3.0: # seconds UD = -0.33 YR = 0.0 FB = 0.0 LR = 0.0 else: UD, YR, FB, LR = status_quo state = PASSIVE beep = True passive_time = time.time() elif state == PASSIVE: UD, YR, FB, LR = status_quo if time.time() - passive_time > 60.0:
                    state = POWEROFF
                    beep = True

            else:
                assert state == POWEROFF, "Should be on poweroff state here!"
                UD, YR, FB, LR = status_quo

            output = struct.pack(pack_format, UD, YR, FB, LR, state, beep)
            client.send(output)
            print "SENT:  UD = %f | YR = %f | FB = %f | LR = %f | status = %d | beep = %d" % (UD, YR, FB, LR, state, beep)


        else:
            #---------------------------------------------------------------------------------------
            # We get here when the server sends us the "running = False"
            #---------------------------------------------------------------------------------------
            client.close()


def server():

    poll = select.poll()

    unpack_format = "=ffffb?"
    unpack_size = struct.calcsize(unpack_format)

    pack_format = "=?"

    server = socket.socket()
    host = socket.gethostname()
    port = 31415
    server.bind((host, port))

    server.listen(5)

    try:
        connection, addr = server.accept()
        connection_fd = connection.fileno()
        poll.register(connection_fd, select.POLLIN | select.POLLPRI)

        #-------------------------------------------------------------------------------------------
        # Tell the client to go-go-go!
        #-------------------------------------------------------------------------------------------
        output = struct.pack(pack_format, True)
        connection.send(output)

        #-------------------------------------------------------------------------------------------
        # Listen to the client and do what it says.
        #-------------------------------------------------------------------------------------------
        while True:
            results = poll.poll(500)

            #---------------------------------------------------------------------------------------
            # Check whether there's I/O from RC
            #---------------------------------------------------------------------------------------
            for fd, event in results:
                assert fd == connection_fd, "WHOSE FD IS THIS?"

                #-----------------------------------------------------------------------------------
                # Unpack the data received
                #-----------------------------------------------------------------------------------
                raw = connection.recv(unpack_size)
                assert (len(raw) == unpack_size), "Invalid data"

                #-----------------------------------------------------------------------------------
                # React on the action
                #-----------------------------------------------------------------------------------
                formatted = struct.unpack(unpack_format, raw)
                assert (len(formatted) == 6), "Bad formatted size"

                UD = formatted[0]
                YR = formatted[1]
                FB = formatted[2]
                LR = formatted[3]
                state = formatted[4]
                beep = formatted[5]

                print "RECEIVED: UD = %f | YR = %f | FB = %f | LR = %f | status = %d | beep = %d" % (UD, YR, FB, LR, state, beep)

    except KeyboardInterrupt:
        #-------------------------------------------------------------------------------------------
        # Tell the client to stop-stop-stop!
        #-------------------------------------------------------------------------------------------
        output = struct.pack(pack_format, False)
        connection.send(output)

    except Exception, err:
        print err

    finally:
       connection.close()


if len(sys.argv) != 2:
    print "Select DRONE or RC"
elif sys.argv[1] == "RC":
    client()
elif sys.argv[1] == "DRONE":
    server()
else:
    print "Select RC or DRONE"

RC piDrone connectivity

The network connection / interaction between the RC and piDrone is complete:

#!/usr/bin/python

import socket
import struct
import sys
import select
import time

def client():

    poll = select.poll()

    go_go_go = False

    pack_format = "=ffffb?"
    pack_size = struct.calcsize(pack_format)

    unpack_format = "=?"
    unpack_size = struct.calcsize(unpack_format)

    client = socket.socket()
    host = socket.gethostname()
    port = 31415

    while True:
        try:
            client.connect((host, port))
        except:
            time.sleep(0.1)
        else:
            break
        continue
        
    client_fd = client.fileno()
    poll.register(client_fd, select.POLLIN | select.POLLPRI)

    UD = 0.0
    state = 27
    beep = True

    try:
        running = True
        while running:
            results = poll.poll(500) # milliseconds

            #----------------------------------------------------------------------------------
            # Check whether there's I/O from RC
            #----------------------------------------------------------------------------------
            for fd, event in results:
                if fd == client_fd:
                    #--------------------------------------------------------------------------
                    # Has piDrone told piRC to start?
                    #--------------------------------------------------------------------------
                    raw = client.recv(unpack_size)
                    assert (len(raw) == unpack_size), "Invalid data"

                    #--------------------------------------------------------------------------
                    # React on the action
                    #--------------------------------------------------------------------------
                    formatted = struct.unpack(unpack_format, raw)

                    if formatted[0]: # is True
                        go_go_go = True
                    else:
                        go_go_go = False
                        running = False

            if not go_go_go:
                continue

            YR = UD + 0.1
            FB = YR + 0.1
            LR = FB + 0.1

            output = struct.pack(pack_format, UD, YR, FB, LR, state, beep)
            client.send(output)
            print "SENT:  UD = %f | YR = %f | FB = %f | LR = %f | status = %d | beep = %d" % (UD, YR, FB, LR, state, beep)

            UD = LR + 1


    except Exception, err:
        print err
    finally:
        client.close()

def server():

    poll = select.poll()

    unpack_format = "=ffffb?"
    unpack_size = struct.calcsize(unpack_format)

    pack_format = "=?"

    server = socket.socket()
    host = socket.gethostname()
    port = 31415
    server.bind((host, port))

    server.listen(5)

    try:
        connection, addr = server.accept()
        connection_fd = connection.fileno()
        poll.register(connection_fd, select.POLLIN | select.POLLPRI)

        #--------------------------------------------------------------------------------------
        # Tell the client to go-go-go!
        #--------------------------------------------------------------------------------------
        output = struct.pack(pack_format, True)
        connection.send(output)

        #--------------------------------------------------------------------------------------
        # Listen to the client and do what it says.
        #--------------------------------------------------------------------------------------
        while True:
            results = poll.poll(500)

            #----------------------------------------------------------------------------------
            # Check whether there's I/O from RC
            #----------------------------------------------------------------------------------
            for fd, event in results:
                assert fd == connection_fd, "WHOSE FD IS THIS?"

                #--------------------------------------------------------------------------
                # Unpack the data received
                #--------------------------------------------------------------------------
                raw = connection.recv(unpack_size)
                assert (len(raw) == unpack_size), "Invalid data"

                #--------------------------------------------------------------------------
                # React on the action
                #--------------------------------------------------------------------------
                formatted = struct.unpack(unpack_format, raw)
                assert (len(formatted) == 6), "Bad formatted size"

                UD = formatted[0]
                YR = formatted[1]
                FB = formatted[2]
                LR = formatted[3]
                state = formatted[4]
                beep = formatted[5]

                print "RECEIVED: UD = %f | YR = %f | FB = %f | LR = %f | status = %d | beep = %d" % (UD, YR, FB, LR, state, beep)

    except KeyboardInterrupt:
        #--------------------------------------------------------------------------------------
        # Tell the client to stop-stop-stop!
        #--------------------------------------------------------------------------------------
        output = struct.pack(pack_format, False)
        connection.send(output)

    except Exception, err:
        print err

    finally:
       connection.close()


if len(sys.argv) != 2:
    print "Select DRONE or RC"
elif sys.argv[1] == "RC":
    client()
elif sys.argv[1] == "DRONE":
    server()
else:
    print "Select RC or DRONE"

Next step is to merge this with this.

piDrone + piRC interaction

I was considering adding the remote controller WiFi connection into the autopilot, but now I’m against it, simply because it’s easier to plug it directly into the Motion process.  Ultimately Autopilot  / Sweep will still play a role, protect again “object collision”, but initially at least, they won’t be implemented.

                +—————+ 
                |  RC |······
                +—————+     ·
+—————+                     ·
|Sweep|———>———+             · 
+—————+       |             ·
        +—————+———+     +———·——+
        |Autopilot|——>——|Motion|
        +—————+———+     +———+——+
+———+         |             |
|GPS|————>————+             |
+———+                       |
                +—————+     |
                |Video|——>——+
                +—————+

The testing code for piDrone ⇔ piRC WiFi interactions is well underway, only hindered by the dither making the decision above, and the ever-slipping release of the Garmin LiDAR-Lite v3HP.  Oh, and my kids are on Easter school holidays and we’re going to my parents, so no updates until Friday at the earlier!

RC software test 1 passed

Courtesy of the creator of SMBus2 on GitHub, the joysticks are working perfectly.  Here’s the code.  Time now to move on to the network connection with Penelope.

#!/usr/bin/python
import time
from smbus2 import SMBusWrapper, i2c_msg 

try:
    with SMBusWrapper(1) as bus:

        bus.write_byte_data(0x40, 0x76, 2)
        bus.write_byte_data(0x41, 0x76, 2)
        
        while True:
            time.sleep(0.5)

            msg = i2c_msg.read(0x40, 2)
            bus.i2c_rdwr(msg)
            data = list(msg)

            assert (len(data) == 2), "Joystick 0 data len: %d" % len(data)

            if (data[0] > 127):
                X_0 = data[0] - 256
            else:
                X_0 = data[0]

            if (data[1] > 127):
                Y_0 = data[1] - 256
            else:
                Y_0 = data[1]
            
            msg = i2c_msg.read(0x41, 2)
            bus.i2c_rdwr(msg)
            data = list(msg)

            assert (len(data) == 2), "Joystick 1 data len: %d" % len(data)

            if (data[0] > 127):
                X_1 = data[0] - 256
            else:
                X_1 = data[0]

            if (data[1] > 127):
                Y_1 = data[1] - 256
            else:
                Y_1 = data[1]

            print "X_0 = %d | Y_0 = %d | X_1 = %d | Y_1 = %d" % (X_0, Y_0, X_1, Y_1)

except KeyboardInterrupt:
    pass

finally:
    pass