Using python for the ultrasonic distance sensor from part one of this post in a project I realized: It is still too slow. In my project I record high queality video using the raspberry pi camera at high resolution. That puts quite some load on the IO. As a result, the timing needed for the distance measurement becomes unreliable. Results vary by more than a meter shot-to shot (without changing the distance to the first obstacle). For higher timing accuracy I decided to do the distance measurement in C or C++. I stole some interrupt based c++ code from here. I modified it to have it measure continuously. The code does one measuerement every 25miliseconds and outputs the result as a binary float to stdout. The result looks like this (download here):

// code based on
// http://stackoverflow.com/questions/22580242/raspberrypi-g-ultrasonic-sensor-not-working
#include<iostream>
#include<wiringPi.h>
#include<errno.h>
#include<string.h>
#include<stdio.h>
#include<stdint.h>       //for uint32_t
using namespace std;
uint32_t time1=0,time2=0;
uint32_t time_diff=0;
float Range_cm=0;
volatile int flag=0;
void show_distance(void);

void myInterrupt(void) // Interrupt routine
 {                     // Called whenever the input pin toggles
 	uint32_t timeTemp=micros();
    if(flag==0) // first toggle? note the time
      {
            time1=timeTemp;
            flag=1;

      }
    else // second toggle? compute the distance
      {
            time2=timeTemp;
            flag=0;
            time_diff=time2-time1;
            Range_cm=time_diff/58.;
       }
  }
void show_distance()// writes the distance as a 
  {                 // binary float to stdout.
	fwrite(&Range_cm,sizeof(float),1,stdout); 
    cout.flush();
  }

int main(void)
  {
    if(wiringPiSetup()<0)
     {
       cout<<"wiringPiSetup failed !!\n";
     }
    pinMode(4,OUTPUT);
    pinMode(5,INPUT);
    pullUpDnControl(5,PUD_DOWN);
    if(wiringPiISR(5,INT_EDGE_BOTH,&myInterrupt) < 0)
            {
            cerr<<"interrupt error ["<<strerror (errno)<< "]:"<<errno<<endl;
            return 1;
            }

    while(1) // this loop starts a new measurement
    {        // every 2500 miliseconds
    	time1=0;
    	time2=0;
    	flag=0;
	digitalWrite(4,0);
	delayMicroseconds(1);
	digitalWrite(4,1);
	delayMicroseconds(10);
	digitalWrite(4,0);
	delayMicroseconds(25000);
	show_distance();
    }
    return 0;
 }

Now we just need to modify our python class to read the regularly arriving distance from the c-code instead of measuring it itsself. To do so I use the following code (download distanceMeter1.py):

# License GPL
import gps
import threading
import subprocess
import time
import numpy
import os
#from xdg.Menu import tmp
# culd be beefed up with code from here http://www.danmandle.com/blog/getting-gpsd-to-work-with-python/
#GPS
class distanceMeter(threading.Thread):
    # the init routine starts the compiled c-code
    # at high priority and initializes variables
    def __init__(self): 
        threading.Thread.__init__(self)
        self.distanceProcess = subprocess.Popen(['./distance'],stdout=subprocess.PIPE, bufsize=500,preexec_fn=lambda : os.nice(-19))
        self.running = True
        self.currentdistance=0.0
        self.lastFive=numpy.zeros(5)
        self.lastFiveValid=numpy.zeros(5)
        self.current=0
        self.currentValid=0
        print "created"
    # the run method of the thread is an infinite loop:
    #   whenever a distance measurement arrives
    #   the result is checked for validity (changes by more
    #   than 5 cm in 25msec are rejected as implausible for
    #   my means). Then mean and standard deviation of last
    #   five samples is computed.
    def run(self):
        print "starting loop"
        while self.running:
            # whenever new binary data comes in
            # put it into a numpy float and
            # process it.
            rawdata=self.distanceProcess.stdout.read(4)
            tmp=numpy.fromstring(rawdata,dtype="<f4")[-1] 
            if (numpy.abs(self.lastFive-tmp)<5).any():
                self.currentdistance=tmp
                self.lastFiveValid[self.currentValid%5]=tmp
                self.currentValid+=1
            self.lastFive[self.current%5]=tmp
            self.current+=1
    def stopController(self):
        self.running = False
        self.distanceProcess.send_signal(2)
        self.distanceProcess.kill()
    @property
    def distance(self):
        return numpy.mean(self.lastFiveValid)
    @property
    def deviation(self):
        return numpy.std(self.lastFiveValid)
# This is just to show how to use the the class above 
if __name__ == '__main__':
    test=distanceReader()
    test.start()
    for i in numpy.arange(1000):
        try:
            print test.distance
            time.sleep(.1)
            # you can put as much code as you like here,
            # test.distance will always contain the current
            # distance measurement, updated every 25msec.
        except KeyboardInterrupt:
            test.stopController()
            test.join()
            break

With this I get reasonable distance measurements even while recording high resolution high quality video using the camera.