Fecking rangefinders

  • SRF02 can’t run at 400kbps I2C baudrate necessary for reading maximum data from the MPU-9250 IMU.
  • TeraRanger needs a 12V power supply and provides 5V serial or I2C meaning level shifting to interface with the Raspberry Pi serial / I2C pins
  • LEDDAR uses weird and slow modbus protocol over serial meaning the IMU FIFO overflows if I’m sampling it above 500Hz – 1kHz works perfectly without LEDDAR.  Essentially, it’s wasting time I’m going to need for Camera, GPS, and Scanse Sweep processing,
  • Garmin LiDAR-Lite supports the necessary 400kbps I2C baudrate at 3.3V, but requires low level I2C access requiring a 20ms gap between sending the read request, and reading the response data.  Arduino provides this low level access, higher level smbus I2C via Python does not.  There are also comments around suggesting no other I2C activity can take place during that 20ms i.e. can’t access IMU during the 20ms!

All the sensors work, it’s just the API to access the data that’s non-standard in every one of these.  Did nobody on the design teams consider using a standard API for modern interface technology?  FFS!

P.S. Yes, I know there’s a URF that supports 400kbps I2C baudrate at 3.3V, but it has a bloody great potentiometer on the underside meaning it’s nigh on impossible to attach it ground-facing under a quadcopter.

P.P.S.  I know I could use the PX4FLOW; I actually have 3 but only one of these (the original) works; the clones both do not.  And anyway, where’s the fun in that compared to a vertical rangefinder, the Raspberry Pi Camera and the MPU-9250 gyro i.e. the three components that make up the PX4FLOW?

Compass and ViDAR

First, ViDAR: I’ve got fed up of calling it video macro-block something or the other, so now it’s Video Detection and Ranging.  I think it’s now working as well as it can without a height detector; it’s a long way from perfect, but until I can rely on stable height, I can’t identify further bugs.  I’m almost certainly going to wait for the Garmin LiDAR-Lite (GLL) to provide the height information.

There’s one bug I’ve fixed which explains why the PX4FLOW only uses a gyro – the angles involved are increments not absolutes.  The picture below tries to show that it’s not the absolute tilt of the quadcopter that needs to be accounted for, but the increment:

ViDAR angles

ViDAR angles

I’ve also got the compass working – I’ve just pulled out the critical bits.  Most of the samples I found use the MPU9250 master I2C to attach to the compass; compass data is then picked from MPU9250 registers.  But I found this version which exposed the compass registers for direct access which is much cleaner and clearer.

#  Gyroscope / Accelerometer class for reading position / movement.  Works with the Invensense IMUs:
#  - MPU-6050
#  - MPU-9150
#  - MPU-9250
#  The compass / magnetometer of the MPU-9250 is not used
class MPU6050:
    def initCompass(self):    
        # Set up the I2C master pass through.
        int_bypass = self.i2c.readU8(self.__MPU6050_RA_INT_PIN_CFG)
        self.i2c.write8(self.__MPU6050_RA_INT_PIN_CFG , int_bypass | 0x02)

        # Connect directly to the bypassed magnetometer, and configured it for 16 bit continuous data
        self.i2c_compass = I2C(0x0C)
        self.i2c_compass.write8(self.__AK893_RA_CNTL1, 0x16);
    def readCompass(self):
        compass_bytes = self.i2c_compass.readList(self.__AK893_RA_X_LO, 7)

        # Convert the array of 6 bytes to 3 shorts - 7th byte kicks off another read
        compass_data = []
        for ii in range(0, 6, 2):
            lobyte = compass_bytes[ii]
            hibyte = compass_bytes[ii + 1]
            if (hibyte > 127):
                hibyte -= 256

            compass_data.append((hibyte << 8) + lobyte)

        [mgx, mgy, mgz] = compass_data
        return mgx, mgy, mgz

mpu6050 = MPU6050()

    while True:
        mx, my, mz = mpu6050.readCompass()
        print "%d, %d, %d" % (mx, my, mz)

except KeyboardInterrupt, e:

I probably won’t include the compass data for yaw and orientation yet, but at least the main hurdle has been overcome.

DisneyLand Paris :-(

We’re off on holiday tomorrow, so I’m leaving myself this note to record the state of play: the new A+ 512MB RAM is overclocked to 1GHz setup with Chloe’s SD card running March Jessie renamed Hermione.  New PCBs have arrives and one is made up.  The new PCB is installed and has passed basic testing of I2C and PWM

To do:

  • install LEDDAR and Pi Camera onto the underside
  • update python picamera to the new version
  • test motion.py on hermione
  • merge motion.py with X8.py
  • work out why udhcpd still doesn’t work on May Jessie-lite (different SD card)

Progress report

Here’s Chloe’s HoG in Hermione’s frame.  You can see I’ve put a large WiFi antenna on one of the side platforms; the other is for GPS in the future.  The frame itself is not quite complete – I still need to install a platform on the underside to hang the sensors off.  In addition, the LID (LiDAR Installation Desktop) needs assembling – it’s just here for show currently.

Chloe's HoG in Hermione's frame

Chloe’s HoG in Hermione’s frame

Here’s a flight with just accelerometer and gyro in control for basic stability testing.

With these 1340 high pitch Chinese CF props, there’s no shortage of lift power despite the fact she weighs 2.8kg, so I’m going to defer the X8 format for a while on financial grounds – 4 new T-motor U3 motors and 40A ESCs costs nearly £400.

The PCBs are on order, and first setup will be for LEDDAR and PX4FLOW.

Oddly, only one of my PX4FLOWs works properly – for some reason, the newer one can’t see the URF, so can’t provide velocities, only angular shift rates; however, LEDDAR will give me the height allowing me to convert the angular rates to horizontal velocities.  If that works, that also opens up the possibility of replacing the PX4FLOW with a Raspi Camera using the H.264 video macro block increments to allow me to do the equivalent of the PX4FLOW motion processing myself, which if possible, would please me greatly.

Still lurking in the background is getting the compass working to overcome the huge yaw you can see in the video.


A walk in the park

I wired up PIX4FLOW to my test rig, knocked together some test code, set up the I2C baudrate to 400kbps to make sure it worked at the same rate as the IMU needs.

PX4FLOW test rig

PX4FLOW test rig

I took it for a walk around the garden: from the office to the garden via the hallway, then an anticlockwise 6m diameter circle around the garden before returning back to the office.  The code was sampling at about 20Hz, with the test rig about 60cm from the ground with the Y axis always pointing away from me.  The walk took about 80s.

Here’s the X, Y distance graph based upon integrating the velocities the PIX4FLOW gives.

Garden plot

Garden plot

A quick walk through:

  • 0,0 is in the office
  • throughout the test the Y axis pointed away from me
  • beyond the 4m point, I walked in an anti-clockwise circle
  • once complete I doubled back and headed back to the office.

I’m delighted with the garden section; because the y axis was always facing forwards, from the PX4FLOW point of view, it always went forwards, but when transformed to the earth frame using the gyro readings, it shows a really accurate plot shape.  Given this was just a green grass surface, I’m amazed the down facing camera did such a good job

Here’s the height graph from the inbuilt URF:


It’s good except for the spikes – I may need LEDDAR to make this better.  On the plus side, the height is not integrated, so the spikes do not have a persistent effect.

There were a few problems or inaccuracies:

  • the sensors should timestamp each read, but the register value did not change so I had to do this myself with time.time() – I have a second sensor on the way to see if it’s the sensors faul (ebay PX4FLOW to find them)
  • the scale of the circle is wrong:  the graph shows the circle to be about 3m diameter, but it should be more like 6m – this may just be a problem in my maths
  • the end of the walk should return to the start, yet it’s 6m out – the shape of the walk out of and back to the office match, but there’s a 30° error by the end of the flight.  I suspect only compass will fix this.

One unexpected positive was that although I’ve heard the I2C SMBus only supports 32 byte buffers, it seemed fine reading the 77 byte registers in a single sequence.

Overall then as a first trial I’m pretty delighted to the extent it’s now worth getting the new PCB for Chloe / Hermione.

Flight of the body snatcher

Body snatcher flight from Andy Baker on Vimeo.

So this is the first flight of Chloe’s HoG running on Hermione’s frame.

She drifts right as Chloe always has but more so, and she doesn’t have LEDDAR installed, so the hover height is wrong. She’s seriously underpowered with the T-motor 1240 props, but the X8 configuration will fix that, but will require new PCBs.

Due to the lack of LEDDAR, she’s running the velocity rather than distance as the outer PID; combined with the lower power, that means she doesn’t get as high as she should.

Finally, you’ll notice significant yaw that eventually corrects as the yaw rate PID i-gain kicks in; again that due to the size of the props and X8 will resolve that.

Next steps are to

  • add code for PX4FLOW
  • get legs to allow underside sensors
  • add PIX4FLOW – LEDDAR is going into (permanent?) retirement as long as the PIX4FLOW provides good vertical height data with it’s URF.
  • bodge up the current PCB to add another 5V output pin for PIX4FLOW
  • bodge up the PX4FLOW I2C wiring for 0.1″ pitch connection to PCB
  • get the PCBs for X8.

Plenty to keep me busy for a while!

Say ‘Hi!’ to Hermione*

Hermione will be the mother of all that’s gone; she’s a bit heartless at the moment – her HoG will be an RPi A3 when it appears on the market.

Hermione flat-out (with banana for scale)

Hermione flat-out (with banana for scale)

Hermione flat-out (with banana for scale)

Hermione tip-toes (with banana for scale)

The main reason behind building yet another quadcopter is the frame:

  • loads more space for extra sensors
  • folding arms so it’s easily transportable
  • beautiful build – lots of CF and CNC machining
  • lots of extension platforms adding lots of options of where to put all the pieces – installed here are a power distribution board (in the plate sandwich), a plate for her HoG, and a GPS plate sticking out to the left.

The one thing that’s missing are legs – I’ve had the ones supplied before, and they crack and break with the down-falls my testing produces.  There are stronger ones I’ll be getting.  I’ve bought from quadframe.com previously for Chloe so I know the high quality of their frames; the previous frame I’d bought was premature, and sadly I ended up selling it.

But this time, I have a purpose for all the extra space: Hermione will be deployed with GPS, LiDAR x 2, PX4FLOW, a RPi Camera and anything else I can get onto her.  Also the frame supports an X8 layout as well as standard quad format; X8 is where each arm has 2 motors and props: the top motor prop is set up as normal; the lower one is upside down, and the prop is installed on the motor upside down.  The topside and underside props spin in opposite directions.  This format provides more power for heavy lifts and protection against motor / prop damage or failure.  Each prop has its own ESC and separate PWM feed.

Doing the above requires the A3 for its extra cores to do a lot more sensor processing and a new PCB to support the new GPIO pin requirements.

GPS, LiDAR and LEDDAR require UART connections; X8 requires another 4 GPIO pins for the 4 ESCs PWM feed.  One of the sensors – probably GPS – can use the USB port, assuming the A3 has built-in WiFi like the B3.  PXFLOW uses I2C.

My thinking is that LEDDAR and PXFLOW provide term fused inputs to the distance PIDs; GPS provides targets for the flight plan; compass is the yaw input with the the flight plan providing direction of travel so that a camera pointing forwards can always track her progress; Scance Sweep provides object detection overriding the GPS flight path short term to avoid objects.

Together, this means I could set a start and end position for a flight using GPS (either just start and finish or with intermediate check points), and then just set her loose autonomously to track against those points, whether this is simply flying from A to B or getting from the entrance to the centre of a maze, logging “where I’ve been” to ensure she always prioritises new paths through the maze.

Here’s the current PCB eagle layout for Phoebe and Chloe:



  • the right hand side 3 vertical PCB tracks are for the LiPo to 5V regulator – I’m planning on Hermione having a BEC on her PDB (power distribution board), so that opens up space for the rear X8 PWM pins.
  • the left hand side 4 vertical PCB track are I2C extensions in place for the URF, but the space is needed for the the front X8 PWM pins so the I2C extension is moved to the bottom edge and will be used for the PIX4FLOW.

Here’s the first draft of the revised PCB:

Hermione PCB beta

Hermione PCB beta

PIN usage:

  • pins 4, 6, 8, 10 and 12 are used for LEDDAR as now but extended outwards
  • the GPIO pins for PWM are completely reworked for X8 format as is the MPU9250 interrupt pin; I’ll probably end up adding X8 before I add Scanse Sweep simply to fill the gap between now and its delivery
  • I2C is extended on the lowerside for the PX4FLOW
  • That leaves a selection of pins and space on the topside for Scanse Sweep – I need to get wiring specs for this; and alternative is via USB using a UART to USB converter – I have one of these already for use configurating LEDDAR by my Windows PC.
  • GPS will also be via USB, assuming the WiFi is now build into the A3 board or I might use a mini USB hub like on of these that I used already for other projects.

Regarding WiFi, to extend the range, I’m going to replace the (hopefully) on board antenna with a HiRose U.FL connector, as per here.  That will then allow me to connect to a U.FL to RP-SMA cable, and place a higher gain antenna on the board.

I’ve still  left a few breadboard pins at the base as they may turn out to be useful for LEDs, buzzers etc.

As you can see, there’s a lot that needs to be done, but it breaks down into lots of separate blocks to keep me busy until the A3 and Scanse Sweep arrive.  The first step is simple: move Chloe’s HoG over to Hermione’s frame, and get her stable.

*Penelope (as in “Pitstop”) was a close runner up, but with my existing HoGWARTs WAP installation notes, Hermione (as in “Granger”) won the ballot outright.


Basic Installation

  • sudo apt-get gpsd gpsd_tools python-gps
  • Edit /etc/default/gpsd and add the GPS device into the boot configuration of gpsd
  • Reboot or restart the GPS daemon gpsd.
  • Try these various tests to make sure it’s working.

Python Script

I’ve written an extension of an Adafruit python script which captures GPS data and writes it to screen and file:

from __future__ import division
import gps
import os
import math

# Listen on port 2947 (gpsd) of localhost
session = gps.gps("localhost", "2947")
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

lat1 = 0.0
lon1 = 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"


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 = longitude => p below
    # lambda = latitude => 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:
            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
                if hasattr(report, 'epx'):   # Estimated longitude error - meters 
                    epx = report.epx

                if hasattr(report, 'lat'):   # Latitude in degrees
                    latitude = report.lat
                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
            lat2 = latitude * math.pi / 180
            lon2 = longitude * math.pi / 180

            dx = (lat2 - lat1) * math.cos((lon1 + lon2) / 2) * R
            dy = (lon2 - lon1) * R
            lon1 = lon2
            lat1 = lat2

            output = "%s, %f, %f, %d, %f, %f, %f, %f, %f, %f" % (time, 

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

Here’s what I get plotting a walk around the garden carrying her – it’s not a great match.

Garden track

Garden track

0.0m is my office window.  Everything beyond is -10m, -20m is the garden;  the walk in the garden was

  • 15m SSW
  • 9m ESE
  • 18m N

That’s an approximate right angle triangle, but the scan doesn’t show that without a lot of imagination.  The scaling is right, but the direction is very noisy.  I’m not sure whether this is a code bug or just pushing the limits of GPS accuracy.  I’ll have a few more goes to see if I can get any better.  GPS samples are at roughly 1Hz.

Code update plans for Chloe

  • ultimately, that output file with be a FIFO read by Chloe’s code.  She’ll use select.select() to monitor the file contents, and sleep, whereas now she just sleeps.
  • I’ll add command line parameters –indoors or –outdoors to select the range of sensors to be used – primarily GPS; PX4FLOW / Scanse Sweep and LEDDAR will work in both environments.
  • If using the GPS, add startup code to ensure at least 4 satellites are feeding the GPS for accurate triangulation – during the walk, I had 4 indoors and 6 outdoors; obviously the more satellites, the greater the accuracy.

LEDDAR shield

These are my notes for building a protective shield around the LEDDAR One based upon their spec.  I don’t have a complete build yet, but I’ll post a photo when I do.

Components and preparation


  • Sticky back paper
  • draw a 60mm diameter circle with a compass – this is the outline of the plates
  • choose 3 points around the edge and draw an arc, 60mm diameter as before – this locates the centre of the circle.
  • from the centre, draw a 45mm diameter circle – the mounting holes lie on this circle
  • choose a point on the 45mm diameter circle, and draw an 38.79mm radius – this identifies the isoceles triangle points for the mounting holes
  • with the 2 new intersects arc 2 more to clearly identify clearly 3 corners to equilateral triangle
  • choose a pair of the equilateral triangle points, and draw a line between them
  • draw a line from the third triangle corner through the centre point of the circles up to the new line to identify the centre of the line – these should cross at 90° if your drawing is accurate
  • From the line between the two triangle corners, draw parallel lines above by 1mm and below by 21mm and 27mm.  These are the centre points for the sensor and the 2 rows of LEDs respectively  .The 6 LEDs are separated horizontally and vertically by 6mm.
  • if you’ve done this correctly, it should look a bit like this:

    LEDDAR cover

    LEDDAR cover

  • carefully cut around the outer circle and stick to the 60mm diameter acrylic discs.

Drilling etc.

  • center-punch all ten marked holes (3 corners, 1 sensor and 6 LEDs)
  • peel off the template and top protective sheet and discard.
  • drill small (1mm) pilot holes through the points of the triangle
  • separate the two disks
  • drill a 2.5mm hole through the bottom plate
  • tap an M3 thread into these bottom plate holes and put aside
  • drill out 3 x 3.2mm holes through the top plate at the 3 corners of the triangle
  • drill a 22mm hole at the sensor center point using a step drill bit set
  • drill 5mm holes for the LEDs
  • file out the LEDs if you wish or need.
  • countersink the triangle holes – do this after the LED holes or drilling the LED holes will slip.

Just FYI, I bought a new Rotring compass, 0.1mm pen, ink and adapter to get this as accurate as possible.

It is possible to get 60mm diameter, 3mm thickness, 10cm tall clear acrylic cylinders / tubes from ebay, to cut to the right height and protect the sides.  I have these on order but have not tried them yet.

Distance / Direction PIDs

I’ve added the distance + direction (DD) PIDs: the targets come from the integrated velocity flight plan, the inputs come from the acceleration integrated twice and fused with LEDDAR height for the Z axis.  The outputs feed the existing velocity PIDs’ targets with a max speed of 0.5m/s set for safety reasons during the initial testing.

First few test flights were a super-sky-soarer (3m off the ground in less than a second before I could react to kill!); a couple of careless bugs fixed (and a rebuilt chassis – falling from 3m leads to a harder impact than the lower PCB can cope with even with the super shock-resistant Tarot legs installed).

It’s still a β version with more tuning – I suspect removing the PID I-gain from the velocity PIDs.  However, in principle, it’s working, and so I’ve updated GitHub.  By default the DD processing is commented out – look around line number 2319.  Simply uncomment them to convert the velocity PID targets from the flight plan to the DD PID output.