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

RC hardware complete…

courtesy of lots of extra layers of the Pimoroni PiBow Ninja.  They also did a couple of custom cuts for Penelope’s PiBow so I could lower her by 2 layers (6mm) making space to include the battery warmer.  Penelope is however still waiting for the Garmin LiDAR-Lite v3HP.

RC hardware

RC hardware

I wish I could say the same about the software.  The problem is how to access these I2C Hall Effect Joysticks.  Although “i2cdetect -y 1” can see them, none of three different I2C python libraries (smbus, smbus2 and pigpio) can read the data correctly from them.  The problem is that the joysticks’ I2C does not need a register, you simply read two bytes from the I2C address, yet none of these libraries successfully doing this.  A google query found my own post high in the rankings which is disappointing when hoping to find someone else’s solution, so I’ve contract Grayhill directly.

Until this is solved, there’s little point adding the code to connect to Penelope (and for her to be listening), nor having Ivy load the code on boot.  Very frustrating.

Remote control

I’d run out of ideas of what to do next, and then it struck me: I could finally add an RC, and with just a few seconds’ thought, I realised it would be reletively simple.

Back at the end of 2013, about a year into my piDrone project, I’d clearly considered this – and yes, that is a Raspberry Pi 1A!!!

Remote Control Prototype

Remote Control Prototype

I’m guessing this was the point I realised I was way too ignorant, and realised autonomous control was easier, ironically.  I still have these I2C Grayhill 67A joysticks, and now RC control is much easier to add safely – it’s just another poll.poll() input to the Autopilot process like the Sweep- and GPS-flight plan inputs are now.  And this is perfect for Penelope.  Hermione will remain the OTT autonomous control, and ‘P’ will have some degree of manual control.  And in a way, this is much like my DJI Mavic – it hands control to the human only when it is safe to do so.

The RC will be based on the new RPI B3+ with it’s improved WiFi.  I’ve already done a prototype of how the joysticks attach to a Pimoroni PiBow Ninja body.

Pibow Joysticks

Pibow Joysticks

And actually, that feeds into the GPS fusion with the IMU ∑∑(acceleration – gravity)δtδt, and LiDARs distance inputs when the LiDARs are out of range.  I’m a lot more comfortable testing it this way rather than flying her over a lake!

My lord, I have a cunning plan!


P.S. I’m calling the RC “Phoebe”, who alongside Penelope make up team “Pi²”.

P.P.S. In one of those convenient coincidences, my one unknown was how to power Phoebe.  Then this appeared via Twitter.  I hope they do another production run.  It’s these synchronised occurrences that force you to consider whether there is a higher being playing chess with one’s life!

P.P.P.S. Changed my mind: the RC is called “Ivy” and together with “Penelope”, they make the “PI” team!

Obstruction avoidance test 2 – PASSED!!!!!

After a minor tweak to the handling resolution of the Scanse Sweep data, all works brilliantly.

This is a five metre forwards flight, with the flight paused and later resumed once the obstacle has been avoided.  Note that ‘H’ tracks the obstruction at about one meter away.  Hence she flies a quarter circle around the circular cardboard tube, before continuing the forward flight when the obstruction is behind her.

The code is updated on GitHub as a result.