{"id":309,"date":"2014-07-29T16:37:24","date_gmt":"2014-07-29T16:37:24","guid":{"rendered":"http:\/\/www.pgoergen.de\/?p=309"},"modified":"2014-07-29T16:37:24","modified_gmt":"2014-07-29T16:37:24","slug":"raspberry-pi-ultrasonic-distance-sensor-part-ii","status":"publish","type":"post","link":"https:\/\/pgoergen.de\/de\/2014\/07\/raspberry-pi-ultrasonic-distance-sensor-part-ii\/","title":{"rendered":"Raspberry Pi – Ultrasonic Distance Sensor part II"},"content":{"rendered":"

Using python for the ultrasonic distance sensor from part one of this post<\/a> 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<\/a>. 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<\/a>):<\/p>\n

\r\n\/\/ code based on\r\n\/\/ http:\/\/stackoverflow.com\/questions\/22580242\/raspberrypi-g-ultrasonic-sensor-not-working\r\n#include<iostream>\r\n#include<wiringPi.h>\r\n#include<errno.h>\r\n#include<string.h>\r\n#include<stdio.h>\r\n#include<stdint.h>       \/\/for uint32_t\r\nusing namespace std;\r\nuint32_t time1=0,time2=0;\r\nuint32_t time_diff=0;\r\nfloat Range_cm=0;\r\nvolatile int flag=0;\r\nvoid show_distance(void);\r\n\r\nvoid myInterrupt(void) \/\/ Interrupt routine\r\n {                     \/\/ Called whenever the input pin toggles\r\n \tuint32_t timeTemp=micros();\r\n    if(flag==0) \/\/ first toggle? note the time\r\n      {\r\n            time1=timeTemp;\r\n            flag=1;\r\n\r\n      }\r\n    else \/\/ second toggle? compute the distance\r\n      {\r\n            time2=timeTemp;\r\n            flag=0;\r\n            time_diff=time2-time1;\r\n            Range_cm=time_diff\/58.;\r\n       }\r\n  }\r\nvoid show_distance()\/\/ writes the distance as a \r\n  {                 \/\/ binary float to stdout.\r\n\tfwrite(&Range_cm,sizeof(float),1,stdout); \r\n    cout.flush();\r\n  }\r\n\r\nint main(void)\r\n  {\r\n    if(wiringPiSetup()<0)\r\n     {\r\n       cout<<"wiringPiSetup failed !!\\n";\r\n     }\r\n    pinMode(4,OUTPUT);\r\n    pinMode(5,INPUT);\r\n    pullUpDnControl(5,PUD_DOWN);\r\n    if(wiringPiISR(5,INT_EDGE_BOTH,&myInterrupt) < 0)\r\n            {\r\n            cerr<<"interrupt error ["<<strerror (errno)<< "]:"<<errno<<endl;\r\n            return 1;\r\n            }\r\n\r\n    while(1) \/\/ this loop starts a new measurement\r\n    {        \/\/ every 2500 miliseconds\r\n    \ttime1=0;\r\n    \ttime2=0;\r\n    \tflag=0;\r\n\tdigitalWrite(4,0);\r\n\tdelayMicroseconds(1);\r\n\tdigitalWrite(4,1);\r\n\tdelayMicroseconds(10);\r\n\tdigitalWrite(4,0);\r\n\tdelayMicroseconds(25000);\r\n\tshow_distance();\r\n    }\r\n    return 0;\r\n }\r\n<\/pre>\n
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<\/a>):<\/p>\n
\r\n# License GPL\r\nimport gps\r\nimport threading\r\nimport subprocess\r\nimport time\r\nimport numpy\r\nimport os\r\n#from xdg.Menu import tmp\r\n# culd be beefed up with code from here http:\/\/www.danmandle.com\/blog\/getting-gpsd-to-work-with-python\/\r\n#GPS\r\nclass distanceMeter(threading.Thread):\r\n    # the init routine starts the compiled c-code\r\n    # at high priority and initializes variables\r\n    def __init__(self): \r\n        threading.Thread.__init__(self)\r\n        self.distanceProcess = subprocess.Popen(['.\/distance'],stdout=subprocess.PIPE, bufsize=500,preexec_fn=lambda : os.nice(-19))\r\n        self.running = True\r\n        self.currentdistance=0.0\r\n        self.lastFive=numpy.zeros(5)\r\n        self.lastFiveValid=numpy.zeros(5)\r\n        self.current=0\r\n        self.currentValid=0\r\n        print "created"\r\n    # the run method of the thread is an infinite loop:\r\n    #   whenever a distance measurement arrives\r\n    #   the result is checked for validity (changes by more\r\n    #   than 5 cm in 25msec are rejected as implausible for\r\n    #   my means). Then mean and standard deviation of last\r\n    #   five samples is computed.\r\n    def run(self):\r\n        print "starting loop"\r\n        while self.running:\r\n            # whenever new binary data comes in\r\n            # put it into a numpy float and\r\n            # process it.\r\n            rawdata=self.distanceProcess.stdout.read(4)\r\n            tmp=numpy.fromstring(rawdata,dtype="<f4")[-1] \r\n            if (numpy.abs(self.lastFive-tmp)<5).any():\r\n                self.currentdistance=tmp\r\n                self.lastFiveValid[self.currentValid%5]=tmp\r\n                self.currentValid+=1\r\n            self.lastFive[self.current%5]=tmp\r\n            self.current+=1\r\n    def stopController(self):\r\n        self.running = False\r\n        self.distanceProcess.send_signal(2)\r\n        self.distanceProcess.kill()\r\n    @property\r\n    def distance(self):\r\n        return numpy.mean(self.lastFiveValid)\r\n    @property\r\n    def deviation(self):\r\n        return numpy.std(self.lastFiveValid)\r\n# This is just to show how to use the the class above \r\nif __name__ == '__main__':\r\n    test=distanceReader()\r\n    test.start()\r\n    for i in numpy.arange(1000):\r\n        try:\r\n            print test.distance\r\n            time.sleep(.1)\r\n            # you can put as much code as you like here,\r\n            # test.distance will always contain the current\r\n            # distance measurement, updated every 25msec.\r\n        except KeyboardInterrupt:\r\n            test.stopController()\r\n            test.join()\r\n            break\r\n<\/pre>\n
With this I get reasonable distance measurements even while recording high resolution high quality video using the camera.<\/p>\n","protected":false},"excerpt":{"rendered":"
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 […]<\/a><\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[1],"tags":[],"class_list":["post-309","post","type-post","status-publish","format-standard","hentry","category-uncategorized"],"_links":{"self":[{"href":"https:\/\/pgoergen.de\/de\/wp-json\/wp\/v2\/posts\/309","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/pgoergen.de\/de\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/pgoergen.de\/de\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/pgoergen.de\/de\/wp-json\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/pgoergen.de\/de\/wp-json\/wp\/v2\/comments?post=309"}],"version-history":[{"count":5,"href":"https:\/\/pgoergen.de\/de\/wp-json\/wp\/v2\/posts\/309\/revisions"}],"predecessor-version":[{"id":316,"href":"https:\/\/pgoergen.de\/de\/wp-json\/wp\/v2\/posts\/309\/revisions\/316"}],"wp:attachment":[{"href":"https:\/\/pgoergen.de\/de\/wp-json\/wp\/v2\/media?parent=309"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/pgoergen.de\/de\/wp-json\/wp\/v2\/categories?post=309"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/pgoergen.de\/de\/wp-json\/wp\/v2\/tags?post=309"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}