OK, so this is weird…

When I first added the autopilot process, it would update the main process at 100Hz with the current distance vector target; the main process couldn’t quite keep up with what it was being fed, but it was close.  The down side was the video processing dropped rate through the floor, building up a big backlog, meaning there was a very late reaction to lateral drift.

So I changed the autopilot process to only send velocity vector targets; that meant autopilot sent an update to the main process every few seconds (i.e. ascent, hover, descent and stop updates) rather than 100 times a second for the distance increments; as a result, video processing was running at full speed again.

But when I turned on diagnostics, the main process can’t keep up with the autopilot despite the fact they are only send once every few seconds.  A print to screen the messages showed they were being sent correctly, but the main process’ select() didn’t pick them up: in a passive flight, it stayed at a fixed ascent velocity for ages – way beyond the point the autopilot prints indicated the hover, descent and stop messages had been sent .  Without diagnostics, the sending and receipt of the messages were absolutely in sync.  Throughout all this, the GPS and video processes’ data rates to the main process were low and worked perfectly.

The common factor between autopilot, GPS, video and diagnostics is that they use shared memory files to store / send their data to the main processor; having more than one with high demand (autopilot at 100Hz distance target or diagnostics at 100Hz) seemed to be the cause for one of the lower frequency shared memory sources simply to not be spotted as far as the main process’ select() was concerned.  I have no idea why this happens and that troubles me.

This useful link shows the tools to query shared memory usage stats.

df -k /dev/shm shows only 1% shared memory is used during a flight

Filesystem 1K-blocks Used Available Use% Mounted on
tmpfs 441384 4 441380 1% /dev/shm

ipcs -pm shows the processes owning the shared memory:

------ Shared Memory Creator/Last-op PIDs --------
shmid owner cpid lpid 
0 root 625 625 
32769 root 625 625 
65538 root 625 625 
98307 root 625 625 
131076 root 625 625

ps -eaf | grep python shows the processes in use by Hermione. Note that none of these’ process IDs are in the list of shared memory owners above:

root 609 599 0 15:43 pts/0 00:00:00 sudo python ./qc.py
root 613 609 12 15:43 pts/0 00:02:21 python ./qc.py
root 624 613 0 15:43 pts/0 00:00:03 python /home/pi/QCAPPlus.pyc GPS
root 717 613 1 16:00 pts/0 00:00:01 python /home/pi/QCAPPlus.pyc MOTION 800 800 10
root 730 613 14 16:01 pts/0 00:00:00 python /home/pi/QCAPPlus.pyc AUTOPILOT fp.csv 100

Oddly, it’s the gps daemon with the shared memory creator process ID:

gpsd 625 1 4 15:43 ? 00:01:00 /usr/sbin/gpsd -N /dev/ttyGPS

I’m not quite sure yet whether there’s anything wrong here.

I could just go ahead with object avoidance; the main process would only have diagnostics as it’s main high speed shared memory usage.  Autopilot can maintain the revised version of ony sending low frequency velocity vector target changes.  Autopilot would get high frequency input from the Sweep, but convert that to changes of low frequency velocity targets sent to the main process.  This way, main has only diagnostics, and autopilot only has sweep as fast inputs.  This is a speculative solution.  But I don’t like the idea of moving forward with an undiagnosed weird problem.

Odds and sods

Progress and thus blog updates have been delayed by the weather: too windy last week; this week, too hot and sunny.  When the sun is high, the camera can’t resolve sufficient contrast between the clover flowers and the grass in the mown lawn  Because sunrise is 5am, that means I can only do test flights in the evening.  It also means that in coming to this conclusion, I’ve broken 5 props so far.  Very frustrated and expensive.

As a result, I’m still to really confirm that autopilot process is working well, and Sweep still lies on my office table.

On the slight plus side, I’ve enhanced the GPS data stream to the main process; I suspect I was throwing too much away by using first ‘string’ and then ‘float’ to pack the data.  I’ve just upped it to a ’64bit float’.  Iff this works as I hope, that may be all that’s necessary to track “where am I?” in accurate GPS units only, using Sweep + compass just to spot orientation of current paths / hedges in the maze. allowing the autopilot to choose “which way now?”.  Any mapping for “have I been here before?” can be added as an enhancement; initially it will be a random choice of the various path directions available regardless of whether they’ve been visited already.  But this is all a long way in the future.


A little later after writing the above, a speckled shade from a large tree was cast over part of the back garden and I managed to collect this GPS plot of a supposed 5m flight NE over the course of a 20s i.e. 20 GPS samples:

GPS resolution

GPS resolution

She struggled with the video tracking and once she left the shade, chaos ensued, but I did get the GPS stats, which clearly shows a much higher resolution initially than I was getting before.  So that’s good.  Yet another prop snapped on the aborted flight as she headed towards the wall. So that’s prop number 6 or £110 in real terms – completely unaffordable.

The multiple breakages are again weather based: two weeks of no rain means once more the lawn is rock solid, and props clipping the ground on an aborted landing snap instead of embedding in soft soil.

Intelligent Autopilot

I’m posting this to crystallise a lot of interrelated parallel thoughts into a working serial plan.  What you read below is probably the 20th draft revision:


Currently, each flight has a flight plan written by hand and saved to file beforehand: lines of 3D velocities plus how long to fly at those velocities e.g.

# - evx is fore / aft velocity in m/s; fore is positive
# - evy is port / starboard velocity in m/s; port is positive 
# - evz is up / down velocity in m/s; up is positive
# - time is how long to maintain that speed in seconds
# - name is an arbitrary name put out to console to show flight progress

# evx, evy, evz, time, name
 0.0, 0.0, 0.3, 2.0, ASCENT
 0.0, 0.0, 0.0, 6.0, HOVER
 0.0, 0.0,-0.3, 2.0, DESCENT

The main motion process uses this to calculate the distance PID targets at a given point in time; it’s efficient, but inflexible – it doesn’t react to the world around it.  To incorporate object avoidance, GPS waypoint targets, and ultimately maze mapping and tracking, I’ve decided to add a well-informed autopilot process to do the hard work, and leave the motion process in ignorant bliss.  As a separate process, the LINUX kernel scheduling should use a different RPi 3B CPU core for the autopilot than that used by the already overworked motion process.

Initially, the autopilot process will use the same input file as before, feeding the latest distances targets over an OS FIFO to be picked up promptly by the motion process much as GPS, video and Sweep do now.  That will ease the load on the motion process slightly as a result.

Once tested, GPS and Sweep will move from the motion process to the autopilot so it can make intelligent decisions dynamically about the distance targets it sends to be motion process; the time-critical motion processing remains oblivious to brick walls, hedges and where it’s been, it just blindly does what the intelligent autopilot tells it based upon the brick walls, hedges and mapping the autopilot knows of.

The only minor problem I can see currently is that the autopilot needs to know the direction the quad is pointing so it can yaw the quad frame Sweep data to align with the earth frame GPS flight data; because the compass is part of the IMU code on the motion process, I either need a FIFO feeding this back to the autopilot, or to access the IMU compass directly from the autopilot via I2C.  Plenty of time to resolve that though while I get the basics working first.

The motivation for doing this is I don’t want to attach the expensive Sweep to Hermione until all future significant code changes have been test and any significant bugs have been made and tested thoroughly.

For the record, the current code on GitHub has been updated.

Another bath time Eureka moment

and also premature, but I’ve just solved the problem of how to get Hermione to find her way through a maze.  It obviously involves GPS to define the centre of the maze, and Scanse Sweep to avoid the hedges, but what’s new is how to uses both to build a map of where she’s been.

Both GPS and Sweep provide input at once a second.  A combination of both’s data is sent to yet another process.  This combination of GPS location, the sweep at that location (and thinking on the fly, also compass orientation) builds a map.  Building and storing the map isn’t trivial, but it’s not going to be difficult either.  That map process can then be queried by the motion processing code i.e. Q: “Where next?”; A: “Left” or maybe “Compass point 118 degrees”.  The answer to the question is determined by areas that have not been explored yet – i.e. areas that aren’t on the map yet.

Once more, some future fun defined.

 

Persistent /dev/tty* names for GPS and Scanse Sweep

This details for this post are stolen from here; I’ve just tweaked it into my context.

The problem it solves is that both GPS and Sweep are USB UART /dev/ttyUSB? devices, and which is which is indeterminate and will change between boots.  The stolen solution below sets up a symlink to each device based upon the device attributes such that the code only ever refers the symlink i.e. ttySWEEP and ttyGPS rather than trying to guess the correct /dev/ttyUSB? underlying these symlinks.  I’ll hand you over now to my edition of the original author’s post:


Persistent names for usb-serial devices

I own a bunch of devices that appear as /dev/ttyUSB? in the system e.g. GPS and Scanse Sweep.  As I plug them in and pull them out from the USB ports, they get names like /dev/ttyUSB0 or ttyUSB1. Sadly the device names are not persistent — whether the Sweep pops up as /dev/ttyUSB0 or /dev/ttyUSB1 depends on the order in which are the devices discovered by the kernel. That makes things difficult — it usually requires a trial and error approach to find out what the hell is the GPS board’s tty name this time.

Wouldn’t it be nice to have persistent, descriptive device name for each of these toys? Like /dev/ttyGPS and /dev/ttySWEEP?

usb-serial devices

To distinguish between them we need some other unique identifier — in this case a vendor, product and serial number. These are the messages recorded at the end of /var/log/messages when Sweep (for example) is plugged in:

May 13 06:22:27 general kernel: [ 30.629485] usb 1-1.4: new full-speed USB device number 5 using dwc_otg
May 13 06:22:27 general kernel: [ 30.756984] usb 1-1.4: New USB device found, idVendor=0403, idProduct=6015
May 13 06:22:27 general kernel: [ 30.757006] usb 1-1.4: New USB device strings: Mfr=1, Product=2, SerialNumber=3
May 13 06:22:27 general kernel: [ 30.757019] usb 1-1.4: Product: FT230X Basic UART
May 13 06:22:27 general kernel: [ 30.757031] usb 1-1.4: Manufacturer: FTDI
May 13 06:22:27 general kernel: [ 30.757044] usb 1-1.4: SerialNumber: DO004VY5

UDEV rules

Now with the list of identifiers in hand let’s create a UDEV ruleset that’ll make a nice symbolic link for each of these devices. UDEV rules are usually scattered into many files in /etc/udev/rules.d. Create a new file called 99-usb-serial.rules and put the following lines in there:

SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", ATTRS{serial}=="011806AE", SYMLINK+="ttyGPS"
SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", ATTRS{serial}=="DO004VY5", SYMLINK+="ttySWEEP"

By now it should be obvious what these lines mean. Perhaps just a note for the last entry on each line — SYMLINK+=”ttySWEEP” means that UDEV should create a symlink /dev/ttySWEEP pointing to the actual /dev/ttyUSB* device. In other words the device names will continue to be assigned ad-hoc but the symbolic links will always point to the right device node.


Right back to me – this is a preemptive strike for when I add the Scanse Sweep to Hermione’s existing GPS.  However, that’s not going to happen until the Garmin LiDAR-Lite stops f’ing up the I2C (again), and I’ve tested all the varients of the GPS flight plan code.

Sweeping the office

Based on yesterday’s layout, I did a sweep. This is just a few seconds of scanning, yielding 8800 samples!  Units are meters.

Sweep the office

Sweep the office

The fuzzy sparse bit at the bottom is me wearing a black, fluffy, I presume non-reflective fleece.  The long extension top left is the view out of the office door into the hallway.  The dent to the left is Babbage, the Raspberry Pi official bear; to the right, my magnifying glass for soldering etc.

Here’s the code:

#!/usr/bin/env python

from __future__ import division
import serial
import math
import os
import struct

with serial.Serial("/dev/ttyUSB0",
                    baudrate = 115200, 
                    parity=serial.PARITY_NONE,  
                    bytesize = serial.EIGHTBITS,
                    stopbits = serial.STOPBITS_ONE,
                    xonxoff = False,
                    rtscts = False,
                    dsrdtr = False) as sweep:

    print "Scanse Sweep open"
    sweep.write("ID\n")
    print "Query device information"
    resp = sweep.readline()
    print "Response: " + resp

    print "Starting scanning...",
    sweep.write("DS\n")
    resp = sweep.readline()
    assert (len(resp) == 6), "Bad data"

    status = resp[2:4]
    if  status == "00":
        print "OK"
    else:
        print "Failed %s" % status

        #-----------------------------------------------------------------------
        # Missing here is stopping the scanning - it will still be running next 
        # time code is initiated and it all gets very messy / confusong separating
        # binary data from ASCII command / response.  Really need to do a subset of
        # the finally: branch below.
        #-----------------------------------------------------------------------
        os.exit()

    log = open("sweep.csv", "wb")
    log.write("angle, distance, x, y\n")

    format = '=' + 'B' * 7

    try:
        while True:
            line = sweep.read(7)
            assert (len(line) == 7), "Bad data read: %d" % len(line)
            data = struct.unpack(format, line)
            assert (len(data) == 7), "Bad data type conversion: %d" % len(data)

            azimuth_lo = data[1]
            azimuth_hi = data[2]
            angle_int = (azimuth_hi << 8) + azimuth_lo
            degrees = (angle_int >> 4) + (angle_int & 15) / 16

            distance_lo = data[3]
            distance_hi = data[4]
            distance = ((distance_hi << 8) + distance_lo) / 100

            x = distance * math.cos(degrees * math.pi / 180)
            y = distance * math.sin(degrees * math.pi / 180)

            log.write("%f, %f, %f, %f\n" % (degrees, distance, x, y))

    #--------------------------------------------------------------------------
    # Catch Ctrl-C
    #--------------------------------------------------------------------------
    except KeyboardInterrupt as e:
        pass        

    #--------------------------------------------------------------------------
    # Catch incorrect assumption bugs
    #--------------------------------------------------------------------------
    except AssertionError as e:
        print e

    #--------------------------------------------------------------------------
    # Cleanup regardless otherwise the next run picks up data from this
    #--------------------------------------------------------------------------
    finally:
    	print "Stop scanning"
    	sweep.write("DX\n")
    	resp = sweep.read()
    	print "Response: %s" % resp
    	log.close()    

For now, that’s job done.  Once I’ve finished the GPS tracking on the piDrone, this code will be incorporated as it’s own process connected via another OS FIFO (as per GPS and down-facing video motion frames) to the piDrone code which will use it for simple object avoidance.

First contact

Camera shot

Camera shot

Screen Shot

Screen Shot

import serial

sweep = serial.Serial("/dev/ttyUSB0",
                      baudrate = 115200, 
                      parity=serial.PARITY_NONE,  
                      bytesize = serial.EIGHTBITS,
                      stopbits = serial.STOPBITS_ONE,
                      xonxoff = False,
                      rtscts = False,
                      dsrdtr = False)
print "Scanse Sweep open"
sweep.write("ID\n")
print "Version requested"
resp = sweep.readline()
print "Response: " + resp

Yet more to distract me from my piDrone stuff!

£55.71 import duty* later…

and this is what UPS handed over to me in return.

Scanse Sweep 2D LiDAR tracker

Scanse Sweep 2D LiDAR tracker

First impressions are extremely good which bodes well for meeting my high requirements and expectations.  The box is sturdy, and the contents are not going to get damaged in transit; in addition to the Sweep itself, there’s

  • a custom UART / USB adaptor that’s small and slick
  • one USB A to micro-B for a PC connection – as cables like this go, this is probably the slickest looking I’ve seen – flat cable with very tidy connectors
  • one JST to bare wires to add your own connection – this is probably what I’ll be using to attach to the RPi UART ttyAMA0 as its the same wires that my Garmin LiDAR-Lite uses so I have spares
  • one cable I have no idea what it’s for – JST at one end clearly for the Sweep, but no idea what the other end’s for
  • a sticker, oddly requiring microwaving to make it sticky – I’ll be keeping this boxed!
  • a quick start guide.

I’m not in a rush to install it on Hermione; it’s too expensive to risk while I’m busy testing out the GPS, compass and yaw code – but I will put together a basic system, probably on my piPad.  That’ll let me check whether my double 2.4A battery bank is sufficient for both a B3 and the 0.5A the Sweep needs – I suspect this will be the first problem I’ll be having to fix; certainly, the raw Garmin LiDAR already runs off it only 2.4A feed due to power problems.


*I’m glad we are part of the EU and the free trade agreement means there is no import duty – oh no, wait, feck, argh – BREXIT! }:-{(>

GPS, Compass and Object Avoidance

It’s time to down tools on

  • Ö – she just can’t run fast enough to test on grass
  • yaw control – I just can’t work out why something goes very wrong three seconds after the yaw control kicks in

and move on to the next steps.

Compass

The compass data is already available, but needs calibrating to allow for magnetic materials in the area; I’ve even got the calibration code written, but it fails; it uses the gyro to track that the compass has turned > 360° to find the maximum and minimum readings, and hence the offset due to the local readings.  The angles from the gyro were utter rubbish, and I have no idea why – I just need to try harder here.

GPS

I’ve put together a hand-held GPS tracker, and took it for a walk outside my house*.  I also took a photo of our house with my Mavic and overlaid the results (it’s worth clicking on the image to see it full size):

GPS tracking

GPS tracking

It’s not perfect, but the shape tracked by the GPS is reasonable enough once the GPS has settled; note the both green splodges are at the same point, but only the end one is in the right position due the first few samples from the GPS – I’ll have to remember this when incorporating this with Hermione’s flight plan.  Ignoring the initial few samples, I think mostly the errors were less than a meter once away from the house.  The GPS code for Hermione will be closely based on what I used for my handheld version:

from __future__ import division
import gps
import os
import math

###################################################################################################
# Set up the tty to used in /etc/default/gpsd - if the GPS is via USB, then this is /dev/ttyUSB0. #
# The python script listens on port 2947 (gpsd) of localhost.  This can be reconfigured in        # 
# /etc/default/gpsd also.                                                                         #
###################################################################################################

session = gps.gps()
session.stream(gps.WATCH_ENABLE | gps.WATCH_NEWSTYLE)

num_sats = 0
latitude = 0.0
longitude = 0.0
time = ""
epx = 0.0
epy = 0.0
epv = 0.0
ept = 0.0
eps = 0.0
climb = 0.0
altitude = 0.0
speed = 0.0
direction = 0.0

lat = 0.0
lon = 0.0
alt = 0.0
new_lat = False
new_lon = False
new_alt = False

base_station_set = False

dx = 0.0
dy = 0.0
dz = 0.0

R = 6371000 # radius of the earth in meters

fp_name = "gpstrack.csv"
header = "time, latitude, longitude, satellites, climb, altitude, speed, direction, dx, dy, dz, epx, epy"

os.system("clear")

print header

with open(fp_name, "wb") as fp:
    fp.write(header + "\n")

    #---------------------------------------------------------------------------------
    # With a based level longitude and latitude in degrees, we can be the current X and Y coordinates
    # relative to the takeoff position thus:
    # psi = latitude => p below
    # lambda = longitude => l below
    # Using equirectangular approximation:
    #
    # x = (l2 - l1) * cos ((p1 + p2) / 2)
    # y = (p2 - p1)
    # d = R * (x*x + y*y) ^ 0.5
    #
    # More at http://www.movable-type.co.uk/scripts/latlong.html
    #---------------------------------------------------------------------------------

    while True:
        try:
            report = session.next()
#            print report
#            os.system("clear")
            if report['class'] == 'TPV':
                if hasattr(report, 'time'):  # Time
                    time = report.time

                if hasattr(report, 'ept'):   # Estimated timestamp error - seconds
                    ept = report.ept

                if hasattr(report, 'lon'):   # Longitude in degrees
                    longitude = report.lon
                    new_lon = True

                if hasattr(report, 'epx'):   # Estimated longitude error - meters
                    epx = report.epx

                if hasattr(report, 'lat'):   # Latitude in degrees
                    latitude = report.lat
                    new_lat = True

                if hasattr(report, 'epy'):   # Estimated latitude error - meters
                    epy = report.epy

                if hasattr(report, 'alt'):   # Altitude - meters
                    altitude = report.alt
                    new_alt = True

                if hasattr(report, 'epv'):   # Estimated altitude error - meters
                    epv = report.epv

                if hasattr(report, 'track'): # Direction - degrees from true north
                    direction = report.track

                if hasattr(report, 'epd'):   # Estimated direction error - degrees
                    epd = report.epd

                if hasattr(report, 'climb'): # Climb velocity - meters per second
                    climb = report.climb

                if hasattr(report, 'epc'):   # Estimated climb error - meters per seconds
                    epc = report.epc

                if hasattr(report, 'speed'): # Speed over ground - meters per second
                    speed = report.speed

                if hasattr(report, 'eps'):   # Estimated speed error - meters per second
                    eps = report.eps


            if report['class'] == 'SKY':
                if hasattr(report, 'satellites'):
                    num_sats = 0
                    for satellite in report.satellites:
                        if hasattr(satellite, 'used') and satellite.used:
                            num_sats += 1

            #-----------------------------------------------------------------------------
            # Calculate the X,Y coordinates in meters
            #-----------------------------------------------------------------------------
            if new_lon and new_lat and new_alt and num_sats > 6:

                new_lon = False
                new_lat = False
                new_alt = False

                lat = latitude * math.pi / 180
                lon = longitude * math.pi / 180
                alt = altitude


                if not base_station_set:
                    base_station_set = True

                    base_lat = lat
                    base_lon = lon
                    base_alt = alt

                dx = (lon - base_lon) * math.cos((lat + base_lat) / 2) * R
                dy = (lat - base_lat) * R
                dz = (alt - base_alt)

            else:
                continue


            output = "%s, %f, %f, %d, %f, %f, %f, %f, %f, %f, %f, %f, %f" % (time,
                                                                             latitude,
                                                                             longitude,
                                                                             num_sats,
                                                                             climb,
                                                                             altitude,
                                                                             speed,
                                                                             direction,
                                                                             dx,
                                                                             dy,
                                                                             dz,
                                                                             epx,
                                                                             epy)




            print output
            fp.write(output + "\n")
        except KeyError:
            pass
        except KeyboardInterrupt:
            break
        except StopIteration:
            session = None
            print "GPSD has terminated"
            break

The main difference will be that while this code writes to file, the final version will write to a shared memory pipe / FIFO much like the camera video macro-blocks are now.  The GPS will run in a separate process, posting new results as ASCII lines into the FIFO; Hermione’s picks up these new results with the select() she already uses.  The advantage of the 2 processes is both that they can be run on difference cores of Hermione’s CPU, and that the 9600 baudrate GPS UART data rate won’t affect the running speed of the main motion processing to get the data from the pipe.

Lateral object avoidance

My Scanse Sweep is very imminently arriving, and based on her specs, I plan to attach her to Hermione’s underside – she’ll have 4 blind spots due to her legs, but otherwise a clear view to detect objects up to 40m away.  Her data comes in over a UART like the GPS, and like the GPS, the data is ASCII text.  That makes it easy to parse.  The LOA does churn out data at 115,200 bps, so it too will be in a separate process.  Only proximity alerts will be passed to Hermione on yet another pipe, again listened to on Hermione’s select(); the LOA code will just log the rest providing a scan of the boundaries where it is.

Pure, unbridled fun!

While building Phoebe, Chloe, Zoe and Hermione over the last 4 years has been fascinating, frustrating, intellectually challenging, educational, satisfying, and critically, a brilliant-boredom-blocker, it’s never once been fun.

When I first started this project, there were many DIY quadcopter projects and very few commercial ones, and the commercial ones absolutely needed a skilled human with good hand-eye coordination in the feedback loop.  4 years later, the DIY market is shrinking because the commercial market has caught up and overtaken them; they now support vast amounts of autonomy to protect themselves from less-competent humans like me.

The best ‘affordable’ one currently is the DJI Mavic Pro.  It has 24-core, GPS, several URF  and video sensors for object avoidance and vertical / horizontal tracking, return to home, tracking a target and a stable gimbal for great photos and videos.  It folds up tiny and so portable.  And it costs £1k; I’ve spent many multiples of this on the development of Phoebe, Chloe, Zoe and Hermione.  So I’ve bought one and it arrived today.  After two hours charging, setting up etc, it was dusk, so I only took it out for 5 minutes.  And came back in beaming from ear to ear!

DJI Mavic Pro

DJI Mavic Pro

P.S. Development of Hermione with compass and GPS, and ultimately Scanse Sweep, will continue in the background, but currently, that’s blocked by the fact the I2C errors came back last week, despite there being no code nor hardware changes between the day she worked and the day she didn’t. Blind paralysed sterile stag (still no-fecking eye deer) why 🙁