├── .gitignore ├── LICENSE ├── README.md ├── libraries ├── MPULib │ ├── MPULib.cpp │ └── MPULib.h ├── MedianFilter │ ├── MedianFilter.cpp │ └── MedianFilter.h ├── PID_v1 │ ├── PID_v1.cpp │ └── PID_v1.h └── PinChangeInt │ └── PinChangeInt.h └── src ├── Makefile ├── debugger.cpp ├── debugger.h ├── flight_controller.cpp ├── flight_controller.h ├── imu.cpp ├── imu.h ├── motor_controller.cpp ├── motor_controller.h ├── quadcopter.ino ├── rc_interrupts.h ├── remote_control.cpp └── remote_control.h /.gitignore: -------------------------------------------------------------------------------- 1 | .build* 2 | .DS_Store 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU 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 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This is an old version of my quad code. [Go here for the latest version.](https://github.com/bolandrm/rmb_multicopter) 2 | -------------------------------------------------------------------------------- /libraries/MPULib/MPULib.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Created by Basel Al-Rudainy, 6 april 2013. 3 | 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) any later version. 8 | This library is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | Lesser General Public License for more details. 12 | */ 13 | 14 | #include "MPULib.h" 15 | #include "Arduino.h" 16 | #include "Wire.h" 17 | 18 | MPULib::MPULib() 19 | { 20 | 21 | } 22 | 23 | void MPULib::init(){ 24 | //init - ADXL345 25 | writeCmd(ADXL_addr,REG_DATA_FORMAT,0x0B); //+-16g 26 | writeCmd(ADXL_addr,REG_PWR_CTL,0x08); //measurement mode 27 | writeCmd(ADXL_addr,REG_BW_RATE,0x09); //REG_BW_RATE rate=50hz, bw=20hz 28 | //1G = 265 29 | //-----end init ADXL345 30 | 31 | //init - L3G4200D 32 | writeCmd(L3G4_addr,CTRL_REG1,L3G4_BW_ENAX); 33 | writeCmd(L3G4_addr,CTRL_REG2,L3G4_LPF); 34 | writeCmd(L3G4_addr,CTRL_REG4,MODE_2000); 35 | writeCmd(L3G4_addr,CTRL_REG5,L3G4_HPF); 36 | //-----end init L3G4200D 37 | 38 | //init - HMC5883 39 | writeCmd(HMC_addr,HMC_mode_reg,HMC_contm_val); 40 | //-----end init HMC5883 41 | } 42 | 43 | void MPULib::getAxlData(int16_t *axl_x, int16_t *axl_y, int16_t *axl_z){ 44 | byte buffer[6]; 45 | readCmd(ADXL_addr,DATAX0,6,buffer); 46 | *axl_y=(buffer[1]<<8) | buffer[0]; 47 | *axl_x=(buffer[3]<<8) | buffer[2]; 48 | *axl_z=(buffer[5]<<8) | buffer[4]; 49 | 50 | } 51 | 52 | void MPULib::getGyroData(float *gyro_x, float *gyro_y, float *gyro_z){ 53 | byte buffer[6]; 54 | readCmd(L3G4_addr,READALLSIX,6,buffer); 55 | *gyro_x = (float)((int)(buffer[1]<<8) | buffer[0])*SCALE_2000; 56 | *gyro_y = (float)-1*((int)(buffer[3]<<8) | buffer[2])*SCALE_2000; 57 | *gyro_z = (float)((int)(buffer[5]<<8) | buffer[4])*SCALE_2000; 58 | } 59 | 60 | void MPULib::getMagData(int buff[]){ 61 | byte buffer[6]; 62 | readCmd(HMC_addr,HMC_X_MSB,6,buffer); 63 | buff[0]=(buffer[0]<<8) | buffer[1]; 64 | buff[2]=(buffer[2]<<8) | buffer[3]; 65 | buff[1]=(buffer[4]<<8) | buffer[5]; 66 | } 67 | 68 | void MPULib::readCmd(byte addr,byte reg,byte num,byte buff[]){ 69 | Wire.beginTransmission(addr); 70 | Wire.write(reg); 71 | Wire.endTransmission(); 72 | Wire.requestFrom(addr, num); 73 | while(Wire.available() 20 | #define ADXL_addr 0x53 //addr-pin LOW 21 | 22 | #define REG_DATA_FORMAT 0x31 23 | #define REG_PWR_CTL 0x2D 24 | #define REG_BW_RATE 0x2C 25 | 26 | #define DATAX0 0x32 //LSB 27 | #define DATAX1 0x33 //MSB 28 | #define DATAY0 0x34 //LSB 29 | #define DATAY1 0x35 //MSB 30 | #define DATAZ0 0x36 //LSB 31 | #define DATAZ1 0x37 //MSB 32 | // <--------------------------------> 33 | 34 | // <---------L3G4200D--------------> 35 | 36 | #define L3G4_addr 0x69 //SDO-pin HIGH 37 | 38 | #define CTRL_REG1 0x20 39 | #define CTRL_REG2 0x21 40 | #define CTRL_REG4 0x23 41 | #define CTRL_REG5 0x24 42 | #define L3G4_HPF 0x13 43 | #define L3G4_LPF 0x34 44 | #define L3G4_BW_ENAX 0x8F 45 | #define MODE_250 0x00 46 | #define MODE_500 ((0x01)<<4) 47 | #define MODE_2000 ((0x03)<<4) 48 | #define SCALE_250 (8.75/1000.0) 49 | #define SCALE_500 (17.5/1000.0) 50 | #define SCALE_2000 (70.0/1000.0) 51 | 52 | #define READALLSIX 0x28 | (1 << 7) 53 | #define OUT_X_L 0x28 54 | #define OUT_X_H 0x29 55 | #define OUT_Y_L 0x2A 56 | #define OUT_Y_H 0x2B 57 | #define OUT_Z_L 0x2C 58 | #define OUT_Z_H 0x2D 59 | // <--------------------------------> 60 | 61 | // <---------HMC5883--------------> 62 | #define HMC_addr 0x1E 63 | #define HMC_mode_reg 0x02 64 | #define HMC_contm_val 0x00 65 | #define HMC_X_MSB 0x03 66 | // <--------------------------------> 67 | 68 | class MPULib 69 | { 70 | public: 71 | MPULib(); 72 | void init(); 73 | void getAxlData(int16_t *, int16_t *, int16_t *); 74 | void getGyroData(float *, float *, float *); 75 | void getMagData(int buff[]); 76 | private: 77 | void readCmd(byte addr,byte reg,byte num,byte buff[]); 78 | void writeCmd(byte addr, byte reg, byte val); 79 | }; 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /libraries/MedianFilter/MedianFilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | MedianFilter.cpp - Median Filter for the Arduino platform. 3 | Copyright (c) 2013 Phillip Schmidt. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | /* 21 | 22 | A median filter object is created by by passing the desired filter window size on object creation. 23 | The window size should be an odd number between 3 and 255. 24 | 25 | New data is added to the median filter by passing the data through the write() method. 26 | The new data will over-write the oldest data point, then be shifted in the array to place it in the correct location. 27 | 28 | The current median value is returned by the read() method. 29 | 30 | !!! All data must be type INT. !!! 31 | 32 | */ 33 | 34 | #include "MedianFilter.h" 35 | 36 | 37 | MedianFilter::MedianFilter(byte size, int seed) 38 | { 39 | 40 | if(size < 3){size = 3;} // prevent undersized windows 41 | if(size > 255){size = 255;} // prevent oversized windows 42 | 43 | medFilterWin = size; // number of samples in sliding median filter window - usually odd # 44 | medDataPointer = size >> 1; // mid point of window 45 | sortedData = (float*) calloc (size, sizeof(float)); // array for data sorted by size 46 | historyMap = (byte*) calloc (size, sizeof(byte)); // array for locations of data in sorted list (arranged in looped age order) 47 | locationMap = (byte*) calloc (size, sizeof(byte)); // array for locations of history data in map list 48 | ODP = 0; // oldest data point location in historyMap 49 | 50 | for(byte i=0; i0; i--){ //index through left adjacent data 66 | int j = i - 1; // neighbour location 67 | if(sortedData[i] < sortedData[j]){ 68 | //Serial.print("<"); 69 | tempData = sortedData[j]; // store neighbour data in temp 70 | tempMap = locationMap[j]; // store position of adjacent data in historyMap 71 | 72 | sortedData[j] = sortedData[i]; // move new data to neighbour location 73 | historyMap[ODP] = j; 74 | locationMap[j] = ODP; 75 | 76 | sortedData[i] = tempData; // swap neighbour data back in 77 | historyMap[tempMap] = i; 78 | locationMap[i] = tempMap; 79 | 80 | dataMoved = true; 81 | } 82 | else{ 83 | i=0; // abort loop if left neighbour is larger (faster than "break;") 84 | } 85 | } 86 | } // end shift data to left 87 | 88 | if(historyMap[ODP] != medFilterWin - 1 && dataMoved == false){ 89 | // don't check right neighbours if at the extreme right or data already moved 90 | for(int i=historyMap[ODP]; i sortedData[j]){ 93 | //Serial.print(">"); 94 | tempData = sortedData[j]; // store neighbour data in temp 95 | tempMap = locationMap[j]; // store position of adjacent data in historyMap 96 | 97 | sortedData[j] = sortedData[i];// move new data to neighbour location 98 | historyMap[ODP] = j; 99 | locationMap[j] = ODP; 100 | 101 | sortedData[i] = tempData; // swap neighbour data back in 102 | historyMap[tempMap] = i; 103 | locationMap[i] = tempMap; 104 | } 105 | else{ 106 | i=medFilterWin; // abort loop if right neighbour is smaller (faster than "break;") 107 | } 108 | } 109 | } // end shift data to right 110 | 111 | ODP++; 112 | if(ODP >= medFilterWin){ODP = 0;} // reset after oldest data point to ring history 113 | 114 | return sortedData[medDataPointer]; 115 | } 116 | 117 | float MedianFilter::out() // return the value of the median data sample 118 | { 119 | return sortedData[medDataPointer]; 120 | } 121 | 122 | /* 123 | void MedianFilter::printData() // display sorting data for debugging 124 | { 125 | for(int i=0; i brettbeauregard.com 4 | * 5 | * This Library is licensed under a GPLv3 License 6 | **********************************************************************************************/ 7 | 8 | #if ARDUINO >= 100 9 | #include "Arduino.h" 10 | #else 11 | #include "WProgram.h" 12 | #endif 13 | 14 | #include 15 | 16 | /*Constructor (...)********************************************************* 17 | * The parameters specified here are those for for which we can't set up 18 | * reliable defaults, so we need to have the user set them. 19 | ***************************************************************************/ 20 | PID::PID(double* Input, double* Output, double* Setpoint, 21 | double Kp, double Ki, double Kd, int ControllerDirection) 22 | { 23 | 24 | ITermMax = 0.0; 25 | myOutput = Output; 26 | myInput = Input; 27 | mySetpoint = Setpoint; 28 | inAuto = false; 29 | 30 | PID::SetOutputLimits(0, 255); //default output limit corresponds to 31 | //the arduino pwm limits 32 | errorBand = 0; 33 | SampleTime = 100; //default Controller Sample Time is 0.1 seconds 34 | 35 | PID::SetControllerDirection(ControllerDirection); 36 | PID::SetTunings(Kp, Ki, Kd); 37 | 38 | lastTime = millis()-SampleTime; 39 | } 40 | 41 | void PID::SetDebugParams(double* pDebugIn, double* iDebugIn, double* dDebugIn) { 42 | pDebug = pDebugIn; 43 | iDebug = iDebugIn; 44 | dDebug = dDebugIn; 45 | } 46 | 47 | void PID::SetITermMax(double iMax) { 48 | ITermMax = iMax; 49 | } 50 | 51 | /* Compute() ********************************************************************** 52 | * This, as they say, is where the magic happens. this function should be called 53 | * every time "void loop()" executes. the function will decide for itself whether a new 54 | * pid Output needs to be computed. returns true when the output is computed, 55 | * false when nothing has been done. 56 | **********************************************************************************/ 57 | bool PID::Compute() 58 | { 59 | if(!inAuto) return false; 60 | unsigned long now = millis(); 61 | unsigned long timeChange = (now - lastTime); 62 | if(timeChange>=SampleTime) 63 | { 64 | /*Compute all the working error variables*/ 65 | double input = *myInput; 66 | double error = *mySetpoint - input; 67 | 68 | if (errorBand != 0 && (error < 0 && error > -errorBand || error > 0 && error < errorBand)) { 69 | error = 0; 70 | } 71 | 72 | ITerm+= (ki * error); 73 | if(ITerm > outMax) ITerm= outMax; 74 | else if(ITerm < outMin) ITerm= outMin; 75 | 76 | if (ITermMax != 0.0) { 77 | if(ITerm > ITermMax) ITerm= ITermMax; 78 | else if(ITerm < -ITermMax) ITerm= -ITermMax; 79 | } 80 | 81 | double dInput = (input - lastInput); 82 | 83 | /*Compute PID Output*/ 84 | 85 | double pTerm = kp * error; 86 | double dTerm = -kd * dInput; 87 | 88 | *pDebug = pTerm; 89 | *iDebug = ITerm; 90 | *dDebug = dTerm; 91 | 92 | double output = pTerm + ITerm + dTerm; 93 | 94 | if(output > outMax) output = outMax; 95 | else if(output < outMin) output = outMin; 96 | *myOutput = output; 97 | 98 | /*Remember some variables for next time*/ 99 | lastInput = input; 100 | lastTime = now; 101 | return true; 102 | } 103 | else return false; 104 | } 105 | 106 | 107 | void PID::SetErrorBand(float band) { 108 | errorBand = band; 109 | } 110 | /* SetTunings(...)************************************************************* 111 | * This function allows the controller's dynamic performance to be adjusted. 112 | * it's called automatically from the constructor, but tunings can also 113 | * be adjusted on the fly during normal operation 114 | ******************************************************************************/ 115 | void PID::SetTunings(double Kp, double Ki, double Kd) 116 | { 117 | if (Kp<0 || Ki<0 || Kd<0) return; 118 | 119 | dispKp = Kp; dispKi = Ki; dispKd = Kd; 120 | 121 | double SampleTimeInSec = ((double)SampleTime)/1000; 122 | kp = Kp; 123 | ki = Ki * SampleTimeInSec; 124 | kd = Kd / SampleTimeInSec; 125 | 126 | if(controllerDirection ==REVERSE) 127 | { 128 | kp = (0 - kp); 129 | ki = (0 - ki); 130 | kd = (0 - kd); 131 | } 132 | } 133 | 134 | /* SetSampleTime(...) ********************************************************* 135 | * sets the period, in Milliseconds, at which the calculation is performed 136 | ******************************************************************************/ 137 | void PID::SetSampleTime(int NewSampleTime) 138 | { 139 | if (NewSampleTime > 0) 140 | { 141 | double ratio = (double)NewSampleTime 142 | / (double)SampleTime; 143 | ki *= ratio; 144 | kd /= ratio; 145 | SampleTime = (unsigned long)NewSampleTime; 146 | } 147 | } 148 | 149 | /* SetOutputLimits(...)**************************************************** 150 | * This function will be used far more often than SetInputLimits. while 151 | * the input to the controller will generally be in the 0-1023 range (which is 152 | * the default already,) the output will be a little different. maybe they'll 153 | * be doing a time window and will need 0-8000 or something. or maybe they'll 154 | * want to clamp it from 0-125. who knows. at any rate, that can all be done 155 | * here. 156 | **************************************************************************/ 157 | void PID::SetOutputLimits(double Min, double Max) 158 | { 159 | if(Min >= Max) return; 160 | outMin = Min; 161 | outMax = Max; 162 | 163 | if(inAuto) 164 | { 165 | if(*myOutput > outMax) *myOutput = outMax; 166 | else if(*myOutput < outMin) *myOutput = outMin; 167 | 168 | if(ITerm > outMax) ITerm= outMax; 169 | else if(ITerm < outMin) ITerm= outMin; 170 | } 171 | } 172 | 173 | /* SetMode(...)**************************************************************** 174 | * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) 175 | * when the transition from manual to auto occurs, the controller is 176 | * automatically initialized 177 | ******************************************************************************/ 178 | void PID::SetMode(int Mode) 179 | { 180 | bool newAuto = (Mode == AUTOMATIC); 181 | if(newAuto == !inAuto) 182 | { /*we just went from manual to auto*/ 183 | PID::Initialize(); 184 | } 185 | inAuto = newAuto; 186 | } 187 | 188 | /* Initialize()**************************************************************** 189 | * does all the things that need to happen to ensure a bumpless transfer 190 | * from manual to automatic mode. 191 | ******************************************************************************/ 192 | void PID::Initialize() 193 | { 194 | ITerm = *myOutput; 195 | lastInput = *myInput; 196 | if(ITerm > outMax) ITerm = outMax; 197 | else if(ITerm < outMin) ITerm = outMin; 198 | } 199 | 200 | /* SetControllerDirection(...)************************************************* 201 | * The PID will either be connected to a DIRECT acting process (+Output leads 202 | * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to 203 | * know which one, because otherwise we may increase the output when we should 204 | * be decreasing. This is called from the constructor. 205 | ******************************************************************************/ 206 | void PID::SetControllerDirection(int Direction) 207 | { 208 | if(inAuto && Direction !=controllerDirection) 209 | { 210 | kp = (0 - kp); 211 | ki = (0 - ki); 212 | kd = (0 - kd); 213 | } 214 | controllerDirection = Direction; 215 | } 216 | 217 | /* Status Funcions************************************************************* 218 | * Just because you set the Kp=-1 doesn't mean it actually happened. these 219 | * functions query the internal state of the PID. they're here for display 220 | * purposes. this are the functions the PID Front-end uses for example 221 | ******************************************************************************/ 222 | double PID::GetKp(){ return dispKp; } 223 | double PID::GetKi(){ return dispKi;} 224 | double PID::GetKd(){ return dispKd;} 225 | int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} 226 | int PID::GetDirection(){ return controllerDirection;} 227 | 228 | // /********************************************************************************************** 229 | // * NOTE - This is a modified version for contuous updates 230 | // * see: https://github.com/br3ttb/Arduino-PID-Library/pull/9 231 | // * 232 | // * Arduino PID Library - Version 1.0.1 233 | // * by Brett Beauregard brettbeauregard.com 234 | // * 235 | // * This Library is licensed under a GPLv3 License 236 | // **********************************************************************************************/ 237 | // 238 | // #if ARDUINO >= 100 239 | // #include "Arduino.h" 240 | // #else 241 | // #include "WProgram.h" 242 | // #endif 243 | // 244 | // #include 245 | // 246 | // /*Constructor (...)********************************************************* 247 | // * The parameters specified here are those for for which we can't set up 248 | // * reliable defaults, so we need to have the user set them. 249 | // ***************************************************************************/ 250 | // PID::PID(double* Input, double* Output, double* Setpoint, 251 | // double Kp, double Ki, double Kd, int ControllerDirection) 252 | // { 253 | // ITermMax = 0.0; 254 | // myOutput = Output; 255 | // myInput = Input; 256 | // mySetpoint = Setpoint; 257 | // inAuto = false; 258 | // 259 | // PID::SetOutputLimits(0, 255); //default output limit corresponds to 260 | // //the arduino pwm limits 261 | // 262 | // SampleTime = 100; //default Controller Sample Time is 0.1 seconds 263 | // 264 | // PID::SetControllerDirection(ControllerDirection); 265 | // PID::SetResolution(MILLIS); // Use a resolution of milliseconds by default 266 | // PID::SetTunings(Kp, Ki, Kd); 267 | // } 268 | // 269 | // 270 | // /* Compute() ********************************************************************** 271 | // * This, as they say, is where the magic happens. this function should be called 272 | // * every time "void loop()" executes. the function will decide for itself whether a new 273 | // * pid Output needs to be computed. returns true when the output is computed, 274 | // * false when nothing has been done. 275 | // **********************************************************************************/ 276 | // bool PID::Compute() 277 | // { 278 | // if(!inAuto) return false; 279 | // unsigned long now = PID::GetTime(); 280 | // timeChange = (now - lastTime); 281 | // if(SampleTime == 0 || timeChange>=SampleTime) 282 | // { 283 | // /*Compute all the working error variables*/ 284 | // double input = *myInput; 285 | // double error = *mySetpoint - input; 286 | // 287 | // double dInput; 288 | // if (SampleTime > 0) { 289 | // ITerm += (ki * error); 290 | // dInput = (input - lastInput); 291 | // } else { 292 | // ITerm += (ki * error)*(((double)timeChange)/secondsDivider); 293 | // dInput = (input - lastInput)/(((double)timeChange)/secondsDivider); 294 | // } 295 | // 296 | // if(ITerm > outMax) ITerm= outMax; 297 | // else if(ITerm < outMin) ITerm= outMin; 298 | // 299 | // if (ITermMax != 0.0) { 300 | // if(ITerm > ITermMax) ITerm= ITermMax; 301 | // else if(ITerm < -ITermMax) ITerm= -ITermMax; 302 | // } 303 | // 304 | // /*Compute PID Output*/ 305 | // double output = kp * error + ITerm- kd * dInput; 306 | // 307 | // if(output > outMax) output = outMax; 308 | // else if(output < outMin) output = outMin; 309 | // *myOutput = output; 310 | // 311 | // /*Remember some variables for next time*/ 312 | // lastInput = input; 313 | // lastTime = now; 314 | // return true; 315 | // } 316 | // else return false; 317 | // } 318 | // 319 | // 320 | // /* SetTunings(...)************************************************************* 321 | // * This function allows the controller's dynamic performance to be adjusted. 322 | // * it's called automatically from the constructor, but tunings can also 323 | // * be adjusted on the fly during normal operation 324 | // ******************************************************************************/ 325 | // void PID::SetTunings(double Kp, double Ki, double Kd) 326 | // { 327 | // if (Kp<0 || Ki<0 || Kd<0) return; 328 | // 329 | // dispKp = Kp; dispKi = Ki; dispKd = Kd; 330 | // 331 | // if (SampleTime > 0) { 332 | // double SampleTimeInSec = ((double)SampleTime)/secondsDivider; 333 | // kp = Kp; 334 | // ki = Ki * SampleTimeInSec; 335 | // kd = Kd / SampleTimeInSec; 336 | // } else { 337 | // kp = Kp; 338 | // ki = Ki; 339 | // kd = Kd; 340 | // } 341 | // 342 | // if(controllerDirection ==REVERSE) 343 | // { 344 | // kp = (0 - kp); 345 | // ki = (0 - ki); 346 | // kd = (0 - kd); 347 | // } 348 | // } 349 | // 350 | // /* SetSampleTime(...) ********************************************************* 351 | // * sets the period, in Milliseconds, at which the calculation is performed 352 | // * If it's set to 0 or a negative value it will computer every time the 353 | // * function is called 354 | // ******************************************************************************/ 355 | // void PID::SetSampleTime(int NewSampleTime) 356 | // { 357 | // if (NewSampleTime > 0) 358 | // { 359 | // double ratio; 360 | // if (SampleTime > 0) 361 | // ratio = (double)NewSampleTime/(double)SampleTime; 362 | // else 363 | // ratio = (double)NewSampleTime/(double)timeChange; // We will assume the user is calling Compute at a regular interval 364 | // 365 | // ki *= ratio; 366 | // kd /= ratio; 367 | // SampleTime = (unsigned long)NewSampleTime; 368 | // } else 369 | // SampleTime = 0; // We will compute every time the function is called 370 | // } 371 | // 372 | // /* SetOutputLimits(...)**************************************************** 373 | // * This function will be used far more often than SetInputLimits. while 374 | // * the input to the controller will generally be in the 0-1023 range (which is 375 | // * the default already,) the output will be a little different. maybe they'll 376 | // * be doing a time window and will need 0-8000 or something. or maybe they'll 377 | // * want to clamp it from 0-125. who knows. at any rate, that can all be done 378 | // * here. 379 | // **************************************************************************/ 380 | // void PID::SetOutputLimits(double Min, double Max) 381 | // { 382 | // if(Min >= Max) return; 383 | // outMin = Min; 384 | // outMax = Max; 385 | // 386 | // if(inAuto) 387 | // { 388 | // if(*myOutput > outMax) *myOutput = outMax; 389 | // else if(*myOutput < outMin) *myOutput = outMin; 390 | // 391 | // if(ITerm > outMax) ITerm= outMax; 392 | // else if(ITerm < outMin) ITerm= outMin; 393 | // } 394 | // } 395 | // 396 | // /* SetMode(...)**************************************************************** 397 | // * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) 398 | // * when the transition from manual to auto occurs, the controller is 399 | // * automatically initialized 400 | // ******************************************************************************/ 401 | // void PID::SetMode(int Mode) 402 | // { 403 | // bool newAuto = (Mode == AUTOMATIC); 404 | // if(newAuto == !inAuto) 405 | // { /*we just went from manual to auto*/ 406 | // PID::Initialize(); 407 | // } 408 | // inAuto = newAuto; 409 | // } 410 | // 411 | // /* Initialize()**************************************************************** 412 | // * does all the things that need to happen to ensure a bumpless transfer 413 | // * from manual to automatic mode. 414 | // ******************************************************************************/ 415 | // void PID::Initialize() 416 | // { 417 | // ITerm = *myOutput; 418 | // lastInput = *myInput; 419 | // if(ITerm > outMax) ITerm = outMax; 420 | // else if(ITerm < outMin) ITerm = outMin; 421 | // } 422 | // 423 | // /* SetControllerDirection(...)************************************************* 424 | // * The PID will either be connected to a DIRECT acting process (+Output leads 425 | // * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to 426 | // * know which one, because otherwise we may increase the output when we should 427 | // * be decreasing. This is called from the constructor. 428 | // ******************************************************************************/ 429 | // void PID::SetControllerDirection(int Direction) 430 | // { 431 | // if(inAuto && Direction !=controllerDirection) 432 | // { 433 | // kp = (0 - kp); 434 | // ki = (0 - ki); 435 | // kd = (0 - kd); 436 | // } 437 | // controllerDirection = Direction; 438 | // } 439 | // 440 | // /* GetTime()******************************************************************* 441 | // * Will get the current time either by using millis() or micros() 442 | // ******************************************************************************/ 443 | // unsigned long PID::GetTime() 444 | // { 445 | // if (secondsDivider == 1000.0) return millis(); 446 | // return micros(); 447 | // } 448 | // 449 | // /* SetResolution(...)********************************************************** 450 | // * Will set the resolution of GetTime(). 451 | // * MILLIS will set the resolution to milliseconds while 452 | // * MICROS will set the resolution to microseconds. 453 | // ******************************************************************************/ 454 | // void PID::SetResolution(int resolution) 455 | // { 456 | // if (resolution == MILLIS) 457 | // secondsDivider = 1000.0; 458 | // else 459 | // secondsDivider = 1000000.0; 460 | // lastTime = PID::GetTime()-SampleTime; // Update last time variable 461 | // } 462 | // 463 | // void PID::SetITermMax(double iMax) { 464 | // ITermMax = iMax; 465 | // } 466 | // 467 | // /* Status Funcions************************************************************* 468 | // * Just because you set the Kp=-1 doesn't mean it actually happened. these 469 | // * functions query the internal state of the PID. they're here for display 470 | // * purposes. this are the functions the PID Front-end uses for example 471 | // ******************************************************************************/ 472 | // double PID::GetKp(){ return dispKp; } 473 | // double PID::GetKi(){ return dispKi;} 474 | // double PID::GetKd(){ return dispKd;} 475 | // int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} 476 | // int PID::GetDirection(){ return controllerDirection;} 477 | -------------------------------------------------------------------------------- /libraries/PID_v1/PID_v1.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_v1_h 2 | #define PID_v1_h 3 | #define LIBRARY_VERSION 1.0.0 4 | 5 | class PID 6 | { 7 | 8 | 9 | public: 10 | 11 | //Constants used in some of the functions below 12 | #define AUTOMATIC 1 13 | #define MANUAL 0 14 | #define DIRECT 0 15 | #define REVERSE 1 16 | 17 | //commonly used functions ************************************************************************** 18 | PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and 19 | double, double, double, int); // Setpoint. Initial tuning parameters are also set here 20 | 21 | void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) 22 | 23 | bool Compute(); // * performs the PID calculation. it should be 24 | // called every time loop() cycles. ON/OFF and 25 | // calculation frequency can be set using SetMode 26 | // SetSampleTime respectively 27 | 28 | void SetOutputLimits(double, double); //clamps the output to a specific range. 0-255 by default, but 29 | //it's likely the user will want to change this depending on 30 | //the application 31 | 32 | void SetITermMax(double); 33 | 34 | void SetDebugParams(double*, double*, double*); 35 | 36 | 37 | //available but not commonly used functions ******************************************************** 38 | void SetTunings(double, double, // * While most users will set the tunings once in the 39 | double); // constructor, this function gives the user the option 40 | // of changing tunings during runtime for Adaptive control 41 | void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT 42 | // means the output will increase when error is positive. REVERSE 43 | // means the opposite. it's very unlikely that this will be needed 44 | // once it is set in the constructor. 45 | void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which 46 | // the PID calculation is performed. default is 100 47 | 48 | void SetErrorBand(float); 49 | 50 | //Display functions **************************************************************** 51 | double GetKp(); // These functions query the pid for interal values. 52 | double GetKi(); // they were created mainly for the pid front-end, 53 | double GetKd(); // where it's important to know what is actually 54 | int GetMode(); // inside the PID. 55 | int GetDirection(); // 56 | 57 | double* pDebug; 58 | double* iDebug; 59 | double* dDebug; 60 | 61 | private: 62 | double ITermMax; 63 | void Initialize(); 64 | 65 | double dispKp; // * we'll hold on to the tuning parameters in user-entered 66 | double errorBand; // * we'll hold on to the tuning parameters in user-entered 67 | double dispKi; // format for display purposes 68 | double dispKd; // 69 | 70 | double kp; // * (P)roportional Tuning Parameter 71 | double ki; // * (I)ntegral Tuning Parameter 72 | double kd; // * (D)erivative Tuning Parameter 73 | 74 | int controllerDirection; 75 | 76 | double *myInput; // * Pointers to the Input, Output, and Setpoint variables 77 | double *myOutput; // This creates a hard link between the variables and the 78 | double *mySetpoint; // PID, freeing the user from having to constantly tell us 79 | // what these values are. with pointers we'll just know. 80 | 81 | unsigned long lastTime; 82 | double ITerm, lastInput; 83 | 84 | unsigned long SampleTime; 85 | double outMin, outMax; 86 | bool inAuto; 87 | }; 88 | #endif 89 | 90 | 91 | 92 | 93 | // /********************************************************************************************** 94 | // * NOTE - This is a modified version for contuous updates 95 | // * see: https://github.com/br3ttb/Arduino-PID-Library/pull/9 96 | // * 97 | // * Arduino PID Library - Version 1.0.1 98 | // * by Brett Beauregard brettbeauregard.com 99 | // * 100 | // * This Library is licensed under a GPLv3 License 101 | // **********************************************************************************************/ 102 | // 103 | // #ifndef PID_v1_h 104 | // #define PID_v1_h 105 | // #define LIBRARY_VERSION 1.0.0 106 | // 107 | // class PID 108 | // { 109 | // 110 | // 111 | // public: 112 | // 113 | // //Constants used in some of the functions below 114 | // #define AUTOMATIC 1 115 | // #define MANUAL 0 116 | // #define DIRECT 0 117 | // #define REVERSE 1 118 | // #define MILLIS 0 119 | // #define MICROS 1 120 | // 121 | // //commonly used functions ************************************************************************** 122 | // PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and 123 | // double, double, double, int); // Setpoint. Initial tuning parameters are also set here 124 | // 125 | // void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) 126 | // 127 | // bool Compute(); // * performs the PID calculation. it should be 128 | // // called every time loop() cycles. ON/OFF and 129 | // // calculation frequency can be set using SetMode 130 | // // SetSampleTime respectively 131 | // 132 | // void SetOutputLimits(double, double); //clamps the output to a specific range. 0-255 by default, but 133 | // //it's likely the user will want to change this depending on 134 | // //the application 135 | // 136 | // 137 | // 138 | // //available but not commonly used functions ******************************************************** 139 | // void SetTunings(double, double, // * While most users will set the tunings once in the 140 | // double); // constructor, this function gives the user the option 141 | // // of changing tunings during runtime for Adaptive control 142 | // void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT 143 | // // means the output will increase when error is positive. REVERSE 144 | // // means the opposite. it's very unlikely that this will be needed 145 | // // once it is set in the constructor. 146 | // void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which 147 | // // the PID calculation is performed. default is 100 148 | // void SetResolution(int); // * Set the resolution of the GetTime() function. 149 | // // MILLIS sets the resolution to milliseconds. 150 | // // MICROS sets the resolution to microseconds. 151 | // 152 | // void SetITermMax(double); 153 | // 154 | // 155 | // //Display functions **************************************************************** 156 | // double GetKp(); // These functions query the pid for interal values. 157 | // double GetKi(); // they were created mainly for the pid front-end, 158 | // double GetKd(); // where it's important to know what is actually 159 | // int GetMode(); // inside the PID. 160 | // int GetDirection(); // 161 | // 162 | // private: 163 | // void Initialize(); 164 | // unsigned long GetTime(); // * This will call either millis() or micros() 165 | // // depending on the used resolution. 166 | // double ITermMax; 167 | // double dispKp; // * we'll hold on to the tuning parameters in user-entered 168 | // double dispKi; // format for display purposes 169 | // double dispKd; // 170 | // 171 | // double kp; // * (P)roportional Tuning Parameter 172 | // double ki; // * (I)ntegral Tuning Parameter 173 | // double kd; // * (D)erivative Tuning Parameter 174 | // 175 | // int controllerDirection; 176 | // 177 | // double *myInput; // * Pointers to the Input, Output, and Setpoint variables 178 | // double *myOutput; // This creates a hard link between the variables and the 179 | // double *mySetpoint; // PID, freeing the user from having to constantly tell us 180 | // // what these values are. with pointers we'll just know. 181 | // 182 | // unsigned long lastTime; 183 | // double ITerm, lastInput; 184 | // unsigned long timeChange; 185 | // 186 | // unsigned long SampleTime; 187 | // double secondsDivider; 188 | // double outMin, outMax; 189 | // bool inAuto; 190 | // }; 191 | // #endif 192 | -------------------------------------------------------------------------------- /libraries/PinChangeInt/PinChangeInt.h: -------------------------------------------------------------------------------- 1 | // Copyright 2010, 2011, 2012, 2013, 2014 Michael Schwager, Lex Talonis, Chris J. Klick 2 | // This file is part of PinChangeInt. 3 | /* 4 | PinChangeInt is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | */ 17 | 18 | // We use 4-character tabstops, so IN VIM: :set ts=4 sw=4 sts=4 19 | // ...that's: ESCAPE key, colon key, then 20 | // "s-e-t SPACE key t-s = 4 SPACE key s-w = 4 SPACE key s-t-s = 4" 21 | 22 | /* 23 | * This is the PinChangeInt library for the Arduino. 24 | This library provides an extension to the interrupt support for arduino by adding pin change 25 | interrupts, giving a way for users to have interrupts drive off of any pin (ATmega328-based 26 | Arduinos) and by the Port B, J, and K pins on the Arduino Mega and its ilk. 27 | 28 | See the README for license, acknowledgements, and other details. 29 | 30 | See google code project for latest, bugs and info http://code.google.com/p/arduino-pinchangeint/ 31 | See github for the bleeding edge code: https://github.com/GreyGnome/PinChangeInt 32 | For more information Refer to avr-gcc header files, arduino source and atmega datasheet. 33 | 34 | This library was inspired by and derived from Chris J. Klick's PCInt Arduino Playground 35 | example here: http://www.arduino.cc/playground/Main/PcInt 36 | Nice job, Chris! 37 | */ 38 | 39 | //-------- define these in your sketch, if applicable ---------------------------------------------------------- 40 | //-------- These must go in your sketch ahead of the #include statement ----------------------- 41 | // You can reduce the memory footprint of this handler by declaring that there will be no pin change interrupts 42 | // on any one or two of the three ports. If only a single port remains, the handler will be declared inline 43 | // reducing the size and latency of the handler. 44 | // #define NO_PORTB_PINCHANGES // to indicate that port b will not be used for pin change interrupts 45 | // #define NO_PORTC_PINCHANGES // to indicate that port c will not be used for pin change interrupts 46 | // #define NO_PORTD_PINCHANGES // to indicate that port d will not be used for pin change interrupts 47 | // --- Mega support --- 48 | // #define NO_PORTB_PINCHANGES // to indicate that port b will not be used for pin change interrupts 49 | // #define NO_PORTJ_PINCHANGES // to indicate that port j will not be used for pin change interrupts 50 | // #define NO_PORTK_PINCHANGES // to indicate that port k will not be used for pin change interrupts 51 | // In the Mega, there is no Port C, no Port D. Instead, you get Port J and Port K. Port B remains. 52 | // Port J, however, is practically useless because there is only 1 pin available for interrupts. Most 53 | // of the Port J pins are not even connected to a header connection. // "Mega Support" notes 54 | // --- Sanguino, Mioduino support --- 55 | // #define NO_PORTA_PINCHANGES // to indicate that port a will not be used for pin change interrupts 56 | 57 | // You can reduce the code size by 20-50 bytes, and you can speed up the interrupt routine 58 | // slightly by declaring that you don't care if the static variables PCintPort::pinState and/or 59 | // PCintPort::arduinoPin are set and made available to your interrupt routine. 60 | // #define NO_PIN_STATE // to indicate that you don't need the pinState 61 | // #define NO_PIN_NUMBER // to indicate that you don't need the arduinoPin 62 | // #define DISABLE_PCINT_MULTI_SERVICE // to limit the handler to servicing a single interrupt per invocation. 63 | // #define GET_PCINT_VERSION // to enable the uint16_t getPCIintVersion () function. 64 | // The following is intended for testing purposes. If defined, then a whole host of static variables can be read 65 | // in your interrupt subroutine. It is not defined by default, and you DO NOT want to define this in 66 | // Production code!: 67 | // #define PINMODE 68 | //-------- define the above in your sketch, if applicable ------------------------------------------------------ 69 | 70 | /* 71 | PinChangeInt.h 72 | ---- VERSIONS --- (NOTE TO SELF: Update the PCINT_VERSION define, below) ----------------- 73 | ...Moved to RELEASE_NOTES. 74 | 75 | See the README file for the License and more details. 76 | */ 77 | 78 | #ifndef PinChangeInt_h 79 | #define PinChangeInt_h 80 | 81 | #define PCINT_VERSION 2190 // This number MUST agree with the version number, above. 82 | 83 | #include "stddef.h" 84 | 85 | // Thanks to Maurice Beelen, nms277, Akesson Karlpetter, and Orly Andico for these fixes. 86 | #if defined(ARDUINO) && ARDUINO >= 100 87 | #include 88 | #include 89 | #include // cby and sbi defined here 90 | #else 91 | #include 92 | #include 93 | #ifndef LIBCALL_PINCHANGEINT 94 | #include "../cppfix/cppfix.h" 95 | #endif 96 | #endif 97 | 98 | 99 | #undef DEBUG 100 | 101 | /* 102 | * Theory: all IO pins on Atmega168 are covered by Pin Change Interrupts. 103 | * The PCINT corresponding to the pin must be enabled and masked, and 104 | * an ISR routine provided. Since PCINTs are per port, not per pin, the ISR 105 | * must use some logic to actually implement a per-pin interrupt service. 106 | */ 107 | 108 | /* Pin to interrupt map: 109 | * D0-D7 = PCINT 16-23 = PCIR2 = PD = PCIE2 = pcmsk2 110 | * D8-D13 = PCINT 0-5 = PCIR0 = PB = PCIE0 = pcmsk0 111 | * A0-A5 (D14-D19) = PCINT 8-13 = PCIR1 = PC = PCIE1 = pcmsk1 112 | */ 113 | 114 | #undef INLINE_PCINT 115 | #define INLINE_PCINT 116 | // Thanks to cserveny...@gmail.com for MEGA support! 117 | #if defined __AVR_ATmega2560__ || defined __AVR_ATmega1280__ || defined __AVR_ATmega1281__ || defined __AVR_ATmega2561__ || defined __AVR_ATmega640__ 118 | #define __USE_PORT_JK 119 | // Mega does not have PORTA, C or D 120 | #define NO_PORTA_PINCHANGES 121 | #define NO_PORTC_PINCHANGES 122 | #define NO_PORTD_PINCHANGES 123 | #if ((defined(NO_PORTB_PINCHANGES) && defined(NO_PORTJ_PINCHANGES)) || \ 124 | (defined(NO_PORTJ_PINCHANGES) && defined(NO_PORTK_PINCHANGES)) || \ 125 | (defined(NO_PORTK_PINCHANGES) && defined(NO_PORTB_PINCHANGES))) 126 | #define INLINE_PCINT inline 127 | #endif 128 | #else 129 | #define NO_PORTJ_PINCHANGES 130 | #define NO_PORTK_PINCHANGES 131 | #if defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) 132 | #ifndef NO_PORTA_PINCHANGES 133 | #define __USE_PORT_A 134 | #endif 135 | #else 136 | #define NO_PORTA_PINCHANGES 137 | #endif 138 | // if defined only D .OR. only C .OR. only B .OR. only A, then inline it 139 | #if ( (defined(NO_PORTA_PINCHANGES) && defined(NO_PORTB_PINCHANGES) && defined(NO_PORTC_PINCHANGES)) || \ 140 | (defined(NO_PORTA_PINCHANGES) && defined(NO_PORTB_PINCHANGES) && defined(NO_PORTD_PINCHANGES)) || \ 141 | (defined(NO_PORTA_PINCHANGES) && defined(NO_PORTC_PINCHANGES) && defined(NO_PORTD_PINCHANGES)) || \ 142 | (defined(NO_PORTB_PINCHANGES) && defined(NO_PORTC_PINCHANGES) && defined(NO_PORTD_PINCHANGES)) ) 143 | #define INLINE_PCINT inline 144 | #endif 145 | #endif 146 | 147 | // Provide drop in compatibility with johnboiles PCInt project at 148 | // http://www.arduino.cc/playground/Main/PcInt 149 | #define PCdetachInterrupt(pin) PCintPort::detachInterrupt(pin) 150 | #define PCattachInterrupt(pin,userFunc,mode) PCintPort::attachInterrupt(pin, userFunc,mode) 151 | #define PCgetArduinoPin() PCintPort::getArduinoPin() 152 | 153 | typedef void (*PCIntvoidFuncPtr)(void); 154 | 155 | class PCintPort { 156 | public: 157 | PCintPort(int index,int pcindex, volatile uint8_t& maskReg) : 158 | portInputReg(*portInputRegister(index)), 159 | portPCMask(maskReg), 160 | PCICRbit(1 << pcindex), 161 | portRisingPins(0), 162 | portFallingPins(0), 163 | firstPin(NULL) 164 | #ifdef PINMODE 165 | ,intrCount(0) 166 | #endif 167 | { 168 | #ifdef FLASH 169 | ledsetup(); 170 | #endif 171 | } 172 | volatile uint8_t& portInputReg; 173 | static int8_t attachInterrupt(uint8_t pin, PCIntvoidFuncPtr userFunc, int mode); 174 | static void detachInterrupt(uint8_t pin); 175 | INLINE_PCINT void PCint(); 176 | static volatile uint8_t curr; 177 | #ifndef NO_PIN_NUMBER 178 | static volatile uint8_t arduinoPin; 179 | #endif 180 | #ifndef NO_PIN_STATE 181 | static volatile uint8_t pinState; 182 | #endif 183 | #ifdef PINMODE 184 | static volatile uint8_t pinmode; 185 | static volatile uint8_t s_portRisingPins; 186 | static volatile uint8_t s_portFallingPins; 187 | static volatile uint8_t s_lastPinView; 188 | static volatile uint8_t s_pmask; 189 | static volatile char s_PORT; 190 | static volatile uint8_t s_changedPins; 191 | static volatile uint8_t s_portRisingPins_nCurr; 192 | static volatile uint8_t s_portFallingPins_nNCurr; 193 | static volatile uint8_t s_currXORlastPinView; 194 | volatile uint8_t intrCount; 195 | static volatile uint8_t s_count; 196 | static volatile uint8_t pcint_multi; 197 | static volatile uint8_t PCIFRbug; 198 | #endif 199 | #ifdef FLASH 200 | static void ledsetup(void); 201 | #endif 202 | 203 | protected: 204 | class PCintPin { 205 | public: 206 | PCintPin() : 207 | PCintFunc((PCIntvoidFuncPtr)NULL), 208 | mode(0) {} 209 | PCIntvoidFuncPtr PCintFunc; 210 | uint8_t mode; 211 | uint8_t mask; 212 | uint8_t arduinoPin; 213 | PCintPin* next; 214 | }; 215 | void enable(PCintPin* pin, PCIntvoidFuncPtr userFunc, uint8_t mode); 216 | int8_t addPin(uint8_t arduinoPin,PCIntvoidFuncPtr userFunc, uint8_t mode); 217 | volatile uint8_t& portPCMask; 218 | const uint8_t PCICRbit; 219 | volatile uint8_t portRisingPins; 220 | volatile uint8_t portFallingPins; 221 | volatile uint8_t lastPinView; 222 | PCintPin* firstPin; 223 | }; 224 | 225 | #ifndef LIBCALL_PINCHANGEINT // LIBCALL_PINCHANGEINT *********************************************** 226 | volatile uint8_t PCintPort::curr=0; 227 | #ifndef NO_PIN_NUMBER 228 | volatile uint8_t PCintPort::arduinoPin=0; 229 | #endif 230 | #ifndef NO_PIN_STATE 231 | volatile uint8_t PCintPort::pinState=0; 232 | #endif 233 | #ifdef PINMODE 234 | volatile uint8_t PCintPort::pinmode=0; 235 | volatile uint8_t PCintPort::s_portRisingPins=0; 236 | volatile uint8_t PCintPort::s_portFallingPins=0; 237 | volatile uint8_t PCintPort::s_lastPinView=0; 238 | volatile uint8_t PCintPort::s_pmask=0; 239 | volatile char PCintPort::s_PORT='x'; 240 | volatile uint8_t PCintPort::s_changedPins=0; 241 | volatile uint8_t PCintPort::s_portRisingPins_nCurr=0; 242 | volatile uint8_t PCintPort::s_portFallingPins_nNCurr=0; 243 | volatile uint8_t PCintPort::s_currXORlastPinView=0; 244 | volatile uint8_t PCintPort::s_count=0; 245 | volatile uint8_t PCintPort::pcint_multi=0; 246 | volatile uint8_t PCintPort::PCIFRbug=0; 247 | #endif 248 | 249 | #ifdef FLASH 250 | #define PINLED 13 251 | volatile uint8_t *led_port; 252 | uint8_t led_mask; 253 | uint8_t not_led_mask; 254 | boolean ledsetup_run=false; 255 | void PCintPort::ledsetup(void) { 256 | if (! ledsetup_run) { 257 | led_port=portOutputRegister(digitalPinToPort(PINLED)); 258 | led_mask=digitalPinToBitMask(PINLED); 259 | not_led_mask=led_mask^0xFF; 260 | pinMode(PINLED, OUTPUT); digitalWrite(PINLED, LOW); 261 | ledsetup_run=true; 262 | } 263 | }; 264 | #endif 265 | 266 | // 267 | // ATMEGA 644 268 | // 269 | #if defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) // Sanguino, Mosquino uino bobino bonanafannafofino, me my momino... 270 | 271 | #ifndef NO_PORTA_PINCHANGES 272 | PCintPort portA=PCintPort(1, 0,PCMSK0); // port PA==1 (from Arduino.h, Arduino version 1.0) 273 | #endif 274 | #ifndef NO_PORTB_PINCHANGES 275 | PCintPort portB=PCintPort(2, 1,PCMSK1); // port PB==2 (from Arduino.h, Arduino version 1.0) 276 | #endif 277 | #ifndef NO_PORTC_PINCHANGES 278 | PCintPort portC=PCintPort(3, 2,PCMSK2); // port PC==3 (also in pins_arduino.c, Arduino version 022) 279 | #endif 280 | #ifndef NO_PORTD_PINCHANGES 281 | PCintPort portD=PCintPort(4, 3,PCMSK3); // port PD==4 282 | #endif 283 | 284 | #else // others 285 | 286 | #ifndef NO_PORTB_PINCHANGES 287 | PCintPort portB=PCintPort(2, 0,PCMSK0); // port PB==2 (from Arduino.h, Arduino version 1.0) 288 | #endif 289 | #ifndef NO_PORTC_PINCHANGES // note: no PORTC on MEGA 290 | PCintPort portC=PCintPort(3, 1,PCMSK1); // port PC==3 (also in pins_arduino.c, Arduino version 022) 291 | #endif 292 | #ifndef NO_PORTD_PINCHANGES // note: no PORTD on MEGA 293 | PCintPort portD=PCintPort(4, 2,PCMSK2); // port PD==4 294 | #endif 295 | 296 | #endif // defined __AVR_ATmega644__ 297 | 298 | #ifdef __USE_PORT_JK 299 | #ifndef NO_PORTJ_PINCHANGES 300 | PCintPort portJ=PCintPort(10,1,PCMSK1); // port PJ==10 301 | #endif 302 | #ifndef NO_PORTK_PINCHANGES 303 | PCintPort portK=PCintPort(11,2,PCMSK2); // port PK==11 304 | #endif 305 | #endif // USE_PORT_JK 306 | 307 | static PCintPort *lookupPortNumToPort( int portNum ) { 308 | PCintPort *port = NULL; 309 | 310 | switch (portNum) { 311 | #ifndef NO_PORTA_PINCHANGES 312 | case 1: 313 | port=&portA; 314 | break; 315 | #endif 316 | #ifndef NO_PORTB_PINCHANGES 317 | case 2: 318 | port=&portB; 319 | break; 320 | #endif 321 | #ifndef NO_PORTC_PINCHANGES 322 | case 3: 323 | port=&portC; 324 | break; 325 | #endif 326 | #ifndef NO_PORTD_PINCHANGES 327 | case 4: 328 | port=&portD; 329 | break; 330 | #endif 331 | #ifdef __USE_PORT_JK 332 | 333 | #ifndef NO_PORTJ_PINCHANGES 334 | case 10: 335 | port=&portJ; 336 | break; 337 | #endif 338 | 339 | #ifndef NO_PORTK_PINCHANGES 340 | case 11: 341 | port=&portK; 342 | break; 343 | #endif 344 | 345 | #endif // __USE_PORT_JK 346 | } 347 | 348 | return port; 349 | } 350 | 351 | 352 | void PCintPort::enable(PCintPin* p, PCIntvoidFuncPtr userFunc, uint8_t mode) { 353 | // Enable the pin for interrupts by adding to the PCMSKx register. 354 | // ...The final steps; at this point the interrupt is enabled on this pin. 355 | p->mode=mode; 356 | p->PCintFunc=userFunc; 357 | #ifndef NO_PORTJ_PINCHANGES 358 | // A big shout out to jrhelbert for this fix! Thanks!!! 359 | if ((p->arduinoPin == 14) || (p->arduinoPin == 15)) { 360 | portPCMask |= (p->mask << 1); // PORTJ's PCMSK1 is a little odd... 361 | } 362 | else { 363 | portPCMask |= p->mask; 364 | } 365 | #else 366 | portPCMask |= p->mask; 367 | #endif 368 | if ((p->mode == RISING) || (p->mode == CHANGE)) portRisingPins |= p->mask; 369 | if ((p->mode == FALLING) || (p->mode == CHANGE)) portFallingPins |= p->mask; 370 | PCICR |= PCICRbit; 371 | } 372 | 373 | int8_t PCintPort::addPin(uint8_t arduinoPin, PCIntvoidFuncPtr userFunc, uint8_t mode) 374 | { 375 | PCintPin* tmp; 376 | 377 | tmp=firstPin; 378 | // Add to linked list, starting with firstPin. If pin already exists, just enable. 379 | if (firstPin != NULL) { 380 | do { 381 | if (tmp->arduinoPin == arduinoPin) { enable(tmp, userFunc, mode); return(0); } 382 | if (tmp->next == NULL) break; 383 | tmp=tmp->next; 384 | } while (true); 385 | } 386 | 387 | // Create pin p: fill in the data. 388 | PCintPin* p=new PCintPin; 389 | if (p == NULL) return(-1); 390 | p->arduinoPin=arduinoPin; 391 | p->mode = mode; 392 | p->next=NULL; 393 | p->mask = digitalPinToBitMask(arduinoPin); // the mask 394 | 395 | if (firstPin == NULL) firstPin=p; 396 | else tmp->next=p; // NOTE that tmp cannot be NULL. 397 | 398 | #ifdef DEBUG 399 | Serial.print("addPin. pin given: "); Serial.print(arduinoPin, DEC); 400 | int addr = (int) p; 401 | Serial.print(" instance addr: "); Serial.println(addr, HEX); 402 | Serial.print("userFunc addr: "); Serial.println((int)p->PCintFunc, HEX); 403 | #endif 404 | 405 | enable(p, userFunc, mode); 406 | #ifdef DEBUG 407 | Serial.print("addPin. pin given: "); Serial.print(arduinoPin, DEC), Serial.print (" pin stored: "); 408 | int addr = (int) p; 409 | Serial.print(" instance addr: "); Serial.println(addr, HEX); 410 | #endif 411 | return(1); 412 | } 413 | 414 | /* 415 | * attach an interrupt to a specific pin using pin change interrupts. 416 | */ 417 | int8_t PCintPort::attachInterrupt(uint8_t arduinoPin, PCIntvoidFuncPtr userFunc, int mode) 418 | { 419 | PCintPort *port; 420 | uint8_t portNum = digitalPinToPort(arduinoPin); 421 | if ((portNum == NOT_A_PORT) || (userFunc == NULL)) return(-1); 422 | 423 | port=lookupPortNumToPort(portNum); 424 | // Added by GreyGnome... must set the initial value of lastPinView for it to be correct on the 1st interrupt. 425 | // ...but even then, how do you define "correct"? Ultimately, the user must specify (not provisioned for yet). 426 | port->lastPinView=port->portInputReg; 427 | #ifdef DEBUG 428 | Serial.print("attachInterrupt- pin: "); Serial.println(arduinoPin, DEC); 429 | #endif 430 | // map pin to PCIR register 431 | return(port->addPin(arduinoPin,userFunc,mode)); 432 | } 433 | 434 | void PCintPort::detachInterrupt(uint8_t arduinoPin) 435 | { 436 | PCintPort *port; 437 | PCintPin* current; 438 | uint8_t mask; 439 | uint8_t portNum = digitalPinToPort(arduinoPin); 440 | if (portNum == NOT_A_PORT) return; 441 | port=lookupPortNumToPort(portNum); 442 | mask=digitalPinToBitMask(arduinoPin); 443 | current=port->firstPin; 444 | while (current) { 445 | if (current->mask == mask) { // found the target 446 | uint8_t oldSREG = SREG; 447 | cli(); // disable interrupts 448 | #ifndef NO_PORTJ_PINCHANGES 449 | // A big shout out to jrhelbert for this fix! Thanks!!! 450 | if ((arduinoPin == 14) || (arduinoPin == 15)) { 451 | port->portPCMask &= ~(mask << 1); // PORTJ's PCMSK1 is a little odd... 452 | } 453 | else { 454 | port->portPCMask &= ~mask; // disable the mask entry. 455 | } 456 | #else 457 | port->portPCMask &= ~mask; // disable the mask entry. 458 | #endif 459 | if (port->portPCMask == 0) PCICR &= ~(port->PCICRbit); 460 | port->portRisingPins &= ~current->mask; port->portFallingPins &= ~current->mask; 461 | // TODO: This is removed until we can add code that frees memory. 462 | // Note that in the addPin() function, above, we do not define a new pin if it was 463 | // once already defined. 464 | // ... ... 465 | // Link the previous' next to the found next. Then remove the found. 466 | //if (prev != NULL) prev->next=current->next; // linked list skips over current. 467 | //else firstPin=current->next; // at the first pin; save the new first pin 468 | SREG = oldSREG; // Restore register; reenables interrupts 469 | return; 470 | } 471 | current=current->next; 472 | } 473 | } 474 | 475 | // common code for isr handler. "port" is the PCINT number. 476 | // there isn't really a good way to back-map ports and masks to pins. 477 | void PCintPort::PCint() { 478 | 479 | #ifdef FLASH 480 | if (*led_port & led_mask) *led_port&=not_led_mask; 481 | else *led_port|=led_mask; 482 | #endif 483 | #ifndef DISABLE_PCINT_MULTI_SERVICE 484 | uint8_t pcifr; 485 | while (true) { 486 | #endif 487 | // get the pin states for the indicated port. 488 | #ifdef PINMODE 489 | PCintPort::s_lastPinView=lastPinView; 490 | intrCount++; 491 | PCintPort::s_count=intrCount; 492 | #endif 493 | // OLD v. 2.01 technique: Test 1: 3163; Test 7: 3993 494 | // From robtillaart online: ------------ (starting v. 2.11beta) 495 | // uint8_t changedPins = PCintPort::curr ^ lastPinView; 496 | // lastPinView = PCintPort::curr; 497 | // uint8_t fastMask = changedPins & ((portRisingPins & PCintPort::curr ) | ( portFallingPins & ~PCintPort::curr )); 498 | // NEW v. 2.11 technique: Test 1: 3270 Test 7: 3987 499 | // ------------------------------------- 500 | // was: uint8_t changedPins = PCintPort::curr ^ lastPinView; 501 | // makes test 6 of the PinChangeIntSpeedTest go from 3867 to 3923. Not good. 502 | uint8_t changedPins = (PCintPort::curr ^ lastPinView) & 503 | ((portRisingPins & PCintPort::curr ) | ( portFallingPins & ~PCintPort::curr )); 504 | 505 | #ifdef PINMODE 506 | PCintPort::s_currXORlastPinView=PCintPort::curr ^ lastPinView; 507 | PCintPort::s_portRisingPins_nCurr=portRisingPins & PCintPort::curr; 508 | PCintPort::s_portFallingPins_nNCurr=portFallingPins & ~PCintPort::curr; 509 | #endif 510 | lastPinView = PCintPort::curr; 511 | 512 | PCintPin* p = firstPin; 513 | while (p) { 514 | // Trigger interrupt if the bit is high and it's set to trigger on mode RISING or CHANGE 515 | // Trigger interrupt if the bit is low and it's set to trigger on mode FALLING or CHANGE 516 | if (p->mask & changedPins) { 517 | #ifndef NO_PIN_STATE 518 | PCintPort::pinState=PCintPort::curr & p->mask ? HIGH : LOW; 519 | #endif 520 | #ifndef NO_PIN_NUMBER 521 | PCintPort::arduinoPin=p->arduinoPin; 522 | #endif 523 | #ifdef PINMODE 524 | PCintPort::pinmode=p->mode; 525 | PCintPort::s_portRisingPins=portRisingPins; 526 | PCintPort::s_portFallingPins=portFallingPins; 527 | PCintPort::s_pmask=p->mask; 528 | PCintPort::s_changedPins=changedPins; 529 | #endif 530 | p->PCintFunc(); 531 | } 532 | p=p->next; 533 | } 534 | #ifndef DISABLE_PCINT_MULTI_SERVICE 535 | pcifr = PCIFR & PCICRbit; 536 | if (pcifr == 0) break; 537 | PCIFR |= PCICRbit; 538 | #ifdef PINMODE 539 | PCintPort::pcint_multi++; 540 | if (PCIFR & PCICRbit) PCintPort::PCIFRbug=1; // PCIFR & PCICRbit should ALWAYS be 0 here! 541 | #endif 542 | PCintPort::curr=portInputReg; 543 | } 544 | #endif 545 | } 546 | 547 | #ifndef NO_PORTA_PINCHANGES 548 | ISR(PCINT0_vect) { 549 | #ifdef PINMODE 550 | PCintPort::s_PORT='A'; 551 | #endif 552 | PCintPort::curr = portA.portInputReg; 553 | portA.PCint(); 554 | } 555 | #define PORTBVECT PCINT1_vect 556 | #define PORTCVECT PCINT2_vect 557 | #define PORTDVECT PCINT3_vect 558 | #else 559 | #define PORTBVECT PCINT0_vect 560 | #define PORTCVECT PCINT1_vect 561 | #define PORTDVECT PCINT2_vect 562 | #endif 563 | 564 | #ifndef NO_PORTB_PINCHANGES 565 | ISR(PORTBVECT) { 566 | #ifdef PINMODE 567 | PCintPort::s_PORT='B'; 568 | #endif 569 | PCintPort::curr = portB.portInputReg; 570 | portB.PCint(); 571 | } 572 | #endif 573 | 574 | #ifndef NO_PORTC_PINCHANGES 575 | ISR(PORTCVECT) { 576 | #ifdef PINMODE 577 | PCintPort::s_PORT='C'; 578 | #endif 579 | PCintPort::curr = portC.portInputReg; 580 | portC.PCint(); 581 | } 582 | #endif 583 | 584 | #ifndef NO_PORTD_PINCHANGES 585 | ISR(PORTDVECT){ 586 | #ifdef PINMODE 587 | PCintPort::s_PORT='D'; 588 | #endif 589 | PCintPort::curr = portD.portInputReg; 590 | portD.PCint(); 591 | } 592 | #endif 593 | 594 | #ifdef __USE_PORT_JK 595 | #ifndef NO_PORTJ_PINCHANGES 596 | ISR(PCINT1_vect) { 597 | #ifdef PINMODE 598 | PCintPort::s_PORT='J'; 599 | #endif 600 | PCintPort::curr = portJ.portInputReg; 601 | portJ.PCint(); 602 | } 603 | #endif 604 | 605 | #ifndef NO_PORTK_PINCHANGES 606 | ISR(PCINT2_vect){ 607 | #ifdef PINMODE 608 | PCintPort::s_PORT='K'; 609 | #endif 610 | PCintPort::curr = portK.portInputReg; 611 | portK.PCint(); 612 | } 613 | #endif 614 | 615 | #endif // __USE_PORT_JK 616 | 617 | #ifdef GET_PCINT_VERSION 618 | uint16_t getPCIntVersion () { 619 | return ((uint16_t) PCINT_VERSION); 620 | } 621 | #endif // GET_PCINT_VERSION 622 | #endif // #ifndef LIBCALL_PINCHANGEINT ************************************************************* 623 | #endif // #ifndef PinChangeInt_h ******************************************************************* 624 | -------------------------------------------------------------------------------- /src/Makefile: -------------------------------------------------------------------------------- 1 | ARDUINO_DIR = /Applications/Arduino.app/Contents/Resources/Java 2 | USER_LIB_PATH = ./../libraries 3 | OBJDIR = ./.build-$(BOARD_TAG) 4 | ARDUINO_LIBS = PinChangeInt PID_v1 Wire MPULib MedianFilter 5 | 6 | #BOARD_TAG = mega2560 7 | BOARD_TAG = pro5v328 8 | MONITOR_BAUDRATE = 115200 9 | #MONITOR_PORT = /dev/tty.usbmodem* 10 | MONITOR_PORT = /dev/tty.usbserial 11 | AVRDDUDE = /usr/local/bin/avrdude 12 | 13 | include ~/hardware/Arduino-Makefile/Arduino.mk 14 | -------------------------------------------------------------------------------- /src/debugger.cpp: -------------------------------------------------------------------------------- 1 | #include "debugger.h" 2 | 3 | void Debugger::init(RemoteControl *_rc, IMU *_imu, FlightController *_fc) { 4 | rc = _rc; 5 | imu = _imu; 6 | fc = _fc; 7 | 8 | last_debug_time = micros(); 9 | } 10 | 11 | void Debugger::print() { 12 | if (millis() - last_debug_time > DEBUG_RATE_MILLIS) { 13 | 14 | if (CHART_DEBUG) { 15 | chart_debug(); 16 | } else { 17 | print_debug(); 18 | } 19 | 20 | last_debug_time = micros(); 21 | } 22 | } 23 | 24 | void Debugger::chart_debug() { 25 | if (fc->mode == RATE) { 26 | Serial.print(rc->get(RC_PITCH)); 27 | Serial.print(" "); 28 | Serial.print(imu->y_rate); 29 | Serial.print(" "); 30 | Serial.print(fc->pitch_rate_pid.GetKp()); 31 | Serial.print(" "); 32 | Serial.print(fc->pitch_rate_pid.GetKi()); 33 | Serial.print(" "); 34 | Serial.print(fc->pitch_rate_pid.GetKd()); 35 | Serial.print(" "); 36 | Serial.print(imu->x_angle); 37 | Serial.print("\r"); 38 | } else { 39 | Serial.print(rc->get(RC_PITCH)); 40 | Serial.print(" "); 41 | Serial.print(imu->y_angle); 42 | Serial.print(" "); 43 | Serial.print(fc->pitch_angle_pid.GetKp()); 44 | Serial.print(" "); 45 | Serial.print(fc->pitch_angle_pid.GetKi()); 46 | Serial.print(" "); 47 | Serial.print(fc->pitch_angle_pid.GetKd()); 48 | Serial.print(" "); 49 | Serial.print(imu->x_angle); 50 | Serial.print("\r"); 51 | } 52 | } 53 | 54 | void Debugger::print_debug() { 55 | Serial.print("x_gyro: "); Serial.print(imu->x_rate); 56 | Serial.print(" \t y_gyro: "); Serial.print(imu->y_rate); 57 | Serial.print(" \t z_gyro: "); Serial.print(imu->z_rate); 58 | Serial.print(" \t x_ang: "); Serial.print(imu->x_angle); 59 | Serial.print(" \t y_ang "); Serial.print(imu->y_angle); 60 | Serial.print(" \t x_ang_raw: "); Serial.print(imu->acc_x_in); 61 | Serial.print(" \t y_ang_raw: "); Serial.print(imu->acc_y_in); 62 | Serial.print(" \t z_ang_raw "); Serial.print(imu->acc_z_in); 63 | Serial.print(" \t z_gyro_raw "); Serial.print(imu->gyro_z_rate); 64 | Serial.println(); 65 | 66 | Serial.print("thrttl: "); Serial.print(rc->get(RC_THROTTLE)); 67 | Serial.print("\t x_tar: "); Serial.print(rc->get(RC_ROLL)); 68 | Serial.print("\t y_tar: "); Serial.print(rc->get(RC_PITCH)); 69 | Serial.print("\t z_tar: "); Serial.print(rc->get(RC_YAW)); 70 | Serial.print("\t pot_a: "); Serial.print(rc->get(RC_POT_A)); 71 | Serial.print("\t pot_b: "); Serial.print(rc->get(RC_POT_B)); 72 | Serial.println(); 73 | 74 | Serial.print("M1_out: "); Serial.print(fc->motors.outputs[M1]); 75 | Serial.print("\t M2_out: "); Serial.print(fc->motors.outputs[M2]); 76 | Serial.print("\t M3_out: "); Serial.print(fc->motors.outputs[M3]); 77 | Serial.print("\t M4_out: "); Serial.print(fc->motors.outputs[M4]); 78 | Serial.println(); 79 | 80 | Serial.print("roll_rate_pid: "); Serial.print(fc->pid_outputs[PID_ROLL_RATE]); 81 | Serial.print("\t pitch_rate_pid: "); Serial.print(fc->pid_outputs[PID_PITCH_RATE]); 82 | Serial.print("\t yaw_rate_pid: "); Serial.print(fc->pid_outputs[PID_YAW_RATE]); 83 | Serial.print("\t pitch_rate_pid.GetKp(), 5); 84 | Serial.print("\t pitch_rate_pid.GetKi(), 5); 85 | Serial.print("\t pitch_rate_pid.GetKd(), 5); 86 | Serial.println(); 87 | 88 | Serial.print("roll_angle_pid: "); Serial.print(fc->pid_outputs[PID_ROLL_ANGLE]); 89 | Serial.print("\t pitch_angle_pid: "); Serial.print(fc->pid_outputs[PID_PITCH_ANGLE]); 90 | Serial.print("\t pitch_angle_pid.GetKp(), 5); 91 | Serial.print("\t pitch_angle_pid.GetKi(), 5); 92 | Serial.print("\t pitch_angle_pid.GetKd(), 5); 93 | Serial.println(); 94 | 95 | if (fc->emergency_stopped) Serial.println("\t EMERGENCY STOPPED!"); 96 | } 97 | -------------------------------------------------------------------------------- /src/debugger.h: -------------------------------------------------------------------------------- 1 | #ifndef debugger_h 2 | #define debugger_h 3 | 4 | #define DEBUG true 5 | #define CHART_DEBUG false 6 | 7 | #if DEBUG 8 | #if CHART_DEBUG 9 | #define DEBUG_RATE_MILLIS 50 10 | #else 11 | #define DEBUG_RATE_MILLIS 500 12 | #endif 13 | #endif 14 | 15 | #include "remote_control.h" 16 | #include "flight_controller.h" 17 | #include "imu.h" 18 | 19 | class Debugger { 20 | public: 21 | void init(RemoteControl*, IMU*, FlightController*); 22 | void print(); 23 | 24 | private: 25 | RemoteControl *rc; 26 | IMU *imu; 27 | FlightController *fc; 28 | 29 | void chart_debug(); 30 | void print_debug(); 31 | 32 | uint32_t last_debug_time; 33 | uint32_t loop_time; 34 | uint32_t loop_start_time; 35 | }; 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /src/flight_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "flight_controller.h" 2 | 3 | void FlightController::init(RemoteControl *_rc, IMU *_imu) { 4 | rc = _rc; 5 | imu = _imu; 6 | 7 | mode = STABILIZE; 8 | safety_mode = UNARMED; 9 | emergency_stopped = false; 10 | gyro_freeze_counter = 0; 11 | 12 | for (int i = 0; i < NUM_PIDS; i++) { 13 | pid_inputs[i] = 0.0; 14 | pid_outputs[i] = 0.0; 15 | pid_setpoints[i] = 0.0; 16 | pid_p_debugs[i] = 0.0; 17 | pid_i_debugs[i] = 0.0; 18 | pid_d_debugs[i] = 0.0; 19 | } 20 | 21 | roll_rate_pid.SetMode(AUTOMATIC); 22 | roll_rate_pid.SetSampleTime(3); 23 | roll_rate_pid.SetDebugParams(&pid_p_debugs[PID_ROLL_RATE], 24 | &pid_i_debugs[PID_ROLL_RATE], 25 | &pid_d_debugs[PID_ROLL_RATE]); 26 | 27 | pitch_rate_pid.SetMode(AUTOMATIC); 28 | pitch_rate_pid.SetSampleTime(3); 29 | pitch_rate_pid.SetDebugParams(&pid_p_debugs[PID_PITCH_RATE], 30 | &pid_i_debugs[PID_PITCH_RATE], 31 | &pid_d_debugs[PID_PITCH_RATE]); 32 | 33 | yaw_rate_pid.SetMode(AUTOMATIC); 34 | yaw_rate_pid.SetSampleTime(3); 35 | yaw_rate_pid.SetDebugParams(&pid_p_debugs[PID_YAW_RATE], 36 | &pid_i_debugs[PID_YAW_RATE], 37 | &pid_d_debugs[PID_YAW_RATE]); 38 | 39 | roll_angle_pid.SetMode(AUTOMATIC); 40 | roll_angle_pid.SetSampleTime(3); 41 | roll_angle_pid.SetDebugParams(&pid_p_debugs[PID_ROLL_ANGLE], 42 | &pid_i_debugs[PID_ROLL_ANGLE], 43 | &pid_d_debugs[PID_ROLL_ANGLE]); 44 | 45 | pitch_angle_pid.SetMode(AUTOMATIC); 46 | pitch_angle_pid.SetSampleTime(3); 47 | pitch_angle_pid.SetDebugParams(&pid_p_debugs[PID_PITCH_ANGLE], 48 | &pid_i_debugs[PID_PITCH_ANGLE], 49 | &pid_d_debugs[PID_PITCH_ANGLE]); 50 | 51 | motors.init(); 52 | } 53 | 54 | void FlightController::process() { 55 | set_safety_mode(); 56 | 57 | set_pid_output_limits(); 58 | adjust_pid_tuning(); 59 | compute_pids(); 60 | 61 | if (safety_mode == ARMED) { 62 | compute_motor_outputs(); 63 | adjust_for_bounds(); 64 | } else { 65 | motors.command_all_off(); 66 | } 67 | 68 | safety_check(); 69 | motors.command(); 70 | } 71 | 72 | void FlightController::adjust_for_bounds() { 73 | int16_t motor_fix = 0; 74 | uint16_t motor_min = motors.outputs[0]; 75 | uint16_t motor_max = motors.outputs[0]; 76 | 77 | for(int i = 1; i < NUM_MOTORS; i++) { 78 | if (motors.outputs[i] < motor_min) motor_min = motors.outputs[i]; 79 | if (motors.outputs[i] > motor_max) motor_max = motors.outputs[i]; 80 | } 81 | 82 | if (motor_min < MOTOR_MIN) { 83 | motor_fix = MOTOR_MIN - motor_min; 84 | } else if (motor_max > MOTOR_MAX) { 85 | motor_fix = MOTOR_MAX - motor_max; 86 | } 87 | 88 | for(int i = 0; i < NUM_MOTORS; i++) { 89 | motors.outputs[i] += motor_fix; 90 | } 91 | } 92 | 93 | void FlightController::compute_motor_outputs() { 94 | 95 | double m1_r_out = rc->get(RC_THROTTLE) + pid_outputs[PID_ROLL_RATE] + pid_outputs[PID_YAW_RATE]; 96 | double m2_l_out = rc->get(RC_THROTTLE) - pid_outputs[PID_ROLL_RATE] + pid_outputs[PID_YAW_RATE]; 97 | double m3_f_out = rc->get(RC_THROTTLE) - pid_outputs[PID_PITCH_RATE] - pid_outputs[PID_YAW_RATE]; 98 | double m4_b_out = rc->get(RC_THROTTLE) + pid_outputs[PID_PITCH_RATE] - pid_outputs[PID_YAW_RATE]; 99 | 100 | // double m1_r_out = rc->get(RC_THROTTLE) + pid_outputs[PID_ROLL_RATE]; 101 | // double m2_l_out = rc->get(RC_THROTTLE) - pid_outputs[PID_ROLL_RATE]; 102 | // double m3_f_out = rc->get(RC_THROTTLE) - pid_outputs[PID_PITCH_RATE]; 103 | // double m4_b_out = rc->get(RC_THROTTLE) + pid_outputs[PID_PITCH_RATE]; 104 | 105 | motors.outputs[M1] = (int16_t)(m1_r_out + 0.5); 106 | motors.outputs[M2] = (int16_t)(m2_l_out + 0.5); 107 | motors.outputs[M3] = (int16_t)(m3_f_out + 0.5); 108 | motors.outputs[M4] = (int16_t)(m4_b_out + 0.5); 109 | } 110 | 111 | void FlightController::reset_pids() { 112 | // this will reset the I term to avoid windup 113 | roll_rate_pid.SetOutputLimits(0.0, 0.01); 114 | pitch_rate_pid.SetOutputLimits(0.0, 0.01); 115 | yaw_rate_pid.SetOutputLimits(0.0, 0.01); 116 | roll_angle_pid.SetOutputLimits(0.0, 0.01); 117 | pitch_angle_pid.SetOutputLimits(0.0, 0.01); 118 | } 119 | 120 | void FlightController::adjust_pid_tuning() { 121 | if (Serial.available() <= 0) return; 122 | byte incomingByte = Serial.read(); 123 | 124 | double kp, kd, ki; 125 | 126 | if (mode == STABILIZE) { 127 | kp = roll_angle_pid.GetKp(); 128 | ki = roll_angle_pid.GetKi(); 129 | kd = roll_angle_pid.GetKd(); 130 | } else { 131 | kp = roll_rate_pid.GetKp(); 132 | ki = roll_rate_pid.GetKi(); 133 | kd = roll_rate_pid.GetKd(); 134 | } 135 | 136 | if (incomingByte == 'a') { 137 | if (kp <= 0.05) kp = 0; 138 | else kp -= 0.05; 139 | } else if (incomingByte == 's') { 140 | if (ki <= 0.05) ki = 0; 141 | else ki -= 0.05; 142 | } else if (incomingByte == 'd') { 143 | if (kd <= 0.05) kd = 0; 144 | else kd -= 0.05; 145 | } else if (incomingByte == 'q') { 146 | if (kp == 0) kp = 0.01; 147 | else kp += 0.05; 148 | } else if (incomingByte == 'w') { 149 | if (ki == 0) ki = 0.01; 150 | else ki += 0.05; 151 | } else if (incomingByte == 'e') { 152 | if (kd == 0) kd = 0.01; 153 | else kd += 0.05; 154 | } 155 | 156 | 157 | if (mode == STABILIZE) { 158 | roll_angle_pid.SetTunings(kp, ki, kd); 159 | pitch_angle_pid.SetTunings(kp, ki, kd); 160 | } else { 161 | roll_rate_pid.SetTunings(kp, ki, kd); 162 | pitch_rate_pid.SetTunings(kp, ki, kd); 163 | } 164 | } 165 | 166 | void FlightController::compute_angle_pids() { 167 | pid_setpoints[PID_ROLL_ANGLE] = rc->get(RC_ROLL); 168 | pid_setpoints[PID_PITCH_ANGLE] = rc->get(RC_PITCH); 169 | 170 | pid_inputs[PID_ROLL_ANGLE] = imu->x_angle; 171 | pid_inputs[PID_PITCH_ANGLE] = imu->y_angle; 172 | 173 | roll_angle_pid.Compute(); 174 | pitch_angle_pid.Compute(); 175 | } 176 | 177 | void FlightController::compute_rate_pids() { 178 | pid_setpoints[PID_YAW_RATE] = rc->get(RC_YAW); 179 | 180 | pid_inputs[PID_ROLL_RATE] = imu->x_rate; 181 | pid_inputs[PID_PITCH_RATE] = imu->y_rate; 182 | pid_inputs[PID_YAW_RATE] = imu->z_rate; 183 | 184 | roll_rate_pid.Compute(); 185 | pitch_rate_pid.Compute(); 186 | yaw_rate_pid.Compute(); 187 | } 188 | 189 | void FlightController::compute_pids() { 190 | if (mode == STABILIZE) { 191 | compute_angle_pids(); 192 | pid_setpoints[PID_ROLL_RATE] = pid_outputs[PID_ROLL_ANGLE]; 193 | pid_setpoints[PID_PITCH_RATE] = pid_outputs[PID_PITCH_ANGLE]; 194 | } else { 195 | pid_setpoints[PID_ROLL_RATE] = rc->get(RC_ROLL); 196 | pid_setpoints[PID_PITCH_RATE] = rc->get(RC_PITCH); 197 | } 198 | 199 | compute_rate_pids(); 200 | } 201 | 202 | void FlightController::debug_output() { 203 | } 204 | 205 | void FlightController::set_pid_output_limits() { 206 | roll_rate_pid.SetOutputLimits(-1000.0, 1000.0); 207 | pitch_rate_pid.SetOutputLimits(-1000.0, 1000.0); 208 | yaw_rate_pid.SetOutputLimits(-1000.0, 1000.0); 209 | roll_angle_pid.SetOutputLimits(-1000.0, 1000.0); 210 | pitch_angle_pid.SetOutputLimits(-1000.0, 1000.0); 211 | } 212 | 213 | void FlightController::emergency_stop() { 214 | emergency_stopped = true; 215 | motors.command_all_off(); 216 | } 217 | 218 | void FlightController::safety_check() { 219 | // watchdog to prevent stale imu values 220 | if (imu->x_rate == last_gyro_value) { 221 | gyro_freeze_counter++; 222 | if (gyro_freeze_counter == 500) emergency_stop(); 223 | } else { 224 | gyro_freeze_counter = 0; 225 | last_gyro_value = imu->x_rate; 226 | } 227 | 228 | if (imu->x_angle > 45.0 || imu->x_angle < -45.0 229 | || imu->y_angle > 45.0 || imu->y_angle < -45.0) { 230 | emergency_stop(); 231 | } 232 | 233 | for(int i = 0; i < NUM_MOTORS; i++) { 234 | if (motors.outputs[i] > INDOOR_SAFE_MOTOR_SPEED) emergency_stop(); 235 | } 236 | } 237 | 238 | void FlightController::set_safety_mode() { 239 | bool throttle_high = rc->get(RC_THROTTLE) > RC_THROTTLE_CUTOFF; 240 | 241 | if (throttle_high && !emergency_stopped) { 242 | if (safety_mode == UNARMED) { 243 | safety_mode = ARMED; 244 | reset_pids(); 245 | } 246 | } else { 247 | safety_mode = UNARMED; 248 | } 249 | } 250 | 251 | FlightController::FlightController() : 252 | roll_rate_pid(&pid_inputs[PID_ROLL_RATE], 253 | &pid_outputs[PID_ROLL_RATE], 254 | &pid_setpoints[PID_ROLL_RATE], 255 | 1.86, 0.36, 0.0, REVERSE), 256 | pitch_rate_pid(&pid_inputs[PID_PITCH_RATE], 257 | &pid_outputs[PID_PITCH_RATE], 258 | &pid_setpoints[PID_PITCH_RATE], 259 | 1.86, 0.36, 0.0, REVERSE), 260 | yaw_rate_pid(&pid_inputs[PID_YAW_RATE], 261 | &pid_outputs[PID_YAW_RATE], 262 | &pid_setpoints[PID_YAW_RATE], 263 | 3.00, 0.00, 0.0, REVERSE), 264 | //1.00, 0.20, 0.0, REVERSE), 265 | roll_angle_pid(&pid_inputs[PID_ROLL_ANGLE], 266 | &pid_outputs[PID_ROLL_ANGLE], 267 | &pid_setpoints[PID_ROLL_ANGLE], 268 | 0.98, 0.08, 0.0, DIRECT), 269 | pitch_angle_pid(&pid_inputs[PID_PITCH_ANGLE], 270 | &pid_outputs[PID_PITCH_ANGLE], 271 | &pid_setpoints[PID_PITCH_ANGLE], 272 | 0.98, 0.08, 0.0, DIRECT) 273 | {} 274 | -------------------------------------------------------------------------------- /src/flight_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef flight_controller_h 2 | #define flight_controller_h 3 | 4 | #include "remote_control.h" 5 | #include "imu.h" 6 | #include "motor_controller.h" 7 | #include "PID_v1.h" 8 | 9 | #define RATE 0 10 | #define STABILIZE 1 11 | 12 | #define NUM_PIDS 5 13 | #define PID_ROLL_RATE 0 14 | #define PID_PITCH_RATE 1 15 | #define PID_YAW_RATE 2 16 | #define PID_ROLL_ANGLE 3 17 | #define PID_PITCH_ANGLE 4 18 | 19 | #define ARMED 1 20 | #define UNARMED 0 21 | #define RC_THROTTLE_CUTOFF 800 22 | #define INDOOR_SAFE_MOTOR_SPEED 1800 //5000 //1800 23 | 24 | class FlightController { 25 | public: 26 | FlightController(); 27 | 28 | void process(); 29 | void init(RemoteControl *, IMU *); 30 | void emergency_stop(); 31 | 32 | int safety_mode; 33 | int mode; 34 | 35 | PID roll_rate_pid, pitch_rate_pid, yaw_rate_pid, 36 | roll_angle_pid, pitch_angle_pid; 37 | MotorController motors; 38 | double pid_outputs[NUM_PIDS]; 39 | bool emergency_stopped; 40 | 41 | private: 42 | void set_pid_output_limits(); 43 | void adjust_pid_tuning(); 44 | void set_safety_mode(); 45 | void safety_check(); 46 | void reset_pids(); 47 | void compute_pids(); 48 | void compute_angle_pids(); 49 | void compute_rate_pids(); 50 | void adjust_for_bounds(); 51 | void compute_motor_outputs(); 52 | void zero_motor_outputs(); 53 | void set_motor_outputs(); 54 | void command_motors(); 55 | void debug_output(); 56 | 57 | RemoteControl *rc; 58 | IMU *imu; 59 | 60 | uint16_t gyro_freeze_counter; 61 | float last_gyro_value; 62 | double pid_inputs[NUM_PIDS]; 63 | double pid_setpoints[NUM_PIDS]; 64 | double pid_p_debugs[NUM_PIDS]; 65 | double pid_i_debugs[NUM_PIDS]; 66 | double pid_d_debugs[NUM_PIDS]; 67 | bool logging; 68 | }; 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /src/imu.cpp: -------------------------------------------------------------------------------- 1 | #include "IMU.h" 2 | #include "Wire.h" 3 | 4 | void IMU::init() { 5 | Wire.begin(); 6 | TWBR = 10; // setup i2c to run at 444 kHz 7 | 8 | Serial.println("Initializing MPU..."); 9 | mpu.init(); 10 | 11 | delay(100); // Wait for sensor to stabilize 12 | } 13 | 14 | bool IMU::update_sensor_values() { 15 | bool updated = false; 16 | 17 | if ((millis() - accel_update_timer) > 20) { // ~50 hz 18 | update_accel(); 19 | accel_update_timer = millis(); 20 | updated = true; 21 | } 22 | 23 | if ((micros() - gyro_update_timer) > 1300) { // ~800 Hz 24 | update_gyro(); 25 | gyro_update_timer = micros(); 26 | updated = true; 27 | } 28 | 29 | if (updated) { 30 | combine(); 31 | 32 | x_angle = comp_angle_x - 180; 33 | y_angle = comp_angle_y - 180; 34 | } 35 | 36 | return updated; 37 | } 38 | 39 | void IMU::update_gyro() { 40 | mpu.getGyroData(&gyro_x_in, &gyro_y_in, &gyro_z_in); 41 | 42 | gyro_x_rate = gyro_x_in + GYRO_X_OFFSET; 43 | gyro_y_rate = gyro_y_in + GYRO_Y_OFFSET; 44 | gyro_z_rate = gyro_z_in + GYRO_Z_OFFSET; 45 | 46 | x_rate = GYRO_ALPHA * gyro_x_rate + (1-GYRO_ALPHA) * x_rate; 47 | y_rate = GYRO_ALPHA * gyro_y_rate + (1-GYRO_ALPHA) * y_rate; 48 | z_rate = GYRO_ALPHA * gyro_z_rate + (1-GYRO_ALPHA) * z_rate; 49 | 50 | //Integration of gyro rates to get the angles 51 | gyro_x_angle += x_rate * (float)(micros() - gyro_update_timer) / 1000000; 52 | gyro_y_angle += y_rate * (float)(micros() - gyro_update_timer) / 1000000; 53 | } 54 | 55 | void IMU::update_accel() { 56 | mpu.getAxlData(&acc_x_in, &acc_y_in, &acc_z_in); 57 | 58 | acc_x_in = acc_x_in + ACCEL_X_OFFSET; 59 | acc_y_in = acc_y_in + ACCEL_Y_OFFSET; 60 | acc_z_in = acc_z_in + ACCEL_Z_OFFSET; 61 | 62 | acc_x_filtered = ACC_ALPHA * acc_x_filter.in(acc_x_in) + (1-ACC_ALPHA) * acc_x_filtered; 63 | acc_y_filtered = ACC_ALPHA * acc_y_filter.in(acc_y_in) + (1-ACC_ALPHA) * acc_y_filtered; 64 | acc_z_filtered = ACC_ALPHA * acc_z_filter.in(acc_z_in) + (1-ACC_ALPHA) * acc_z_filtered; 65 | 66 | acc_x_angle = (atan2(acc_x_filtered, acc_z_filtered) + PI) * RAD_TO_DEG; 67 | acc_y_angle = (atan2(acc_y_filtered, acc_z_filtered) + PI) * RAD_TO_DEG; 68 | } 69 | 70 | void IMU::combine() { 71 | //Angle calculation through Complementary filter 72 | 73 | float dt = (float)(micros() - combination_update_timer) / 1000000.0; 74 | 75 | comp_angle_x = GYRO_PART * (comp_angle_x + (x_rate * dt)) + ACC_PART * acc_x_angle; 76 | comp_angle_y = GYRO_PART * (comp_angle_y + (y_rate * dt)) + ACC_PART * acc_y_angle; 77 | 78 | combination_update_timer = micros(); 79 | } 80 | 81 | IMU::IMU() : 82 | acc_x_filter(7, 0.0), 83 | acc_y_filter(7, 0.0), 84 | acc_z_filter(7, 0.0) 85 | {} 86 | -------------------------------------------------------------------------------- /src/imu.h: -------------------------------------------------------------------------------- 1 | // Much of this was adapted from: 2 | // https://github.com/RomainGoussault/quadcopter 3 | 4 | #ifndef IMU_h 5 | #define IMU_h 6 | 7 | #include 8 | #include "MPULib.h" 9 | #include "MedianFilter.h" 10 | 11 | #define GYRO_PART 0.985 12 | #define ACC_PART (1.0 - GYRO_PART) 13 | 14 | #define GYRO_ALPHA 0.9 15 | #define ACC_ALPHA 0.9 16 | 17 | #define GYRO_X_OFFSET 0.0; 18 | #define GYRO_Y_OFFSET 0.0; 19 | #define GYRO_Z_OFFSET 0.40; 20 | 21 | #define ACCEL_X_OFFSET 6; 22 | #define ACCEL_Y_OFFSET 0; 23 | #define ACCEL_Z_OFFSET 42; // Want 256 (1G) 24 | 25 | class IMU { 26 | public: 27 | IMU(); 28 | 29 | void init(); 30 | bool update_sensor_values(); 31 | 32 | float x_angle, y_angle; 33 | float x_rate, y_rate, z_rate; 34 | 35 | // Debugging only: 36 | float acc_x_angle, acc_y_angle; 37 | int16_t acc_x_in, acc_y_in, acc_z_in; 38 | float gyro_x_angle, gyro_y_angle; 39 | float gyro_x_rate, gyro_y_rate, gyro_z_rate; 40 | 41 | private: 42 | void setup_initial_angles(); 43 | void update_gyro(); 44 | void update_accel(); 45 | void combine(); 46 | 47 | MPULib mpu; 48 | 49 | float gyro_x_in, gyro_y_in, gyro_z_in; 50 | 51 | float comp_angle_x, comp_angle_y; 52 | float acc_x_filtered, acc_y_filtered, acc_z_filtered; 53 | MedianFilter acc_x_filter, acc_y_filter, acc_z_filter; 54 | 55 | uint32_t gyro_update_timer, accel_update_timer, combination_update_timer; 56 | float gyro_x_offset, gyro_y_offset, gyro_z_offset; 57 | }; 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /src/motor_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "motor_controller.h" 2 | 3 | #define M1_PIN 3 4 | #define M2_PIN 9 5 | #define M3_PIN 10 6 | #define M4_PIN 11 7 | 8 | #define M1_OUTPUT_REG OCR2B 9 | #define M2_OUTPUT_REG OCR1A 10 | #define M3_OUTPUT_REG OCR1B 11 | #define M4_OUTPUT_REG OCR2A 12 | 13 | void MotorController::command() { 14 | #ifdef ALLOW_MOTORS 15 | M1_OUTPUT_REG = outputs[M1] / 16; 16 | M2_OUTPUT_REG = outputs[M2] * 2; 17 | M3_OUTPUT_REG = outputs[M3] * 2; 18 | M4_OUTPUT_REG = outputs[M4] / 16; 19 | #else 20 | M1_OUTPUT_REG = 0; 21 | M2_OUTPUT_REG = 0; 22 | M3_OUTPUT_REG = 0; 23 | M4_OUTPUT_REG = 0; 24 | #endif 25 | } 26 | 27 | void MotorController::command_all_off() { 28 | zero_outputs(); 29 | command(); 30 | } 31 | 32 | void MotorController::zero_outputs() { 33 | for (int i = 0; i < NUM_MOTORS; i++) { 34 | outputs[i] = MOTOR_SAFE_OFF; 35 | } 36 | } 37 | 38 | void MotorController::init() { 39 | zero_outputs(); 40 | 41 | pinMode(M1_PIN, OUTPUT); 42 | pinMode(M2_PIN, OUTPUT); 43 | pinMode(M3_PIN, OUTPUT); 44 | pinMode(M4_PIN, OUTPUT); 45 | 46 | // Init PWM Timer 1 16 bit 47 | // // Clear OCnA/OCnB/OCnC on compare match, set OCnA/OCnB/OCnC at BOTTOM (non-inverting mode) 48 | // // Prescaler set to 8, that gives us a resolution of 0.5us 49 | TCCR1A = (1< 21 | 22 | class MotorController { 23 | public: 24 | void command_all_off(); 25 | void command(); 26 | void init(); 27 | 28 | uint16_t outputs[4]; 29 | 30 | private: 31 | void zero_outputs(); 32 | }; 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /src/quadcopter.ino: -------------------------------------------------------------------------------- 1 | #define SERIAL_PORT_SPEED 115200 2 | 3 | #include "imu.h" 4 | #include "remote_control.h" 5 | #include "rc_interrupts.h" 6 | #include "flight_controller.h" 7 | #include "debugger.h" 8 | 9 | IMU imu; 10 | RemoteControl rc; 11 | FlightController flight_controller; 12 | Debugger debugger; 13 | 14 | void setup() { 15 | Serial.begin(SERIAL_PORT_SPEED); 16 | 17 | imu.init(); 18 | bind_rc_interrupts(); 19 | flight_controller.init(&rc, &imu); 20 | 21 | debugger.init(&rc, &imu, &flight_controller); 22 | } 23 | 24 | void loop() { 25 | while(!imu.update_sensor_values()); 26 | 27 | rc.read_values(); 28 | flight_controller.process(); 29 | 30 | debugger.print(); 31 | } 32 | -------------------------------------------------------------------------------- /src/rc_interrupts.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define RC_CH1_INPUT A0 4 | #define RC_CH2_INPUT A1 5 | #define RC_CH3_INPUT A2 6 | #define RC_CH4_INPUT A3 7 | #define RC_CH5_INPUT 7 8 | #define RC_CH6_INPUT 8 9 | 10 | void calc_ch_1() { RemoteControl::calc_input(RC_CH1, RC_CH1_INPUT); } 11 | void calc_ch_2() { RemoteControl::calc_input(RC_CH2, RC_CH2_INPUT); } 12 | void calc_ch_3() { RemoteControl::calc_input(RC_CH3, RC_CH3_INPUT); } 13 | void calc_ch_4() { RemoteControl::calc_input(RC_CH4, RC_CH4_INPUT); } 14 | void calc_ch_5() { RemoteControl::calc_input(RC_CH5, RC_CH5_INPUT); } 15 | void calc_ch_6() { RemoteControl::calc_input(RC_CH6, RC_CH6_INPUT); } 16 | 17 | void bind_rc_interrupts() { 18 | PCintPort::attachInterrupt(RC_CH1_INPUT, calc_ch_1, CHANGE); 19 | PCintPort::attachInterrupt(RC_CH2_INPUT, calc_ch_2, CHANGE); 20 | PCintPort::attachInterrupt(RC_CH3_INPUT, calc_ch_3, CHANGE); 21 | PCintPort::attachInterrupt(RC_CH4_INPUT, calc_ch_4, CHANGE); 22 | PCintPort::attachInterrupt(RC_CH5_INPUT, calc_ch_5, CHANGE); 23 | PCintPort::attachInterrupt(RC_CH6_INPUT, calc_ch_6, CHANGE); 24 | } 25 | -------------------------------------------------------------------------------- /src/remote_control.cpp: -------------------------------------------------------------------------------- 1 | #include "remote_control.h" 2 | 3 | int16_t RemoteControl::rc_in_min[] = { RC_CH1_IN_MIN, RC_CH2_IN_MIN, RC_CH3_IN_MIN, 4 | RC_CH4_IN_MIN, RC_CH5_IN_MIN, RC_CH6_IN_MIN }; 5 | int16_t RemoteControl::rc_in_max[] = { RC_CH1_IN_MAX, RC_CH2_IN_MAX, RC_CH3_IN_MAX, 6 | RC_CH4_IN_MAX, RC_CH5_IN_MAX, RC_CH6_IN_MAX }; 7 | int16_t RemoteControl::rc_out_min[] = { RC_CH1_OUT_MIN, RC_CH2_OUT_MIN, RC_CH3_OUT_MIN, 8 | RC_CH4_OUT_MIN, RC_CH5_OUT_MIN, RC_CH6_OUT_MIN }; 9 | int16_t RemoteControl::rc_out_max[] = { RC_CH1_OUT_MAX, RC_CH2_OUT_MAX, RC_CH3_OUT_MAX, 10 | RC_CH4_OUT_MAX, RC_CH5_OUT_MAX, RC_CH6_OUT_MAX }; 11 | 12 | uint32_t RemoteControl::last_update_time = 0; 13 | uint16_t RemoteControl::rc_values[] = {0}; 14 | uint32_t RemoteControl::rc_start[] = {0}; 15 | volatile uint16_t RemoteControl::rc_shared[] = {0}; 16 | 17 | void RemoteControl::read_values() { 18 | if ((millis() - last_update_time) > RC_TIMEOUT) { 19 | // If we don't get an input for RC_TIMEOUT, set all vals to 0 20 | for (int i = 0; i < NUM_CHANNELS; i++) { 21 | rc_values[i] = 0; 22 | } 23 | } else { 24 | noInterrupts(); 25 | memcpy(rc_values, (const void *)rc_shared, sizeof(rc_shared)); 26 | interrupts(); 27 | } 28 | 29 | for (int i = 0; i < NUM_CHANNELS; i++) { 30 | process_channel_value(i); 31 | } 32 | } 33 | 34 | void RemoteControl::process_channel_value(int channel) { 35 | int16_t value = rc_values[channel]; 36 | value = constrain(value, rc_in_min[channel], rc_in_max[channel]); 37 | value = map(value, rc_in_min[channel], rc_in_max[channel], rc_out_min[channel], rc_out_max[channel]); 38 | 39 | if ((channel == RC_CH1 || channel == RC_CH2 || channel == RC_CH4) 40 | && (value > 0 && value < 5 || value < 0 && value > -5) ) { 41 | value = 0; 42 | } 43 | 44 | if (channel == RC_CH1 || channel == RC_CH2) { 45 | value = -value; // invert roll & pitch 46 | } 47 | 48 | if (channel == RC_CH3 && value < 1070 && value > 750) { 49 | value = 1070; 50 | } 51 | 52 | rc_out_values[channel] = value; 53 | } 54 | 55 | int16_t RemoteControl::get(int channel) { 56 | return rc_out_values[channel]; 57 | } 58 | 59 | void RemoteControl::calc_input(int channel, int input_pin) { 60 | if (digitalRead(input_pin) == HIGH) { 61 | rc_start[channel] = micros(); 62 | } else { 63 | uint32_t rc_compare = (uint16_t)(micros() - rc_start[channel]); 64 | rc_shared[channel] = rc_compare; 65 | } 66 | 67 | last_update_time = millis(); 68 | } 69 | 70 | RemoteControl::RemoteControl() {} 71 | -------------------------------------------------------------------------------- /src/remote_control.h: -------------------------------------------------------------------------------- 1 | #ifndef remote_control_h 2 | #define remote_control_h 3 | 4 | #include 5 | 6 | #define NUM_CHANNELS 6 7 | #define RC_TIMEOUT 1000 // milliseconds 8 | 9 | #define RC_CH1_IN_MIN 1154 10 | #define RC_CH1_IN_MAX 1898 11 | #define RC_CH1_OUT_MIN -15 12 | #define RC_CH1_OUT_MAX 15 13 | 14 | #define RC_CH2_IN_MIN 1192 15 | #define RC_CH2_IN_MAX 1824 16 | #define RC_CH2_OUT_MIN -15 17 | #define RC_CH2_OUT_MAX 15 18 | 19 | #define RC_CH3_IN_MIN 1000 20 | #define RC_CH3_IN_MAX 1812 21 | #define RC_CH3_OUT_MIN 600 22 | #define RC_CH3_OUT_MAX 1600 // 1864 is max motor input 23 | 24 | #define RC_CH4_IN_MIN 1171 25 | #define RC_CH4_IN_MAX 1871 26 | #define RC_CH4_OUT_MIN -15 27 | #define RC_CH4_OUT_MAX 15 28 | 29 | #define RC_CH5_IN_MIN 996 30 | #define RC_CH5_IN_MAX 2000 31 | #define RC_CH5_OUT_MIN 0 32 | #define RC_CH5_OUT_MAX 100 33 | 34 | #define RC_CH6_IN_MIN 996 35 | #define RC_CH6_IN_MAX 2000 36 | #define RC_CH6_OUT_MIN 0 37 | #define RC_CH6_OUT_MAX 100 38 | 39 | #define RC_CH1 0 40 | #define RC_CH2 1 41 | #define RC_CH3 2 42 | #define RC_CH4 3 43 | #define RC_CH5 4 44 | #define RC_CH6 5 45 | 46 | #define RC_ROLL RC_CH1 47 | #define RC_PITCH RC_CH2 48 | #define RC_THROTTLE RC_CH3 49 | #define RC_YAW RC_CH4 50 | #define RC_POT_A RC_CH5 51 | #define RC_POT_B RC_CH6 52 | 53 | class RemoteControl { 54 | public: 55 | RemoteControl(); 56 | 57 | void read_values(); 58 | int16_t get(int); 59 | 60 | static void calc_input(int, int); 61 | 62 | private: 63 | void process_channel_value(int channel); 64 | 65 | static uint32_t last_update_time; 66 | static int16_t rc_in_min[6]; 67 | static int16_t rc_in_max[6]; 68 | static int16_t rc_out_min[6]; 69 | static int16_t rc_out_max[6]; 70 | static uint32_t rc_start[6]; 71 | static volatile uint16_t rc_shared[6]; 72 | static uint16_t rc_values[6]; 73 | uint16_t rc_out_values[6]; 74 | }; 75 | 76 | #endif 77 | --------------------------------------------------------------------------------