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 🙁

 

She cannae take any more, Captain!

Zoe is now maxed out;  any increased video frame size above the current 400 x 400 pixels at 10 fps leads to a IMU FIFO overflow i.e. the video processing simply takes too long.  She’s also run out of physical space on her frame for more sensors.

On the other hand, Hermione has loads of physical space, but is plagued with I2C problems on her A+.

To add GPS for flight plans, and Scanse Sweep for object avoidance, I need more cores.  For the moment that means a B2 or B3.  Currently I’m leaning towards a B2 as I don’t need the 64 bit kernel of the B3, nor the built in WiFi or Bluetooth – I’d rather continue to use a faster WiFi USB dongle.  With the extra cores, I can move the video processing out of the motion processing into a different process, and have it feed the latest values to the motion processing when available.  Hopefully that will mean support for higher video frame size and rate and perhaps also increase the IMU sampling rate back to the 1kHz – I’ve had to reduce it to 500Hz currently.  Having the extra cores and 4 USB ports means GPS and Scanse Sweep should be much easier to add – I doubt the A3 (if it ever appears) will support those extra USB ports.

So it’s a B2.  For the sake of up to date build instructions, I’ll be installing her from scratch and blogging the instructions once complete.

 

Salad bowl

Nothing exciting to report I’m afraid – still waiting for the rain to stop to take the girls out.  So I thought I’d show you what they look like.  Both are now kitted out with LiDAR and RaspiCam for vertical and lateral distance sampling.  They run the same software – the code enables 8 ESC objects if the hostname is hermione.local or 4 otherwise:

Salad bowl

Salad bowl

As you may have guess, Hermione’s lid is a Salad Bowl from Italy – I spotted one we had and it fitted perfectly, so I got myself an orange one – £5 + £15 shipping to avoid the Italian postal service stealing it in transit.  The bowl is concave on the underside (in its role as a salad bowl).  Luckily, it’s 10cm diameter matching some black acrylic disks I have (one of which is on Hermione’s underside to which attaches the Garmin LiDAR Lite and the0 RaspiCam.  Ultimately, the Scanse Sweep will be attached to this top black disc.

On that train of thought, I need to get a move on as the Scanse Sweep could well arrive before Christmas according to the latest update note.

Hermione’s progress

Here’s Hermione with her new PCB.  It’s passed the passive tests; next step is to make sure each of the 8 motor ESCs are connected the right way to the respective PWM output on the PCB, and finally, I’ll do a quick flight with only the MPU-9250 as the sensors to tune the X8 PID gains.  Then she’s getting shelved.

Hermione's progress

Hermione’s progress

Zoe’s getting a new PCB so I can run the camera and Garmin LiDAR-Lite V3 on her too.  Hermione is huge compared to Zoe, and with the winter weather setting in, I’m going to need a system that’s small enough to test indoors.

Hermione will still be built – I need her extra size to incorporate the Scance Sweep and GPS, but I suspect only when an A3 arrives on the market – Hermione’s processing with a new 512MB A+ overclocked to 1GHz is nearly maxed out with the camera and diagnostics.  She’s probably just about got CPU space for the compass and Garmin LiDAR lite over I2C but I think that’s it until the A3 comes to market.  My hope for the A3 is that it uses the same 4 core CPU as the B2 with built in Bluetooth and WiFi as per the B3 but no USB / ethernet hub to save power.  Fingers crossed.