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!

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.

WAP RPi 3B+ and Stretch

So Hermione is a 3B running Jessie from February 2017 due to network and I2C problems added in the March release.

Penelope is now a 3B+ running Stretch.  As a result, I want to update to the latest WAP software and I2C.  The biggest problem for me is setting up the isolated Wireless Access Point.  I’ve finally solved it with a lot of help from my friends on the RPi Forum.  Here’s how:

To set up an isolated AP:

  • Follow these instructions up to but not including “ADD ROUTING AND MASQUERADE” section.
  • Comment out any “NETWORK={” entries in /etc/wpa_supplicant/wpa_supplicant.conf
  • reboot

To disable the isolated AP to contact the internet for apt-get updates etc:

  • In /etc/default/hostapd set DAEMON_CONF=””
  • Reinstate the “NETWORK={” entries in /etc/wpa_supplicant/wpa_supplicant.conf for your home network ssid etc
  • Comment out the AP interface IP details in /etc/dhcpcd.conf
  • reboot

To solve the I2C, I’m waiting for the Garmin LiDAR-Lite v3HP which has been consistently delayed by up to 8 weeks since (at least) the start of February i.e. it’s still delayed by up to 8 weeks, two months later!  The Garmin LiDAR-Lite v3 exposed the I2C problem: it was strictly running I2C rules whereas the RPi I2C included an incompatible bend of the rules commonly supported many other devices e.g the MPU-9250 IMU.  I’m hoping the delay is due to the fact they are updating their I2C implementation to support the bent rules.

THBAPSA


P.S. Penelope has a new salad bowl:

Penelope's Salad Bowl

Penelope’s Salad Bowl

OA test 1 analysis

My fault: when an obstacle is detected, the autopilot tells ‘H’ to move 90°, parallel to the obstacle, until she reaches the safe zone.  That’s not what the logs from the autopilot say:

AP: PHASE CHANGE: RTF
AP: PHASE CHANGE: TAKEOFF
AP: PHASE CHANGE: HOVER
AP: FILE FLIGHT PLAN
AP: PHASE CHANGE: FORE
AP: AVOIDING OBSTACLE @ 6 DEGREES.
AP: PHASE CHANGE: AVOID @ 103 DEGREES
AP: OBSTACLE AVOIDED, RESUME PAUSED
AP: PHASE CHANGE: FORE
AP: AVOIDING OBSTACLE @ 7 DEGREES.
AP: PHASE CHANGE: AVOID @ 94 DEGREES
AP: OBSTACLE AVOIDED, RESUME PAUSED
AP: PHASE CHANGE: FORE
AP: AVOIDING OBSTACLE @ 13 DEGREES.
AP: PHASE CHANGE: AVOID @ 81 DEGREES
AP: AVOIDING OBSTACLE @ 11 DEGREES.
AP: PHASE CHANGE: AVOID @ 79 DEGREES
AP: OBSTACLE AVOIDED, RESUME PAUSED
AP: PHASE CHANGE: FORE
AP: AVOIDING OBSTACLE @ 15 DEGREES.
AP: PHASE CHANGE: AVOID @ 76 DEGREES
AP: AVOIDING OBSTACLE @ 15 DEGREES.
AP: PHASE CHANGE: AVOID @ 72 DEGREES
AP: OBSTACLE AVOIDED, RESUME PAUSED
AP: PHASE CHANGE: FORE
AP: AVOIDING OBSTACLE @ 14 DEGREES.
AP: PHASE CHANGE: AVOID @ 70 DEGREES
AP: AVOIDING OBSTACLE @ 18 DEGREES.
AP: PHASE CHANGE: AVOID @ 72 DEGREES
AP: OBSTACLE AVOIDED, RESUME PAUSED
AP: PHASE CHANGE: FORE
AP: AVOIDING OBSTACLE @ 23 DEGREES.
AP: PHASE CHANGE: AVOID @ 72 DEGREES
AP: OBSTACLE AVOIDED, RESUME PAUSED
AP: PHASE CHANGE: FORE
AP: AVOIDING OBSTACLE @ 31 DEGREES.
AP: PHASE CHANGE: AVOID @ 73 DEGREES
AP: OBSTACLE AVOIDED, RESUME PAUSED
AP: PHASE CHANGE: FORE
AP: AVOIDING OBSTACLE @ 31 DEGREES.
AP: PHASE CHANGE: AVOID @ 65 DEGREES
AP: AVOIDING OBSTACLE @ 37 DEGREES.
AP: PHASE CHANGE: AVOID @ 59 DEGREES
AP: OBSTACLE AVOIDED, RESUME PAUSED
AP: PHASE CHANGE: FORE
AP: AVOIDING OBSTACLE @ 40 DEGREES.
AP: PHASE CHANGE: AVOID @ 71 DEGREES
AP: PROXIMITY LANDING 1.46 METERS
AP: PHASE CHANGE: PROXIMITY CRITICAL 0.67m
AP: LANDING COMPLETE
AP: FINISHED

Frankly, it’s a miracle the video looked as good as it did! Clearly, there’s something wrong with compensating direction angles, as the OBSTACLE and AVOID angles should be ±90°.  Superficially this should be simple to fix, fingers crossed.


P.S. Based on the video, Sweep angles are correct; it’s my code calculation that’s wrong; for example, the final sample of the object at +40°should result of a result of -50° not 71°.

P.P.S.  The bug was crass; the log used the distance not direction, and the functional code is correct.  I suspect the problem’s solution is fine-tuning of the critical vs. warning ranges.