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.

Zoe resurrected

I’ve only just boxed up Zoe having reached her limit, but now the Raspberry Pi Zero W has been released with WiFi and Bluetooth built in, freeing up her one micro-USB socket and thus opening up the possibility of adding GPS tracking to her too.  I’ve just ordered one from The PiHut to try it out.

This isn’t going to affect her maximum camera video size of 480 x 480 pixels, so she’s never going to work as well on gravel like Hermione, but it’s a useful upgrade nonetheless.

Bodes well!

The new PCB arrived, and I’ve done a few indoor tests, and I’ve only seen one I²C error over a total of 60s testing.  Previously, the I²C problem would occur in a fraction of a second.  So my power spike / noise speculation was probably right but needs refining.  Next step is to take Hermione out to build up confidence and to check what video frame resolution she can handle – currently it’s 640 x 640 pixels.  Annoyingly there are gales blowing and forecast to stay for the next few days.

I’ve already been designing the layout for the next PCB revision before this one arrived.  The main change is that the PCB is fed 5V independently from the Raspberry Pi.  Obviously their grounds are connected.  The power comes from a dual port (2.5A each) LiPo Battery Bank – one of these already powers my piPad B3 and RPi touch screen independently and beautifully.

In addition, but unrelated to the PCB power solution, the new PCB design includes a button and LED.  These are for use in the next phase: setting up a series of points in the flight plan based upon GPS targets.

Essentially, prior to a flight, Hermione is set down in several places, and the button pressed; the LED flashes while a sufficient number of GPS satellites have been acquired, at which point the LED goes on for 1 second, the GPS position is saved to file and the LED goes off.  This can be repeated several times to construct a list of GPS positions saved to file as the flight plan.

At the start of a flight, the LED flash / on / off sequence is repeated to acquires enough satellites and record the take-off GPS position; Hermione then climbs to 1m height, and hovers there for a second, yawing to face the first GPS target point saved previously in the flight plan file.  Then she heads there at 1m/s.  Once at the first GPS point, she hovers again, yawing to point towards the second GPS target, and off she goes again, with the sequence repeated for all of the prerecorded GPS targets.  On reaching the final pre-recorded GPS point, once more she hovers for a second while she yaws to face the takeoff GPS point taken at the start of the flight, and back home she goes.  Simples!

End of the line?

Status:

  • Zoe works as best as she can, but she doesn’t have the performance to run the video above 480 x 480 pixels at 20 fps.  This means lateral motion over the IKEA play mat works, but not on the gravel on the drive, never mind the grass.  I have found a micro-USB GPS dongle which I may add but I’ve yet to find a micro-USB hub to allow power, WiFi and GPS to go into the single micro USB port (the other is blocked by the frame)
  • Hermione can process video macro-blocks at 640 x 640 pixels @ 20 fps (and perhaps higher) which may be enough for the gravel drive, but I’m completely out of ideas for the source of the I²C errors from the Garmin LiDAR-Lite V3 when it is connected even if it’s not run.

I’ll continue working on the I²C to the end of the month, but after that I’ll be downing drone development and move on to other things, probably still Pi related nonetheless.

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 🙁

 

The GPS + compass plan

My intent with GPS and compass it that Hermione flies from an arbitrary take-off location to a predetermined target GPS location, oriented in the direction she’s flying.

Breaking that down into a little more detail.

  • Turn Hermione on and calibrate the compass, and wait for enough GPS satellites to be acquired.
  • Carry her to the destination landing point and capture the GPS coordinated, saving them to file.
  • Move to a random place in the open flying area and kick off the flight.
  • Before take-off, acquire the GPS coordinates of the starting point, and from that and the target coordinates, get the 3D flight direction vector
  • On takeoff, climb to 1m, and while hovering, yaw to point in the direction of the destination target vector using the compass as the only tool to give a N(X), W(Y), Up(Z) orientation vector – some account needs to be taken for magnetic north (compass) vs. true north (GPS)
  • Once done, fly towards the target, always pointing in the way she’s flying (i.e. yaw target is linked to velocity sensor input), current GPS position changing during the flight always realigning the direction target vector to the destination position.
  • On arrival at the target GPS location, she hovers for a second (i.e. braking) and decends.

There’s a lot of detail hidden in the summary above, not least the fact that GPS provides yet another feed for 3D distance and velocity vectors to be fused with the accelerometer / PiCamera / LiDAR, so I’m going to have to go through it step by step

The first is to fly a square again, but with her oriented to the next direction at the hover, and once moving to the next corner, have yaw follow the direction of movement.  Next comes compass calibration, and flight plan based upon magnetic north west and up.

However, someone’s invoked Murphy’s / Sod’s law on me again: Hermione is seeing the I2C errors again despite no hardware or software changes in this area.  Zoe is behaving better, and I’m trying to double the motion tracking by doubling the video frame rate / sampling rate for the Garmin LiDAR-Lite; the rate change is working for both, but the LiDAR readings see to be duff, reading 60cm when the flight height is less than 10cm.  Grr 🙁

Hermione’s proof of the pudding

Here finally is her flying in a stable hover for a long time without rocketing off into space.  Yes, she’s wobbly, but that’s a simple pitch / roll rotation rate PID tune much like I had to do with Zoe.  She’s running the video at 560 x 560 pixels at 10 fps, hence no need for the IKEA play mat.

Finally I can move on to adding the compass and GPS into the mix.

Hermione’s progress

I’ve upgraded Hermione to a B2, and installed the very latest level of the Raspian Lite OS from scratch, and her electronics is now driven from a 2.4A battery bank.  She’s running the video at 560 x 560 pixels at 10 fps with the B2 overclocked to 1GHz, and has 1GB memory.  With these changes, she’s working without error, at least in passive mode: she didn’t do this with the A+ and lower current power supply.

Hermione refit

Hermione refit

I’ve tidied up the WiFi wiring by hacking open an antenna base station, removing the magnet (so it doesn’t mess up the nearby compass), and replacing the wiring with a shorter version RP-SMA connector which is now routed through the frame thus keeping it nicely tucked away.

I’ve just bought a USB GPS module; it’s currently being tested, but soon, it’ll be on the same arm as the WiFi, with the WiFi antenna attached to the underside pointing down, and the GPS on top.  Ultimately, I’ll do the same with the GPS as I’ve done with the WiFi: strip the cable down to exactly the right length.

To use the GPS, I’ve installed the python gps package:

sudo apt-get install python-gps

This starts a GPS daemon, by default on /dev/ttyAMA01; this needs changing in /etc/default/gpsd to /dev/ttyUSB0.

Here’s the code – it’s almost a direct copy of an adafruit sample:

from __future__ import division
import gps
import os
import math

# Listen on port 2947 (gpsd) of localhost
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
new_lat = False
new_lon = False
base_lon = 0.0
base_lat = 0.0

dx = 0.0
dy = 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, 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
                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:

                new_lon = False
                new_lat = False

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

                if base_lat == 0.0 and base_lon == 0.0:
                    base_lat = lat
                    base_lon = lon
                    continue

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

            else:
                continue


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




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

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.

 

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.