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 Zoe saw.

I’ve finally found and almost resolved the instability problem with Zoe.  Zoe is a Pi0W compared to Hermione’s B3.  As a result, she’s a single CPU at lower frequency and she’s struggling to keep up.  In particular the LiDAR / Video distance increasing lag behind the IMU:

Distance

Distance

As a result, once fused, Zoe is reacting to historic movements and so drifting.  The IMU data processing takes priority and by slowing it down to 333Hz (from 500), that allows enough space to process the LiDAR / Video distance to stay in sync with the IMU.  Here’s the result for a simple 10 second hover.

There is still lagging drift but much less than seen previously; this drift is still cumulative; hence my next step is to reduce the video frame size a little more.

While this might not be the reason behind the RC drift, it cannot have been helping.


By the way, the fact the incrementally lagging drift is consistently left / right suggests strongly that I need to reduce the port / starboard PID due to the weight balance, primarily the LiPo battery aligned fore / aft in the frame.  On the plus side, without this flaw, I’d never have been able to diagnose the drift problem so clearly and quickly!

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.