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, 
                    bytesize = serial.EIGHTBITS,
                    stopbits = serial.STOPBITS_ONE,
                    xonxoff = False,
                    rtscts = False,
                    dsrdtr = False) as sweep:

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

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

    status = resp[2:4]
    if  status == "00":
        print "OK"
        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.

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

    format = '=' + 'B' * 7

        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:

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

    # Cleanup regardless otherwise the next run picks up data from this
    	print "Stop scanning"
    	resp = sweep.read()
    	print "Response: %s" % resp

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, 
                      bytesize = serial.EIGHTBITS,
                      stopbits = serial.STOPBITS_ONE,
                      xonxoff = False,
                      rtscts = False,
                      dsrdtr = False)
print "Scanse Sweep open"
print "Version requested"
resp = sweep.readline()
print "Response: " + resp

Yet more to distract me from my piDrone stuff!

Motion vectors from Raspicam videos

After a gentle nudge from Jeremy and his pointy stick yesterday, I had a quick play with Kitty++.  Follow the link for what she does and how, but in summary, she uses the Raspicam video to churn out the ‘motion vectors’ between video frames as part of the h.264 video compression algorithm.  Because the video frames are at a fixed rate (10fps in my case), then how ‘far’ the frame have moved is actually how ‘fast’ – i.e. a velocity vector that could be merged / blended / fused with the accelerometer velocity as a way to constrain drift.

The sub-project stalled last year due to lack of believable data, but yesterday, after a couple of bug fixes, here’s a chart showing the movement over a 10s ‘flight’ of my prototyping Pi.  I was moving the Pi forwards and backwards and left and right in a cross shape – you can see the movement, but the cross shape gets corrupted as periodically the video compression algorithm resets itself to maintain accuracy – a problem for this integrated distance chart, but not for the velocity vector I need.

Raspicam motion detection

Raspicam motion detection

If you follow the line from the 0,0 point, you can see the line travelling left / right first, then up / down.  This is 100 frames over 10 seconds.

There’s a lot of details to be tweaked to make this work properly especially to get units correct but it’s now another viable solution in addition to the laser dot tracking for motion control.  The advantage of this over the laser tracking is I can get lots more frames a second so the motion fusion can happen much faster than the 1s limit I’m seeing using the laser tracking.

P.S. The only real down side of this is that without a camera, Zoe can’t use it, and Zoe will continue to be my preferred show-and-tell quad as she’s easier to transport.  So I will continue to work out why Zoe drift the way she shouldn’t be according to the 0g offsets.  I do have one idea to test later today if I get the chance.

Another step of fine tuning

With the IMU sample rate set to 250 Hz (a 4ms gap between samples), there should be enough time to run motion processing for each sample without missing any; based on experience, motion processing takes 2 to 3ms. I’m currently averaging 5 samples before invoking motion processing (50Hz updates to the props).  Today I did some testing setting this to 2 and 1 (i.e. 125Hz and 250Hz updates to the props).

Setting 5 samples per motion processing gives 3430 samples @ 250Hz = 13.720 seconds of flight. time.time() says it took 13.77s = 99.63% of samples were caught.

Setting 2 samples per motion processing gave 3503 samples @ 250Hz = 14.012 seconds of flight. time.time() says it took 14.08s = 99.5% of samples were caught.

Setting 1 sample per motion processing gave 3502 samples @ 250Hz = 14.08 seconds of flight time. time.time() says it took 14.22s = 98.5% of samples were caught.

I’m guessing the slight decline is due to Linux scheduling; I chose to opt for 2 samples per motion processing, which updates the props at 125Hz or every 8ms.

And boy was the flight much smoother by having the more frequent, smaller increments to the props.

And I reckoned with these faster, less-lag updates to the motors, I might be able to trust the gyro readings for longer, so I changed the complementary filter tau (incrementally) to 5s from its previous 0.5s.

The sharp sighted of you may have already seen the results in the numbers: I’ve now breached my 10s flight time target by 2 seconds (the other couple of seconds is warm-up time), with the same level of drift I could only get in 6 second flights a few weeks ago. That 10s target was what I’d set myself to then look feeding other motion processing sensors based upon the RaspiCam, either laser (Kitty) or MPEG macro-block (Kitty++) tracking.

Only down side – it’s rained nearly all day, so no change of capturing one of these long flights on video. Perhaps tomorrow?

Motion sensors

Other than a few test flights, I’ve now put Phoebe on hold so as not to break her before the CotswoldJam at the end of September.  I’ve bought her a new case so I can carry her around safely:

Phoebe's Peli 1600 case

Phoebe’s Peli 1600 case

So this morning, I picked up doing motion processing using the RaspiCam YUV macro-block output – kitty++.py, partly triggered by a pingback yesterday.  The motion processing (in a very simple form) is working fine, but it only produces a CSV file as output allowing me to see the data as a graph in Excel:

Zig-zagging for 10 seconds

Zig-zagging for 10 seconds

Ideally for testing, I’d want a small screen to show the direction / speed the motion processing is detecting.  And as she’s headless, I’d like to add a button to press so that I can do various tests on demand while she’s headless.  In one of the twists of fate, the post brought my new E-paper HAT.  Here’s it installed on Kitty:

Kitty's E-paper screen

Kitty’s E-paper screen

The camera is stuck underneath in one of the super-slim cases I found.

I now need to install the drivers, and update the code to draw variable length arrows for the orientation / speed vector.

After that, I need to add the ultrasonic range finder to get the distance from the ground.  I’ve got a few of these – they’ll do for Kitty, but with their 100kbps I2C baudrate, they’re not good for Phoebe who needs 400kbps I2C baudrate to capture all the sensor data.

Should keep me out of trouble for a while!


I mentioned a year ago using the Raspberry Pi Camera to provide motion data; this motion data is a bi-product of the compression of h.264 video frames.

Each frame in a video is broken up into 16 x 16 pixel blocks (macro-blocks) which are compared to the blocks from the previous frame to find nearest matches.  Then an X, Y vectors is produced for each macro-blocks for the best matching block in the previous frame along with a score of absolute differences (SAD) – essentially a score of trustworthyness of the vector.  This happens at the frame rate for the video as part of the h.264 video compression algorithm, and is produced by the GPU alone.

Because of the fixed frame rate, these are effectively velocity vectors, and because the camera would be strapped under the quadcopter body, the velocities are in the quadcopter reference frame.  A crude processing of this data would simply be to average all the X,Y vectors based upon each SAD values to come up with a best guess estimate of an overall linear vector per frame.  Better processing would need to accommodate yaw also.

All this function is now available from the Python picamera interface making it very easy to create a motion tracking process which feeds the macro-block vectors + SAD data to the HoG code which can do the averaging and produce the quadframe velocity PID inputs for the X and Y axes.  The velocity PID targets are set as now to be earth-frame speeds reorientated to the quad-frame.


  • frame speed and processing on the GPU
  • no integration of (accelerometer – subtraction of gravity rotated to the quadframe) to get velocity hence no cumulative error from offsets


  • need some height information – possibly just best estimate as now

This opens up the possibility of indoor and outdoor motion processing over varying contrast surfaces, or laser tracking over surfaces without significant contrast difference.

First step is to adapt the existing Kitty code to churn out these motion vectors and stream them to a dummy module to produce the resultant velocity PID inputs.  More thoughts anon.

Motion tracking*

or Motion flow – the ability to track motion direction and speed – in this particular case, to fill the gap between the point where integration / calibration errors in the accelerometer are too large and the point where GPS becomes useful.

Here’s my overview of what’s required.

Altimeter / Baromoter – needed for level flight beyond the time frame that integrated accelerometer readings can be trusted – interesting because these two sets of data come from separate reference frames, and will need moving to the same frame, and merged with a complementary filter.

Compass / Magnetomer – needed for orientation and another source of pitch, roll and yaw angles because the compass has 3 axis so you can detect reorentation of the quad wrt the static location of the earth’s magnetic core.

Motion tracking – needed to track distances / directions beyond the scope of the integrated acclerometer readings’ accuracy and the point where GPS is useful:

  • RaspiCam per frame video movement vectors is the raw data (units of pixels measuring rotation, direction and distance traveled per frame, but then this needs to be interpreted from pixel movement into real units requiring
  • details of the lens focal length – i.e. how physically large is landscape the video is capturing at a given height from the ground meaning we need
  • height above the ground measurement – barometer will do for level ground, ultrasonic proximity will do for sloping / uneven ground, GPS will have to do beyond the scope of the ultrasound
  • Angles of pitch and roll – what the compass can kindly provide.

This definitely is sounding like a project of it’s own right, but which can be done in lots of little increments, some of which can be applied directly by adding new sensors to Phoebe / Chloe, and others done independently using one of my raspberry pi’s already decked up with a RaspiCam.

I’m starting to get excited by this as it allows me to progress while I wait for someone to come up with a half decent design for the MPU9250 + barometer to site on Chlöe’s Beret**.

*See these bits and bobs for more details.

**Beret – Breadboard Replacement Technology – A non-conformist HAT, lacking EEPROM, CSI and DSI slots.  Chlöe took offence at me calling her a Mad Hatter!