Camera motion tracking code

I’ve reworked the code so that the video is collected in a daemonized process and fed into a shared memory FIFO. The main code for processing the camera output is now organised to be easily integrated into the quadcopter code. This’ll happen for Hermione once I’ve built her with a new PCB and 512MB memory A+. I suspect the processing overhead of this is very light, given that most of the video processing happens in the GPU, and the motion processing is some very simple averaging.

#!/usr/bin/python
from __future__ import division
import os
import sys
import picamera
import select
import struct
import subprocess
import signal
import time

####################################################################################################
#
# Motion.py - motion tracker based upon video-frame macro-blocks.  Note this happens per flight - 
#             nothing is carried between flights when this is merged with the quadcopter code.
#
####################################################################################################

#--------------------------------------------------------------------------------------------------
# Video at 10fps. Each frame is 320 x 320 pixels.  Each macro-block is 16 x 16 pixels.  Due to an 
# extra column of macro-blocks (dunno why), that means each frame breaks down into 21 columns by 
# 20 rows = 420 macro-blocks, each of which is 4 bytes - 1 signed byte X, 1 signed byte Y and 2 unsigned
# bytes SAD (sum of absolute differences). 
#--------------------------------------------------------------------------------------------------
def RecordVideo():
    print "Video process: started"
    with picamera.PiCamera() as camera:
        camera.resolution = (320, 320)
        camera.framerate = 10

        camera.start_recording('/dev/null', format='h264', motion_output="/dev/shm/motion_stream", quality=23)

        try:
            while True:
                camera.wait_recording(1.0)
        except KeyboardInterrupt:
            pass
        finally:            
            try:
                camera.stop_recording()
            except IOError:
                pass
    print "Video process: stopped"

#---------------------------------------------------------------------------------------------------
# Check if I am the video process            
#---------------------------------------------------------------------------------------------------
if len(sys.argv) > 1 and sys.argv[1] == "video":
    RecordVideo()
    sys.exit()

#---------------------------------------------------------------------------------------------------
# Setup a shared memory based data stream for the PiCamera video motion output
#---------------------------------------------------------------------------------------------------
os.mkfifo("/dev/shm/motion_stream")

#---------------------------------------------------------------------------------------------------
# Start up the video camera as a new process. Run it in its own process group so that Ctrl-C doesn't
# get through.
#---------------------------------------------------------------------------------------------------
def Daemonize():
    os.setpgrp()
video = subprocess.Popen(["python", "motion.py", "video"], preexec_fn =  Daemonize)

#---------------------------------------------------------------------------------------------------
# Off we go; set up the format for parsing a frame of macro blocks
#---------------------------------------------------------------------------------------------------
format = '=' + 'bbH' * int(1680 / 4)

#---------------------------------------------------------------------------------------------------
# Wait until we can open the FIFO from the video process
#---------------------------------------------------------------------------------------------------
camera_installed = True
read_list = []
write_list = []
exception_list = []

if camera_installed:
    while True:
        try:
            py_fifo = open("/dev/shm/motion_stream", "rb")
        except:
            continue
        else:
            break
    print "Main process: fifo opened"
    read_list = [py_fifo]


motion_data = open("motion_data.csv", "w")
motion_data.write("idx, idy, adx, ady, sad\n")

total_bytes = 0
frame_rate = 10
scale = 10000

#---------------------------------------------------------------------------------------------------
# Per frame distance and velocity increments
#---------------------------------------------------------------------------------------------------
ivx = 0.0
ivy = 0.0
idx = 0.0
idy = 0.0

#---------------------------------------------------------------------------------------------------
# Per flight absolute distance and velocity integrals
#---------------------------------------------------------------------------------------------------
avx = 0.0
avy = 0.0
adx = 0.0
ady = 0.0

start_time = time.time()

try:
    while True:
        #-------------------------------------------------------------------------------------------
        # The sleep time for select is defined by how many batches of data are sitting in the FIFO
        # compared to how many we want to process per motion processing (samples per motion)
        #
        # timeout = (samples_per_motion - mpu6050.numFIFOSamles()) / sampling_rate
        # check for negative timeout.
        #-------------------------------------------------------------------------------------------

        #-------------------------------------------------------------------------------------------
        # Wait for the next whole frame
        #-------------------------------------------------------------------------------------------
        read_out, write_out, exception_out = select.select(read_list, write_list, exception_list)

        if camera_installed and len(read_out) != 0:
            #---------------------------------------------------------------------------------------
            # We have new data on the video FIFO; get it, and make sure it really is a whole frame of 
            # 21 columns x 20 rows x 4 bytes for macro block.
            #---------------------------------------------------------------------------------------
            frame = py_fifo.read(1680)
            if len(frame) != 1680:
                print "ERROR: incomplete frame received"
                break

            #---------------------------------------------------------------------------------------
            # Convert to byte, byte, ushort of x, y, sad
            #---------------------------------------------------------------------------------------
            iframe = struct.unpack(format, frame)

            #---------------------------------------------------------------------------------------
            # Iterate through the 21 x 20 macro blocks averaging the X and Y vectors of the frame based
            # upon the SAD (sum of absolute differences, lower is better).  
            #---------------------------------------------------------------------------------------
            ivx = 0.0
            ivy = 0.0
            sad = 0

            for ii in range(0, 420 * 3, 3):
                ivy += iframe[ii]
                ivx += iframe[ii + 1]
                sad += iframe[ii + 2]

            #---------------------------------------------------------------------------------------
            # Scale the macro block values to the speed increment in meters per second
            #---------------------------------------------------------------------------------------
            ivx /= scale
            ivy /= scale     

            #---------------------------------------------------------------------------------------
            # Use the frame rate to convert velocity increment to distance increment
            #---------------------------------------------------------------------------------------
            idt = 1 / frame_rate
            idx = ivx * idt
            idy = ivy * idt

            #---------------------------------------------------------------------------------------
            # Integrate (sum due to fixed frame rate) the increments to produce total distance and velocity.
            # Note that when producing the diagnostic earth frame distance, it's the increment in distance
            # that's rotated and added to the total ed*
            #---------------------------------------------------------------------------------------
            avx += ivx
            avy += ivy
            adx += idx
            ady += idy

            time_stamp = time.time() - start_time

            motion_data.write("%f, %f, %f, %f, %f, %d\n" % (time_stamp, idx, idy, adx, ady, sad))

except:
    pass


#---------------------------------------------------------------------------------------------------
# Stop the video process
#---------------------------------------------------------------------------------------------------
video.send_signal(signal.SIGINT)

motion_data.flush()
motion_data.close()

py_fifo.close()
os.unlink("/dev/shm/motion_stream")

 

‘Scoping the Modbus

Using the LEDDAR One on Phoebe, I had to drop her sampling rate to 500Hz to prevent FIFO overflow.  This isn’t bad, but I decided to investigate why adding LEDDAR into the mix has slowed the processing down significantly.  Out with my OSCIUM iMSO-104 iPad scope.

The following allows a crude estimate of the the baud rate to be at least 50kbps:

Baudrate

Baudrate

This shows the time the LEDDAR One takes for a request / response processing – about 3ms

Request / Response timing

Request / Response timing

This shows the reaction time to the rising edge data ready interrupt (DRI); since the Quadcopter and the test code only poll the interrupt every 10ms, this is fine too.

Interrupt response

Interrupt response

What’s also visible is the cost of doing 3 individual reads of the registers (about 20ms) compared to reading 5 registers in a single shot which drops it down to about 5ms as shown below:

One-shot registers read

One-shot registers read

I’ve swapped Phoebe to use the one-shot read as above, and as a result, she’s running the IMU at 1kHz again while running the LEDDAR.

Sober as a judge.

Phoebe is no longer legless:

Phoebe's new legs

Phoebe’s new legs

Finally found legs I like – strong, flexible, and anchored securely to the frame without risking damaging it. Search e-bay for Tarot TL2749-02 for the brackets which are the critical bits. The legs and feet are available from Tarot too, but others are available – they just need to have clips for 8mm CF rods at the bottom and 10mm clips at the top which attach to the brackets.

The only problem with these and all other legs is that with any drift, then Phoebe trips over her toes on landing – the few flights I tested these this morning, I killed the flight just as she was a centimetre or two off the ground so when she tripped, her props were not powered up.

I2C is error proof

Yesterday was proof of error, today is proof of the protective fix:

    def readSensors(self):
        #-------------------------------------------------------------------------------------------
        # For the sake of always getting good date wrap the interrupt and the register read in a try
        # except loop.
        #-------------------------------------------------------------------------------------------
        while True:
            try:
                #-----------------------------------------------------------------------------------
                # Wait for the data ready interrupt
                #-----------------------------------------------------------------------------------
                RPIO.edge_detect_wait(RPIO_DATA_READY_INTERRUPT)

                #-----------------------------------------------------------------------------------
                # For speed of reading, read all the sensors and parse to SHORTs after.  This also
                # ensures a self consistent set of sensor data compared to reading each individually
                # where the sensor data registers could be updated between reads.
                #-----------------------------------------------------------------------------------
                sensor_data = self.i2c.readList(self.__MPU6050_RA_ACCEL_XOUT_H, 14)
            except:
                self.num_i2c_errs += 1
                continue

            #---------------------------------------------------------------------------------------
            # Convert the array of 14 bytes to 7 shorts
            #---------------------------------------------------------------------------------------
            for index in range(0, 14, 2):
                if (sensor_data[index] > 127):
                    sensor_data[index] -= 256
                self.result_array[int(index / 2)] = (sensor_data[index] << 8) + sensor_data[index + 1]

            #-------------------------------------------------------------------------------------------
            # +/- 4g * 16 bit range for the accelerometer
            # +/- 250 degrees per second * 16 bit range for the gyroscope
            #-------------------------------------------------------------------------------------------
            [ax, ay, az, temp, gx, gy, gz] = self.result_array
            if gz == -1 and gy == -1 and gx == -1 and temp == -1 and az == -1:
               self.num_data_errs += 1
               continue 
            break

        return ax, ay, az, gx, gy, gz

And here’s the proof duff data was caught triggering another sampling loop:

lps: 990.767934
mpu6050 2 misses, i2c 3 misses

Next step is to take her out and fly her!

What do you get if you cross an accelerometer with an ultrasonic range finder?

Fusion.

Pink is the range finder distance differentiated against time giving velocity in the quad Z axis – note how spikey it is.

Green is the accelerometer output integrated over time giving velocity in the quad Z axis – note that it’s drifting downwards between 6 and 10 seconds when the quad is actually hovering – I know because I’m holding it in my hands.

Orange is what you get when you fuse these together with a complementary filter – note it doesn’t drift downwards, and much of the spikiness is gone.

Velocity fusion

Velocity fusion

Time for real test flight tomorrow – weather is looking good too.

à bientôt

Off on family hols so it’s going to be quiet here for a bit.  Back in a week.

TTFN

And no, we’re not off to France – we’re going camping to Devon with the kids.  It’s going to be hell, and the weather is crap too. 🙁

Trends

Gyro and accelerometer trends

Gyro and accelerometer trends

Just a quicky: I spent yesterday gathering these higher quality trends mapping gyro and accelerometer offsets to temperature.

The trend lines are a lot more closely tied to the data points, so hopefully that’ll mean more accuracy.  I’ll give it a try today or tomorrow – I’m hoping that the net result will be even more limited drift, plus longer flight (different sides of the same coin essentially).

I used to be indecisive…

but now I’m not so sure…

whether I actually need a horizontal acceleration PID.  I’d considered adding the horizontal acceleration PID between the horizontal speed PID and the pitch / roll angle PIDs.  Its output would set the angle target for those PIDs, but I’ve come to the conclusion that there is a direct mathematical link between horizontal acceleration (the output from the horizontal speed PID) and the tilt angle target set for the angle PIDs

Consider we want 1g acceleration horizontally, and 1g acceleration vertically to give us horizontal acceleration – i.e. no rise or fall in flight height.  Then the power must be applied equally horizontally and vertically, meaning a 45º tilt.  Or putting it slightly clearer, the acceleration from the accelerometer is measured in g’s, so if we setup the gains of the horizontal speed PID to output in (approximate) g’s, and denote earth axis acceleration along the x and z axes as aeax and aeaz respectively then

tan(θ) = aeax / aeaz

But for horizontal flight aeaz = 1g. So

θ = atan(aeax)

This means the target acceleration from the horizontal speed PID output (i.e. target acceleration) if measured in units of g maps directly to a target angle for the pitch / roll angle PIDs which is “a good thing”TM as it’s much better than adding a noisy accelerometer PID into the system.