├── 20s_hover_fp.csv
├── 2m_square_fp.csv
├── GPS_spec.pdf
├── IMU_spec.pdf
├── Joystick_spec.pdf
├── LICENSE
├── LiDAR_spec.pdf
├── LiDARv3HP_spec.pdf
├── PhoebePresentationCamJamSept14.odp
├── Quadcopter.py
├── RC.py
├── README.md
├── RPIO.tgz
├── RPi0-PCB.brd
├── RPi3-PCB.brd
├── Sweep_spec.pdf
├── piDrones.pdf
├── pidrone
├── qc.py
└── smbus2.zip
/20s_hover_fp.csv:
--------------------------------------------------------------------------------
1 | # - evx is fore / aft velocity in m/s; fore is positive
2 | # - evy is port / starboard velocity in m/s; port is positive
3 | # - evz is up / down velocity in m/s; up is positive
4 | # - time is how long to maintain that speed in seconds
5 | # - name is an arbitrary name put out to console to show flight progress
6 |
7 | # evx, evy, evz, time, name
8 | 0.0, 0.0, 0.0, 20.0, HOVER
9 |
--------------------------------------------------------------------------------
/2m_square_fp.csv:
--------------------------------------------------------------------------------
1 | # - evx is fore / aft velocity in m/s; fore is positive
2 | # - evy is port / starboard velocity in m/s; port is positive
3 | # - evz is up / down velocity in m/s; up is positive
4 | # - time is how long to maintain that speed in seconds
5 | # - name is an arbitrary name put out to console to show flight progress
6 |
7 | # evx, evy, evz, time, name
8 |
9 | 0.0, 0.0, 0.0, 1.0, HOVER
10 | 0.25, 0.0, 0.0, 4.0, FORE
11 | 0.0, 0.0, 0.0, 1.0, HOVER
12 | 0.0, 0.25, 0.0, 4.0, PORT
13 | 0.0, 0.0, 0.0, 1.0, HOVER
14 | -0.25, 0.0, 0.0, 4.0, AFT
15 | 0.0, 0.0, 0.0, 1.0, HOVER
16 | 0.0,-0.25, 0.0, 4.0, STARBOARD
17 | 0.0, 0.0, 0.0, 1.0, HOVER
18 |
--------------------------------------------------------------------------------
/GPS_spec.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PiStuffing/Quadcopter/fa3a2e7b0d37d54ff5acc32c6752d3a31834240e/GPS_spec.pdf
--------------------------------------------------------------------------------
/IMU_spec.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PiStuffing/Quadcopter/fa3a2e7b0d37d54ff5acc32c6752d3a31834240e/IMU_spec.pdf
--------------------------------------------------------------------------------
/Joystick_spec.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PiStuffing/Quadcopter/fa3a2e7b0d37d54ff5acc32c6752d3a31834240e/Joystick_spec.pdf
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | GNU GENERAL PUBLIC LICENSE
2 | Version 2, June 1991
3 |
4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc.,
5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
6 | Everyone is permitted to copy and distribute verbatim copies
7 | of this license document, but changing it is not allowed.
8 |
9 | Preamble
10 |
11 | The licenses for most software are designed to take away your
12 | freedom to share and change it. By contrast, the GNU General Public
13 | License is intended to guarantee your freedom to share and change free
14 | software--to make sure the software is free for all its users. This
15 | General Public License applies to most of the Free Software
16 | Foundation's software and to any other program whose authors commit to
17 | using it. (Some other Free Software Foundation software is covered by
18 | the GNU Lesser General Public License instead.) You can apply it to
19 | your programs, too.
20 |
21 | When we speak of free software, we are referring to freedom, not
22 | price. Our General Public Licenses are designed to make sure that you
23 | have the freedom to distribute copies of free software (and charge for
24 | this service if you wish), that you receive source code or can get it
25 | if you want it, that you can change the software or use pieces of it
26 | in new free programs; and that you know you can do these things.
27 |
28 | To protect your rights, we need to make restrictions that forbid
29 | anyone to deny you these rights or to ask you to surrender the rights.
30 | These restrictions translate to certain responsibilities for you if you
31 | distribute copies of the software, or if you modify it.
32 |
33 | For example, if you distribute copies of such a program, whether
34 | gratis or for a fee, you must give the recipients all the rights that
35 | you have. You must make sure that they, too, receive or can get the
36 | source code. And you must show them these terms so they know their
37 | rights.
38 |
39 | We protect your rights with two steps: (1) copyright the software, and
40 | (2) offer you this license which gives you legal permission to copy,
41 | distribute and/or modify the software.
42 |
43 | Also, for each author's protection and ours, we want to make certain
44 | that everyone understands that there is no warranty for this free
45 | software. If the software is modified by someone else and passed on, we
46 | want its recipients to know that what they have is not the original, so
47 | that any problems introduced by others will not reflect on the original
48 | authors' reputations.
49 |
50 | Finally, any free program is threatened constantly by software
51 | patents. We wish to avoid the danger that redistributors of a free
52 | program will individually obtain patent licenses, in effect making the
53 | program proprietary. To prevent this, we have made it clear that any
54 | patent must be licensed for everyone's free use or not licensed at all.
55 |
56 | The precise terms and conditions for copying, distribution and
57 | modification follow.
58 |
59 | GNU GENERAL PUBLIC LICENSE
60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
61 |
62 | 0. This License applies to any program or other work which contains
63 | a notice placed by the copyright holder saying it may be distributed
64 | under the terms of this General Public License. The "Program", below,
65 | refers to any such program or work, and a "work based on the Program"
66 | means either the Program or any derivative work under copyright law:
67 | that is to say, a work containing the Program or a portion of it,
68 | either verbatim or with modifications and/or translated into another
69 | language. (Hereinafter, translation is included without limitation in
70 | the term "modification".) Each licensee is addressed as "you".
71 |
72 | Activities other than copying, distribution and modification are not
73 | covered by this License; they are outside its scope. The act of
74 | running the Program is not restricted, and the output from the Program
75 | is covered only if its contents constitute a work based on the
76 | Program (independent of having been made by running the Program).
77 | Whether that is true depends on what the Program does.
78 |
79 | 1. You may copy and distribute verbatim copies of the Program's
80 | source code as you receive it, in any medium, provided that you
81 | conspicuously and appropriately publish on each copy an appropriate
82 | copyright notice and disclaimer of warranty; keep intact all the
83 | notices that refer to this License and to the absence of any warranty;
84 | and give any other recipients of the Program a copy of this License
85 | along with the Program.
86 |
87 | You may charge a fee for the physical act of transferring a copy, and
88 | you may at your option offer warranty protection in exchange for a fee.
89 |
90 | 2. You may modify your copy or copies of the Program or any portion
91 | of it, thus forming a work based on the Program, and copy and
92 | distribute such modifications or work under the terms of Section 1
93 | above, provided that you also meet all of these conditions:
94 |
95 | a) You must cause the modified files to carry prominent notices
96 | stating that you changed the files and the date of any change.
97 |
98 | b) You must cause any work that you distribute or publish, that in
99 | whole or in part contains or is derived from the Program or any
100 | part thereof, to be licensed as a whole at no charge to all third
101 | parties under the terms of this License.
102 |
103 | c) If the modified program normally reads commands interactively
104 | when run, you must cause it, when started running for such
105 | interactive use in the most ordinary way, to print or display an
106 | announcement including an appropriate copyright notice and a
107 | notice that there is no warranty (or else, saying that you provide
108 | a warranty) and that users may redistribute the program under
109 | these conditions, and telling the user how to view a copy of this
110 | License. (Exception: if the Program itself is interactive but
111 | does not normally print such an announcement, your work based on
112 | the Program is not required to print an announcement.)
113 |
114 | These requirements apply to the modified work as a whole. If
115 | identifiable sections of that work are not derived from the Program,
116 | and can be reasonably considered independent and separate works in
117 | themselves, then this License, and its terms, do not apply to those
118 | sections when you distribute them as separate works. But when you
119 | distribute the same sections as part of a whole which is a work based
120 | on the Program, the distribution of the whole must be on the terms of
121 | this License, whose permissions for other licensees extend to the
122 | entire whole, and thus to each and every part regardless of who wrote it.
123 |
124 | Thus, it is not the intent of this section to claim rights or contest
125 | your rights to work written entirely by you; rather, the intent is to
126 | exercise the right to control the distribution of derivative or
127 | collective works based on the Program.
128 |
129 | In addition, mere aggregation of another work not based on the Program
130 | with the Program (or with a work based on the Program) on a volume of
131 | a storage or distribution medium does not bring the other work under
132 | the scope of this License.
133 |
134 | 3. You may copy and distribute the Program (or a work based on it,
135 | under Section 2) in object code or executable form under the terms of
136 | Sections 1 and 2 above provided that you also do one of the following:
137 |
138 | a) Accompany it with the complete corresponding machine-readable
139 | source code, which must be distributed under the terms of Sections
140 | 1 and 2 above on a medium customarily used for software interchange; or,
141 |
142 | b) Accompany it with a written offer, valid for at least three
143 | years, to give any third party, for a charge no more than your
144 | cost of physically performing source distribution, a complete
145 | machine-readable copy of the corresponding source code, to be
146 | distributed under the terms of Sections 1 and 2 above on a medium
147 | customarily used for software interchange; or,
148 |
149 | c) Accompany it with the information you received as to the offer
150 | to distribute corresponding source code. (This alternative is
151 | allowed only for noncommercial distribution and only if you
152 | received the program in object code or executable form with such
153 | an offer, in accord with Subsection b above.)
154 |
155 | The source code for a work means the preferred form of the work for
156 | making modifications to it. For an executable work, complete source
157 | code means all the source code for all modules it contains, plus any
158 | associated interface definition files, plus the scripts used to
159 | control compilation and installation of the executable. However, as a
160 | special exception, the source code distributed need not include
161 | anything that is normally distributed (in either source or binary
162 | form) with the major components (compiler, kernel, and so on) of the
163 | operating system on which the executable runs, unless that component
164 | itself accompanies the executable.
165 |
166 | If distribution of executable or object code is made by offering
167 | access to copy from a designated place, then offering equivalent
168 | access to copy the source code from the same place counts as
169 | distribution of the source code, even though third parties are not
170 | compelled to copy the source along with the object code.
171 |
172 | 4. You may not copy, modify, sublicense, or distribute the Program
173 | except as expressly provided under this License. Any attempt
174 | otherwise to copy, modify, sublicense or distribute the Program is
175 | void, and will automatically terminate your rights under this License.
176 | However, parties who have received copies, or rights, from you under
177 | this License will not have their licenses terminated so long as such
178 | parties remain in full compliance.
179 |
180 | 5. You are not required to accept this License, since you have not
181 | signed it. However, nothing else grants you permission to modify or
182 | distribute the Program or its derivative works. These actions are
183 | prohibited by law if you do not accept this License. Therefore, by
184 | modifying or distributing the Program (or any work based on the
185 | Program), you indicate your acceptance of this License to do so, and
186 | all its terms and conditions for copying, distributing or modifying
187 | the Program or works based on it.
188 |
189 | 6. Each time you redistribute the Program (or any work based on the
190 | Program), the recipient automatically receives a license from the
191 | original licensor to copy, distribute or modify the Program subject to
192 | these terms and conditions. You may not impose any further
193 | restrictions on the recipients' exercise of the rights granted herein.
194 | You are not responsible for enforcing compliance by third parties to
195 | this License.
196 |
197 | 7. If, as a consequence of a court judgment or allegation of patent
198 | infringement or for any other reason (not limited to patent issues),
199 | conditions are imposed on you (whether by court order, agreement or
200 | otherwise) that contradict the conditions of this License, they do not
201 | excuse you from the conditions of this License. If you cannot
202 | distribute so as to satisfy simultaneously your obligations under this
203 | License and any other pertinent obligations, then as a consequence you
204 | may not distribute the Program at all. For example, if a patent
205 | license would not permit royalty-free redistribution of the Program by
206 | all those who receive copies directly or indirectly through you, then
207 | the only way you could satisfy both it and this License would be to
208 | refrain entirely from distribution of the Program.
209 |
210 | If any portion of this section is held invalid or unenforceable under
211 | any particular circumstance, the balance of the section is intended to
212 | apply and the section as a whole is intended to apply in other
213 | circumstances.
214 |
215 | It is not the purpose of this section to induce you to infringe any
216 | patents or other property right claims or to contest validity of any
217 | such claims; this section has the sole purpose of protecting the
218 | integrity of the free software distribution system, which is
219 | implemented by public license practices. Many people have made
220 | generous contributions to the wide range of software distributed
221 | through that system in reliance on consistent application of that
222 | system; it is up to the author/donor to decide if he or she is willing
223 | to distribute software through any other system and a licensee cannot
224 | impose that choice.
225 |
226 | This section is intended to make thoroughly clear what is believed to
227 | be a consequence of the rest of this License.
228 |
229 | 8. If the distribution and/or use of the Program is restricted in
230 | certain countries either by patents or by copyrighted interfaces, the
231 | original copyright holder who places the Program under this License
232 | may add an explicit geographical distribution limitation excluding
233 | those countries, so that distribution is permitted only in or among
234 | countries not thus excluded. In such case, this License incorporates
235 | the limitation as if written in the body of this License.
236 |
237 | 9. The Free Software Foundation may publish revised and/or new versions
238 | of the General Public License from time to time. Such new versions will
239 | be similar in spirit to the present version, but may differ in detail to
240 | address new problems or concerns.
241 |
242 | Each version is given a distinguishing version number. If the Program
243 | specifies a version number of this License which applies to it and "any
244 | later version", you have the option of following the terms and conditions
245 | either of that version or of any later version published by the Free
246 | Software Foundation. If the Program does not specify a version number of
247 | this License, you may choose any version ever published by the Free Software
248 | Foundation.
249 |
250 | 10. If you wish to incorporate parts of the Program into other free
251 | programs whose distribution conditions are different, write to the author
252 | to ask for permission. For software which is copyrighted by the Free
253 | Software Foundation, write to the Free Software Foundation; we sometimes
254 | make exceptions for this. Our decision will be guided by the two goals
255 | of preserving the free status of all derivatives of our free software and
256 | of promoting the sharing and reuse of software generally.
257 |
258 | NO WARRANTY
259 |
260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN
262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS
266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE
267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
268 | REPAIR OR CORRECTION.
269 |
270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
278 | POSSIBILITY OF SUCH DAMAGES.
279 |
280 | END OF TERMS AND CONDITIONS
281 |
282 | How to Apply These Terms to Your New Programs
283 |
284 | If you develop a new program, and you want it to be of the greatest
285 | possible use to the public, the best way to achieve this is to make it
286 | free software which everyone can redistribute and change under these terms.
287 |
288 | To do so, attach the following notices to the program. It is safest
289 | to attach them to the start of each source file to most effectively
290 | convey the exclusion of warranty; and each file should have at least
291 | the "copyright" line and a pointer to where the full notice is found.
292 |
293 | Python project for Phoebe the quadcopter
294 | Copyright (C) 2013 PiStuffing
295 |
296 | This program is free software; you can redistribute it and/or modify
297 | it under the terms of the GNU General Public License as published by
298 | the Free Software Foundation; either version 2 of the License, or
299 | (at your option) any later version.
300 |
301 | This program is distributed in the hope that it will be useful,
302 | but WITHOUT ANY WARRANTY; without even the implied warranty of
303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
304 | GNU General Public License for more details.
305 |
306 | You should have received a copy of the GNU General Public License along
307 | with this program; if not, write to the Free Software Foundation, Inc.,
308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
309 |
310 | Also add information on how to contact you by electronic and paper mail.
311 |
312 | If the program is interactive, make it output a short notice like this
313 | when it starts in an interactive mode:
314 |
315 | Gnomovision version 69, Copyright (C) year name of author
316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
317 | This is free software, and you are welcome to redistribute it
318 | under certain conditions; type `show c' for details.
319 |
320 | The hypothetical commands `show w' and `show c' should show the appropriate
321 | parts of the General Public License. Of course, the commands you use may
322 | be called something other than `show w' and `show c'; they could even be
323 | mouse-clicks or menu items--whatever suits your program.
324 |
325 | You should also get your employer (if you work as a programmer) or your
326 | school, if any, to sign a "copyright disclaimer" for the program, if
327 | necessary. Here is a sample; alter the names:
328 |
329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program
330 | `Gnomovision' (which makes passes at compilers) written by James Hacker.
331 |
332 | {signature of Ty Coon}, 1 April 1989
333 | Ty Coon, President of Vice
334 |
335 | This General Public License does not permit incorporating your program into
336 | proprietary programs. If your program is a subroutine library, you may
337 | consider it more useful to permit linking proprietary applications with the
338 | library. If this is what you want to do, use the GNU Lesser General
339 | Public License instead of this License.
340 |
--------------------------------------------------------------------------------
/LiDAR_spec.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PiStuffing/Quadcopter/fa3a2e7b0d37d54ff5acc32c6752d3a31834240e/LiDAR_spec.pdf
--------------------------------------------------------------------------------
/LiDARv3HP_spec.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PiStuffing/Quadcopter/fa3a2e7b0d37d54ff5acc32c6752d3a31834240e/LiDARv3HP_spec.pdf
--------------------------------------------------------------------------------
/PhoebePresentationCamJamSept14.odp:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/PiStuffing/Quadcopter/fa3a2e7b0d37d54ff5acc32c6752d3a31834240e/PhoebePresentationCamJamSept14.odp
--------------------------------------------------------------------------------
/RC.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/python
2 |
3 | ####################################################################################################
4 | ####################################################################################################
5 | ## ##
6 | ## Hove's Raspberry Pi Python Quadcopter Flight Controller. Open Source @ GitHub ##
7 | ## PiStuffing/Quadcopter under GPL for non-commercial application. Any code derived from ##
8 | ## this should retain this copyright comment. ##
9 | ## ##
10 | ## Copyright 2012 - 2018 Andy Baker (Hove) - andy@pistuffing.co.uk ##
11 | ## ##
12 | ####################################################################################################
13 | ####################################################################################################
14 |
15 | from __future__ import division
16 | from smbus2 import SMBusWrapper, i2c_msg
17 | import math
18 | import socket
19 | import struct
20 | import sys
21 | import select
22 | import time
23 | import RPi.GPIO as GPIO
24 |
25 |
26 | def client():
27 |
28 | ################################################################################################
29 | # MAKE CONNECTION TO SERVER
30 | ################################################################################################
31 |
32 | poll = select.poll()
33 |
34 | go_go_go = False
35 |
36 | pack_format = "=ffffb?"
37 | pack_size = struct.calcsize(pack_format)
38 |
39 | unpack_format = "=?"
40 | unpack_size = struct.calcsize(unpack_format)
41 |
42 | client = socket.socket()
43 | addr = "192.168.42.1"
44 | port = 31415
45 |
46 | '''
47 | client_fd = client.fileno()
48 | poll.register(client_fd, select.POLLIN | select.POLLPRI)
49 | '''
50 |
51 | ################################################################################################
52 | # SET UP THE JOYSTICKS FSM
53 | ################################################################################################
54 |
55 | RC_PASSIVE = 0
56 | RC_TAKEOFF = 1
57 | RC_FLYING = 2
58 | RC_LANDING = 3
59 | RC_DONE = 4
60 | RC_ABORT = 5
61 |
62 | state_name = ["PASSIVE", "TAKEOFF", "FLYING", "LANDING", "DONE", "ABORT"]
63 |
64 | status_quo = [0.0, 0.0, 0.0, 0.0]
65 |
66 | #-----------------------------------------------------------------------------------------------
67 | # Set up GPIO for button and buzzer
68 | #-----------------------------------------------------------------------------------------------
69 | GPIO.setmode(GPIO.BCM)
70 |
71 | GPIO_BUTTON = 10
72 | GPIO.setup(GPIO_BUTTON, GPIO.IN, GPIO.PUD_DOWN) # GPIO button pushed pulls GPIO high
73 |
74 | GPIO_BUZZER = 24
75 | GPIO.setup(GPIO_BUZZER, GPIO.OUT) # GPIO buzzer set high sounds the buzzer
76 | GPIO.output(GPIO_BUZZER, GPIO.LOW)
77 |
78 | #-----------------------------------------------------------------------------------------------
79 | # Acquire contact with the piDrone; only continue below once acquired.
80 | #-----------------------------------------------------------------------------------------------
81 |
82 | with SMBusWrapper(1) as bus:
83 | try:
84 | while True:
85 |
86 | #-----------------------------------------------------------------------------------
87 | # Connect to the server
88 | #-----------------------------------------------------------------------------------
89 | print "CONNECTING..."
90 | while True:
91 | try:
92 | client.connect((addr, port))
93 | except:
94 | time.sleep(0.1)
95 | else:
96 | break
97 | print "CONNECTED."
98 |
99 | #-----------------------------------------------------------------------------------
100 | # We are connected to the piDrone.
101 | #-----------------------------------------------------------------------------------
102 | client_fd = client.fileno()
103 | poll.register(client_fd, select.POLLIN | select.POLLPRI)
104 | connected = True
105 |
106 | #-----------------------------------------------------------------------------------
107 | # Wait for a piDrone status update for go / stop
108 | #-----------------------------------------------------------------------------------
109 | go_go_go = False
110 | while connected:
111 |
112 | ################################################################################
113 | # HANDLE MESSAGES FROM THE SERVER
114 | ################################################################################
115 |
116 | #-------------------------------------------------------------------------------
117 | # Sleep pending an input for the piDrone
118 | #-------------------------------------------------------------------------------
119 | results = poll.poll(200) # milliseconds
120 |
121 | #-------------------------------------------------------------------------------
122 | # Check whether there's I/O from RC
123 | #-------------------------------------------------------------------------------
124 | for fd, event in results:
125 | assert fd == client_fd, "WTF HAPPENED HERE"
126 |
127 | #---------------------------------------------------------------------------
128 | # Has piDrone told piRC to start?
129 | #---------------------------------------------------------------------------
130 | raw = client.recv(unpack_size)
131 | assert (len(raw) == unpack_size), "Invalid data"
132 |
133 | #---------------------------------------------------------------------------
134 | # React on the action from the message from the piDrone
135 | #---------------------------------------------------------------------------
136 | formatted = struct.unpack(unpack_format, raw)
137 |
138 | if formatted[0]: # is True
139 | GPIO.add_event_detect(GPIO_BUTTON, GPIO.FALLING)
140 |
141 | #-----------------------------------------------------------------------
142 | # piDrone is connected, start the flight
143 | #-----------------------------------------------------------------------
144 | go_go_go = True
145 | state = RC_TAKEOFF
146 | takeoff_time = time.time()
147 |
148 | print "GO GO GO"
149 |
150 | else:
151 | GPIO.remove_event_detect(GPIO_BUTTON)
152 | connected = False
153 |
154 | #-----------------------------------------------------------------------
155 | # piDrone is disconnected
156 | #-----------------------------------------------------------------------
157 | go_go_go = False
158 | poll.unregister(client_fd)
159 | client.close()
160 |
161 | print "STOP STOP STOP"
162 |
163 | #---------------------------------------------------------------------------
164 | # If we're now not connected, report back.
165 | #---------------------------------------------------------------------------
166 | if not connected:
167 | continue
168 |
169 | #---------------------------------------------------------------------------
170 | # Are we ready to go?
171 | #---------------------------------------------------------------------------
172 | if not go_go_go:
173 | continue
174 |
175 | #-------------------------------------------------------------------------------
176 | # The piDrone is live and kicking; set the next flight increment.
177 | #
178 | # UD = Up / Down - upwards is positive
179 | # YR = Yaw Rate - anticlockwise is positive
180 | # LR = Left / Right - leftwards is positive
181 | # FB = Forwards / Backwards - forwards is positive
182 | #-------------------------------------------------------------------------------
183 | msg = i2c_msg.read(0x40, 2)
184 | bus.i2c_rdwr(msg)
185 | data = list(msg)
186 |
187 | assert (len(data) == 2), "Joystick 0 data len: %d" % len(data)
188 |
189 | if (data[0] > 127):
190 | UD = data[0] - 256
191 | else:
192 | UD = data[0]
193 |
194 | if (data[1] > 127):
195 | YR = data[1] - 256
196 | else:
197 | YR = data[1]
198 |
199 | msg = i2c_msg.read(0x41, 2)
200 | bus.i2c_rdwr(msg)
201 | data = list(msg)
202 |
203 | assert (len(data) == 2), "Joystick 1 data len: %d" % len(data)
204 |
205 | if (data[0] > 127):
206 | LR = data[0] - 256
207 | else:
208 | LR = data[0]
209 |
210 | if (data[1] > 127):
211 | FB = -(data[1] - 256)
212 | else:
213 | FB = -data[1]
214 |
215 | #===============================================================================
216 | # FSM INPUT, STATES, OUTPUT
217 | #===============================================================================
218 | beep = False
219 |
220 | #-------------------------------------------------------------------------------
221 | # Have we have a falling edge from the button?
222 | #-------------------------------------------------------------------------------
223 | if GPIO.event_detected(GPIO_BUTTON):
224 |
225 | #---------------------------------------------------------------------------
226 | # If we're flying, start landing otherise abort
227 | #---------------------------------------------------------------------------
228 | if state == RC_FLYING:
229 | print "landing-landing-landing"
230 | state = RC_LANDING
231 | landing_time = time.time()
232 | else:
233 | print "abort-abort-abort"
234 | state = RC_ABORT
235 |
236 | #===============================================================================
237 | # FSM INPUTS
238 | #===============================================================================
239 | if state == RC_TAKEOFF:
240 | if time.time() - takeoff_time < 3.0: # seconds
241 | UD = 0.33
242 | YR = 0.0
243 | LR = 0.0
244 | FB = 0.0
245 | else:
246 | UD, YR, FB, LR = status_quo
247 | state = RC_FLYING
248 | beep = True
249 |
250 | #-----------------------------------------------------------------------
251 | # Take the timestamp of flight, and initiate initial height
252 | #-----------------------------------------------------------------------
253 | before = time.time()
254 | height = 1.0
255 |
256 | elif state == RC_FLYING:
257 | #-----------------------------------------------------------------------
258 | # Joysticks are +/- 80, convert these to +/- 1m/s. The exception is the
259 | # yaw rate where +/-80 maps to +/- 90 degrees (pi/2) per second
260 | #-----------------------------------------------------------------------
261 | UD /= 80
262 | YR /= (80 * 2 / math.pi)
263 | FB /= 80
264 | LR /= 80
265 |
266 | #-----------------------------------------------------------------------
267 | # Integrate the height increment and mark the landing timestamo
268 | #-----------------------------------------------------------------------
269 | now = time.time()
270 | dt = now - before
271 | before = now
272 | height += UD * dt #RC! Flawed: future velocity x historic dt
273 | landing_period = height / 0.33 + 1 #RC! OK?
274 |
275 | elif state == RC_LANDING:
276 | if time.time() - landing_time < landing_period: # seconds
277 | UD = -0.33
278 | YR = 0.0
279 | FB = 0.0
280 | LR = 0.0
281 | beep = True
282 | else:
283 | UD, YR, FB, LR = status_quo
284 | state = RC_DONE
285 | beep = False
286 |
287 | else:
288 | assert state == RC_ABORT, "Should be on abort here!"
289 | UD, YR, FB, LR = status_quo
290 |
291 | output = struct.pack(pack_format, UD, YR, FB, LR, state, beep)
292 | client.send(output)
293 | print "SENT: UD = %f | YR = %f | FB = %f | LR = %f | status = %s | beep = %d" % (UD, YR, FB, LR, state_name[state], beep)
294 |
295 | else:
296 | print "DISCONNECTED DISCONNECTED DISCONNECTED"
297 |
298 | except KeyboardInterrupt:
299 | pass
300 |
301 | else:
302 | pass
303 |
304 | finally:
305 | pass
306 |
307 | client()
308 |
309 | '''
310 | def server():
311 |
312 | poll = select.poll()
313 |
314 | unpack_format = "=ffffb?"
315 | unpack_size = struct.calcsize(unpack_format)
316 |
317 | pack_format = "=?"
318 |
319 | server = socket.socket()
320 | addr = "192.168.42.1"
321 | port = 31415
322 | server.bind((addr, port))
323 | server.listen(5)
324 |
325 | raw_input("Start the flight?")
326 |
327 | try:
328 | connection, addr = server.accept()
329 | connection_fd = connection.fileno()
330 | poll.register(connection_fd, select.POLLIN | select.POLLPRI)
331 |
332 | #-------------------------------------------------------------------------------------------
333 | # Tell the client to go-go-go!
334 | #-------------------------------------------------------------------------------------------
335 | output = struct.pack(pack_format, True)
336 | connection.send(output)
337 |
338 | #-------------------------------------------------------------------------------------------
339 | # Listen to the client and do what it says.
340 | #-------------------------------------------------------------------------------------------
341 | while True:
342 | results = poll.poll(200)
343 |
344 | #---------------------------------------------------------------------------------------
345 | # Check whether there's I/O from RC
346 | #---------------------------------------------------------------------------------------
347 | for fd, event in results:
348 | assert fd == connection_fd, "WHOSE FD IS THIS?"
349 |
350 | #-----------------------------------------------------------------------------------
351 | # Unpack the data received
352 | #-----------------------------------------------------------------------------------
353 | raw = connection.recv(unpack_size)
354 | assert (len(raw) == unpack_size), "Invalid data"
355 |
356 | #-----------------------------------------------------------------------------------
357 | # React on the action
358 | #-----------------------------------------------------------------------------------
359 | formatted = struct.unpack(unpack_format, raw)
360 | assert (len(formatted) == 6), "Bad formatted size"
361 |
362 | UD = formatted[0]
363 | YR = formatted[1]
364 | FB = formatted[2]
365 | LR = formatted[3]
366 | state = formatted[4]
367 | beep = formatted[5]
368 |
369 | print "RECEIVED: UD = %f | YR = %f | FB = %f | LR = %f | status = %s | beep = %d" % (UD, YR, FB, LR, state_name[status], beep)
370 |
371 | except KeyboardInterrupt:
372 | #-------------------------------------------------------------------------------------------
373 | # Tell the client to stop-stop-stop!
374 | #-------------------------------------------------------------------------------------------
375 | output = struct.pack(pack_format, False)
376 | connection.send(output)
377 |
378 | except Exception, err:
379 | print err
380 |
381 | finally:
382 | connection.close()
383 |
384 |
385 | if len(sys.argv) != 2:
386 | print "Select DRONE or RC"
387 | elif sys.argv[1] == "RC":
388 | client()
389 | elif sys.argv[1] == "DRONE":
390 | server()
391 | else:
392 | print "Select RC or DRONE"
393 | '''
394 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | Quadcopter
2 | ==========
3 |
4 | Python project for an Raspberry Pi Autonomous Quadcopter Flight Controller
5 |
6 |
7 |
8 | Licensing
9 | ---------
10 |
This work is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License.
11 |
12 | Index
13 | -----
14 | Documentation
15 |