├── .gitignore ├── LICENSE.md ├── README.md ├── display ├── gauge.py └── px4flow_display.py ├── px4flow ├── __init__.py └── mavlink_parser.py └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | PyPX4Flow 2 | ========= 3 | 4 | A simple Python package for reading from the PX4Flow optical-flow sensor 5 | 6 |

7 | 8 | I needed an easy way to access data from the PX4Flow sensor using 9 | Python, so I wrote this little package, which 10 | you can 11 | download from github. 12 | It runs on Windows, Linux, and 13 | OS X in Python 2 and 3. As with the other Python packages I've 14 | written, the API is very simple. For example, this program prints the computed X and Y flow velocities from the sensor: 15 | 16 | 17 | 

 18 | from px4flow import PX4Flow
 19 | 
 20 | class ShowFlow(PX4Flow):
 21 | 
 22 |     def update(self):
 23 | 
 24 |             print(self.getFlowComp())
 25 | 
 26 | 
 27 | if __name__ == '__main__':
 28 | 
 29 |     sensor = ShowFlow('/dev/ttyACM0')
 30 | 
 31 |     while True:
 32 | 
 33 |         sensor.refresh()
 34 | 
 35 |
36 | 37 | 38 | As this example shows, PX4Flow is an abstract class that you subclass with a class implementing the 39 | update method. To see the other methods avaiable, look at the 40 | documentation. 41 | 42 |

43 | 44 | I've also included a program that will display the sensor data in a useful way (depicted above) and log the data to 45 | a .CSV file for Excel. 46 | 47 |

48 | 49 |

Instructions

50 | 51 | You will need: 52 | 53 |
    54 |
  1. A PX4Flow kit. I recommend 55 | following the Image Quality and Output 56 | instructions to make sure the sensor is working properly. 57 |

  2. The ability to program in Python 58 |

  3. The PySerial package installed on your computer. 59 | Like many people, I found it easiest to install from source. 60 |

  4. Administrator (root) privileges on your computer 61 |

  5. The PyPX4Flow repository. 62 |
63 | 64 | Once you've downloaded the repositry, use a terminal (Linux or OS X) or command shell 65 | (Windows) to change to the directory where you put it, and issue the command 66 | 67 | 68 | 
 69 |   python setup.py install
 70 |
71 | 72 | 73 | On Linux or OS X, you may need to issue this command as root: 74 | 75 | 76 | 
 77 |   sudo python setup.py install
 78 |
79 | 80 | 81 | Then you should be able to run the px4flow_display.py program in the display folder. 82 | 83 |

84 | 85 |

Known issues

86 | 87 |
    88 | 89 |
  1. Some users have reported a bug in which the display program crashes immediately with a stack trace ending in the following output: 90 | 91 | 
     92 |  File "/usr/local/lib/python2.7/dist-packages/px4flow/mavlink_parser.py", line 99, in unpack
 93 |     return struct.unpack(fmt, self.msg[lo:hi])[0:n]
 94 | error: unpack requires a string argument of length 4
 95 |
    96 | 97 | So far this bug seem to occur only with versions of Python below 2.7.5, so I've added a 98 | branch to the repository that 99 | should work with such earlier versions. If you are running 2.7.5 or above and still experience this problem, 100 | please let me know. 101 | 102 |

  2. At startup, the display program occasionally computes and displays bogus large values for the distance traveled. 103 | This may have to do with the fact that I am not using the checksum to validate each message. 104 | 105 |
106 | 107 |

Copyright and licensing

108 | 109 | Copyright and licensing information (Gnu 110 | LGPL) 111 | can be found in the header of each source file. 112 | 113 |

Acknowledgments

114 | 115 | This work was supported in part by a Commonwealth Research Commercialization Fund 116 | grant from the Center for Innovative Technology (CRCF #MF14F-011-MS) and a 117 | Lenfest summer research grant from Washington and Lee University. 118 | 119 | 120 | -------------------------------------------------------------------------------- /display/gauge.py: -------------------------------------------------------------------------------- 1 | ''' 2 | gauge.py - Gauge-display classes for Tkinter projects 3 | 4 | Copyright (C) 2014 Simon D. Levy 5 | 6 | This program is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as 8 | published by the Free Software Foundation, either version 3 of the 9 | License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with this code. If not, see . 18 | ''' 19 | 20 | from sys import version 21 | 22 | if version[0] == '3': 23 | import tkinter as tk 24 | else: 25 | import Tkinter as tk 26 | 27 | class VerticalGauge(object): 28 | ''' 29 | A class for vertical gauges. 30 | ''' 31 | 32 | def __init__(self, canvas, left, bottom, width, height, color, label, minval, maxval, fmt): 33 | ''' 34 | Creates a labeled vertical gauge of specified dimensions on a specified canvas. 35 | FMT for min and max values is as for print(); for example '+%3.3f', '%d', etc. 36 | ''' 37 | 38 | right = left + width 39 | 40 | self.height = height 41 | self.canvas = canvas 42 | self.minval = minval 43 | self.maxval = maxval 44 | 45 | top = bottom - self.height 46 | bbox = (left, bottom, right, top) 47 | self.bbox = bbox 48 | self.rect = canvas.create_rectangle(bbox, fill=color) 49 | canvas.create_rectangle((bbox[0]-1, bbox[1]-1, bbox[2]+1, bbox[3]+1), outline='white') 50 | self._create_label(left, bottom+20, text=label) 51 | self._create_label(left-30, bottom, text= fmt % minval) 52 | self._create_label(left-30, top , text= fmt % maxval) 53 | 54 | def update(self, newval): 55 | ''' 56 | Updates the VerticalGauge with a new value. 57 | ''' 58 | 59 | new_height = self.height * (newval-self.minval) / (self.maxval - self.minval) 60 | bbox = self.bbox 61 | self.canvas.coords(self.rect, (bbox[0], bbox[1], bbox[2], bbox[1]-new_height)) 62 | 63 | def _create_label(self, x, y, text): 64 | 65 | return self.canvas.create_text(x, y, anchor=tk.W, font=('Helvetica', 12), fill='white', text=text) 66 | -------------------------------------------------------------------------------- /display/px4flow_display.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python 2 | 3 | ''' 4 | px4flow_display.py - Display / logging program for PyPX4Flow package 5 | 6 | Copyright (C) 2014 Simon D. Levy 7 | 8 | This program is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU Lesser General Public License as 10 | published by the Free Software Foundation, either version 3 of the 11 | License, or (at your option) any later version. 12 | 13 | This program is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public License 19 | along with this code. If not, see . 20 | ''' 21 | 22 | # Pick your port 23 | PORT = '/dev/ttyACM0' # Linux 24 | #PORT = 'COM5' # Windows 25 | #PORT = '/dev/tty.usbmodem1431' # Mac OS X 26 | 27 | # Sensor params (units are meters, seconds) 28 | 29 | GRND_DIST_MIN = 0.3 30 | GRND_DIST_MAX = 5.0 31 | 32 | VELOCITY_MAX = 5.0 33 | 34 | DISTANCE_MAX = 10 35 | 36 | # Display params (Width, Height, X, Y, Radius, Distance) 37 | 38 | DISPLAY_W = 1000 39 | DISPLAY_H = 300 40 | 41 | GAUGE_Y = DISPLAY_H - 60 42 | GAUGE_W = 90 43 | GAUGE_H = 200 44 | 45 | VELOCITY_X = 450 46 | VELOCITY_Y = DISPLAY_H / 2 47 | VELOCITY_R = 5 48 | VELOCITY_D = 100 49 | 50 | DISTANCE_X = 800 51 | DISTANCE_Y = VELOCITY_Y 52 | DISTANCE_D = VELOCITY_D 53 | DISTANCE_R = 5 54 | 55 | from px4flow import PX4Flow 56 | from gauge import VerticalGauge 57 | 58 | from sys import exit, version 59 | from time import time, strftime 60 | 61 | if version[0] == '3': 62 | import tkinter as tk 63 | else: 64 | import Tkinter as tk 65 | 66 | class PX4FlowPlotter: 67 | 68 | def __init__(self, reader): 69 | 70 | # Store the PX4Flow reader 71 | self.reader = reader 72 | 73 | # Set up the frame 74 | self.root = tk.Tk() 75 | self.frame = tk.Frame(self.root, borderwidth=4, width=DISPLAY_W, height=DISPLAY_H, relief='sunken') 76 | self.frame.master.title('PX4Flow: Hit ESC to quit') 77 | 78 | # Add a canvas for drawing 79 | self.canvas = tk.Canvas(self.frame, width = DISPLAY_W, height = DISPLAY_H, background = 'black') 80 | 81 | # Add a text item for reporting acquisition rate 82 | self.rate_label = self.create_label(DISPLAY_W-140, DISPLAY_H-20, color='red') 83 | 84 | # Add a gauge for grnd_dist 85 | self.grnd_dist_gauge = VerticalGauge(self.canvas, 40, GAUGE_Y, GAUGE_W, GAUGE_H, 'blue', 'Grnd dist (m)', GRND_DIST_MIN, GRND_DIST_MAX, '%2.1f') 86 | 87 | # Add a gauge for quality 88 | self.quality_gauge = VerticalGauge(self.canvas, 200, GAUGE_Y, GAUGE_W, GAUGE_H, 'yellow', ' Quality', 0, 255, '%d') 89 | 90 | # Add axes for velocity 91 | self.create_axis_line(-1, 0) 92 | self.create_axis_line(+1, 0) 93 | self.create_axis_line(0, -1) 94 | self.create_axis_line(0, +1) 95 | 96 | # Add labels for velocity 97 | self.create_label(VELOCITY_X+120, VELOCITY_Y, 'X m/sec') 98 | self.create_label(VELOCITY_X-30, DISPLAY_H-275, 'Y m/sec') 99 | 100 | # Add lines for displaying velocities 101 | self.x_line = self.create_null_line() 102 | self.y_line = self.create_null_line() 103 | 104 | # Add widgets for displaying distance traveled 105 | box_lx = DISTANCE_X - DISTANCE_D 106 | box_rx = DISTANCE_X + DISTANCE_D 107 | box_uy = DISTANCE_Y - DISTANCE_D 108 | box_ly = DISTANCE_Y + DISTANCE_D 109 | self.canvas.create_rectangle((box_lx, box_uy, box_rx, box_ly), outline='white') 110 | self.create_label(box_lx+DISTANCE_D/2+30, box_ly+20, 'X (m)') 111 | self.create_label(box_rx+10, DISTANCE_Y, 'Y (m)') 112 | self.create_label(box_rx+10, box_uy-10, '+%d' % DISTANCE_MAX) 113 | self.create_label(box_lx-30, box_ly+10, '-%d' % DISTANCE_MAX) 114 | self.location_circle = self.canvas.create_oval(self.location_to_oval(0, 0), width=1, outline='red') 115 | self.location_pix_prev = None 116 | 117 | # Pack the widgets into the canvas 118 | self.canvas.pack(fill=tk.BOTH, expand=1) 119 | 120 | # Set up a key event for exit on ESC 121 | self.frame.bind("", self.key) 122 | self.frame.pack() 123 | 124 | # This call gives the frame focus so that it receives input 125 | self.frame.focus_set() 126 | 127 | self.failcount = 0 128 | 129 | def location_to_oval(self, xpix, ypix): 130 | 131 | return xpix-DISTANCE_R, ypix-DISTANCE_R, xpix+DISTANCE_R, ypix+DISTANCE_R, 132 | 133 | def location_to_pixels(self): 134 | 135 | xpix = DISTANCE_X + (self.reader.X_accum/ DISTANCE_MAX) * DISTANCE_D 136 | ypix = DISTANCE_Y + (self.reader.Y_accum/ DISTANCE_MAX) * DISTANCE_D 137 | 138 | return xpix, ypix 139 | 140 | def create_null_line(self): 141 | 142 | return self.canvas.create_line(VELOCITY_X, VELOCITY_Y, VELOCITY_X, VELOCITY_Y, width=4) 143 | 144 | def create_axis_line(self, dx, dy): 145 | 146 | self.canvas.create_line(VELOCITY_X, VELOCITY_Y, VELOCITY_X+dx*VELOCITY_D, VELOCITY_Y+dy*VELOCITY_D, fill='white') 147 | 148 | def create_label(self, x, y, text=None, color='white'): 149 | 150 | return self.canvas.create_text(x, y, anchor=tk.W, font=('Helvetica', 12), fill=color, text=text) 151 | 152 | def run(self): 153 | 154 | # Start timing 155 | self.start_sec = time() 156 | 157 | # Start the recursive timer-task 158 | self.task() 159 | 160 | # Set Tkinter's main loop 161 | self.root.mainloop() 162 | 163 | def task(self): 164 | 165 | reader = self.reader 166 | 167 | # Refresh PX4Flow data 168 | reader.refresh() 169 | 170 | # Ground distance will be None on sensor fail 171 | grnd_dist = reader.H 172 | 173 | # Update sensor display 174 | if grnd_dist: 175 | 176 | xpix, ypix = self.location_to_pixels() 177 | self.canvas.coords(self.location_circle, self.location_to_oval(xpix, ypix)) 178 | if self.location_pix_prev: 179 | self.canvas.create_line(self.location_pix_prev[0], self.location_pix_prev[1], xpix, ypix, fill='red') 180 | self.location_pix_prev = xpix, ypix 181 | 182 | self.grnd_dist_gauge.update(grnd_dist) 183 | self.quality_gauge.update(reader.Quality) 184 | 185 | x = VELOCITY_X + 2 186 | y = VELOCITY_Y + 2 187 | 188 | self.canvas.coords(self.x_line, (x, y, int(VELOCITY_X+reader.X/VELOCITY_MAX*VELOCITY_D), y)) 189 | self.canvas.itemconfigure(self.x_line, fill = 'red' if reader.X < 0 else 'green') 190 | 191 | self.canvas.coords(self.y_line, (x, y, x, int(VELOCITY_Y+reader.Y/VELOCITY_MAX*VELOCITY_D))) 192 | self.canvas.itemconfigure(self.y_line, fill = 'red' if reader.Y < 0 else 'green') 193 | 194 | else: 195 | self.failcount += 1 196 | print('Fail %d' % self.failcount) 197 | 198 | # Report speed after several seconds 199 | elapsed_sec = time() - self.start_sec 200 | if elapsed_sec > 5: 201 | self.canvas.itemconfigure(self.rate_label, text='%d updates / sec' % \ 202 | int(reader.count/elapsed_sec)) 203 | 204 | # Reschedule this task immediately 205 | self.frame.after(1, self.task) 206 | 207 | def click(self, event): 208 | print("Clicked at: ", event.x, event.y) 209 | 210 | def key(self, event): 211 | 212 | # Make sure the frame is receiving input! 213 | self.frame.focus_force() 214 | if event.keysym == 'Escape': 215 | exit(0) 216 | 217 | class PX4FlowReader(PX4Flow): 218 | 219 | def __init__(self, port): 220 | 221 | PX4Flow.__init__(self, port) 222 | 223 | # No readings yet 224 | self.SensorX,self.SensorY = None, None 225 | self.X,self.Y = None, None 226 | self.H = None 227 | self.Quality = None 228 | 229 | # Create logfile named by current date / time 230 | filename = 'px4flow_' + strftime('%d_%b_%Y_%H_%M_%S') + '.csv' 231 | self.logfile = open(filename, 'w') 232 | self.write_and_flush('Time (sec), Ground Dist (m), Flow X, Flow Y, Flow Comp X (m), Flow Comp Y (m), Quality (/255),,') 233 | self.write_and_flush('X accum (m), Y accum (m)\n') 234 | 235 | # These will get the accumulated X,Y distances in meters 236 | self.timeSecPrev = None 237 | self.X_accum = 0 238 | self.Y_accum = 0 239 | 240 | self.count = 0 241 | 242 | # Implements missing method in parent class 243 | def update(self): 244 | 245 | # Grab raw sensor X,Y 246 | self.SensorX,self.SensorY = self.getFlow() 247 | 248 | # Grab computed X,Y in meters 249 | self.X,self.Y = self.getFlowComp() 250 | 251 | # Grab ground distance (height) in meters 252 | self.H = self.getGroundDistance() 253 | 254 | # Grab quality in percent 255 | self.Quality = self.getQuality() 256 | 257 | # Time in seconds 258 | timeSec = self.getTime() / 1e6 259 | 260 | # Computed flow in meters per second 261 | flowCompX, flowCompY = self.getFlowComp() 262 | 263 | self.write_and_flush('%6.3f, %+3.3f, %d, %d, %+3.3f, %+3.3f, %d' % \ 264 | (timeSec, self.H, self.SensorX, self.SensorY, self.X, self.Y, self.Quality)) 265 | 266 | # After first iteration, compute accumualted distances 267 | if self.count: 268 | 269 | # Compute distance if elapsed time available 270 | if self.timeSecPrev: 271 | 272 | elapsedSec = timeSec - self.timeSecPrev 273 | 274 | # Elapsed time should never be more than a small fraction of a second 275 | if elapsedSec < 0.1: 276 | self.X_accum += flowCompX * elapsedSec 277 | self.Y_accum += flowCompY * elapsedSec 278 | 279 | self.timeSecPrev = timeSec 280 | 281 | self.write_and_flush(',,%+3.3f, %+3.3f' % (self.X_accum, self.Y_accum)) 282 | 283 | self.write_and_flush('\n') 284 | 285 | # Update count for speed reporting 286 | self.count += 1 287 | self.timeSecPrev = timeSec 288 | 289 | def write_and_flush(self, s): 290 | 291 | self.logfile.write(s) 292 | self.logfile.flush() 293 | 294 | if __name__ == "__main__": 295 | 296 | reader = PX4FlowReader(PORT) 297 | 298 | plotter = PX4FlowPlotter(reader) 299 | 300 | plotter.run() 301 | -------------------------------------------------------------------------------- /px4flow/__init__.py: -------------------------------------------------------------------------------- 1 | ''' 2 | PX4Flow - a Python class for collecting data from the PX4Flow sensor 3 | 4 | Copyright (C) 2014 Simon D. Levy 5 | 6 | This code is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as 8 | published by the Free Software Foundation, either version 3 of the 9 | License, or (at your option) any later version. 10 | 11 | This code is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with this code. If not, see . 18 | ''' 19 | 20 | # Message ID fom https://pixhawk.ethz.ch/mavlink/ 21 | MSG_OPTICAL_FLOW = 100 22 | 23 | # Arbitrary 24 | _BUFSIZE = 2048 25 | 26 | import serial 27 | import platform 28 | 29 | from px4flow.mavlink_parser import MAVLinkParser 30 | 31 | class PX4Flow(MAVLinkParser): 32 | ''' 33 | An abstract class for reading from the PX4Flow optical flow sensor. Your subclass should provide an 34 | update(self) method that calls one or more of the accessor methods below. 35 | ''' 36 | 37 | def __init__(self, port): 38 | ''' 39 | Creates a new PX4Flow object on the specified port. 40 | ''' 41 | 42 | # Baud rate is unspecified 43 | self.dev = serial.Serial(port) 44 | 45 | # Create MAVLink object for parsing 46 | MAVLinkParser.__init__(self, self, MSG_OPTICAL_FLOW) 47 | 48 | # Require refresh before first reading 49 | self._refreshed = False 50 | 51 | def close(self): 52 | ''' 53 | Closes the port on which the sensor was opened. 54 | ''' 55 | self.dev.close() 56 | 57 | def refresh(self): 58 | ''' 59 | Refreshes the optical flow reading. 60 | ''' 61 | 62 | self._refreshed = True 63 | 64 | # Grab bytes from the device 65 | bytes = self.dev.read(_BUFSIZE) 66 | 67 | # Check for MAVLINK messages in bytes 68 | MAVLinkParser.process(self, bytes) 69 | 70 | def getFlow(self): 71 | ''' 72 | Returns raw sensor X,Y. 73 | ''' 74 | 75 | self._check_refreshed() 76 | 77 | return MAVLinkParser.unpack(self, 'hh', 20, 24, 2) 78 | 79 | def getFlowComp(self): 80 | ''' 81 | Returns computed X,Y in meters. 82 | ''' 83 | 84 | self._check_refreshed() 85 | 86 | return MAVLinkParser.unpack(self, 'ff', 8, 16, 2) 87 | 88 | def getGroundDistance(self): 89 | ''' 90 | Returns ground distance (height) in meters. 91 | ''' 92 | 93 | self._check_refreshed() 94 | 95 | return MAVLinkParser.unpack1(self, 'f', 16, 20) 96 | 97 | def getQuality(self): 98 | ''' 99 | Returns quality in percent. 100 | ''' 101 | 102 | self._check_refreshed() 103 | 104 | return MAVLinkParser.unpack_uint8(self, 25) 105 | 106 | def getTime(self): 107 | ''' 108 | Returns current time in microseconds. 109 | ''' 110 | 111 | self._check_refreshed() 112 | 113 | return MAVLinkParser.unpack1(self, 'Q', 0, 8) 114 | 115 | def _check_refreshed(self): 116 | 117 | if not self._refreshed: 118 | 119 | raise Exception('Attempt to read without refresh()') 120 | -------------------------------------------------------------------------------- /px4flow/mavlink_parser.py: -------------------------------------------------------------------------------- 1 | ''' 2 | mavlink_parser.py - Simon's homebrew code for parsing MAVLink messages. 3 | DOES NOT VERIFY CHECKSUM!!! 4 | 5 | Based on http://en.wikipedia.org/wiki/MAVLink, but hoping to replace it 6 | with pymavlink, so we can get checksum and other features. 7 | 8 | Copyright (C) 2014 Simon D. Levy 9 | 10 | This code is free software: you can redistribute it and/or modify 11 | it under the terms of the GNU Lesser General Public License as 12 | published by the Free Software Foundation, either version 3 of the 13 | License, or (at your option) any later version. 14 | 15 | This code is distributed in the hope that it will be useful, 16 | but WITHOUT ANY WARRANTY without even the implied warranty of 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | GNU General Public License for more details. 19 | 20 | You should have received a copy of the GNU Lesser General Public License 21 | along with this code. If not, see . 22 | ''' 23 | 24 | import struct 25 | 26 | # States for message parsing 27 | # From http://qgroundcontrol.org/mavlink/start#packet_anatomy 28 | STATE_DFLT = 0 29 | STATE_STX = 1 30 | STATE_LEN = 2 31 | STATE_SEQ = 3 32 | STATE_SYS = 4 33 | STATE_COMP = 5 34 | STATE_MSG = 6 35 | STATE_CKA = 7 36 | STATE_CKB = 8 37 | 38 | # STX delimiter byte for Mavlink 1.0 39 | STX_BYTE = 0XFE 40 | 41 | class MAVLinkParser(object): 42 | 43 | def process(self, buf): 44 | 45 | for b in bytearray(buf): 46 | 47 | if b == STX_BYTE: 48 | self.state = STATE_STX 49 | 50 | elif self.state == STATE_STX: 51 | self.msglen = b 52 | self.state = STATE_LEN 53 | 54 | elif self.state == STATE_LEN: 55 | self.state = STATE_SEQ 56 | 57 | elif self.state == STATE_SEQ: 58 | self.state = STATE_SYS 59 | 60 | elif self.state == STATE_SYS: 61 | self.state = STATE_COMP 62 | 63 | elif self.state == STATE_COMP: 64 | self.msgid = b 65 | self.msg = bytearray('', 'utf8') 66 | self.state = STATE_MSG 67 | 68 | elif self.state == STATE_MSG: 69 | self.msg.append(b) 70 | if len(self.msg) == self.msglen: 71 | self.state = STATE_CKA 72 | 73 | elif self.state == STATE_CKA: 74 | self.state = STATE_CKB 75 | 76 | elif self.state == STATE_CKB: 77 | if self.msgid == self.tgtid: 78 | self.handler.update() 79 | self.msg = bytearray('', 'utf8') 80 | self.state = STATE_DFLT 81 | 82 | def __init__(self, handler, targetid): 83 | 84 | self.state = STATE_DFLT 85 | self.msglen = 0 86 | self.msgid = 0 87 | self.msg = bytearray('', 'utf8') 88 | 89 | self.handler = handler 90 | self.tgtid = targetid 91 | 92 | def unpack_uint8(self, lo): 93 | return self.unpack1('B', lo, lo+1) 94 | 95 | def unpack1(self, fmt, lo, hi): 96 | return self.unpack(fmt, lo, hi, 1)[0] 97 | 98 | def unpack(self, fmt, lo, hi, n): 99 | return struct.unpack(fmt, self.msg[lo:hi])[0:n] 100 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | setup.py - Python distutils setup file for PyPX4Flow package. 5 | 6 | Copyright (C) 2014 Simon D. Levy 7 | 8 | This code is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU Lesser General Public License as 10 | published by the Free Software Foundation, either version 3 of the 11 | License, or (at your option) any later version. 12 | 13 | This code is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public License 19 | along with this code. If not, see . 20 | ''' 21 | 22 | from distutils.core import setup 23 | 24 | setup (name = 'PyPX4Flow', 25 | version = '0.1', 26 | description = 'PX4Flow sensor utility', 27 | packages = ['px4flow'], 28 | author='Simon D. Levy', 29 | author_email='simon.d.levy@gmail.com', 30 | url='http://home.wlu.edu/~levys/software/pypx4flow', 31 | license='LGPL', 32 | platforms='Linux; Windows; OS X' 33 | ) 34 | --------------------------------------------------------------------------------