From 8565b7ef851586c3781f9d89b302f9602f98e5e2 Mon Sep 17 00:00:00 2001 From: Brian Jones Date: Thu, 23 Jan 2025 12:50:29 -0600 Subject: [PATCH 1/6] Added support for LSM6DSOX IMU from Adafruit. Added some shortcuts around SBUS failsafe. --- .../dRehmFlight_Teensy_BETA_1.4/COPYING.txt | 674 ++++ .../dRehmFlight_Teensy_BETA_1.4.ino | 1861 +++++++++ .../dRehmFlight_Teensy_BETA_1.4/radioComm.ino | 198 + .../src/DSMRX/DSMRX.cpp | 116 + .../src/DSMRX/DSMRX.h | 85 + .../src/MPU6050/I2Cdev.cpp | 1468 ++++++++ .../src/MPU6050/I2Cdev.h | 283 ++ .../src/MPU6050/MPU6050.cpp | 3330 +++++++++++++++++ .../src/MPU6050/MPU6050.h | 1041 ++++++ .../src/MPU6050/MPU6050_6Axis_MotionApps20.h | 617 +++ .../MPU6050/MPU6050_6Axis_MotionApps_V6_12.h | 617 +++ .../src/MPU6050/MPU6050_9Axis_MotionApps41.h | 887 +++++ .../src/MPU6050/helper_3dmath.h | 216 ++ .../src/MPU9250/MPU9250.cpp | 1202 ++++++ .../src/MPU9250/MPU9250.h | 313 ++ .../src/SBUS/SBUS.cpp | 376 ++ .../src/SBUS/SBUS.h | 82 + .../src/SBUS/elapsedMillis.h | 81 + .../src/TFMPlus/TFMPlus.cpp | 385 ++ .../src/TFMPlus/TFMPlus.h | 227 ++ 20 files changed, 14059 insertions(+) create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/COPYING.txt create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/dRehmFlight_Teensy_BETA_1.4.ino create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/radioComm.ino create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/DSMRX/DSMRX.cpp create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/DSMRX/DSMRX.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/I2Cdev.cpp create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/I2Cdev.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050.cpp create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050_6Axis_MotionApps20.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050_9Axis_MotionApps41.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/helper_3dmath.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU9250/MPU9250.cpp create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU9250/MPU9250.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/SBUS/SBUS.cpp create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/SBUS/SBUS.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/SBUS/elapsedMillis.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/TFMPlus/TFMPlus.cpp create mode 100644 Versions/dRehmFlight_Teensy_BETA_1.4/src/TFMPlus/TFMPlus.h diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/COPYING.txt b/Versions/dRehmFlight_Teensy_BETA_1.4/COPYING.txt new file mode 100644 index 00000000..80e659dc --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/COPYING.txt @@ -0,0 +1,674 @@ +GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/dRehmFlight_Teensy_BETA_1.4.ino b/Versions/dRehmFlight_Teensy_BETA_1.4/dRehmFlight_Teensy_BETA_1.4.ino new file mode 100644 index 00000000..8ca9024e --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/dRehmFlight_Teensy_BETA_1.4.ino @@ -0,0 +1,1861 @@ +//Arduino/Teensy Flight Controller - dRehmFlight +//Author: Nicholas Rehm +//Project Start: 1/6/2020 +//Last Updated: 7/29/2022 +//Version: Beta 1.3 + +//========================================================================================================================// + +//CREDITS + SPECIAL THANKS +/* +Some elements inspired by: +http://www.brokking.net/ymfc-32_main.html + +Madgwick filter function adapted from: +https://github.com/arduino-libraries/MadgwickAHRS + +MPU9250 implementation based on MPU9250 library by: +brian.taylor@bolderflight.com +http://www.bolderflight.com + +Adafruit LSM6DSOX implementation added by: +bjones@aggiejones.com +http://nvrtd.design + +Thank you to: +RcGroups 'jihlein' - IMU implementation overhaul + SBUS implementation. +Everyone that sends me pictures and videos of your flying creations! -Nick + +*/ + + + +//========================================================================================================================// +// USER-SPECIFIED DEFINES // +//========================================================================================================================// + +//Uncomment only one receiver type +#define USE_PWM_RX +//#define USE_PPM_RX +//#define USE_SBUS_RX +//#define USE_DSM_RX +static const uint8_t num_DSM_channels = 6; //If using DSM RX, change this to match the number of transmitter channels you have + +//Uncomment only one IMU +#define USE_MPU6050_I2C //Default +//#define USE_MPU9250_SPI +//#define USE_LSM6DSOX_SPI + +//Uncomment only one full scale gyro range (deg/sec) +#define GYRO_250DPS //Default +//#define GYRO_500DPS +//#define GYRO_1000DPS +//#define GYRO_2000DPS + +//Uncomment only one full scale accelerometer range (G's) +#define ACCEL_2G //Default +//#define ACCEL_4G +//#define ACCEL_8G +//#define ACCEL_16G + + + +//========================================================================================================================// + + + +//REQUIRED LIBRARIES (included with download in main sketch folder) + +#include //I2c communication +#include //SPI communication +#include //Commanding any extra actuators, installed with teensyduino installer + +#if defined USE_SBUS_RX + #include "src/SBUS/SBUS.h" //sBus interface +#endif + +#if defined USE_DSM_RX + #include "src/DSMRX/DSMRX.h" +#endif + +#if defined USE_MPU6050_I2C + #include "src/MPU6050/MPU6050.h" + MPU6050 mpu6050; +#elif defined USE_MPU9250_SPI + #include "src/MPU9250/MPU9250.h" + MPU9250 mpu9250(SPI2,36); +#elif defined USE_LSM6DSOX_SPI + #include + Adafruit_LSM6DSOX lsm6dsox; + + #define LSM_CS 36 + #define LSM_SCK 37 + #define LSM_MISO 34 + #define LSM_MOSI 35 + + sensors_event_t accel; + sensors_event_t gyro; + sensors_event_t temp; //unused, but could be interesting +#else + #error No MPU defined... +#endif + + + +//========================================================================================================================// + + + +//Setup gyro and accel full scale value selection and scale factor + +#if defined USE_MPU6050_I2C + #define GYRO_FS_SEL_250 MPU6050_GYRO_FS_250 + #define GYRO_FS_SEL_500 MPU6050_GYRO_FS_500 + #define GYRO_FS_SEL_1000 MPU6050_GYRO_FS_1000 + #define GYRO_FS_SEL_2000 MPU6050_GYRO_FS_2000 + #define ACCEL_FS_SEL_2 MPU6050_ACCEL_FS_2 + #define ACCEL_FS_SEL_4 MPU6050_ACCEL_FS_4 + #define ACCEL_FS_SEL_8 MPU6050_ACCEL_FS_8 + #define ACCEL_FS_SEL_16 MPU6050_ACCEL_FS_16 +#elif defined USE_MPU9250_SPI + #define GYRO_FS_SEL_250 mpu9250.GYRO_RANGE_250DPS + #define GYRO_FS_SEL_500 mpu9250.GYRO_RANGE_500DPS + #define GYRO_FS_SEL_1000 mpu9250.GYRO_RANGE_1000DPS + #define GYRO_FS_SEL_2000 mpu9250.GYRO_RANGE_2000DPS + #define ACCEL_FS_SEL_2 mpu9250.ACCEL_RANGE_2G + #define ACCEL_FS_SEL_4 mpu9250.ACCEL_RANGE_4G + #define ACCEL_FS_SEL_8 mpu9250.ACCEL_RANGE_8G + #define ACCEL_FS_SEL_16 mpu9250.ACCEL_RANGE_16G +#elif defined USE_LSM6DSOX_SPI + #define GYRO_FS_SEL_250 lsm6ds_gyro_range_t::LSM6DS_GYRO_RANGE_250_DPS //there is also a 125 DPS option + #define GYRO_FS_SEL_500 lsm6ds_gyro_range_t::LSM6DS_GYRO_RANGE_500_DPS + #define GYRO_FS_SEL_1000 lsm6ds_gyro_range_t::LSM6DS_GYRO_RANGE_1000_DPS + #define GYRO_FS_SEL_2000 lsm6ds_gyro_range_t::LSM6DS_GYRO_RANGE_2000_DPS + #define ACCEL_FS_SEL_2 lsm6ds_accel_range_t::LSM6DS_ACCEL_RANGE_2_G + #define ACCEL_FS_SEL_4 lsm6ds_accel_range_t::LSM6DS_ACCEL_RANGE_4_G + #define ACCEL_FS_SEL_8 lsm6ds_accel_range_t::LSM6DS_ACCEL_RANGE_8_G + #define ACCEL_FS_SEL_16 lsm6ds_accel_range_t::LSM6DS_ACCEL_RANGE_16_G +#endif + +#if defined GYRO_250DPS + #define GYRO_SCALE GYRO_FS_SEL_250 + #define GYRO_SCALE_FACTOR 131.0 +#elif defined GYRO_500DPS + #define GYRO_SCALE GYRO_FS_SEL_500 + #define GYRO_SCALE_FACTOR 65.5 +#elif defined GYRO_1000DPS + #define GYRO_SCALE GYRO_FS_SEL_1000 + #define GYRO_SCALE_FACTOR 32.8 +#elif defined GYRO_2000DPS + #define GYRO_SCALE GYRO_FS_SEL_2000 + #define GYRO_SCALE_FACTOR 16.4 +#endif + +#if defined ACCEL_2G + #define ACCEL_SCALE ACCEL_FS_SEL_2 + #define ACCEL_SCALE_FACTOR 16384.0 +#elif defined ACCEL_4G + #define ACCEL_SCALE ACCEL_FS_SEL_4 + #define ACCEL_SCALE_FACTOR 8192.0 +#elif defined ACCEL_8G + #define ACCEL_SCALE ACCEL_FS_SEL_8 + #define ACCEL_SCALE_FACTOR 4096.0 +#elif defined ACCEL_16G + #define ACCEL_SCALE ACCEL_FS_SEL_16 + #define ACCEL_SCALE_FACTOR 2048.0 +#endif + + + +//========================================================================================================================// +// USER-SPECIFIED VARIABLES // +//========================================================================================================================// + +//Radio failsafe values for every channel in the event that bad reciever data is detected. Recommended defaults: +unsigned long channel_1_fs = 1000; //thro +unsigned long channel_2_fs = 1500; //ail +unsigned long channel_3_fs = 1500; //elev +unsigned long channel_4_fs = 1500; //rudd +unsigned long channel_5_fs = 2000; //gear, greater than 1500 = throttle cut +unsigned long channel_6_fs = 2000; //aux1 + +//Filter parameters - Defaults tuned for 2kHz loop rate; Do not touch unless you know what you are doing: +float B_madgwick = 0.04; //Madgwick filter parameter +float B_accel = 0.14; //Accelerometer LP filter paramter, (MPU6050 default: 0.14. MPU9250 default: 0.2) +float B_gyro = 0.1; //Gyro LP filter paramter, (MPU6050 default: 0.1. MPU9250 default: 0.17) +float B_mag = 1.0; //Magnetometer LP filter parameter + +//Magnetometer calibration parameters - if using MPU9250, uncomment calibrateMagnetometer() in void setup() to get these values, else just ignore these +float MagErrorX = 0.0; +float MagErrorY = 0.0; +float MagErrorZ = 0.0; +float MagScaleX = 1.0; +float MagScaleY = 1.0; +float MagScaleZ = 1.0; + +//IMU calibration parameters - calibrate IMU using calculate_IMU_error() in the void setup() to get these values, then comment out calculate_IMU_error() +float AccErrorX = 0.0; +float AccErrorY = 0.0; +float AccErrorZ = 0.0; +float GyroErrorX = 0.0; +float GyroErrorY= 0.0; +float GyroErrorZ = 0.0; + +//Controller parameters (take note of defaults before modifying!): +float i_limit = 25.0; //Integrator saturation level, mostly for safety (default 25.0) +float maxRoll = 30.0; //Max roll angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode +float maxPitch = 30.0; //Max pitch angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode +float maxYaw = 160.0; //Max yaw rate in deg/sec + +float Kp_roll_angle = 0.2; //Roll P-gain - angle mode +float Ki_roll_angle = 0.3; //Roll I-gain - angle mode +float Kd_roll_angle = 0.05; //Roll D-gain - angle mode (has no effect on controlANGLE2) +float B_loop_roll = 0.9; //Roll damping term for controlANGLE2(), lower is more damping (must be between 0 to 1) +float Kp_pitch_angle = 0.2; //Pitch P-gain - angle mode +float Ki_pitch_angle = 0.3; //Pitch I-gain - angle mode +float Kd_pitch_angle = 0.05; //Pitch D-gain - angle mode (has no effect on controlANGLE2) +float B_loop_pitch = 0.9; //Pitch damping term for controlANGLE2(), lower is more damping (must be between 0 to 1) + +float Kp_roll_rate = 0.15; //Roll P-gain - rate mode +float Ki_roll_rate = 0.2; //Roll I-gain - rate mode +float Kd_roll_rate = 0.0002; //Roll D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) +float Kp_pitch_rate = 0.15; //Pitch P-gain - rate mode +float Ki_pitch_rate = 0.2; //Pitch I-gain - rate mode +float Kd_pitch_rate = 0.0002; //Pitch D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) + +float Kp_yaw = 0.3; //Yaw P-gain +float Ki_yaw = 0.05; //Yaw I-gain +float Kd_yaw = 0.00015; //Yaw D-gain (be careful when increasing too high, motors will begin to overheat!) + + + +//========================================================================================================================// +// DECLARE PINS // +//========================================================================================================================// + +//NOTE: Pin 13 is reserved for onboard LED, pins 18 and 19 are reserved for the MPU6050 IMU for default setup +//Radio: +//Note: If using SBUS, connect to pin 21 (RX5), if using DSM, connect to pin 15 (RX3) +const int ch1Pin = 15; //throttle +const int ch2Pin = 16; //ail +const int ch3Pin = 17; //ele +const int ch4Pin = 20; //rudd +const int ch5Pin = 21; //gear (throttle cut) +const int ch6Pin = 22; //aux1 (free aux channel) +const int PPM_Pin = 23; +//OneShot125 ESC pin outputs: +const int m1Pin = 0; +const int m2Pin = 1; +const int m3Pin = 2; +const int m4Pin = 3; +const int m5Pin = 4; +const int m6Pin = 5; +//PWM servo or ESC outputs: +const int servo1Pin = 6; +const int servo2Pin = 7; +const int servo3Pin = 8; +const int servo4Pin = 9; +const int servo5Pin = 10; +const int servo6Pin = 11; +const int servo7Pin = 12; +PWMServo servo1; //Create servo objects to control a servo or ESC with PWM +PWMServo servo2; +PWMServo servo3; +PWMServo servo4; +PWMServo servo5; +PWMServo servo6; +PWMServo servo7; + + + +//========================================================================================================================// + + + +//DECLARE GLOBAL VARIABLES + +//General stuff +float dt; +unsigned long current_time, prev_time; +unsigned long print_counter, serial_counter; +unsigned long blink_counter, blink_delay; +bool blinkAlternate; + +//Radio communication: +unsigned long channel_1_pwm, channel_2_pwm, channel_3_pwm, channel_4_pwm, channel_5_pwm, channel_6_pwm; +unsigned long channel_1_pwm_prev, channel_2_pwm_prev, channel_3_pwm_prev, channel_4_pwm_prev; + +#if defined USE_SBUS_RX + SBUS sbus(Serial5); + uint16_t sbusChannels[16]; + bool sbusFailSafe; + bool sbusLostFrame; +#endif +#if defined USE_DSM_RX + DSM1024 DSM; +#endif + +//IMU: +float AccX, AccY, AccZ; +float AccX_prev, AccY_prev, AccZ_prev; +float GyroX, GyroY, GyroZ; +float GyroX_prev, GyroY_prev, GyroZ_prev; +float MagX, MagY, MagZ; +float MagX_prev, MagY_prev, MagZ_prev; +float roll_IMU, pitch_IMU, yaw_IMU; +float roll_IMU_prev, pitch_IMU_prev; +float q0 = 1.0f; //Initialize quaternion for madgwick filter +float q1 = 0.0f; +float q2 = 0.0f; +float q3 = 0.0f; + +//Normalized desired state: +float thro_des, roll_des, pitch_des, yaw_des; +float roll_passthru, pitch_passthru, yaw_passthru; + +//Controller: +float error_roll, error_roll_prev, roll_des_prev, integral_roll, integral_roll_il, integral_roll_ol, integral_roll_prev, integral_roll_prev_il, integral_roll_prev_ol, derivative_roll, roll_PID = 0; +float error_pitch, error_pitch_prev, pitch_des_prev, integral_pitch, integral_pitch_il, integral_pitch_ol, integral_pitch_prev, integral_pitch_prev_il, integral_pitch_prev_ol, derivative_pitch, pitch_PID = 0; +float error_yaw, error_yaw_prev, integral_yaw, integral_yaw_prev, derivative_yaw, yaw_PID = 0; + +//Mixer +float m1_command_scaled, m2_command_scaled, m3_command_scaled, m4_command_scaled, m5_command_scaled, m6_command_scaled; +int m1_command_PWM, m2_command_PWM, m3_command_PWM, m4_command_PWM, m5_command_PWM, m6_command_PWM; +float s1_command_scaled, s2_command_scaled, s3_command_scaled, s4_command_scaled, s5_command_scaled, s6_command_scaled, s7_command_scaled; +int s1_command_PWM, s2_command_PWM, s3_command_PWM, s4_command_PWM, s5_command_PWM, s6_command_PWM, s7_command_PWM; + +//Flight status +bool armedFly = false; + +//========================================================================================================================// +// VOID SETUP // +//========================================================================================================================// + +void setup() { + Serial.begin(500000); //USB serial + delay(500); + + //Initialize all pins + pinMode(13, OUTPUT); //Pin 13 LED blinker on board, do not modify + pinMode(m1Pin, OUTPUT); + pinMode(m2Pin, OUTPUT); + pinMode(m3Pin, OUTPUT); + pinMode(m4Pin, OUTPUT); + pinMode(m5Pin, OUTPUT); + pinMode(m6Pin, OUTPUT); + servo1.attach(servo1Pin, 900, 2100); //Pin, min PWM value, max PWM value + servo2.attach(servo2Pin, 900, 2100); + servo3.attach(servo3Pin, 900, 2100); + servo4.attach(servo4Pin, 900, 2100); + servo5.attach(servo5Pin, 900, 2100); + servo6.attach(servo6Pin, 900, 2100); + servo7.attach(servo7Pin, 900, 2100); + + //Set built in LED to turn on to signal startup + digitalWrite(13, HIGH); + + delay(5); + + //Initialize radio communication + radioSetup(); + + //Set radio channels to default (safe) values before entering main loop + channel_1_pwm = channel_1_fs; + channel_2_pwm = channel_2_fs; + channel_3_pwm = channel_3_fs; + channel_4_pwm = channel_4_fs; + channel_5_pwm = channel_5_fs; + channel_6_pwm = channel_6_fs; + + //Initialize IMU communication + IMUinit(); + + delay(5); + + //Get IMU error to zero accelerometer and gyro readings, assuming vehicle is level when powered up + //calculate_IMU_error(); //Calibration parameters printed to serial monitor. Paste these in the user specified variables section, then comment this out forever. + + //Arm servo channels + servo1.write(0); //Command servo angle from 0-180 degrees (1000 to 2000 PWM) + servo2.write(0); //Set these to 90 for servos if you do not want them to briefly max out on startup + servo3.write(0); //Keep these at 0 if you are using servo outputs for motors + servo4.write(0); + servo5.write(0); + servo6.write(0); + servo7.write(0); + + delay(5); + + //calibrateESCs(); //PROPS OFF. Uncomment this to calibrate your ESCs by setting throttle stick to max, powering on, and lowering throttle to zero after the beeps + //Code will not proceed past here if this function is uncommented! + + //Arm OneShot125 motors + m1_command_PWM = 125; //Command OneShot125 ESC from 125 to 250us pulse length + m2_command_PWM = 125; + m3_command_PWM = 125; + m4_command_PWM = 125; + m5_command_PWM = 125; + m6_command_PWM = 125; + armMotors(); //Loop over commandMotors() until ESCs happily arm + + //Indicate entering main loop with 3 quick blinks + setupBlink(3,160,70); //numBlinks, upTime (ms), downTime (ms) + + //If using MPU9250 IMU, uncomment for one-time magnetometer calibration (may need to repeat for new locations) + //calibrateMagnetometer(); //Generates magentometer error and scale factors to be pasted in user-specified variables section + +} + + + +//========================================================================================================================// +// MAIN LOOP // +//========================================================================================================================// + +void loop() { + //Keep track of what time it is and how much time has elapsed since the last loop + prev_time = current_time; + current_time = micros(); + dt = (current_time - prev_time)/1000000.0; + + loopBlink(); //Indicate we are in main loop with short blink every 1.5 seconds + + //Print data at 100hz (uncomment one at a time for troubleshooting) - SELECT ONE: + //printRadioData(); //Prints radio pwm values (expected: 1000 to 2000) + //printDesiredState(); //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle) + //printGyroData(); //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest) + //printAccelData(); //Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level) + //printMagData(); //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300) + //printRollPitchYaw(); //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level) + //printPIDoutput(); //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1) + //printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250) + //printServoCommands(); //Prints the values being written to the servos (expected: 0 to 180) + //printLoopRate(); //Prints the time between loops in microseconds (expected: microseconds between loop iterations) + + // Get arming status + armedStatus(); //Check if the throttle cut is off and throttle is low. + + //Get vehicle state + getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise + Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU angle estimates (degrees) + + //Compute desired state + getDesState(); //Convert raw commands to normalized values based on saturated control limits + + //PID Controller - SELECT ONE: + controlANGLE(); //Stabilize on angle setpoint + //controlANGLE2(); //Stabilize on angle setpoint using cascaded method. Rate controller must be tuned well first! + //controlRATE(); //Stabilize on rate setpoint + + //Actuator mixing and scaling to PWM values + controlMixer(); //Mixes PID outputs to scaled actuator commands -- custom mixing assignments done here + scaleCommands(); //Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) + + //Throttle cut check + throttleCut(); //Directly sets motor commands to low based on state of ch5 + + //Command actuators + commandMotors(); //Sends command pulses to each motor pin using OneShot125 protocol + servo1.write(s1_command_PWM); //Writes PWM value to servo object + servo2.write(s2_command_PWM); + servo3.write(s3_command_PWM); + servo4.write(s4_command_PWM); + servo5.write(s5_command_PWM); + servo6.write(s6_command_PWM); + servo7.write(s7_command_PWM); + + //Get vehicle commands for next loop iteration + getCommands(); //Pulls current available radio commands + failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup + + //Regulate loop rate + loopRate(2000); //Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default +} + + + +//========================================================================================================================// +// FUNCTIONS // +//========================================================================================================================// + + + +void controlMixer() { + //DESCRIPTION: Mixes scaled commands from PID controller to actuator outputs based on vehicle configuration + /* + * Takes roll_PID, pitch_PID, and yaw_PID computed from the PID controller and appropriately mixes them for the desired + * vehicle configuration. For example on a quadcopter, the left two motors should have +roll_PID while the right two motors + * should have -roll_PID. Front two should have -pitch_PID and the back two should have +pitch_PID etc... every motor has + * normalized (0 to 1) thro_des command for throttle control. Can also apply direct unstabilized commands from the transmitter with + * roll_passthru, pitch_passthru, and yaw_passthu. mX_command_scaled and sX_command scaled variables are used in scaleCommands() + * in preparation to be sent to the motor ESCs and servos. + * + *Relevant variables: + *thro_des - direct thottle control + *roll_PID, pitch_PID, yaw_PID - stabilized axis variables + *roll_passthru, pitch_passthru, yaw_passthru - direct unstabilized command passthrough + *channel_6_pwm - free auxillary channel, can be used to toggle things with an 'if' statement + */ + + //Quad mixing - EXAMPLE + m1_command_scaled = thro_des - pitch_PID + roll_PID + yaw_PID; //Front Left + m2_command_scaled = thro_des - pitch_PID - roll_PID - yaw_PID; //Front Right + m3_command_scaled = thro_des + pitch_PID - roll_PID + yaw_PID; //Back Right + m4_command_scaled = thro_des + pitch_PID + roll_PID - yaw_PID; //Back Left + m5_command_scaled = 0; + m6_command_scaled = 0; + + //0.5 is centered servo, 0.0 is zero throttle if connecting to ESC for conventional PWM, 1.0 is max throttle + s1_command_scaled = 0; + s2_command_scaled = 0; + s3_command_scaled = 0; + s4_command_scaled = 0; + s5_command_scaled = 0; + s6_command_scaled = 0; + s7_command_scaled = 0; + +} + +void armedStatus() { + //DESCRIPTION: Check if the throttle cut is off and the throttle input is low to prepare for flight. + if ((channel_5_pwm < 1500) && (channel_1_pwm < 1050)) { + armedFly = true; + } +} + +void IMUinit() { + //DESCRIPTION: Initialize IMU + /* + * Don't worry about how this works. + */ + #if defined USE_MPU6050_I2C + Wire.begin(); + Wire.setClock(1000000); //Note this is 2.5 times the spec sheet 400 kHz max... + + mpu6050.initialize(); + + if (mpu6050.testConnection() == false) { + Serial.print("DeviceID: "); + Serial.println(mpu6050.getDeviceID()); + Serial.println("MPU6050 initialization unsuccessful"); + Serial.println("Check MPU6050 wiring or try cycling power"); + while(1) {} + } + else { + Serial.print("OK! DeviceID: "); + Serial.println(mpu6050.getDeviceID()); + Serial.println("MPU6050 initialization successful"); + } + + //From the reset state all registers should be 0x00, so we should be at + //max sample rate with digital low pass filter(s) off. All we need to + //do is set the desired fullscale ranges + mpu6050.setFullScaleGyroRange(GYRO_SCALE); + mpu6050.setFullScaleAccelRange(ACCEL_SCALE); + + #elif defined USE_MPU9250_SPI + int status = mpu9250.begin(); + + if (status < 0) { + Serial.println("MPU9250 initialization unsuccessful"); + Serial.println("Check MPU9250 wiring or try cycling power"); + Serial.print("Status: "); + Serial.println(status); + while(1) {} + } + + //From the reset state all registers should be 0x00, so we should be at + //max sample rate with digital low pass filter(s) off. All we need to + //do is set the desired fullscale ranges + mpu9250.setGyroRange(GYRO_SCALE); + mpu9250.setAccelRange(ACCEL_SCALE); + mpu9250.setMagCalX(MagErrorX, MagScaleX); + mpu9250.setMagCalY(MagErrorY, MagScaleY); + mpu9250.setMagCalZ(MagErrorZ, MagScaleZ); + mpu9250.setSrd(0); //sets gyro and accel read to 1khz, magnetometer read to 100hz + + #elif defined USE_LSM6DSOX_SPI + if (!lsm6dsox.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) { + Serial.println("Failed to find LSM6DSOX chip"); + Serial.println("LSM6DSOX initialization unsuccessful"); + Serial.println("Check LSM6DSOX wiring or try cycling power"); + while (1) {} + } + else { + Serial.println("LSM6DSOX Found!"); + } + lsm6dsox.setGyroRange(GYRO_SCALE); + lsm6dsox.setAccelRange(ACCEL_SCALE); + // Set the data rate of the board higher than stock 104Hz + // lsm6dsox.setAccelDataRate(LSM6DS_RATE_6_66K_HZ); + // lsm6dsox.setGyroDataRate(LSM6DS_RATE_6_66K_HZ); + #endif + +} + +void getIMUdata() { + //DESCRIPTION: Request full dataset from IMU and LP filter gyro, accelerometer, and magnetometer data + /* + * Reads accelerometer, gyro, and magnetometer data from IMU as AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, MagZ. + * These values are scaled according to the IMU datasheet to put them into correct units of g's, deg/sec, and uT. A simple first-order + * low-pass filter is used to get rid of high frequency noise in these raw signals. Generally you want to cut + * off everything past 80Hz, but if your loop rate is not fast enough, the low pass filter will cause a lag in + * the readings. The filter parameters B_gyro and B_accel are set to be good for a 2kHz loop rate. Finally, + * the constant errors found in calculate_IMU_error() on startup are subtracted from the accelerometer and gyro readings. + */ + #ifndef USE_LSM6DSOX_SPI + int16_t AcX = 0; + int16_t AcY = 0; + int16_t AcZ = 0; + int16_t GyX = 0; + int16_t GyY = 0; + int16_t GyZ = 0; + int16_t MgX = 0; + int16_t MgY = 0; + int16_t MgZ = 0; + #endif + + #if defined USE_MPU6050_I2C + mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); + #elif defined USE_MPU9250_SPI + mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); + #elif defined USE_LSM6DSOX_SPI + lsm6dsox.getEvent(&accel, &gyro, &temp); + AccX = accel.acceleration.x / 9.80665; // Convert m/s to G-factor + AccY = accel.acceleration.y / 9.80665; + AccZ = accel.acceleration.z / 9.80665; + GyroX = gyro.gyro.x * 57.29578; // convert rad/s back to deg/sec + GyroY = gyro.gyro.y * 57.29578; + GyroZ = gyro.gyro.z * 57.29578; + #endif + + // The MPU6050 and MPU9250 IMUs have a different scale factor from the LSM6DSOX + #ifndef USE_LSM6DSOX_SPI + //Accelerometer + AccX = AcX / ACCEL_SCALE_FACTOR; //G's + AccY = AcY / ACCEL_SCALE_FACTOR; + AccZ = AcZ / ACCEL_SCALE_FACTOR; + + //Gyro + GyroX = GyX / GYRO_SCALE_FACTOR; //deg/sec + GyroY = GyY / GYRO_SCALE_FACTOR; + GyroZ = GyZ / GYRO_SCALE_FACTOR; + + //Magnetometer + MagX = MgX/6.0; //uT + MagY = MgY/6.0; + MagZ = MgZ/6.0; + #endif + + //Correct the outputs with the calculated error values + AccX = AccX - AccErrorX; + AccY = AccY - AccErrorY; + AccZ = AccZ - AccErrorZ; + //LP filter accelerometer data + AccX = (1.0 - B_accel)*AccX_prev + B_accel*AccX; + AccY = (1.0 - B_accel)*AccY_prev + B_accel*AccY; + AccZ = (1.0 - B_accel)*AccZ_prev + B_accel*AccZ; + AccX_prev = AccX; + AccY_prev = AccY; + AccZ_prev = AccZ; + + //Correct the outputs with the calculated error values + GyroX = GyroX - GyroErrorX; + GyroY = GyroY - GyroErrorY; + GyroZ = GyroZ - GyroErrorZ; + //LP filter gyro data + GyroX = (1.0 - B_gyro)*GyroX_prev + B_gyro*GyroX; + GyroY = (1.0 - B_gyro)*GyroY_prev + B_gyro*GyroY; + GyroZ = (1.0 - B_gyro)*GyroZ_prev + B_gyro*GyroZ; + GyroX_prev = GyroX; + GyroY_prev = GyroY; + GyroZ_prev = GyroZ; + + //Correct the outputs with the calculated error values + MagX = (MagX - MagErrorX)*MagScaleX; + MagY = (MagY - MagErrorY)*MagScaleY; + MagZ = (MagZ - MagErrorZ)*MagScaleZ; + //LP filter magnetometer data + MagX = (1.0 - B_mag)*MagX_prev + B_mag*MagX; + MagY = (1.0 - B_mag)*MagY_prev + B_mag*MagY; + MagZ = (1.0 - B_mag)*MagZ_prev + B_mag*MagZ; + MagX_prev = MagX; + MagY_prev = MagY; + MagZ_prev = MagZ; +} + +void calculate_IMU_error() { + //DESCRIPTION: Computes IMU accelerometer and gyro error on startup. Note: vehicle should be powered up on flat surface + /* + * Don't worry too much about what this is doing. The error values it computes are applied to the raw gyro and + * accelerometer values AccX, AccY, AccZ, GyroX, GyroY, GyroZ in getIMUdata(). This eliminates drift in the + * measurement. + */ + #ifndef USE_LSM6DSOX_SPI + int16_t AcX = 0; + int16_t AcY = 0; + int16_t AcZ = 0; + int16_t GyX = 0; + int16_t GyY = 0; + int16_t GyZ = 0; + #endif + AccErrorX = 0.0; + AccErrorY = 0.0; + AccErrorZ = 0.0; + GyroErrorX = 0.0; + GyroErrorY= 0.0; + GyroErrorZ = 0.0; + + //Read IMU values 12000 times + int c = 0; + while (c < 12000) { + #if defined USE_MPU6050_I2C + mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); + #elif defined USE_MPU9250_SPI + mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); + #elif defined USE_LSM6DSOX_SPI + lsm6dsox.getEvent(&accel, &gyro, &temp); + AccX = accel.acceleration.x / 9.80665; // Convert m/s to G-factor + AccY = accel.acceleration.y / 9.80665; + AccZ = accel.acceleration.z / 9.80665; + GyroX = gyro.gyro.x * 57.29578; // convert rad/s back to deg/sec + GyroY = gyro.gyro.y * 57.29578; + GyroZ = gyro.gyro.z * 57.29578; + #endif + + // The MPU6050 and MPU9250 IMUs have a different scale factor from the LSM6DSOX + #ifndef USE_LSM6DSOX_SPI + //Accelerometer + AccX = AcX / ACCEL_SCALE_FACTOR; //G's + AccY = AcY / ACCEL_SCALE_FACTOR; + AccZ = AcZ / ACCEL_SCALE_FACTOR; + + //Gyro + GyroX = GyX / GYRO_SCALE_FACTOR; //deg/sec + GyroY = GyY / GYRO_SCALE_FACTOR; + GyroZ = GyZ / GYRO_SCALE_FACTOR; + #endif + + //Sum all readings + AccErrorX = AccErrorX + AccX; + AccErrorY = AccErrorY + AccY; + AccErrorZ = AccErrorZ + AccZ; + GyroErrorX = GyroErrorX + GyroX; + GyroErrorY = GyroErrorY + GyroY; + GyroErrorZ = GyroErrorZ + GyroZ; + c++; + } + //Divide the sum by 12000 to get the error value + AccErrorX = AccErrorX / c; + AccErrorY = AccErrorY / c; + AccErrorZ = AccErrorZ / c - 1.0; + GyroErrorX = GyroErrorX / c; + GyroErrorY = GyroErrorY / c; + GyroErrorZ = GyroErrorZ / c; + + Serial.print("float AccErrorX = "); + Serial.print(AccErrorX); + Serial.println(";"); + Serial.print("float AccErrorY = "); + Serial.print(AccErrorY); + Serial.println(";"); + Serial.print("float AccErrorZ = "); + Serial.print(AccErrorZ); + Serial.println(";"); + + Serial.print("float GyroErrorX = "); + Serial.print(GyroErrorX); + Serial.println(";"); + Serial.print("float GyroErrorY = "); + Serial.print(GyroErrorY); + Serial.println(";"); + Serial.print("float GyroErrorZ = "); + Serial.print(GyroErrorZ); + Serial.println(";"); + + Serial.println("Paste these values in user specified variables section and comment out calculate_IMU_error() in void setup."); + while(1); +} + +void calibrateAttitude() { + //DESCRIPTION: Used to warm up the main loop to allow the madwick filter to converge before commands can be sent to the actuators + //Assuming vehicle is powered up on level surface! + /* + * This function is used on startup to warm up the attitude estimation and is what causes startup to take a few seconds + * to boot. + */ + //Warm up IMU and madgwick filter in simulated main loop + for (int i = 0; i <= 10000; i++) { + prev_time = current_time; + current_time = micros(); + dt = (current_time - prev_time)/1000000.0; + getIMUdata(); + Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); + loopRate(2000); //do not exceed 2000Hz + } +} + +void Madgwick(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float invSampleFreq) { + //DESCRIPTION: Attitude estimation through sensor fusion - 9DOF + /* + * This function fuses the accelerometer gyro, and magnetometer readings AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, and MagZ for attitude estimation. + * Don't worry about the math. There is a tunable parameter B_madgwick in the user specified variable section which basically + * adjusts the weight of gyro data in the state estimate. Higher beta leads to noisier estimate, lower + * beta leads to slower to respond estimate. It is currently tuned for 2kHz loop rate. This function updates the roll_IMU, + * pitch_IMU, and yaw_IMU variables which are in degrees. If magnetometer data is not available, this function calls Madgwick6DOF() instead. + */ + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float hx, hy; + float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; + + //use 6DOF algorithm if MPU6050 is being used + #if defined USE_MPU6050_I2C + Madgwick6DOF(gx, gy, gz, ax, ay, az, invSampleFreq); + return; + #endif + + //Use 6DOF algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) + if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + Madgwick6DOF(gx, gy, gz, ax, ay, az, invSampleFreq); + return; + } + + //Convert gyroscope degrees/sec to radians/sec + gx *= 0.0174533f; + gy *= 0.0174533f; + gz *= 0.0174533f; + + //Rate of change of quaternion from gyroscope + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + + //Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + //Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + //Normalise magnetometer measurement + recipNorm = invSqrt(mx * mx + my * my + mz * mz); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; + + //Auxiliary variables to avoid repeated arithmetic + _2q0mx = 2.0f * q0 * mx; + _2q0my = 2.0f * q0 * my; + _2q0mz = 2.0f * q0 * mz; + _2q1mx = 2.0f * q1 * mx; + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _2q0q2 = 2.0f * q0 * q2; + _2q2q3 = 2.0f * q2 * q3; + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q3q3 = q3 * q3; + + //Reference direction of Earth's magnetic field + hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; + hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; + _2bx = sqrtf(hx * hx + hy * hy); + _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + //Gradient decent algorithm corrective step + s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + //Apply feedback step + qDot1 -= B_madgwick * s0; + qDot2 -= B_madgwick * s1; + qDot3 -= B_madgwick * s2; + qDot4 -= B_madgwick * s3; + } + + //Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * invSampleFreq; + q1 += qDot2 * invSampleFreq; + q2 += qDot3 * invSampleFreq; + q3 += qDot4 * invSampleFreq; + + //Normalize quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + //compute angles - NWU + roll_IMU = atan2 (q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2) *57.29577951; //degrees + pitch_IMU = -asin (constrain(-2.0f * (q1*q3 - q0*q2),-0.999999,0.999999))*57.29577951; //degrees + yaw_IMU = -atan2(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3) *57.29577951; //degrees +} + +void Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, float invSampleFreq) { + //DESCRIPTION: Attitude estimation through sensor fusion - 6DOF + /* + * See description of Madgwick() for more information. This is a 6DOF implimentation for when magnetometer data is not + * available (for example when using the recommended MPU6050 IMU for the default setup). + */ + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + + //Convert gyroscope degrees/sec to radians/sec + gx *= 0.0174533f; + gy *= 0.0174533f; + gz *= 0.0174533f; + + //Rate of change of quaternion from gyroscope + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + + //Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + //Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + //Auxiliary variables to avoid repeated arithmetic + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _4q0 = 4.0f * q0; + _4q1 = 4.0f * q1; + _4q2 = 4.0f * q2; + _8q1 = 8.0f * q1; + _8q2 = 8.0f * q2; + q0q0 = q0 * q0; + q1q1 = q1 * q1; + q2q2 = q2 * q2; + q3q3 = q3 * q3; + + //Gradient decent algorithm corrective step + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); //normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + //Apply feedback step + qDot1 -= B_madgwick * s0; + qDot2 -= B_madgwick * s1; + qDot3 -= B_madgwick * s2; + qDot4 -= B_madgwick * s3; + } + + //Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * invSampleFreq; + q1 += qDot2 * invSampleFreq; + q2 += qDot3 * invSampleFreq; + q3 += qDot4 * invSampleFreq; + + //Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + //Compute angles + roll_IMU = atan2 (q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2) *57.29577951; //degrees + pitch_IMU = -asin (constrain(-2.0f * (q1*q3 - q0*q2),-0.999999,0.999999))*57.29577951; //degrees + yaw_IMU = -atan2(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3) *57.29577951; //degrees +} + +void getDesState() { + //DESCRIPTION: Normalizes desired control values to appropriate values + /* + * Updates the desired state variables thro_des, roll_des, pitch_des, and yaw_des. These are computed by using the raw + * RC pwm commands and scaling them to be within our limits defined in setup. thro_des stays within 0 to 1 range. + * roll_des and pitch_des are scaled to be within max roll/pitch amount in either degrees (angle mode) or degrees/sec + * (rate mode). yaw_des is scaled to be within max yaw in degrees/sec. Also creates roll_passthru, pitch_passthru, and + * yaw_passthru variables, to be used in commanding motors/servos with direct unstabilized commands in controlMixer(). + */ + thro_des = (channel_1_pwm - 1000.0)/1000.0; //Between 0 and 1 + roll_des = (channel_2_pwm - 1500.0)/500.0; //Between -1 and 1 + pitch_des = (channel_3_pwm - 1500.0)/500.0; //Between -1 and 1 + yaw_des = (channel_4_pwm - 1500.0)/500.0; //Between -1 and 1 + roll_passthru = roll_des/2.0; //Between -0.5 and 0.5 + pitch_passthru = pitch_des/2.0; //Between -0.5 and 0.5 + yaw_passthru = yaw_des/2.0; //Between -0.5 and 0.5 + + //Constrain within normalized bounds + thro_des = constrain(thro_des, 0.0, 1.0); //Between 0 and 1 + roll_des = constrain(roll_des, -1.0, 1.0)*maxRoll; //Between -maxRoll and +maxRoll + pitch_des = constrain(pitch_des, -1.0, 1.0)*maxPitch; //Between -maxPitch and +maxPitch + yaw_des = constrain(yaw_des, -1.0, 1.0)*maxYaw; //Between -maxYaw and +maxYaw + roll_passthru = constrain(roll_passthru, -0.5, 0.5); + pitch_passthru = constrain(pitch_passthru, -0.5, 0.5); + yaw_passthru = constrain(yaw_passthru, -0.5, 0.5); +} + +void controlANGLE() { + //DESCRIPTION: Computes control commands based on state error (angle) + /* + * Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des computed in + * getDesState(). Error is simply the desired state minus the actual state (ex. roll_des - roll_IMU). Two safety features + * are implimented here regarding the I terms. The I terms are saturated within specified limits on startup to prevent + * excessive buildup. This can be seen by holding the vehicle at an angle and seeing the motors ramp up on one side until + * they've maxed out throttle...saturating I to a specified limit fixes this. The second feature defaults the I terms to 0 + * if the throttle is at the minimum setting. This means the motors will not start spooling up on the ground, and the I + * terms will always start from 0 on takeoff. This function updates the variables roll_PID, pitch_PID, and yaw_PID which + * can be thought of as 1-D stablized signals. They are mixed to the configuration of the vehicle in controlMixer(). + */ + + //Roll + error_roll = roll_des - roll_IMU; + integral_roll = integral_roll_prev + error_roll*dt; + if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low + integral_roll = 0; + } + integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_roll = GyroX; + roll_PID = 0.01*(Kp_roll_angle*error_roll + Ki_roll_angle*integral_roll - Kd_roll_angle*derivative_roll); //Scaled by .01 to bring within -1 to 1 range + + //Pitch + error_pitch = pitch_des - pitch_IMU; + integral_pitch = integral_pitch_prev + error_pitch*dt; + if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low + integral_pitch = 0; + } + integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_pitch = GyroY; + pitch_PID = .01*(Kp_pitch_angle*error_pitch + Ki_pitch_angle*integral_pitch - Kd_pitch_angle*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range + + //Yaw, stablize on rate from GyroZ + error_yaw = yaw_des - GyroZ; + integral_yaw = integral_yaw_prev + error_yaw*dt; + if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low + integral_yaw = 0; + } + integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_yaw = (error_yaw - error_yaw_prev)/dt; + yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range + + //Update roll variables + integral_roll_prev = integral_roll; + //Update pitch variables + integral_pitch_prev = integral_pitch; + //Update yaw variables + error_yaw_prev = error_yaw; + integral_yaw_prev = integral_yaw; +} + +void controlANGLE2() { + //DESCRIPTION: Computes control commands based on state error (angle) in cascaded scheme + /* + * Gives better performance than controlANGLE() but requires much more tuning. Not reccommended for first-time setup. + * See the documentation for tuning this controller. + */ + //Outer loop - PID on angle + float roll_des_ol, pitch_des_ol; + //Roll + error_roll = roll_des - roll_IMU; + integral_roll_ol = integral_roll_prev_ol + error_roll*dt; + if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low + integral_roll_ol = 0; + } + integral_roll_ol = constrain(integral_roll_ol, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_roll = (roll_IMU - roll_IMU_prev)/dt; + roll_des_ol = Kp_roll_angle*error_roll + Ki_roll_angle*integral_roll_ol;// - Kd_roll_angle*derivative_roll; + + //Pitch + error_pitch = pitch_des - pitch_IMU; + integral_pitch_ol = integral_pitch_prev_ol + error_pitch*dt; + if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low + integral_pitch_ol = 0; + } + integral_pitch_ol = constrain(integral_pitch_ol, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup + derivative_pitch = (pitch_IMU - pitch_IMU_prev)/dt; + pitch_des_ol = Kp_pitch_angle*error_pitch + Ki_pitch_angle*integral_pitch_ol;// - Kd_pitch_angle*derivative_pitch; + + //Apply loop gain, constrain, and LP filter for artificial damping + float Kl = 30.0; + roll_des_ol = Kl*roll_des_ol; + pitch_des_ol = Kl*pitch_des_ol; + roll_des_ol = constrain(roll_des_ol, -240.0, 240.0); + pitch_des_ol = constrain(pitch_des_ol, -240.0, 240.0); + roll_des_ol = (1.0 - B_loop_roll)*roll_des_prev + B_loop_roll*roll_des_ol; + pitch_des_ol = (1.0 - B_loop_pitch)*pitch_des_prev + B_loop_pitch*pitch_des_ol; + + //Inner loop - PID on rate + //Roll + error_roll = roll_des_ol - GyroX; + integral_roll_il = integral_roll_prev_il + error_roll*dt; + if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low + integral_roll_il = 0; + } + integral_roll_il = constrain(integral_roll_il, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_roll = (error_roll - error_roll_prev)/dt; + roll_PID = 0.01*(Kp_roll_rate*error_roll + Ki_roll_rate*integral_roll_il + Kd_roll_rate*derivative_roll); //Scaled by .01 to bring within -1 to 1 range + + //Pitch + error_pitch = pitch_des_ol - GyroY; + integral_pitch_il = integral_pitch_prev_il + error_pitch*dt; + if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low + integral_pitch_il = 0; + } + integral_pitch_il = constrain(integral_pitch_il, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_pitch = (error_pitch - error_pitch_prev)/dt; + pitch_PID = 0.01*(Kp_pitch_rate*error_pitch + Ki_pitch_rate*integral_pitch_il + Kd_pitch_rate*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range + + //Yaw + error_yaw = yaw_des - GyroZ; + integral_yaw = integral_yaw_prev + error_yaw*dt; + if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low + integral_yaw = 0; + } + integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_yaw = (error_yaw - error_yaw_prev)/dt; + yaw_PID = 0.01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range + + //Update roll variables + integral_roll_prev_ol = integral_roll_ol; + integral_roll_prev_il = integral_roll_il; + error_roll_prev = error_roll; + roll_IMU_prev = roll_IMU; + roll_des_prev = roll_des_ol; + //Update pitch variables + integral_pitch_prev_ol = integral_pitch_ol; + integral_pitch_prev_il = integral_pitch_il; + error_pitch_prev = error_pitch; + pitch_IMU_prev = pitch_IMU; + pitch_des_prev = pitch_des_ol; + //Update yaw variables + error_yaw_prev = error_yaw; + integral_yaw_prev = integral_yaw; + +} + +void controlRATE() { + //DESCRIPTION: Computes control commands based on state error (rate) + /* + * See explanation for controlANGLE(). Everything is the same here except the error is now the desired rate - raw gyro reading. + */ + //Roll + error_roll = roll_des - GyroX; + integral_roll = integral_roll_prev + error_roll*dt; + if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low + integral_roll = 0; + } + integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_roll = (error_roll - error_roll_prev)/dt; + roll_PID = .01*(Kp_roll_rate*error_roll + Ki_roll_rate*integral_roll + Kd_roll_rate*derivative_roll); //Scaled by .01 to bring within -1 to 1 range + + //Pitch + error_pitch = pitch_des - GyroY; + integral_pitch = integral_pitch_prev + error_pitch*dt; + if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low + integral_pitch = 0; + } + integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_pitch = (error_pitch - error_pitch_prev)/dt; + pitch_PID = .01*(Kp_pitch_rate*error_pitch + Ki_pitch_rate*integral_pitch + Kd_pitch_rate*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range + + //Yaw, stablize on rate from GyroZ + error_yaw = yaw_des - GyroZ; + integral_yaw = integral_yaw_prev + error_yaw*dt; + if (channel_1_pwm < 1060) { //Don't let integrator build if throttle is too low + integral_yaw = 0; + } + integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_yaw = (error_yaw - error_yaw_prev)/dt; + yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range + + //Update roll variables + error_roll_prev = error_roll; + integral_roll_prev = integral_roll; + GyroX_prev = GyroX; + //Update pitch variables + error_pitch_prev = error_pitch; + integral_pitch_prev = integral_pitch; + GyroY_prev = GyroY; + //Update yaw variables + error_yaw_prev = error_yaw; + integral_yaw_prev = integral_yaw; +} + +void scaleCommands() { + //DESCRIPTION: Scale normalized actuator commands to values for ESC/Servo protocol + /* + * mX_command_scaled variables from the mixer function are scaled to 125-250us for OneShot125 protocol. sX_command_scaled variables from + * the mixer function are scaled to 0-180 for the servo library using standard PWM. + * mX_command_PWM are updated here which are used to command the motors in commandMotors(). sX_command_PWM are updated + * which are used to command the servos. + */ + //Scaled to 125us - 250us for oneshot125 protocol + m1_command_PWM = m1_command_scaled*125 + 125; + m2_command_PWM = m2_command_scaled*125 + 125; + m3_command_PWM = m3_command_scaled*125 + 125; + m4_command_PWM = m4_command_scaled*125 + 125; + m5_command_PWM = m5_command_scaled*125 + 125; + m6_command_PWM = m6_command_scaled*125 + 125; + //Constrain commands to motors within oneshot125 bounds + m1_command_PWM = constrain(m1_command_PWM, 125, 250); + m2_command_PWM = constrain(m2_command_PWM, 125, 250); + m3_command_PWM = constrain(m3_command_PWM, 125, 250); + m4_command_PWM = constrain(m4_command_PWM, 125, 250); + m5_command_PWM = constrain(m5_command_PWM, 125, 250); + m6_command_PWM = constrain(m6_command_PWM, 125, 250); + + //Scaled to 0-180 for servo library + s1_command_PWM = s1_command_scaled*180; + s2_command_PWM = s2_command_scaled*180; + s3_command_PWM = s3_command_scaled*180; + s4_command_PWM = s4_command_scaled*180; + s5_command_PWM = s5_command_scaled*180; + s6_command_PWM = s6_command_scaled*180; + s7_command_PWM = s7_command_scaled*180; + //Constrain commands to servos within servo library bounds + s1_command_PWM = constrain(s1_command_PWM, 0, 180); + s2_command_PWM = constrain(s2_command_PWM, 0, 180); + s3_command_PWM = constrain(s3_command_PWM, 0, 180); + s4_command_PWM = constrain(s4_command_PWM, 0, 180); + s5_command_PWM = constrain(s5_command_PWM, 0, 180); + s6_command_PWM = constrain(s6_command_PWM, 0, 180); + s7_command_PWM = constrain(s7_command_PWM, 0, 180); + +} + +void getCommands() { + //DESCRIPTION: Get raw PWM values for every channel from the radio + /* + * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of + * the loop. If using a PWM or PPM receiver, the radio commands are retrieved from a function in the readPWM file separate from this one which + * is running a bunch of interrupts to continuously update the radio readings. If using an SBUS receiver, the alues are pulled from the SBUS library directly. + * The raw radio commands are filtered with a first order low-pass filter to eliminate any really high frequency noise. + */ + + #if defined USE_PPM_RX || defined USE_PWM_RX + channel_1_pwm = getRadioPWM(1); + channel_2_pwm = getRadioPWM(2); + channel_3_pwm = getRadioPWM(3); + channel_4_pwm = getRadioPWM(4); + channel_5_pwm = getRadioPWM(5); + channel_6_pwm = getRadioPWM(6); + + #elif defined USE_SBUS_RX + if (sbus.read(&sbusChannels[0], &sbusFailSafe, &sbusLostFrame)) { + //sBus scaling below is for Taranis-Plus and X4R-SB + float scale = 0.615; + float bias = 895.0; + channel_1_pwm = sbusChannels[0] * scale + bias; + channel_2_pwm = sbusChannels[1] * scale + bias; + channel_3_pwm = sbusChannels[2] * scale + bias; + channel_4_pwm = sbusChannels[3] * scale + bias; + channel_5_pwm = sbusChannels[4] * scale + bias; + channel_6_pwm = sbusChannels[5] * scale + bias; + } + + #elif defined USE_DSM_RX + if (DSM.timedOut(micros())) { + //Serial.println("*** DSM RX TIMED OUT ***"); + } + else if (DSM.gotNewFrame()) { + uint16_t values[num_DSM_channels]; + DSM.getChannelValues(values, num_DSM_channels); + + channel_1_pwm = values[0]; + channel_2_pwm = values[1]; + channel_3_pwm = values[2]; + channel_4_pwm = values[3]; + channel_5_pwm = values[4]; + channel_6_pwm = values[5]; + } + #endif + + //Low-pass the critical commands and update previous values + float b = 0.7; //Lower=slower, higher=noiser + channel_1_pwm = (1.0 - b)*channel_1_pwm_prev + b*channel_1_pwm; + channel_2_pwm = (1.0 - b)*channel_2_pwm_prev + b*channel_2_pwm; + channel_3_pwm = (1.0 - b)*channel_3_pwm_prev + b*channel_3_pwm; + channel_4_pwm = (1.0 - b)*channel_4_pwm_prev + b*channel_4_pwm; + channel_1_pwm_prev = channel_1_pwm; + channel_2_pwm_prev = channel_2_pwm; + channel_3_pwm_prev = channel_3_pwm; + channel_4_pwm_prev = channel_4_pwm; +} + +void failSafe() { + //DESCRIPTION: If radio gives garbage values, set all commands to default values + /* + * Radio connection failsafe used to check if the getCommands() function is returning acceptable pwm values. If any of + * the commands are lower than 800 or higher than 2200, then we can be certain that there is an issue with the radio + * connection (most likely hardware related). If any of the channels show this failure, then all of the radio commands + * channel_x_pwm are set to default failsafe values specified in the setup. Comment out this function when troubleshooting + * your radio connection in case any extreme values are triggering this function to overwrite the printed variables. + * + * When using SBUS, the library handles the detection of lost frames and failsafe. Some receivers handle failsafe differently. + * Testing with FrSky SBUS receivers immediately triggered failsafe when Tx disconnected. A Radiomaster RP3-H ELRS receiver would + * not trigger failsafe when set to "No Pulses", but would trigger when set to "Last Position" after a few seconds. + * Test your receiver's behavior! You can do this with the example sketch in the "Bolder Flight Systems SBUS" library. + */ + + //If using SBUS, this will only check the boolean failsafe value from the SBUS library. If using PWM RX, it checks each channel. + #ifndef USE_SBUS_RX + unsigned minVal = 800; + unsigned maxVal = 2200; + int check1 = 0; + int check2 = 0; + int check3 = 0; + int check4 = 0; + int check5 = 0; + int check6 = 0; + + //Triggers for failure criteria + if (channel_1_pwm > maxVal || channel_1_pwm < minVal) check1 = 1; + if (channel_2_pwm > maxVal || channel_2_pwm < minVal) check2 = 1; + if (channel_3_pwm > maxVal || channel_3_pwm < minVal) check3 = 1; + if (channel_4_pwm > maxVal || channel_4_pwm < minVal) check4 = 1; + if (channel_5_pwm > maxVal || channel_5_pwm < minVal) check5 = 1; + if (channel_6_pwm > maxVal || channel_6_pwm < minVal) check6 = 1; + #endif + + //If any failures, set to default failsafe values + #ifdef USE_SBUS_RX + if (sbusFailSafe) { + #elif + if ((check1 + check2 + check3 + check4 + check5 + check6) > 0) { + #endif + channel_1_pwm = channel_1_fs; + channel_2_pwm = channel_2_fs; + channel_3_pwm = channel_3_fs; + channel_4_pwm = channel_4_fs; + channel_5_pwm = channel_5_fs; + channel_6_pwm = channel_6_fs; + } +} + +void commandMotors() { + //DESCRIPTION: Send pulses to motor pins, oneshot125 protocol + /* + * My crude implimentation of OneShot125 protocol which sends 125 - 250us pulses to the ESCs (mXPin). The pulselengths being + * sent are mX_command_PWM, computed in scaleCommands(). This may be replaced by something more efficient in the future. + */ + int wentLow = 0; + int pulseStart, timer; + int flagM1 = 0; + int flagM2 = 0; + int flagM3 = 0; + int flagM4 = 0; + int flagM5 = 0; + int flagM6 = 0; + + //Write all motor pins high + digitalWrite(m1Pin, HIGH); + digitalWrite(m2Pin, HIGH); + digitalWrite(m3Pin, HIGH); + digitalWrite(m4Pin, HIGH); + digitalWrite(m5Pin, HIGH); + digitalWrite(m6Pin, HIGH); + pulseStart = micros(); + + //Write each motor pin low as correct pulse length is reached + while (wentLow < 6 ) { //Keep going until final (6th) pulse is finished, then done + timer = micros(); + if ((m1_command_PWM <= timer - pulseStart) && (flagM1==0)) { + digitalWrite(m1Pin, LOW); + wentLow = wentLow + 1; + flagM1 = 1; + } + if ((m2_command_PWM <= timer - pulseStart) && (flagM2==0)) { + digitalWrite(m2Pin, LOW); + wentLow = wentLow + 1; + flagM2 = 1; + } + if ((m3_command_PWM <= timer - pulseStart) && (flagM3==0)) { + digitalWrite(m3Pin, LOW); + wentLow = wentLow + 1; + flagM3 = 1; + } + if ((m4_command_PWM <= timer - pulseStart) && (flagM4==0)) { + digitalWrite(m4Pin, LOW); + wentLow = wentLow + 1; + flagM4 = 1; + } + if ((m5_command_PWM <= timer - pulseStart) && (flagM5==0)) { + digitalWrite(m5Pin, LOW); + wentLow = wentLow + 1; + flagM5 = 1; + } + if ((m6_command_PWM <= timer - pulseStart) && (flagM6==0)) { + digitalWrite(m6Pin, LOW); + wentLow = wentLow + 1; + flagM6 = 1; + } + } +} + +void armMotors() { + //DESCRIPTION: Sends many command pulses to the motors, to be used to arm motors in the void setup() + /* + * Loops over the commandMotors() function 50 times with a delay in between, simulating how the commandMotors() + * function is used in the main loop. Ensures motors arm within the void setup() where there are some delays + * for other processes that sometimes prevent motors from arming. + */ + for (int i = 0; i <= 50; i++) { + commandMotors(); + delay(2); + } +} + +void calibrateESCs() { + //DESCRIPTION: Used in void setup() to allow standard ESC calibration procedure with the radio to take place. + /* + * Simulates the void loop(), but only for the purpose of providing throttle pass through to the motors, so that you can + * power up with throttle at full, let ESCs begin arming sequence, and lower throttle to zero. This function should only be + * uncommented when performing an ESC calibration. + */ + while (true) { + prev_time = current_time; + current_time = micros(); + dt = (current_time - prev_time)/1000000.0; + + digitalWrite(13, HIGH); //LED on to indicate we are not in main loop + + getCommands(); //Pulls current available radio commands + failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup + getDesState(); //Convert raw commands to normalized values based on saturated control limits + getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise + Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU (degrees) + getDesState(); //Convert raw commands to normalized values based on saturated control limits + + m1_command_scaled = thro_des; + m2_command_scaled = thro_des; + m3_command_scaled = thro_des; + m4_command_scaled = thro_des; + m5_command_scaled = thro_des; + m6_command_scaled = thro_des; + s1_command_scaled = thro_des; + s2_command_scaled = thro_des; + s3_command_scaled = thro_des; + s4_command_scaled = thro_des; + s5_command_scaled = thro_des; + s6_command_scaled = thro_des; + s7_command_scaled = thro_des; + scaleCommands(); //Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) + + //throttleCut(); //Directly sets motor commands to low based on state of ch5 + + servo1.write(s1_command_PWM); + servo2.write(s2_command_PWM); + servo3.write(s3_command_PWM); + servo4.write(s4_command_PWM); + servo5.write(s5_command_PWM); + servo6.write(s6_command_PWM); + servo7.write(s7_command_PWM); + commandMotors(); //Sends command pulses to each motor pin using OneShot125 protocol + + //printRadioData(); //Radio pwm values (expected: 1000 to 2000) + + loopRate(2000); //Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default + } +} + +float floatFaderLinear(float param, float param_min, float param_max, float fadeTime, int state, int loopFreq){ + //DESCRIPTION: Linearly fades a float type variable between min and max bounds based on desired high or low state and time + /* + * Takes in a float variable, desired minimum and maximum bounds, fade time, high or low desired state, and the loop frequency + * and linearly interpolates that param variable between the maximum and minimum bounds. This function can be called in controlMixer() + * and high/low states can be determined by monitoring the state of an auxillarly radio channel. For example, if channel_6_pwm is being + * monitored to switch between two dynamic configurations (hover and forward flight), this function can be called within the logical + * statements in order to fade controller gains, for example between the two dynamic configurations. The 'state' (1 or 0) can be used + * to designate the two final options for that control gain based on the dynamic configuration assignment to the auxillary radio channel. + * + */ + float diffParam = (param_max - param_min)/(fadeTime*loopFreq); //Difference to add or subtract from param for each loop iteration for desired fadeTime + + if (state == 1) { //Maximum param bound desired, increase param by diffParam for each loop iteration + param = param + diffParam; + } + else if (state == 0) { //Minimum param bound desired, decrease param by diffParam for each loop iteration + param = param - diffParam; + } + + param = constrain(param, param_min, param_max); //Constrain param within max bounds + + return param; +} + +float floatFaderLinear2(float param, float param_des, float param_lower, float param_upper, float fadeTime_up, float fadeTime_down, int loopFreq){ + //DESCRIPTION: Linearly fades a float type variable from its current value to the desired value, up or down + /* + * Takes in a float variable to be modified, desired new position, upper value, lower value, fade time, and the loop frequency + * and linearly fades that param variable up or down to the desired value. This function can be called in controlMixer() + * to fade up or down between flight modes monitored by an auxillary radio channel. For example, if channel_6_pwm is being + * monitored to switch between two dynamic configurations (hover and forward flight), this function can be called within the logical + * statements in order to fade controller gains, for example between the two dynamic configurations. + * + */ + if (param > param_des) { //Need to fade down to get to desired + float diffParam = (param_upper - param_des)/(fadeTime_down*loopFreq); + param = param - diffParam; + } + else if (param < param_des) { //Need to fade up to get to desired + float diffParam = (param_des - param_lower)/(fadeTime_up*loopFreq); + param = param + diffParam; + } + + param = constrain(param, param_lower, param_upper); //Constrain param within max bounds + + return param; +} + +void switchRollYaw(int reverseRoll, int reverseYaw) { + //DESCRIPTION: Switches roll_des and yaw_des variables for tailsitter-type configurations + /* + * Takes in two integers (either 1 or -1) corresponding to the desired reversing of the roll axis and yaw axis, respectively. + * Reversing of the roll or yaw axis may be needed when switching between the two for some dynamic configurations. Inputs of 1, 1 does not + * reverse either of them, while -1, 1 will reverse the output corresponding to the new roll axis. + * This function may be replaced in the future by a function that switches the IMU data instead (so that angle can also be estimated with the + * IMU tilted 90 degrees from default level). + */ + float switch_holder; + + switch_holder = yaw_des; + yaw_des = reverseYaw*roll_des; + roll_des = reverseRoll*switch_holder; +} + +void throttleCut() { + //DESCRIPTION: Directly set actuator outputs to minimum value if triggered + /* + Monitors the state of radio command channel_5_pwm and directly sets the mx_command_PWM values to minimum (120 is + minimum for oneshot125 protocol, 0 is minimum for standard PWM servo library used) if channel 5 is high. This is the last function + called before commandMotors() is called so that the last thing checked is if the user is giving permission to command + the motors to anything other than minimum value. Safety first. + + channel_5_pwm is LOW then throttle cut is OFF and throttle value can change. (ThrottleCut is DEACTIVATED) + channel_5_pwm is HIGH then throttle cut is ON and throttle value = 120 only. (ThrottleCut is ACTIVATED), (drone is DISARMED) + */ + if ((channel_5_pwm > 1500) || (armedFly == false)) { + armedFly = false; + m1_command_PWM = 120; + m2_command_PWM = 120; + m3_command_PWM = 120; + m4_command_PWM = 120; + m5_command_PWM = 120; + m6_command_PWM = 120; + + //Uncomment if using servo PWM variables to control motor ESCs + //s1_command_PWM = 0; + //s2_command_PWM = 0; + //s3_command_PWM = 0; + //s4_command_PWM = 0; + //s5_command_PWM = 0; + //s6_command_PWM = 0; + //s7_command_PWM = 0; + } +} + +void calibrateMagnetometer() { + #if defined USE_MPU9250_SPI + float success; + Serial.println("Beginning magnetometer calibration in"); + Serial.println("3..."); + delay(1000); + Serial.println("2..."); + delay(1000); + Serial.println("1..."); + delay(1000); + Serial.println("Rotate the IMU about all axes until complete."); + Serial.println(" "); + success = mpu9250.calibrateMag(); + if(success) { + Serial.println("Calibration Successful!"); + Serial.println("Please comment out the calibrateMagnetometer() function and copy these values into the code:"); + Serial.print("float MagErrorX = "); + Serial.print(mpu9250.getMagBiasX_uT()); + Serial.println(";"); + Serial.print("float MagErrorY = "); + Serial.print(mpu9250.getMagBiasY_uT()); + Serial.println(";"); + Serial.print("float MagErrorZ = "); + Serial.print(mpu9250.getMagBiasZ_uT()); + Serial.println(";"); + Serial.print("float MagScaleX = "); + Serial.print(mpu9250.getMagScaleFactorX()); + Serial.println(";"); + Serial.print("float MagScaleY = "); + Serial.print(mpu9250.getMagScaleFactorY()); + Serial.println(";"); + Serial.print("float MagScaleZ = "); + Serial.print(mpu9250.getMagScaleFactorZ()); + Serial.println(";"); + Serial.println(" "); + Serial.println("If you are having trouble with your attitude estimate at a new flying location, repeat this process as needed."); + } + else { + Serial.println("Calibration Unsuccessful. Please reset the board and try again."); + } + + while(1); //Halt code so it won't enter main loop until this function commented out + #endif + Serial.println("Error: MPU9250 not selected. Cannot calibrate non-existent magnetometer."); + while(1); //Halt code so it won't enter main loop until this function commented out +} + +void loopRate(int freq) { + //DESCRIPTION: Regulate main loop rate to specified frequency in Hz + /* + * It's good to operate at a constant loop rate for filters to remain stable and whatnot. Interrupt routines running in the + * background cause the loop rate to fluctuate. This function basically just waits at the end of every loop iteration until + * the correct time has passed since the start of the current loop for the desired loop rate in Hz. 2kHz is a good rate to + * be at because the loop nominally will run between 2.8kHz - 4.2kHz. This lets us have a little room to add extra computations + * and remain above 2kHz, without needing to retune all of our filtering parameters. + */ + float invFreq = 1.0/freq*1000000.0; + unsigned long checker = micros(); + + //Sit in loop until appropriate time has passed + while (invFreq > (checker - current_time)) { + checker = micros(); + } +} + +void loopBlink() { + //DESCRIPTION: Blink LED on board to indicate main loop is running + /* + * It looks cool. + */ + if (current_time - blink_counter > blink_delay) { + blink_counter = micros(); + digitalWrite(13, blinkAlternate); //Pin 13 is built in LED + + if (blinkAlternate == 1) { + blinkAlternate = 0; + blink_delay = 100000; + } + else if (blinkAlternate == 0) { + blinkAlternate = 1; + blink_delay = 2000000; + } + } +} + +void setupBlink(int numBlinks,int upTime, int downTime) { + //DESCRIPTION: Simple function to make LED on board blink as desired + for (int j = 1; j<= numBlinks; j++) { + digitalWrite(13, LOW); + delay(downTime); + digitalWrite(13, HIGH); + delay(upTime); + } +} + +void printRadioData() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:1000.0")); + Serial.print(F(" HIGH:2000.0")); + Serial.print(F(" CH1:")); + Serial.print(channel_1_pwm); + Serial.print(F(" CH2:")); + Serial.print(channel_2_pwm); + Serial.print(F(" CH3:")); + Serial.print(channel_3_pwm); + Serial.print(F(" CH4:")); + Serial.print(channel_4_pwm); + Serial.print(F(" CH5:")); + Serial.print(channel_5_pwm); + Serial.print(F(" CH6:")); + Serial.println(channel_6_pwm); + } +} + +void printDesiredState() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-180.0")); + Serial.print(F(" HIGH:180.0")); + Serial.print(F(" thro_des:")); + Serial.print(thro_des); + Serial.print(F(" roll_des:")); + Serial.print(roll_des); + Serial.print(F(" pitch_des:")); + Serial.print(pitch_des); + Serial.print(F(" yaw_des:")); + Serial.println(yaw_des); + } +} + +void printGyroData() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-300.0")); + Serial.print(F(" HIGH:300.0")); + Serial.print(F(" GyroX:")); + Serial.print(GyroX); + Serial.print(F(" GyroY:")); + Serial.print(GyroY); + Serial.print(F(" GyroZ:")); + Serial.println(GyroZ); + } +} + +void printAccelData() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-2.0")); + Serial.print(F(" HIGH:2.0")); + Serial.print(F(" AccX:")); + Serial.print(AccX); + Serial.print(F(" AccY:")); + Serial.print(AccY); + Serial.print(F(" AccZ:")); + Serial.println(AccZ); + } +} + +void printMagData() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-300.0")); + Serial.print(F(" HIGH:300.0")); + Serial.print(F(" MagX:")); + Serial.print(MagX); + Serial.print(F(" MagY:")); + Serial.print(MagY); + Serial.print(F(" MagZ:")); + Serial.println(MagZ); + } +} + +void printRollPitchYaw() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-180.0")); + Serial.print(F(" HIGH:180.0")); + Serial.print(F(" roll:")); + Serial.print(roll_IMU); + Serial.print(F(" pitch:")); + Serial.print(pitch_IMU); + Serial.print(F(" yaw:")); + Serial.println(yaw_IMU); + } +} + +void printPIDoutput() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-2.0")); + Serial.print(F(" HIGH:2.0")); + Serial.print(F(" roll_PID:")); + Serial.print(roll_PID); + Serial.print(F(" pitch_PID:")); + Serial.print(pitch_PID); + Serial.print(F(" yaw_PID:")); + Serial.println(yaw_PID); + } +} + +void printMotorCommands() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:125.0")); + Serial.print(F(" HIGH:250.0")); + Serial.print(F(" m1_command:")); + Serial.print(m1_command_PWM); + Serial.print(F(" m2_command:")); + Serial.print(m2_command_PWM); + Serial.print(F(" m3_command:")); + Serial.print(m3_command_PWM); + Serial.print(F(" m4_command:")); + Serial.print(m4_command_PWM); + Serial.print(F(" m5_command:")); + Serial.print(m5_command_PWM); + Serial.print(F(" m6_command:")); + Serial.println(m6_command_PWM); + } +} + +void printServoCommands() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:0.0")); + Serial.print(F(" HIGH:180.0")); + Serial.print(F(" s1_command:")); + Serial.print(s1_command_PWM); + Serial.print(F(" s2_command:")); + Serial.print(s2_command_PWM); + Serial.print(F(" s3_command:")); + Serial.print(s3_command_PWM); + Serial.print(F(" s4_command:")); + Serial.print(s4_command_PWM); + Serial.print(F(" s5_command:")); + Serial.print(s5_command_PWM); + Serial.print(F(" s6_command:")); + Serial.print(s6_command_PWM); + Serial.print(F(" s7_command:")); + Serial.println(s7_command_PWM); + } +} + +void printLoopRate() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("dt:")); + Serial.println(dt*1000000.0); + } +} + +//=========================================================================================// + +//HELPER FUNCTIONS + +float invSqrt(float x) { + //Fast inverse sqrt for madgwick filter + /* + float halfx = 0.5f * x; + float y = x; + long i = *(long*)&y; + i = 0x5f3759df - (i>>1); + y = *(float*)&i; + y = y * (1.5f - (halfx * y * y)); + y = y * (1.5f - (halfx * y * y)); + return y; + */ + /* + //alternate form: + unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1); + float tmp = *(float*)&i; + float y = tmp * (1.69000231f - 0.714158168f * x * tmp * tmp); + return y; + */ + return 1.0/sqrtf(x); //Teensy is fast enough to just take the compute penalty lol suck it arduino nano +} diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/radioComm.ino b/Versions/dRehmFlight_Teensy_BETA_1.4/radioComm.ino new file mode 100644 index 00000000..eef7f1ec --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/radioComm.ino @@ -0,0 +1,198 @@ +//Arduino/Teensy Flight Controller - dRehmFlight +//Author: Nicholas Rehm +//Project Start: 1/6/2020 +//Last Updated: 7/29/2022 +//Version: Beta 1.3 + +//========================================================================================================================// + +//This file contains all necessary functions and code used for radio communication to avoid cluttering the main code + +unsigned long rising_edge_start_1, rising_edge_start_2, rising_edge_start_3, rising_edge_start_4, rising_edge_start_5, rising_edge_start_6; +unsigned long channel_1_raw, channel_2_raw, channel_3_raw, channel_4_raw, channel_5_raw, channel_6_raw; +int ppm_counter = 0; +unsigned long time_ms = 0; + +void radioSetup() { + //PPM Receiver + #if defined USE_PPM_RX + //Declare interrupt pin + pinMode(PPM_Pin, INPUT_PULLUP); + delay(20); + //Attach interrupt and point to corresponding ISR function + attachInterrupt(digitalPinToInterrupt(PPM_Pin), getPPM, CHANGE); + + //PWM Receiver + #elif defined USE_PWM_RX + //Declare interrupt pins + pinMode(ch1Pin, INPUT_PULLUP); + pinMode(ch2Pin, INPUT_PULLUP); + pinMode(ch3Pin, INPUT_PULLUP); + pinMode(ch4Pin, INPUT_PULLUP); + pinMode(ch5Pin, INPUT_PULLUP); + pinMode(ch6Pin, INPUT_PULLUP); + delay(20); + //Attach interrupt and point to corresponding ISR functions + attachInterrupt(digitalPinToInterrupt(ch1Pin), getCh1, CHANGE); + attachInterrupt(digitalPinToInterrupt(ch2Pin), getCh2, CHANGE); + attachInterrupt(digitalPinToInterrupt(ch3Pin), getCh3, CHANGE); + attachInterrupt(digitalPinToInterrupt(ch4Pin), getCh4, CHANGE); + attachInterrupt(digitalPinToInterrupt(ch5Pin), getCh5, CHANGE); + attachInterrupt(digitalPinToInterrupt(ch6Pin), getCh6, CHANGE); + delay(20); + + //SBUS Recevier + #elif defined USE_SBUS_RX + sbus.begin(); + + //DSM receiver + #elif defined USE_DSM_RX + Serial3.begin(115000); + #else + #error No RX type defined... + #endif +} + +unsigned long getRadioPWM(int ch_num) { + //DESCRIPTION: Get current radio commands from interrupt routines + unsigned long returnPWM = 0; + + if (ch_num == 1) { + returnPWM = channel_1_raw; + } + else if (ch_num == 2) { + returnPWM = channel_2_raw; + } + else if (ch_num == 3) { + returnPWM = channel_3_raw; + } + else if (ch_num == 4) { + returnPWM = channel_4_raw; + } + else if (ch_num == 5) { + returnPWM = channel_5_raw; + } + else if (ch_num == 6) { + returnPWM = channel_6_raw; + } + + return returnPWM; +} + +//For DSM type receivers +void serialEvent3(void) +{ + #if defined USE_DSM_RX + while (Serial3.available()) { + DSM.handleSerialEvent(Serial3.read(), micros()); + } + #endif +} + + + +//========================================================================================================================// + + + +//INTERRUPT SERVICE ROUTINES (for reading PWM and PPM) + +void getPPM() { + unsigned long dt_ppm; + int trig = digitalRead(PPM_Pin); + if (trig==1) { //Only care about rising edge + dt_ppm = micros() - time_ms; + time_ms = micros(); + + + if (dt_ppm > 5000) { //Waiting for long pulse to indicate a new pulse train has arrived + ppm_counter = 0; + } + + if (ppm_counter == 1) { //First pulse + channel_1_raw = dt_ppm; + } + + if (ppm_counter == 2) { //Second pulse + channel_2_raw = dt_ppm; + } + + if (ppm_counter == 3) { //Third pulse + channel_3_raw = dt_ppm; + } + + if (ppm_counter == 4) { //Fourth pulse + channel_4_raw = dt_ppm; + } + + if (ppm_counter == 5) { //Fifth pulse + channel_5_raw = dt_ppm; + } + + if (ppm_counter == 6) { //Sixth pulse + channel_6_raw = dt_ppm; + } + + ppm_counter = ppm_counter + 1; + } +} + +void getCh1() { + int trigger = digitalRead(ch1Pin); + if(trigger == 1) { + rising_edge_start_1 = micros(); + } + else if(trigger == 0) { + channel_1_raw = micros() - rising_edge_start_1; + } +} + +void getCh2() { + int trigger = digitalRead(ch2Pin); + if(trigger == 1) { + rising_edge_start_2 = micros(); + } + else if(trigger == 0) { + channel_2_raw = micros() - rising_edge_start_2; + } +} + +void getCh3() { + int trigger = digitalRead(ch3Pin); + if(trigger == 1) { + rising_edge_start_3 = micros(); + } + else if(trigger == 0) { + channel_3_raw = micros() - rising_edge_start_3; + } +} + +void getCh4() { + int trigger = digitalRead(ch4Pin); + if(trigger == 1) { + rising_edge_start_4 = micros(); + } + else if(trigger == 0) { + channel_4_raw = micros() - rising_edge_start_4; + } +} + +void getCh5() { + int trigger = digitalRead(ch5Pin); + if(trigger == 1) { + rising_edge_start_5 = micros(); + } + else if(trigger == 0) { + channel_5_raw = micros() - rising_edge_start_5; + } +} + +void getCh6() { + int trigger = digitalRead(ch6Pin); + if(trigger == 1) { + rising_edge_start_6 = micros(); + } + else if(trigger == 0) { + channel_6_raw = micros() - rising_edge_start_6; + } +} diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/DSMRX/DSMRX.cpp b/Versions/dRehmFlight_Teensy_BETA_1.4/src/DSMRX/DSMRX.cpp new file mode 100644 index 00000000..f26ee9cd --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/DSMRX/DSMRX.cpp @@ -0,0 +1,116 @@ +/* + Spektrum DSM receiver implementation + + Copyright (C) Simon D. Levy 2017 + + This file is part of DSMRX. + + DSMRX is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + DSMRX is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with DSMRX. If not, see . + */ + +#include "DSMRX.h" + +DSMRX::DSMRX(uint8_t rcChans, uint8_t chanShift, uint8_t chanMask, uint8_t valShift) +{ + _rcChans = rcChans; + _chanShift = chanShift; + _chanMask = chanMask; + _valShift = valShift; + + _gotNewFrame = false; + _lastInterruptMicros = 0; +} + +void DSMRX::handleSerialEvent(uint8_t value, uint32_t usec) +{ + // Reset time + _lastInterruptMicros = usec; + + // check for new frame, i.e. more than 2.5ms passed + static uint32_t spekTimeLast; + uint32_t spekTimeNow = usec; + uint32_t spekInterval = spekTimeNow - spekTimeLast; + spekTimeLast = spekTimeNow; + if (spekInterval > 2500) { + _rxBufPos = 0; + } + + // put the data in buffer + if (_rxBufPos < BUFFER_SIZE) { + _rxBuf[_rxBufPos++] = value; + } + + // parse frame if done + if (_rxBufPos == BUFFER_SIZE) { + + // grab fade count + _fadeCount = _rxBuf[0]; + + // convert to channel data in [0,1024] + for (int b = 2; b < BUFFER_SIZE; b += 2) { + uint8_t bh = _rxBuf[b]; + uint8_t bl = _rxBuf[b+1]; + uint8_t spekChannel = 0x0F & (bh >> _chanShift); + if (spekChannel < _rcChans) { + _rcValue[spekChannel] = ((((uint16_t)(bh & _chanMask) << 8) + bl) >> _valShift); + } + } + + // we have a new frame + _gotNewFrame = true; + } +} + + +bool DSMRX::gotNewFrame(void) +{ + bool retval = _gotNewFrame; + if (_gotNewFrame) { + _gotNewFrame = false; + } + return retval; +} + +void DSMRX::getChannelValues(uint16_t values[], uint8_t count) +{ + for (uint8_t k=0; k maxMicros; +} + +DSM1024::DSM1024(void) : DSMRX(7, 2, 0x03, 0) +{ +} + +DSM2048::DSM2048(void) : DSMRX(8, 3, 0x07, 1) +{ +} diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/DSMRX/DSMRX.h b/Versions/dRehmFlight_Teensy_BETA_1.4/src/DSMRX/DSMRX.h new file mode 100644 index 00000000..8e3da9db --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/DSMRX/DSMRX.h @@ -0,0 +1,85 @@ +/* + Spektrum DSM receiver class + + Copyright (C) Simon D. Levy 2017 + + This file is part of DSMRX. + + DSMRX is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + DSMRX is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with DSMRX. If not, see . + */ + +#pragma once + +#include + +class DSMRX { + + private: + + static const uint8_t BUFFER_SIZE = 16; + static const uint8_t MAX_CHANS = 8; + + // Modified by serial-event handler + volatile uint8_t _rxBuf[BUFFER_SIZE]; + volatile uint8_t _rxBufPos; + volatile uint16_t _rcValue[MAX_CHANS]; + volatile uint32_t _lastInterruptMicros; + + uint8_t _rcChans; + uint8_t _chanShift; + uint8_t _chanMask; + uint8_t _valShift; + uint8_t _fadeCount; + + bool _gotNewFrame; + + protected: + + DSMRX(uint8_t rc, uint8_t cs, uint8_t cm, uint8_t vs); + + public: + + void handleSerialEvent(uint8_t value, uint32_t usec); + + bool gotNewFrame(void); + + /** + * Returns channel values in [1000,2000] interval + */ + void getChannelValues(uint16_t values[], uint8_t count=8); + + /** + * Returns channel values in [-1,+1] interval + */ + void getChannelValuesNormalized(float values[], uint8_t count=8); + + uint8_t getFadeCount(void); + + bool timedOut(uint32_t usec, uint32_t maxMicros=40000); +}; + +class DSM1024 : public DSMRX { + + public: + + DSM1024(void); + +}; + +class DSM2048 : public DSMRX { + + public: + + DSM2048(void); +}; + diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/I2Cdev.cpp b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/I2Cdev.cpp new file mode 100644 index 00000000..a22474dd --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/I2Cdev.cpp @@ -0,0 +1,1468 @@ +// I2Cdev library collection - Main I2C device class +// Abstracts bit and byte I2C R/W functions into a convenient class +// 2013-06-05 by Jeff Rowberg +// +// Changelog: +// 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications +// 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) +// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire +// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation +// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) +// 2011-10-03 - added automatic Arduino version detection for ease of use +// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications +// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) +// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default +// 2011-08-02 - added support for 16-bit registers +// - fixed incorrect Doxygen comments on some methods +// - added timeout value for read operations (thanks mem @ Arduino forums) +// 2011-07-30 - changed read/write function structures to return success or byte counts +// - made all methods static for multi-device memory savings +// 2011-07-28 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2013 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#include "I2Cdev.h" + +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE + + #ifdef I2CDEV_IMPLEMENTATION_WARNINGS + #if ARDUINO < 100 + #warning Using outdated Arduino IDE with Wire library is functionally limiting. + #warning Arduino IDE v1.6.5+ with I2Cdev Fastwire implementation is recommended. + #warning This I2Cdev implementation does not support: + #warning - Repeated starts conditions + #warning - Timeout detection (some Wire requests block forever) + #elif ARDUINO == 100 + #warning Using outdated Arduino IDE with Wire library is functionally limiting. + #warning Arduino IDE v1.6.5+ with I2Cdev Fastwire implementation is recommended. + #warning This I2Cdev implementation does not support: + #warning - Repeated starts conditions + #warning - Timeout detection (some Wire requests block forever) + #elif ARDUINO > 100 + /*#warning Using current Arduino IDE with Wire library is functionally limiting. + #warning Arduino IDE v1.6.5+ with I2CDEV_BUILTIN_FASTWIRE implementation is recommended. + #warning This I2Cdev implementation does not support: + #warning - Timeout detection (some Wire requests block forever)*/ + #endif + #endif + +#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + + //#error The I2CDEV_BUILTIN_FASTWIRE implementation is known to be broken right now. Patience, Iago! + +#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + + #ifdef I2CDEV_IMPLEMENTATION_WARNINGS + #warning Using I2CDEV_BUILTIN_NBWIRE implementation may adversely affect interrupt detection. + #warning This I2Cdev implementation does not support: + #warning - Repeated starts conditions + #endif + + // NBWire implementation based heavily on code by Gene Knight + // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html + // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html + TwoWire Wire; + +#endif + +#ifndef BUFFER_LENGTH +// band-aid fix for platforms without Wire-defined BUFFER_LENGTH (removed from some official implementations) +#define BUFFER_LENGTH 32 +#endif + +/** Default constructor. + */ +I2Cdev::I2Cdev() { +} + +/** Read a single bit from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitNum Bit position to read (0-7) + * @param data Container for single bit value + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) { + uint8_t b; + uint8_t count = readByte(devAddr, regAddr, &b, timeout); + *data = b & (1 << bitNum); + return count; +} + +/** Read a single bit from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitNum Bit position to read (0-15) + * @param data Container for single bit value + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) { + uint16_t b; + uint8_t count = readWord(devAddr, regAddr, &b, timeout); + *data = b & (1 << bitNum); + return count; +} + +/** Read multiple bits from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitStart First bit position to read (0-7) + * @param length Number of bits to read (not more than 8) + * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) { + // 01101001 read byte + // 76543210 bit numbers + // xxx args: bitStart=4, length=3 + // 010 masked + // -> 010 shifted + uint8_t count, b; + if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) { + uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); + b &= mask; + b >>= (bitStart - length + 1); + *data = b; + } + return count; +} + +/** Read multiple bits from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitStart First bit position to read (0-15) + * @param length Number of bits to read (not more than 16) + * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (1 = success, 0 = failure, -1 = timeout) + */ +int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) { + // 1101011001101001 read byte + // fedcba9876543210 bit numbers + // xxx args: bitStart=12, length=3 + // 010 masked + // -> 010 shifted + uint8_t count; + uint16_t w; + if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) { + uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); + w &= mask; + w >>= (bitStart - length + 1); + *data = w; + } + return count; +} + +/** Read single byte from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param data Container for byte value read from device + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) { + return readBytes(devAddr, regAddr, 1, data, timeout); +} + +/** Read single word from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param data Container for word value read from device + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) { + return readWords(devAddr, regAddr, 1, data, timeout); +} + +/** Read multiple bytes from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register regAddr to read from + * @param length Number of bytes to read + * @param data Buffer to store read data in + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Number of bytes read (-1 indicates failure) + */ +int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") reading "); + Serial.print(length, DEC); + Serial.print(" bytes from 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + + int8_t count = 0; + uint32_t t1 = millis(); + + #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE) + + #if (ARDUINO < 100) + // Arduino v00xx (before v1.0), Wire library + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.send(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); + + for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = Wire.receive(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + } + + Wire.endTransmission(); + } + #elif (ARDUINO == 100) + // Arduino v1.0.0, Wire library + // Adds standardized write() and read() stream methods instead of send() and receive() + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); + + for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + } + + Wire.endTransmission(); + } + #elif (ARDUINO > 100) + // Arduino v1.0.1+, Wire library + // Adds official support for repeated start condition, yay! + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); + + for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + } + } + #endif + + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + + // Fastwire library + // no loop required for fastwire + uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, data, length); + if (status == 0) { + count = length; // success + } else { + count = -1; // error + } + + #endif + + // check for timeout + if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout + + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(". Done ("); + Serial.print(count, DEC); + Serial.println(" read)."); + #endif + + return count; +} + +/** Read multiple words from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register regAddr to read from + * @param length Number of words to read + * @param data Buffer to store read data in + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Number of words read (-1 indicates failure) + */ +int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") reading "); + Serial.print(length, DEC); + Serial.print(" words from 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + + int8_t count = 0; + uint32_t t1 = millis(); + +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE + + #if (ARDUINO < 100) + // Arduino v00xx (before v1.0), Wire library + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.send(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + + bool msb = true; // starts with MSB, then LSB + for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + if (msb) { + // first byte is bits 15-8 (MSb=15) + data[count] = Wire.receive() << 8; + } else { + // second byte is bits 7-0 (LSb=0) + data[count] |= Wire.receive(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + count++; + } + msb = !msb; + } + + Wire.endTransmission(); + } + #elif (ARDUINO == 100) + // Arduino v1.0.0, Wire library + // Adds standardized write() and read() stream methods instead of send() and receive() + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + + bool msb = true; // starts with MSB, then LSB + for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + if (msb) { + // first byte is bits 15-8 (MSb=15) + data[count] = Wire.read() << 8; + } else { + // second byte is bits 7-0 (LSb=0) + data[count] |= Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + count++; + } + msb = !msb; + } + + Wire.endTransmission(); + } + #elif (ARDUINO > 100) + // Arduino v1.0.1+, Wire library + // Adds official support for repeated start condition, yay! + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + + bool msb = true; // starts with MSB, then LSB + for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + if (msb) { + // first byte is bits 15-8 (MSb=15) + data[count] = Wire.read() << 8; + } else { + // second byte is bits 7-0 (LSb=0) + data[count] |= Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + count++; + } + msb = !msb; + } + + Wire.endTransmission(); + } + #endif + + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + + // Fastwire library + // no loop required for fastwire + uint16_t intermediate[(uint8_t)length]; + uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, (uint8_t *)intermediate, (uint8_t)(length * 2)); + if (status == 0) { + count = length; // success + for (uint8_t i = 0; i < length; i++) { + data[i] = (intermediate[2*i] << 8) | intermediate[2*i + 1]; + } + } else { + count = -1; // error + } + + #endif + + if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout + + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(". Done ("); + Serial.print(count, DEC); + Serial.println(" read)."); + #endif + + return count; +} + +/** write a single bit in an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitNum Bit position to write (0-7) + * @param value New bit value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) { + uint8_t b; + readByte(devAddr, regAddr, &b); + b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); + return writeByte(devAddr, regAddr, b); +} + +/** write a single bit in a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitNum Bit position to write (0-15) + * @param value New bit value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) { + uint16_t w; + readWord(devAddr, regAddr, &w); + w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum)); + return writeWord(devAddr, regAddr, w); +} + +/** Write multiple bits in an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitStart First bit position to write (0-7) + * @param length Number of bits to write (not more than 8) + * @param data Right-aligned value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { + // 010 value to write + // 76543210 bit numbers + // xxx args: bitStart=4, length=3 + // 00011100 mask byte + // 10101111 original value (sample) + // 10100011 original & ~mask + // 10101011 masked | value + uint8_t b; + if (readByte(devAddr, regAddr, &b) != 0) { + uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); + data <<= (bitStart - length + 1); // shift data into correct position + data &= mask; // zero all non-important bits in data + b &= ~(mask); // zero all important bits in existing byte + b |= data; // combine data with existing byte + return writeByte(devAddr, regAddr, b); + } else { + return false; + } +} + +/** Write multiple bits in a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitStart First bit position to write (0-15) + * @param length Number of bits to write (not more than 16) + * @param data Right-aligned value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) { + // 010 value to write + // fedcba9876543210 bit numbers + // xxx args: bitStart=12, length=3 + // 0001110000000000 mask word + // 1010111110010110 original value (sample) + // 1010001110010110 original & ~mask + // 1010101110010110 masked | value + uint16_t w; + if (readWord(devAddr, regAddr, &w) != 0) { + uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); + data <<= (bitStart - length + 1); // shift data into correct position + data &= mask; // zero all non-important bits in data + w &= ~(mask); // zero all important bits in existing word + w |= data; // combine data with existing word + return writeWord(devAddr, regAddr, w); + } else { + return false; + } +} + +/** Write single byte to an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register address to write to + * @param data New byte value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) { + return writeBytes(devAddr, regAddr, 1, &data); +} + +/** Write single word to a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register address to write to + * @param data New word value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) { + return writeWords(devAddr, regAddr, 1, &data); +} + +/** Write multiple bytes to an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register address to write to + * @param length Number of bytes to write + * @param data Buffer to copy new data from + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") writing "); + Serial.print(length, DEC); + Serial.print(" bytes to 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + uint8_t status = 0; + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.beginTransmission(devAddr); + Wire.send((uint8_t) regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ + || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) + Wire.beginTransmission(devAddr); + Wire.write((uint8_t) regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::beginTransmission(devAddr); + Fastwire::write(regAddr); + #endif + for (uint8_t i = 0; i < length; i++) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[i], HEX); + if (i + 1 < length) Serial.print(" "); + #endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.send((uint8_t) data[i]); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ + || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) + Wire.write((uint8_t) data[i]); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::write((uint8_t) data[i]); + #endif + } + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ + || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) + status = Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::stop(); + //status = Fastwire::endTransmission(); + #endif + #ifdef I2CDEV_SERIAL_DEBUG + Serial.println(". Done."); + #endif + return status == 0; +} + +/** Write multiple words to a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register address to write to + * @param length Number of words to write + * @param data Buffer to copy new data from + * @return Status of operation (true = success) + */ +bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") writing "); + Serial.print(length, DEC); + Serial.print(" words to 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + uint8_t status = 0; + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.beginTransmission(devAddr); + Wire.send(regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ + || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) + Wire.beginTransmission(devAddr); + Wire.write(regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::beginTransmission(devAddr); + Fastwire::write(regAddr); + #endif + for (uint8_t i = 0; i < length; i++) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[i], HEX); + if (i + 1 < length) Serial.print(" "); + #endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.send((uint8_t)(data[i] >> 8)); // send MSB + Wire.send((uint8_t)data[i]); // send LSB + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ + || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) + Wire.write((uint8_t)(data[i] >> 8)); // send MSB + Wire.write((uint8_t)data[i]); // send LSB + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::write((uint8_t)(data[i] >> 8)); // send MSB + status = Fastwire::write((uint8_t)data[i]); // send LSB + if (status != 0) break; + #endif + } + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ + || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) + status = Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::stop(); + //status = Fastwire::endTransmission(); + #endif + #ifdef I2CDEV_SERIAL_DEBUG + Serial.println(". Done."); + #endif + return status == 0; +} + +/** Default timeout value for read operations. + * Set this to 0 to disable timeout detection. + */ +uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + // I2C library + ////////////////////// + // Copyright(C) 2012 + // Francesco Ferrara + // ferrara[at]libero[point]it + ////////////////////// + + /* + FastWire + - 0.24 added stop + - 0.23 added reset + + This is a library to help faster programs to read I2C devices. + Copyright(C) 2012 Francesco Ferrara + occhiobello at gmail dot com + [used by Jeff Rowberg for I2Cdevlib with permission] + */ + + boolean Fastwire::waitInt() { + int l = 250; + while (!(TWCR & (1 << TWINT)) && l-- > 0); + return l > 0; + } + + void Fastwire::setup(int khz, boolean pullup) { + TWCR = 0; + #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__) + // activate internal pull-ups for twi (PORTC bits 4 & 5) + // as per note from atmega8 manual pg167 + if (pullup) PORTC |= ((1 << 4) | (1 << 5)); + else PORTC &= ~((1 << 4) | (1 << 5)); + #elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) + // activate internal pull-ups for twi (PORTC bits 0 & 1) + if (pullup) PORTC |= ((1 << 0) | (1 << 1)); + else PORTC &= ~((1 << 0) | (1 << 1)); + #else + // activate internal pull-ups for twi (PORTD bits 0 & 1) + // as per note from atmega128 manual pg204 + if (pullup) PORTD |= ((1 << 0) | (1 << 1)); + else PORTD &= ~((1 << 0) | (1 << 1)); + #endif + + TWSR = 0; // no prescaler => prescaler = 1 + TWBR = ((16000L / khz) - 16) / 2; // change the I2C clock rate + TWCR = 1 << TWEN; // enable twi module, no interrupt + } + + // added by Jeff Rowberg 2013-05-07: + // Arduino Wire-style "beginTransmission" function + // (takes 7-bit device address like the Wire method, NOT 8-bit: 0x68, not 0xD0/0xD1) + byte Fastwire::beginTransmission(byte device) { + byte twst, retry; + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 2; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device << 1; // send device address without read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 3; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 4; + return 0; + } + + byte Fastwire::writeBuf(byte device, byte address, byte *data, byte num) { + byte twst, retry; + + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 2; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device & 0xFE; // send device address without read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 3; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 4; + + //Serial.print(address, HEX); + //Serial.print(" "); + TWDR = address; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 5; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 6; + + for (byte i = 0; i < num; i++) { + //Serial.print(data[i], HEX); + //Serial.print(" "); + TWDR = data[i]; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 7; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 8; + } + //Serial.print("\n"); + + return 0; + } + + byte Fastwire::write(byte value) { + byte twst; + //Serial.println(value, HEX); + TWDR = value; // send data + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 2; + return 0; + } + + byte Fastwire::readBuf(byte device, byte address, byte *data, byte num) { + byte twst, retry; + + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 16; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 17; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device & 0xfe; // send device address to write + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 18; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 19; + + //Serial.print(address, HEX); + //Serial.print(" "); + TWDR = address; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 20; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 21; + + /***/ + + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 22; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 23; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device | 0x01; // send device address with the read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 24; + twst = TWSR & 0xF8; + } while (twst == TW_MR_SLA_NACK && retry-- > 0); + if (twst != TW_MR_SLA_ACK) return 25; + + for (uint8_t i = 0; i < num; i++) { + if (i == num - 1) + TWCR = (1 << TWINT) | (1 << TWEN); + else + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA); + if (!waitInt()) return 26; + twst = TWSR & 0xF8; + if (twst != TW_MR_DATA_ACK && twst != TW_MR_DATA_NACK) return twst; + data[i] = TWDR; + //Serial.print(data[i], HEX); + //Serial.print(" "); + } + //Serial.print("\n"); + stop(); + + return 0; + } + + void Fastwire::reset() { + TWCR = 0; + } + + byte Fastwire::stop() { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO); + if (!waitInt()) return 1; + return 0; + } +#endif + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + // NBWire implementation based heavily on code by Gene Knight + // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html + // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html + + /* + call this version 1.0 + + Offhand, the only funky part that I can think of is in nbrequestFrom, where the buffer + length and index are set *before* the data is actually read. The problem is that these + are variables local to the TwoWire object, and by the time we actually have read the + data, and know what the length actually is, we have no simple access to the object's + variables. The actual bytes read *is* given to the callback function, though. + + The ISR code for a slave receiver is commented out. I don't have that setup, and can't + verify it at this time. Save it for 2.0! + + The handling of the read and write processes here is much like in the demo sketch code: + the process is broken down into sequential functions, where each registers the next as a + callback, essentially. + + For example, for the Read process, twi_read00 just returns if TWI is not yet in a + ready state. When there's another interrupt, and the interface *is* ready, then it + sets up the read, starts it, and registers twi_read01 as the function to call after + the *next* interrupt. twi_read01, then, just returns if the interface is still in a + "reading" state. When the reading is done, it copies the information to the buffer, + cleans up, and calls the user-requested callback function with the actual number of + bytes read. + + The writing is similar. + + Questions, comments and problems can go to Gene@Telobot.com. + + Thumbs Up! + Gene Knight + + */ + + uint8_t TwoWire::rxBuffer[NBWIRE_BUFFER_LENGTH]; + uint8_t TwoWire::rxBufferIndex = 0; + uint8_t TwoWire::rxBufferLength = 0; + + uint8_t TwoWire::txAddress = 0; + uint8_t TwoWire::txBuffer[NBWIRE_BUFFER_LENGTH]; + uint8_t TwoWire::txBufferIndex = 0; + uint8_t TwoWire::txBufferLength = 0; + + //uint8_t TwoWire::transmitting = 0; + void (*TwoWire::user_onRequest)(void); + void (*TwoWire::user_onReceive)(int); + + static volatile uint8_t twi_transmitting; + static volatile uint8_t twi_state; + static uint8_t twi_slarw; + static volatile uint8_t twi_error; + static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH]; + static volatile uint8_t twi_masterBufferIndex; + static uint8_t twi_masterBufferLength; + static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH]; + static volatile uint8_t twi_rxBufferIndex; + //static volatile uint8_t twi_Interrupt_Continue_Command; + static volatile uint8_t twi_Return_Value; + static volatile uint8_t twi_Done; + void (*twi_cbendTransmissionDone)(int); + void (*twi_cbreadFromDone)(int); + + void twi_init() { + // initialize state + twi_state = TWI_READY; + + // activate internal pull-ups for twi + // as per note from atmega8 manual pg167 + sbi(PORTC, 4); + sbi(PORTC, 5); + + // initialize twi prescaler and bit rate + cbi(TWSR, TWPS0); // TWI Status Register - Prescaler bits + cbi(TWSR, TWPS1); + + /* twi bit rate formula from atmega128 manual pg 204 + SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR)) + note: TWBR should be 10 or higher for master mode + It is 72 for a 16mhz Wiring board with 100kHz TWI */ + + TWBR = ((CPU_FREQ / TWI_FREQ) - 16) / 2; // bitrate register + // enable twi module, acks, and twi interrupt + + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); + + /* TWEN - TWI Enable Bit + TWIE - TWI Interrupt Enable + TWEA - TWI Enable Acknowledge Bit + TWINT - TWI Interrupt Flag + TWSTA - TWI Start Condition + */ + } + + typedef struct { + uint8_t address; + uint8_t* data; + uint8_t length; + uint8_t wait; + uint8_t i; + } twi_Write_Vars; + + twi_Write_Vars *ptwv = 0; + static void (*fNextInterruptFunction)(void) = 0; + + void twi_Finish(byte bRetVal) { + if (ptwv) { + free(ptwv); + ptwv = 0; + } + twi_Done = 0xFF; + twi_Return_Value = bRetVal; + fNextInterruptFunction = 0; + } + + uint8_t twii_WaitForDone(uint16_t timeout) { + uint32_t endMillis = millis() + timeout; + while (!twi_Done && (timeout == 0 || millis() < endMillis)) continue; + return twi_Return_Value; + } + + void twii_SetState(uint8_t ucState) { + twi_state = ucState; + } + + void twii_SetError(uint8_t ucError) { + twi_error = ucError ; + } + + void twii_InitBuffer(uint8_t ucPos, uint8_t ucLength) { + twi_masterBufferIndex = 0; + twi_masterBufferLength = ucLength; + } + + void twii_CopyToBuf(uint8_t* pData, uint8_t ucLength) { + uint8_t i; + for (i = 0; i < ucLength; ++i) { + twi_masterBuffer[i] = pData[i]; + } + } + + void twii_CopyFromBuf(uint8_t *pData, uint8_t ucLength) { + uint8_t i; + for (i = 0; i < ucLength; ++i) { + pData[i] = twi_masterBuffer[i]; + } + } + + void twii_SetSlaRW(uint8_t ucSlaRW) { + twi_slarw = ucSlaRW; + } + + void twii_SetStart() { + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA); + } + + void twi_write01() { + if (TWI_MTX == twi_state) return; // blocking test + twi_transmitting = 0 ; + if (twi_error == 0xFF) + twi_Finish (0); // success + else if (twi_error == TW_MT_SLA_NACK) + twi_Finish (2); // error: address send, nack received + else if (twi_error == TW_MT_DATA_NACK) + twi_Finish (3); // error: data send, nack received + else + twi_Finish (4); // other twi error + if (twi_cbendTransmissionDone) return twi_cbendTransmissionDone(twi_Return_Value); + return; + } + + + void twi_write00() { + if (TWI_READY != twi_state) return; // blocking test + if (TWI_BUFFER_LENGTH < ptwv -> length) { + twi_Finish(1); // end write with error 1 + return; + } + twi_Done = 0x00; // show as working + twii_SetState(TWI_MTX); // to transmitting + twii_SetError(0xFF); // to No Error + twii_InitBuffer(0, ptwv -> length); // pointer and length + twii_CopyToBuf(ptwv -> data, ptwv -> length); // get the data + twii_SetSlaRW((ptwv -> address << 1) | TW_WRITE); // write command + twii_SetStart(); // start the cycle + fNextInterruptFunction = twi_write01; // next routine + return twi_write01(); + } + + void twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait) { + uint8_t i; + ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars)); + ptwv -> address = address; + ptwv -> data = data; + ptwv -> length = length; + ptwv -> wait = wait; + fNextInterruptFunction = twi_write00; + return twi_write00(); + } + + void twi_read01() { + if (TWI_MRX == twi_state) return; // blocking test + if (twi_masterBufferIndex < ptwv -> length) ptwv -> length = twi_masterBufferIndex; + twii_CopyFromBuf(ptwv -> data, ptwv -> length); + twi_Finish(ptwv -> length); + if (twi_cbreadFromDone) return twi_cbreadFromDone(twi_Return_Value); + return; + } + + void twi_read00() { + if (TWI_READY != twi_state) return; // blocking test + if (TWI_BUFFER_LENGTH < ptwv -> length) twi_Finish(0); // error return + twi_Done = 0x00; // show as working + twii_SetState(TWI_MRX); // reading + twii_SetError(0xFF); // reset error + twii_InitBuffer(0, ptwv -> length - 1); // init to one less than length + twii_SetSlaRW((ptwv -> address << 1) | TW_READ); // read command + twii_SetStart(); // start cycle + fNextInterruptFunction = twi_read01; + return twi_read01(); + } + + void twi_readFrom(uint8_t address, uint8_t* data, uint8_t length) { + uint8_t i; + + ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars)); + ptwv -> address = address; + ptwv -> data = data; + ptwv -> length = length; + fNextInterruptFunction = twi_read00; + return twi_read00(); + } + + void twi_reply(uint8_t ack) { + // transmit master read ready signal, with or without ack + if (ack){ + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA); + } else { + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT); + } + } + + void twi_stop(void) { + // send stop condition + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO); + + // wait for stop condition to be exectued on bus + // TWINT is not set after a stop condition! + while (TWCR & _BV(TWSTO)) { + continue; + } + + // update twi state + twi_state = TWI_READY; + } + + void twi_releaseBus(void) { + // release bus + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT); + + // update twi state + twi_state = TWI_READY; + } + + SIGNAL(TWI_vect) { + switch (TW_STATUS) { + // All Master + case TW_START: // sent start condition + case TW_REP_START: // sent repeated start condition + // copy device address and r/w bit to output register and ack + TWDR = twi_slarw; + twi_reply(1); + break; + + // Master Transmitter + case TW_MT_SLA_ACK: // slave receiver acked address + case TW_MT_DATA_ACK: // slave receiver acked data + // if there is data to send, send it, otherwise stop + if (twi_masterBufferIndex < twi_masterBufferLength) { + // copy data to output register and ack + TWDR = twi_masterBuffer[twi_masterBufferIndex++]; + twi_reply(1); + } else { + twi_stop(); + } + break; + + case TW_MT_SLA_NACK: // address sent, nack received + twi_error = TW_MT_SLA_NACK; + twi_stop(); + break; + + case TW_MT_DATA_NACK: // data sent, nack received + twi_error = TW_MT_DATA_NACK; + twi_stop(); + break; + + case TW_MT_ARB_LOST: // lost bus arbitration + twi_error = TW_MT_ARB_LOST; + twi_releaseBus(); + break; + + // Master Receiver + case TW_MR_DATA_ACK: // data received, ack sent + // put byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + + case TW_MR_SLA_ACK: // address sent, ack received + // ack if more bytes are expected, otherwise nack + if (twi_masterBufferIndex < twi_masterBufferLength) { + twi_reply(1); + } else { + twi_reply(0); + } + break; + + case TW_MR_DATA_NACK: // data received, nack sent + // put final byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + + case TW_MR_SLA_NACK: // address sent, nack received + twi_stop(); + break; + + // TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case + + // Slave Receiver (NOT IMPLEMENTED YET) + /* + case TW_SR_SLA_ACK: // addressed, returned ack + case TW_SR_GCALL_ACK: // addressed generally, returned ack + case TW_SR_ARB_LOST_SLA_ACK: // lost arbitration, returned ack + case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack + // enter slave receiver mode + twi_state = TWI_SRX; + + // indicate that rx buffer can be overwritten and ack + twi_rxBufferIndex = 0; + twi_reply(1); + break; + + case TW_SR_DATA_ACK: // data received, returned ack + case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack + // if there is still room in the rx buffer + if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) { + // put byte in buffer and ack + twi_rxBuffer[twi_rxBufferIndex++] = TWDR; + twi_reply(1); + } else { + // otherwise nack + twi_reply(0); + } + break; + + case TW_SR_STOP: // stop or repeated start condition received + // put a null char after data if there's room + if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) { + twi_rxBuffer[twi_rxBufferIndex] = 0; + } + + // sends ack and stops interface for clock stretching + twi_stop(); + + // callback to user defined callback + twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex); + + // since we submit rx buffer to "wire" library, we can reset it + twi_rxBufferIndex = 0; + + // ack future responses and leave slave receiver state + twi_releaseBus(); + break; + + case TW_SR_DATA_NACK: // data received, returned nack + case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack + // nack back at master + twi_reply(0); + break; + + // Slave Transmitter + case TW_ST_SLA_ACK: // addressed, returned ack + case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack + // enter slave transmitter mode + twi_state = TWI_STX; + + // ready the tx buffer index for iteration + twi_txBufferIndex = 0; + + // set tx buffer length to be zero, to verify if user changes it + twi_txBufferLength = 0; + + // request for txBuffer to be filled and length to be set + // note: user must call twi_transmit(bytes, length) to do this + twi_onSlaveTransmit(); + + // if they didn't change buffer & length, initialize it + if (0 == twi_txBufferLength) { + twi_txBufferLength = 1; + twi_txBuffer[0] = 0x00; + } + + // transmit first byte from buffer, fall through + + case TW_ST_DATA_ACK: // byte sent, ack returned + // copy data to output register + TWDR = twi_txBuffer[twi_txBufferIndex++]; + + // if there is more to send, ack, otherwise nack + if (twi_txBufferIndex < twi_txBufferLength) { + twi_reply(1); + } else { + twi_reply(0); + } + break; + + case TW_ST_DATA_NACK: // received nack, we are done + case TW_ST_LAST_DATA: // received ack, but we are done already! + // ack future responses + twi_reply(1); + // leave slave receiver state + twi_state = TWI_READY; + break; + */ + + // all + case TW_NO_INFO: // no state information + break; + + case TW_BUS_ERROR: // bus error, illegal stop/start + twi_error = TW_BUS_ERROR; + twi_stop(); + break; + } + + if (fNextInterruptFunction) return fNextInterruptFunction(); + } + + TwoWire::TwoWire() { } + + void TwoWire::begin(void) { + rxBufferIndex = 0; + rxBufferLength = 0; + + txBufferIndex = 0; + txBufferLength = 0; + + twi_init(); + } + + void TwoWire::beginTransmission(uint8_t address) { + //beginTransmission((uint8_t)address); + + // indicate that we are transmitting + twi_transmitting = 1; + + // set address of targeted slave + txAddress = address; + + // reset tx buffer iterator vars + txBufferIndex = 0; + txBufferLength = 0; + } + + uint8_t TwoWire::endTransmission(uint16_t timeout) { + // transmit buffer (blocking) + //int8_t ret = + twi_cbendTransmissionDone = NULL; + twi_writeTo(txAddress, txBuffer, txBufferLength, 1); + int8_t ret = twii_WaitForDone(timeout); + + // reset tx buffer iterator vars + txBufferIndex = 0; + txBufferLength = 0; + + // indicate that we are done transmitting + // twi_transmitting = 0; + return ret; + } + + void TwoWire::nbendTransmission(void (*function)(int)) { + twi_cbendTransmissionDone = function; + twi_writeTo(txAddress, txBuffer, txBufferLength, 1); + return; + } + + void TwoWire::send(uint8_t data) { + if (twi_transmitting) { + // in master transmitter mode + // don't bother if buffer is full + if (txBufferLength >= NBWIRE_BUFFER_LENGTH) { + return; + } + + // put byte in tx buffer + txBuffer[txBufferIndex] = data; + ++txBufferIndex; + + // update amount in buffer + txBufferLength = txBufferIndex; + } else { + // in slave send mode + // reply to master + //twi_transmit(&data, 1); + } + } + + uint8_t TwoWire::receive(void) { + // default to returning null char + // for people using with char strings + uint8_t value = 0; + + // get each successive byte on each call + if (rxBufferIndex < rxBufferLength) { + value = rxBuffer[rxBufferIndex]; + ++rxBufferIndex; + } + + return value; + } + + uint8_t TwoWire::requestFrom(uint8_t address, int quantity, uint16_t timeout) { + // clamp to buffer length + if (quantity > NBWIRE_BUFFER_LENGTH) { + quantity = NBWIRE_BUFFER_LENGTH; + } + + // perform blocking read into buffer + twi_cbreadFromDone = NULL; + twi_readFrom(address, rxBuffer, quantity); + uint8_t read = twii_WaitForDone(timeout); + + // set rx buffer iterator vars + rxBufferIndex = 0; + rxBufferLength = read; + + return read; + } + + void TwoWire::nbrequestFrom(uint8_t address, int quantity, void (*function)(int)) { + // clamp to buffer length + if (quantity > NBWIRE_BUFFER_LENGTH) { + quantity = NBWIRE_BUFFER_LENGTH; + } + + // perform blocking read into buffer + twi_cbreadFromDone = function; + twi_readFrom(address, rxBuffer, quantity); + //uint8_t read = twii_WaitForDone(); + + // set rx buffer iterator vars + //rxBufferIndex = 0; + //rxBufferLength = read; + + rxBufferIndex = 0; + rxBufferLength = quantity; // this is a hack + + return; //read; + } + + uint8_t TwoWire::available(void) { + return rxBufferLength - rxBufferIndex; + } + +#endif diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/I2Cdev.h b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/I2Cdev.h new file mode 100644 index 00000000..0bff39ef --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/I2Cdev.h @@ -0,0 +1,283 @@ +// I2Cdev library collection - Main I2C device class header file +// Abstracts bit and byte I2C R/W functions into a convenient class +// 2013-06-05 by Jeff Rowberg +// +// Changelog: +// 2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1 +// 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications +// 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) +// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire +// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation +// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) +// 2011-10-03 - added automatic Arduino version detection for ease of use +// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications +// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) +// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default +// 2011-08-02 - added support for 16-bit registers +// - fixed incorrect Doxygen comments on some methods +// - added timeout value for read operations (thanks mem @ Arduino forums) +// 2011-07-30 - changed read/write function structures to return success or byte counts +// - made all methods static for multi-device memory savings +// 2011-07-28 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2013 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _I2CDEV_H_ +#define _I2CDEV_H_ + +// ----------------------------------------------------------------------------- +// I2C interface implementation setting +// ----------------------------------------------------------------------------- +#ifndef I2CDEV_IMPLEMENTATION +#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE +//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_SBWIRE +//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE +#endif // I2CDEV_IMPLEMENTATION + +// comment this out if you are using a non-optimal IDE/implementation setting +// but want the compiler to shut up about it +#define I2CDEV_IMPLEMENTATION_WARNINGS + +// ----------------------------------------------------------------------------- +// I2C interface implementation options +// ----------------------------------------------------------------------------- +#define I2CDEV_ARDUINO_WIRE 1 // Wire object from Arduino +#define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project + // ^^^ NBWire implementation is still buggy w/some interrupts! +#define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project +#define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library +#define I2CDEV_BUILTIN_SBWIRE 5 // I2C object from Shuning (Steve) Bian's SBWire Library at https://github.com/freespace/SBWire + +// ----------------------------------------------------------------------------- +// Arduino-style "Serial.print" debug constant (uncomment to enable) +// ----------------------------------------------------------------------------- +//#define I2CDEV_SERIAL_DEBUG + +#ifdef ARDUINO + #if ARDUINO < 100 + #include "WProgram.h" + #else + #include "Arduino.h" + #endif + #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + #include + #endif + #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY + #include + #endif + #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE + #include "SBWire.h" + #endif +#endif + +#ifdef SPARK + #include + #define ARDUINO 101 +#endif + + +// 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];") +#define I2CDEV_DEFAULT_READ_TIMEOUT 1000 + +class I2Cdev { + public: + I2Cdev(); + + static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); + static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); + static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); + static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); + static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); + static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); + static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); + static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); + + static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data); + static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data); + static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); + static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data); + static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data); + static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data); + static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data); + static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data); + + static uint16_t readTimeout; +}; + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + ////////////////////// + // FastWire 0.24 + // This is a library to help faster programs to read I2C devices. + // Copyright(C) 2012 + // Francesco Ferrara + ////////////////////// + + /* Master */ + #define TW_START 0x08 + #define TW_REP_START 0x10 + + /* Master Transmitter */ + #define TW_MT_SLA_ACK 0x18 + #define TW_MT_SLA_NACK 0x20 + #define TW_MT_DATA_ACK 0x28 + #define TW_MT_DATA_NACK 0x30 + #define TW_MT_ARB_LOST 0x38 + + /* Master Receiver */ + #define TW_MR_ARB_LOST 0x38 + #define TW_MR_SLA_ACK 0x40 + #define TW_MR_SLA_NACK 0x48 + #define TW_MR_DATA_ACK 0x50 + #define TW_MR_DATA_NACK 0x58 + + #define TW_OK 0 + #define TW_ERROR 1 + + class Fastwire { + private: + static boolean waitInt(); + + public: + static void setup(int khz, boolean pullup); + static byte beginTransmission(byte device); + static byte write(byte value); + static byte writeBuf(byte device, byte address, byte *data, byte num); + static byte readBuf(byte device, byte address, byte *data, byte num); + static void reset(); + static byte stop(); + }; +#endif + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + // NBWire implementation based heavily on code by Gene Knight + // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html + // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html + + #define NBWIRE_BUFFER_LENGTH 32 + + class TwoWire { + private: + static uint8_t rxBuffer[]; + static uint8_t rxBufferIndex; + static uint8_t rxBufferLength; + + static uint8_t txAddress; + static uint8_t txBuffer[]; + static uint8_t txBufferIndex; + static uint8_t txBufferLength; + + // static uint8_t transmitting; + static void (*user_onRequest)(void); + static void (*user_onReceive)(int); + static void onRequestService(void); + static void onReceiveService(uint8_t*, int); + + public: + TwoWire(); + void begin(); + void begin(uint8_t); + void begin(int); + void beginTransmission(uint8_t); + //void beginTransmission(int); + uint8_t endTransmission(uint16_t timeout=0); + void nbendTransmission(void (*function)(int)) ; + uint8_t requestFrom(uint8_t, int, uint16_t timeout=0); + //uint8_t requestFrom(int, int); + void nbrequestFrom(uint8_t, int, void (*function)(int)); + void send(uint8_t); + void send(uint8_t*, uint8_t); + //void send(int); + void send(char*); + uint8_t available(void); + uint8_t receive(void); + void onReceive(void (*)(int)); + void onRequest(void (*)(void)); + }; + + #define TWI_READY 0 + #define TWI_MRX 1 + #define TWI_MTX 2 + #define TWI_SRX 3 + #define TWI_STX 4 + + #define TW_WRITE 0 + #define TW_READ 1 + + #define TW_MT_SLA_NACK 0x20 + #define TW_MT_DATA_NACK 0x30 + + #define CPU_FREQ 16000000L + #define TWI_FREQ 100000L + #define TWI_BUFFER_LENGTH 32 + + /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */ + + #define TW_STATUS_MASK (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3)) + #define TW_STATUS (TWSR & TW_STATUS_MASK) + #define TW_START 0x08 + #define TW_REP_START 0x10 + #define TW_MT_SLA_ACK 0x18 + #define TW_MT_SLA_NACK 0x20 + #define TW_MT_DATA_ACK 0x28 + #define TW_MT_DATA_NACK 0x30 + #define TW_MT_ARB_LOST 0x38 + #define TW_MR_ARB_LOST 0x38 + #define TW_MR_SLA_ACK 0x40 + #define TW_MR_SLA_NACK 0x48 + #define TW_MR_DATA_ACK 0x50 + #define TW_MR_DATA_NACK 0x58 + #define TW_ST_SLA_ACK 0xA8 + #define TW_ST_ARB_LOST_SLA_ACK 0xB0 + #define TW_ST_DATA_ACK 0xB8 + #define TW_ST_DATA_NACK 0xC0 + #define TW_ST_LAST_DATA 0xC8 + #define TW_SR_SLA_ACK 0x60 + #define TW_SR_ARB_LOST_SLA_ACK 0x68 + #define TW_SR_GCALL_ACK 0x70 + #define TW_SR_ARB_LOST_GCALL_ACK 0x78 + #define TW_SR_DATA_ACK 0x80 + #define TW_SR_DATA_NACK 0x88 + #define TW_SR_GCALL_DATA_ACK 0x90 + #define TW_SR_GCALL_DATA_NACK 0x98 + #define TW_SR_STOP 0xA0 + #define TW_NO_INFO 0xF8 + #define TW_BUS_ERROR 0x00 + + //#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr)) + //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr)) + + #ifndef sbi // set bit + #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) + #endif // sbi + + #ifndef cbi // clear bit + #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) + #endif // cbi + + extern TwoWire Wire; + +#endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + +#endif /* _I2CDEV_H_ */ diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050.cpp b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050.cpp new file mode 100644 index 00000000..ae7a555e --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050.cpp @@ -0,0 +1,3330 @@ +// I2Cdev library collection - MPU6050 I2C device class +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 8/24/2011 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2019-07-08 - Added Auto Calibration routine +// ... - ongoing debug release + +// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE +// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF +// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING. + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#include "MPU6050.h" + +/** Specific address constructor. + * @param address I2C address, uses default I2C address if none is specified + * @see MPU6050_DEFAULT_ADDRESS + * @see MPU6050_ADDRESS_AD0_LOW + * @see MPU6050_ADDRESS_AD0_HIGH + */ +MPU6050::MPU6050(uint8_t address):devAddr(address) { +} + +/** Power on and prepare for general usage. + * This will activate the device and take it out of sleep mode (which must be done + * after start-up). This function also sets both the accelerometer and the gyroscope + * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets + * the clock source to use the X Gyro for reference, which is slightly better than + * the default internal clock source. + */ +void MPU6050::initialize() { + setClockSource(MPU6050_CLOCK_PLL_XGYRO); + setFullScaleGyroRange(MPU6050_GYRO_FS_250); + setFullScaleAccelRange(MPU6050_ACCEL_FS_2); + setSleepEnabled(false); // thanks to Jack Elston for pointing this one out! +} + +/** Verify the I2C connection. + * Make sure the device is connected and responds as expected. + * @return True if connection is valid, false otherwise + */ +bool MPU6050::testConnection() { + return getDeviceID() == 0x34; +} + +// AUX_VDDIO register (InvenSense demo code calls this RA_*G_OFFS_TC) + +/** Get the auxiliary I2C supply voltage level. + * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to + * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to + * the MPU-6000, which does not have a VLOGIC pin. + * @return I2C supply voltage level (0=VLOGIC, 1=VDD) + */ +uint8_t MPU6050::getAuxVDDIOLevel() { + I2Cdev::readBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, buffer); + return buffer[0]; +} +/** Set the auxiliary I2C supply voltage level. + * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to + * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to + * the MPU-6000, which does not have a VLOGIC pin. + * @param level I2C supply voltage level (0=VLOGIC, 1=VDD) + */ +void MPU6050::setAuxVDDIOLevel(uint8_t level) { + I2Cdev::writeBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level); +} + +// SMPLRT_DIV register + +/** Get gyroscope output rate divider. + * The sensor register output, FIFO output, DMP sampling, Motion detection, Zero + * Motion detection, and Free Fall detection are all based on the Sample Rate. + * The Sample Rate is generated by dividing the gyroscope output rate by + * SMPLRT_DIV: + * + * Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) + * + * where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or + * 7), and 1kHz when the DLPF is enabled (see Register 26). + * + * Note: The accelerometer output rate is 1kHz. This means that for a Sample + * Rate greater than 1kHz, the same accelerometer sample may be output to the + * FIFO, DMP, and sensor registers more than once. + * + * For a diagram of the gyroscope and accelerometer signal paths, see Section 8 + * of the MPU-6000/MPU-6050 Product Specification document. + * + * @return Current sample rate + * @see MPU6050_RA_SMPLRT_DIV + */ +uint8_t MPU6050::getRate() { + I2Cdev::readByte(devAddr, MPU6050_RA_SMPLRT_DIV, buffer); + return buffer[0]; +} +/** Set gyroscope sample rate divider. + * @param rate New sample rate divider + * @see getRate() + * @see MPU6050_RA_SMPLRT_DIV + */ +void MPU6050::setRate(uint8_t rate) { + I2Cdev::writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, rate); +} + +// CONFIG register + +/** Get external FSYNC configuration. + * Configures the external Frame Synchronization (FSYNC) pin sampling. An + * external signal connected to the FSYNC pin can be sampled by configuring + * EXT_SYNC_SET. Signal changes to the FSYNC pin are latched so that short + * strobes may be captured. The latched FSYNC signal will be sampled at the + * Sampling Rate, as defined in register 25. After sampling, the latch will + * reset to the current FSYNC signal state. + * + * The sampled value will be reported in place of the least significant bit in + * a sensor data register determined by the value of EXT_SYNC_SET according to + * the following table. + * + * 
+ * EXT_SYNC_SET | FSYNC Bit Location
+ * -------------+-------------------
+ * 0            | Input disabled
+ * 1            | TEMP_OUT_L[0]
+ * 2            | GYRO_XOUT_L[0]
+ * 3            | GYRO_YOUT_L[0]
+ * 4            | GYRO_ZOUT_L[0]
+ * 5            | ACCEL_XOUT_L[0]
+ * 6            | ACCEL_YOUT_L[0]
+ * 7            | ACCEL_ZOUT_L[0]
+ *
+ * + * @return FSYNC configuration value + */ +uint8_t MPU6050::getExternalFrameSync() { + I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer); + return buffer[0]; +} +/** Set external FSYNC configuration. + * @see getExternalFrameSync() + * @see MPU6050_RA_CONFIG + * @param sync New FSYNC configuration value + */ +void MPU6050::setExternalFrameSync(uint8_t sync) { + I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync); +} +/** Get digital low-pass filter configuration. + * The DLPF_CFG parameter sets the digital low pass filter configuration. It + * also determines the internal sampling rate used by the device as shown in + * the table below. + * + * Note: The accelerometer output rate is 1kHz. This means that for a Sample + * Rate greater than 1kHz, the same accelerometer sample may be output to the + * FIFO, DMP, and sensor registers more than once. + * + * 
+ *          |   ACCELEROMETER    |           GYROSCOPE
+ * DLPF_CFG | Bandwidth | Delay  | Bandwidth | Delay  | Sample Rate
+ * ---------+-----------+--------+-----------+--------+-------------
+ * 0        | 260Hz     | 0ms    | 256Hz     | 0.98ms | 8kHz
+ * 1        | 184Hz     | 2.0ms  | 188Hz     | 1.9ms  | 1kHz
+ * 2        | 94Hz      | 3.0ms  | 98Hz      | 2.8ms  | 1kHz
+ * 3        | 44Hz      | 4.9ms  | 42Hz      | 4.8ms  | 1kHz
+ * 4        | 21Hz      | 8.5ms  | 20Hz      | 8.3ms  | 1kHz
+ * 5        | 10Hz      | 13.8ms | 10Hz      | 13.4ms | 1kHz
+ * 6        | 5Hz       | 19.0ms | 5Hz       | 18.6ms | 1kHz
+ * 7        |   -- Reserved --   |   -- Reserved --   | Reserved
+ *
+ * + * @return DLFP configuration + * @see MPU6050_RA_CONFIG + * @see MPU6050_CFG_DLPF_CFG_BIT + * @see MPU6050_CFG_DLPF_CFG_LENGTH + */ +uint8_t MPU6050::getDLPFMode() { + I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, buffer); + return buffer[0]; +} +/** Set digital low-pass filter configuration. + * @param mode New DLFP configuration setting + * @see getDLPFBandwidth() + * @see MPU6050_DLPF_BW_256 + * @see MPU6050_RA_CONFIG + * @see MPU6050_CFG_DLPF_CFG_BIT + * @see MPU6050_CFG_DLPF_CFG_LENGTH + */ +void MPU6050::setDLPFMode(uint8_t mode) { + I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode); +} + +// GYRO_CONFIG register + +/** Get full-scale gyroscope range. + * The FS_SEL parameter allows setting the full-scale range of the gyro sensors, + * as described in the table below. + * + * 
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ *
+ * + * @return Current full-scale gyroscope range setting + * @see MPU6050_GYRO_FS_250 + * @see MPU6050_RA_GYRO_CONFIG + * @see MPU6050_GCONFIG_FS_SEL_BIT + * @see MPU6050_GCONFIG_FS_SEL_LENGTH + */ +uint8_t MPU6050::getFullScaleGyroRange() { + I2Cdev::readBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, buffer); + return buffer[0]; +} +/** Set full-scale gyroscope range. + * @param range New full-scale gyroscope range value + * @see getFullScaleRange() + * @see MPU6050_GYRO_FS_250 + * @see MPU6050_RA_GYRO_CONFIG + * @see MPU6050_GCONFIG_FS_SEL_BIT + * @see MPU6050_GCONFIG_FS_SEL_LENGTH + */ +void MPU6050::setFullScaleGyroRange(uint8_t range) { + I2Cdev::writeBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range); +} + +// SELF TEST FACTORY TRIM VALUES + +/** Get self-test factory trim value for accelerometer X axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_X + */ +uint8_t MPU6050::getAccelXSelfTestFactoryTrim() { + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, &buffer[0]); + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1]); + return (buffer[0]>>3) | ((buffer[1]>>4) & 0x03); +} + +/** Get self-test factory trim value for accelerometer Y axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_Y + */ +uint8_t MPU6050::getAccelYSelfTestFactoryTrim() { + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, &buffer[0]); + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1]); + return (buffer[0]>>3) | ((buffer[1]>>2) & 0x03); +} + +/** Get self-test factory trim value for accelerometer Z axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_Z + */ +uint8_t MPU6050::getAccelZSelfTestFactoryTrim() { + I2Cdev::readBytes(devAddr, MPU6050_RA_SELF_TEST_Z, 2, buffer); + return (buffer[0]>>3) | (buffer[1] & 0x03); +} + +/** Get self-test factory trim value for gyro X axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_X + */ +uint8_t MPU6050::getGyroXSelfTestFactoryTrim() { + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, buffer); + return (buffer[0] & 0x1F); +} + +/** Get self-test factory trim value for gyro Y axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_Y + */ +uint8_t MPU6050::getGyroYSelfTestFactoryTrim() { + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, buffer); + return (buffer[0] & 0x1F); +} + +/** Get self-test factory trim value for gyro Z axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_Z + */ +uint8_t MPU6050::getGyroZSelfTestFactoryTrim() { + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Z, buffer); + return (buffer[0] & 0x1F); +} + +// ACCEL_CONFIG register + +/** Get self-test enabled setting for accelerometer X axis. + * @return Self-test enabled value + * @see MPU6050_RA_ACCEL_CONFIG + */ +bool MPU6050::getAccelXSelfTest() { + I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, buffer); + return buffer[0]; +} +/** Get self-test enabled setting for accelerometer X axis. + * @param enabled Self-test enabled value + * @see MPU6050_RA_ACCEL_CONFIG + */ +void MPU6050::setAccelXSelfTest(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, enabled); +} +/** Get self-test enabled value for accelerometer Y axis. + * @return Self-test enabled value + * @see MPU6050_RA_ACCEL_CONFIG + */ +bool MPU6050::getAccelYSelfTest() { + I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, buffer); + return buffer[0]; +} +/** Get self-test enabled value for accelerometer Y axis. + * @param enabled Self-test enabled value + * @see MPU6050_RA_ACCEL_CONFIG + */ +void MPU6050::setAccelYSelfTest(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, enabled); +} +/** Get self-test enabled value for accelerometer Z axis. + * @return Self-test enabled value + * @see MPU6050_RA_ACCEL_CONFIG + */ +bool MPU6050::getAccelZSelfTest() { + I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, buffer); + return buffer[0]; +} +/** Set self-test enabled value for accelerometer Z axis. + * @param enabled Self-test enabled value + * @see MPU6050_RA_ACCEL_CONFIG + */ +void MPU6050::setAccelZSelfTest(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, enabled); +} +/** Get full-scale accelerometer range. + * The FS_SEL parameter allows setting the full-scale range of the accelerometer + * sensors, as described in the table below. + * + * 
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ *
+ * + * @return Current full-scale accelerometer range setting + * @see MPU6050_ACCEL_FS_2 + * @see MPU6050_RA_ACCEL_CONFIG + * @see MPU6050_ACONFIG_AFS_SEL_BIT + * @see MPU6050_ACONFIG_AFS_SEL_LENGTH + */ +uint8_t MPU6050::getFullScaleAccelRange() { + I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, buffer); + return buffer[0]; +} +/** Set full-scale accelerometer range. + * @param range New full-scale accelerometer range setting + * @see getFullScaleAccelRange() + */ +void MPU6050::setFullScaleAccelRange(uint8_t range) { + I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range); +} +/** Get the high-pass filter configuration. + * The DHPF is a filter module in the path leading to motion detectors (Free + * Fall, Motion threshold, and Zero Motion). The high pass filter output is not + * available to the data registers (see Figure in Section 8 of the MPU-6000/ + * MPU-6050 Product Specification document). + * + * The high pass filter has three modes: + * + * 
+ *    Reset: The filter output settles to zero within one sample. This
+ *           effectively disables the high pass filter. This mode may be toggled
+ *           to quickly settle the filter.
+ *
+ *    On:    The high pass filter will pass signals above the cut off frequency.
+ *
+ *    Hold:  When triggered, the filter holds the present sample. The filter
+ *           output will be the difference between the input sample and the held
+ *           sample.
+ *
+ * + * 
+ * ACCEL_HPF | Filter Mode | Cut-off Frequency
+ * ----------+-------------+------------------
+ * 0         | Reset       | None
+ * 1         | On          | 5Hz
+ * 2         | On          | 2.5Hz
+ * 3         | On          | 1.25Hz
+ * 4         | On          | 0.63Hz
+ * 7         | Hold        | None
+ *
+ * + * @return Current high-pass filter configuration + * @see MPU6050_DHPF_RESET + * @see MPU6050_RA_ACCEL_CONFIG + */ +uint8_t MPU6050::getDHPFMode() { + I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, buffer); + return buffer[0]; +} +/** Set the high-pass filter configuration. + * @param bandwidth New high-pass filter configuration + * @see setDHPFMode() + * @see MPU6050_DHPF_RESET + * @see MPU6050_RA_ACCEL_CONFIG + */ +void MPU6050::setDHPFMode(uint8_t bandwidth) { + I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, bandwidth); +} + +// FF_THR register + +/** Get free-fall event acceleration threshold. + * This register configures the detection threshold for Free Fall event + * detection. The unit of FF_THR is 1LSB = 2mg. Free Fall is detected when the + * absolute value of the accelerometer measurements for the three axes are each + * less than the detection threshold. This condition increments the Free Fall + * duration counter (Register 30). The Free Fall interrupt is triggered when the + * Free Fall duration counter reaches the time specified in FF_DUR. + * + * For more details on the Free Fall detection interrupt, see Section 8.2 of the + * MPU-6000/MPU-6050 Product Specification document as well as Registers 56 and + * 58 of this document. + * + * @return Current free-fall acceleration threshold value (LSB = 2mg) + * @see MPU6050_RA_FF_THR + */ +uint8_t MPU6050::getFreefallDetectionThreshold() { + I2Cdev::readByte(devAddr, MPU6050_RA_FF_THR, buffer); + return buffer[0]; +} +/** Get free-fall event acceleration threshold. + * @param threshold New free-fall acceleration threshold value (LSB = 2mg) + * @see getFreefallDetectionThreshold() + * @see MPU6050_RA_FF_THR + */ +void MPU6050::setFreefallDetectionThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, MPU6050_RA_FF_THR, threshold); +} + +// FF_DUR register + +/** Get free-fall event duration threshold. + * This register configures the duration counter threshold for Free Fall event + * detection. The duration counter ticks at 1kHz, therefore FF_DUR has a unit + * of 1 LSB = 1 ms. + * + * The Free Fall duration counter increments while the absolute value of the + * accelerometer measurements are each less than the detection threshold + * (Register 29). The Free Fall interrupt is triggered when the Free Fall + * duration counter reaches the time specified in this register. + * + * For more details on the Free Fall detection interrupt, see Section 8.2 of + * the MPU-6000/MPU-6050 Product Specification document as well as Registers 56 + * and 58 of this document. + * + * @return Current free-fall duration threshold value (LSB = 1ms) + * @see MPU6050_RA_FF_DUR + */ +uint8_t MPU6050::getFreefallDetectionDuration() { + I2Cdev::readByte(devAddr, MPU6050_RA_FF_DUR, buffer); + return buffer[0]; +} +/** Get free-fall event duration threshold. + * @param duration New free-fall duration threshold value (LSB = 1ms) + * @see getFreefallDetectionDuration() + * @see MPU6050_RA_FF_DUR + */ +void MPU6050::setFreefallDetectionDuration(uint8_t duration) { + I2Cdev::writeByte(devAddr, MPU6050_RA_FF_DUR, duration); +} + +// MOT_THR register + +/** Get motion detection event acceleration threshold. + * This register configures the detection threshold for Motion interrupt + * generation. The unit of MOT_THR is 1LSB = 2mg. Motion is detected when the + * absolute value of any of the accelerometer measurements exceeds this Motion + * detection threshold. This condition increments the Motion detection duration + * counter (Register 32). The Motion detection interrupt is triggered when the + * Motion Detection counter reaches the time count specified in MOT_DUR + * (Register 32). + * + * The Motion interrupt will indicate the axis and polarity of detected motion + * in MOT_DETECT_STATUS (Register 97). + * + * For more details on the Motion detection interrupt, see Section 8.3 of the + * MPU-6000/MPU-6050 Product Specification document as well as Registers 56 and + * 58 of this document. + * + * @return Current motion detection acceleration threshold value (LSB = 2mg) + * @see MPU6050_RA_MOT_THR + */ +uint8_t MPU6050::getMotionDetectionThreshold() { + I2Cdev::readByte(devAddr, MPU6050_RA_MOT_THR, buffer); + return buffer[0]; +} +/** Set motion detection event acceleration threshold. + * @param threshold New motion detection acceleration threshold value (LSB = 2mg) + * @see getMotionDetectionThreshold() + * @see MPU6050_RA_MOT_THR + */ +void MPU6050::setMotionDetectionThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_THR, threshold); +} + +// MOT_DUR register + +/** Get motion detection event duration threshold. + * This register configures the duration counter threshold for Motion interrupt + * generation. The duration counter ticks at 1 kHz, therefore MOT_DUR has a unit + * of 1LSB = 1ms. The Motion detection duration counter increments when the + * absolute value of any of the accelerometer measurements exceeds the Motion + * detection threshold (Register 31). The Motion detection interrupt is + * triggered when the Motion detection counter reaches the time count specified + * in this register. + * + * For more details on the Motion detection interrupt, see Section 8.3 of the + * MPU-6000/MPU-6050 Product Specification document. + * + * @return Current motion detection duration threshold value (LSB = 1ms) + * @see MPU6050_RA_MOT_DUR + */ +uint8_t MPU6050::getMotionDetectionDuration() { + I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DUR, buffer); + return buffer[0]; +} +/** Set motion detection event duration threshold. + * @param duration New motion detection duration threshold value (LSB = 1ms) + * @see getMotionDetectionDuration() + * @see MPU6050_RA_MOT_DUR + */ +void MPU6050::setMotionDetectionDuration(uint8_t duration) { + I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_DUR, duration); +} + +// ZRMOT_THR register + +/** Get zero motion detection event acceleration threshold. + * This register configures the detection threshold for Zero Motion interrupt + * generation. The unit of ZRMOT_THR is 1LSB = 2mg. Zero Motion is detected when + * the absolute value of the accelerometer measurements for the 3 axes are each + * less than the detection threshold. This condition increments the Zero Motion + * duration counter (Register 34). The Zero Motion interrupt is triggered when + * the Zero Motion duration counter reaches the time count specified in + * ZRMOT_DUR (Register 34). + * + * Unlike Free Fall or Motion detection, Zero Motion detection triggers an + * interrupt both when Zero Motion is first detected and when Zero Motion is no + * longer detected. + * + * When a zero motion event is detected, a Zero Motion Status will be indicated + * in the MOT_DETECT_STATUS register (Register 97). When a motion-to-zero-motion + * condition is detected, the status bit is set to 1. When a zero-motion-to- + * motion condition is detected, the status bit is set to 0. + * + * For more details on the Zero Motion detection interrupt, see Section 8.4 of + * the MPU-6000/MPU-6050 Product Specification document as well as Registers 56 + * and 58 of this document. + * + * @return Current zero motion detection acceleration threshold value (LSB = 2mg) + * @see MPU6050_RA_ZRMOT_THR + */ +uint8_t MPU6050::getZeroMotionDetectionThreshold() { + I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_THR, buffer); + return buffer[0]; +} +/** Set zero motion detection event acceleration threshold. + * @param threshold New zero motion detection acceleration threshold value (LSB = 2mg) + * @see getZeroMotionDetectionThreshold() + * @see MPU6050_RA_ZRMOT_THR + */ +void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_THR, threshold); +} + +// ZRMOT_DUR register + +/** Get zero motion detection event duration threshold. + * This register configures the duration counter threshold for Zero Motion + * interrupt generation. The duration counter ticks at 16 Hz, therefore + * ZRMOT_DUR has a unit of 1 LSB = 64 ms. The Zero Motion duration counter + * increments while the absolute value of the accelerometer measurements are + * each less than the detection threshold (Register 33). The Zero Motion + * interrupt is triggered when the Zero Motion duration counter reaches the time + * count specified in this register. + * + * For more details on the Zero Motion detection interrupt, see Section 8.4 of + * the MPU-6000/MPU-6050 Product Specification document, as well as Registers 56 + * and 58 of this document. + * + * @return Current zero motion detection duration threshold value (LSB = 64ms) + * @see MPU6050_RA_ZRMOT_DUR + */ +uint8_t MPU6050::getZeroMotionDetectionDuration() { + I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_DUR, buffer); + return buffer[0]; +} +/** Set zero motion detection event duration threshold. + * @param duration New zero motion detection duration threshold value (LSB = 1ms) + * @see getZeroMotionDetectionDuration() + * @see MPU6050_RA_ZRMOT_DUR + */ +void MPU6050::setZeroMotionDetectionDuration(uint8_t duration) { + I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_DUR, duration); +} + +// FIFO_EN register + +/** Get temperature FIFO enabled value. + * When set to 1, this bit enables TEMP_OUT_H and TEMP_OUT_L (Registers 65 and + * 66) to be written into the FIFO buffer. + * @return Current temperature FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getTempFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set temperature FIFO enabled value. + * @param enabled New temperature FIFO enabled value + * @see getTempFIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setTempFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, enabled); +} +/** Get gyroscope X-axis FIFO enabled value. + * When set to 1, this bit enables GYRO_XOUT_H and GYRO_XOUT_L (Registers 67 and + * 68) to be written into the FIFO buffer. + * @return Current gyroscope X-axis FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getXGyroFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set gyroscope X-axis FIFO enabled value. + * @param enabled New gyroscope X-axis FIFO enabled value + * @see getXGyroFIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setXGyroFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, enabled); +} +/** Get gyroscope Y-axis FIFO enabled value. + * When set to 1, this bit enables GYRO_YOUT_H and GYRO_YOUT_L (Registers 69 and + * 70) to be written into the FIFO buffer. + * @return Current gyroscope Y-axis FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getYGyroFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set gyroscope Y-axis FIFO enabled value. + * @param enabled New gyroscope Y-axis FIFO enabled value + * @see getYGyroFIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setYGyroFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, enabled); +} +/** Get gyroscope Z-axis FIFO enabled value. + * When set to 1, this bit enables GYRO_ZOUT_H and GYRO_ZOUT_L (Registers 71 and + * 72) to be written into the FIFO buffer. + * @return Current gyroscope Z-axis FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getZGyroFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set gyroscope Z-axis FIFO enabled value. + * @param enabled New gyroscope Z-axis FIFO enabled value + * @see getZGyroFIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setZGyroFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, enabled); +} +/** Get accelerometer FIFO enabled value. + * When set to 1, this bit enables ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H, + * ACCEL_YOUT_L, ACCEL_ZOUT_H, and ACCEL_ZOUT_L (Registers 59 to 64) to be + * written into the FIFO buffer. + * @return Current accelerometer FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getAccelFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set accelerometer FIFO enabled value. + * @param enabled New accelerometer FIFO enabled value + * @see getAccelFIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setAccelFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, enabled); +} +/** Get Slave 2 FIFO enabled value. + * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) + * associated with Slave 2 to be written into the FIFO buffer. + * @return Current Slave 2 FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getSlave2FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set Slave 2 FIFO enabled value. + * @param enabled New Slave 2 FIFO enabled value + * @see getSlave2FIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setSlave2FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, enabled); +} +/** Get Slave 1 FIFO enabled value. + * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) + * associated with Slave 1 to be written into the FIFO buffer. + * @return Current Slave 1 FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getSlave1FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set Slave 1 FIFO enabled value. + * @param enabled New Slave 1 FIFO enabled value + * @see getSlave1FIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setSlave1FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, enabled); +} +/** Get Slave 0 FIFO enabled value. + * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) + * associated with Slave 0 to be written into the FIFO buffer. + * @return Current Slave 0 FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getSlave0FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set Slave 0 FIFO enabled value. + * @param enabled New Slave 0 FIFO enabled value + * @see getSlave0FIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setSlave0FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, enabled); +} + +// I2C_MST_CTRL register + +/** Get multi-master enabled value. + * Multi-master capability allows multiple I2C masters to operate on the same + * bus. In circuits where multi-master capability is required, set MULT_MST_EN + * to 1. This will increase current drawn by approximately 30uA. + * + * In circuits where multi-master capability is required, the state of the I2C + * bus must always be monitored by each separate I2C Master. Before an I2C + * Master can assume arbitration of the bus, it must first confirm that no other + * I2C Master has arbitration of the bus. When MULT_MST_EN is set to 1, the + * MPU-60X0's bus arbitration detection logic is turned on, enabling it to + * detect when the bus is available. + * + * @return Current multi-master enabled value + * @see MPU6050_RA_I2C_MST_CTRL + */ +bool MPU6050::getMultiMasterEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, buffer); + return buffer[0]; +} +/** Set multi-master enabled value. + * @param enabled New multi-master enabled value + * @see getMultiMasterEnabled() + * @see MPU6050_RA_I2C_MST_CTRL + */ +void MPU6050::setMultiMasterEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, enabled); +} +/** Get wait-for-external-sensor-data enabled value. + * When the WAIT_FOR_ES bit is set to 1, the Data Ready interrupt will be + * delayed until External Sensor data from the Slave Devices are loaded into the + * EXT_SENS_DATA registers. This is used to ensure that both the internal sensor + * data (i.e. from gyro and accel) and external sensor data have been loaded to + * their respective data registers (i.e. the data is synced) when the Data Ready + * interrupt is triggered. + * + * @return Current wait-for-external-sensor-data enabled value + * @see MPU6050_RA_I2C_MST_CTRL + */ +bool MPU6050::getWaitForExternalSensorEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, buffer); + return buffer[0]; +} +/** Set wait-for-external-sensor-data enabled value. + * @param enabled New wait-for-external-sensor-data enabled value + * @see getWaitForExternalSensorEnabled() + * @see MPU6050_RA_I2C_MST_CTRL + */ +void MPU6050::setWaitForExternalSensorEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, enabled); +} +/** Get Slave 3 FIFO enabled value. + * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) + * associated with Slave 3 to be written into the FIFO buffer. + * @return Current Slave 3 FIFO enabled value + * @see MPU6050_RA_MST_CTRL + */ +bool MPU6050::getSlave3FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set Slave 3 FIFO enabled value. + * @param enabled New Slave 3 FIFO enabled value + * @see getSlave3FIFOEnabled() + * @see MPU6050_RA_MST_CTRL + */ +void MPU6050::setSlave3FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, enabled); +} +/** Get slave read/write transition enabled value. + * The I2C_MST_P_NSR bit configures the I2C Master's transition from one slave + * read to the next slave read. If the bit equals 0, there will be a restart + * between reads. If the bit equals 1, there will be a stop followed by a start + * of the following read. When a write transaction follows a read transaction, + * the stop followed by a start of the successive write will be always used. + * + * @return Current slave read/write transition enabled value + * @see MPU6050_RA_I2C_MST_CTRL + */ +bool MPU6050::getSlaveReadWriteTransitionEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, buffer); + return buffer[0]; +} +/** Set slave read/write transition enabled value. + * @param enabled New slave read/write transition enabled value + * @see getSlaveReadWriteTransitionEnabled() + * @see MPU6050_RA_I2C_MST_CTRL + */ +void MPU6050::setSlaveReadWriteTransitionEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, enabled); +} +/** Get I2C master clock speed. + * I2C_MST_CLK is a 4 bit unsigned value which configures a divider on the + * MPU-60X0 internal 8MHz clock. It sets the I2C master clock speed according to + * the following table: + * + * 
+ * I2C_MST_CLK | I2C Master Clock Speed | 8MHz Clock Divider
+ * ------------+------------------------+-------------------
+ * 0           | 348kHz                 | 23
+ * 1           | 333kHz                 | 24
+ * 2           | 320kHz                 | 25
+ * 3           | 308kHz                 | 26
+ * 4           | 296kHz                 | 27
+ * 5           | 286kHz                 | 28
+ * 6           | 276kHz                 | 29
+ * 7           | 267kHz                 | 30
+ * 8           | 258kHz                 | 31
+ * 9           | 500kHz                 | 16
+ * 10          | 471kHz                 | 17
+ * 11          | 444kHz                 | 18
+ * 12          | 421kHz                 | 19
+ * 13          | 400kHz                 | 20
+ * 14          | 381kHz                 | 21
+ * 15          | 364kHz                 | 22
+ *
+ * + * @return Current I2C master clock speed + * @see MPU6050_RA_I2C_MST_CTRL + */ +uint8_t MPU6050::getMasterClockSpeed() { + I2Cdev::readBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, buffer); + return buffer[0]; +} +/** Set I2C master clock speed. + * @reparam speed Current I2C master clock speed + * @see MPU6050_RA_I2C_MST_CTRL + */ +void MPU6050::setMasterClockSpeed(uint8_t speed) { + I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, speed); +} + +// I2C_SLV* registers (Slave 0-3) + +/** Get the I2C address of the specified slave (0-3). + * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read + * operation, and if it is cleared, then it's a write operation. The remaining + * bits (6-0) are the 7-bit device address of the slave device. + * + * In read mode, the result of the read is placed in the lowest available + * EXT_SENS_DATA register. For further information regarding the allocation of + * read results, please refer to the EXT_SENS_DATA register description + * (Registers 73 - 96). + * + * The MPU-6050 supports a total of five slaves, but Slave 4 has unique + * characteristics, and so it has its own functions (getSlave4* and setSlave4*). + * + * I2C data transactions are performed at the Sample Rate, as defined in + * Register 25. The user is responsible for ensuring that I2C data transactions + * to and from each enabled Slave can be completed within a single period of the + * Sample Rate. + * + * The I2C slave access rate can be reduced relative to the Sample Rate. This + * reduced access rate is determined by I2C_MST_DLY (Register 52). Whether a + * slave's access rate is reduced relative to the Sample Rate is determined by + * I2C_MST_DELAY_CTRL (Register 103). + * + * The processing order for the slaves is fixed. The sequence followed for + * processing the slaves is Slave 0, Slave 1, Slave 2, Slave 3 and Slave 4. If a + * particular Slave is disabled it will be skipped. + * + * Each slave can either be accessed at the sample rate or at a reduced sample + * rate. In a case where some slaves are accessed at the Sample Rate and some + * slaves are accessed at the reduced rate, the sequence of accessing the slaves + * (Slave 0 to Slave 4) is still followed. However, the reduced rate slaves will + * be skipped if their access rate dictates that they should not be accessed + * during that particular cycle. For further information regarding the reduced + * access rate, please refer to Register 52. Whether a slave is accessed at the + * Sample Rate or at the reduced rate is determined by the Delay Enable bits in + * Register 103. + * + * @param num Slave number (0-3) + * @return Current address for specified slave + * @see MPU6050_RA_I2C_SLV0_ADDR + */ +uint8_t MPU6050::getSlaveAddress(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, buffer); + return buffer[0]; +} +/** Set the I2C address of the specified slave (0-3). + * @param num Slave number (0-3) + * @param address New address for specified slave + * @see getSlaveAddress() + * @see MPU6050_RA_I2C_SLV0_ADDR + */ +void MPU6050::setSlaveAddress(uint8_t num, uint8_t address) { + if (num > 3) return; + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, address); +} +/** Get the active internal register for the specified slave (0-3). + * Read/write operations for this slave will be done to whatever internal + * register address is stored in this MPU register. + * + * The MPU-6050 supports a total of five slaves, but Slave 4 has unique + * characteristics, and so it has its own functions. + * + * @param num Slave number (0-3) + * @return Current active register for specified slave + * @see MPU6050_RA_I2C_SLV0_REG + */ +uint8_t MPU6050::getSlaveRegister(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, buffer); + return buffer[0]; +} +/** Set the active internal register for the specified slave (0-3). + * @param num Slave number (0-3) + * @param reg New active register for specified slave + * @see getSlaveRegister() + * @see MPU6050_RA_I2C_SLV0_REG + */ +void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg) { + if (num > 3) return; + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, reg); +} +/** Get the enabled value for the specified slave (0-3). + * When set to 1, this bit enables Slave 0 for data transfer operations. When + * cleared to 0, this bit disables Slave 0 from data transfer operations. + * @param num Slave number (0-3) + * @return Current enabled value for specified slave + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +bool MPU6050::getSlaveEnabled(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, buffer); + return buffer[0]; +} +/** Set the enabled value for the specified slave (0-3). + * @param num Slave number (0-3) + * @param enabled New enabled value for specified slave + * @see getSlaveEnabled() + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +void MPU6050::setSlaveEnabled(uint8_t num, bool enabled) { + if (num > 3) return; + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, enabled); +} +/** Get word pair byte-swapping enabled for the specified slave (0-3). + * When set to 1, this bit enables byte swapping. When byte swapping is enabled, + * the high and low bytes of a word pair are swapped. Please refer to + * I2C_SLV0_GRP for the pairing convention of the word pairs. When cleared to 0, + * bytes transferred to and from Slave 0 will be written to EXT_SENS_DATA + * registers in the order they were transferred. + * + * @param num Slave number (0-3) + * @return Current word pair byte-swapping enabled value for specified slave + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +bool MPU6050::getSlaveWordByteSwap(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, buffer); + return buffer[0]; +} +/** Set word pair byte-swapping enabled for the specified slave (0-3). + * @param num Slave number (0-3) + * @param enabled New word pair byte-swapping enabled value for specified slave + * @see getSlaveWordByteSwap() + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled) { + if (num > 3) return; + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled); +} +/** Get write mode for the specified slave (0-3). + * When set to 1, the transaction will read or write data only. When cleared to + * 0, the transaction will write a register address prior to reading or writing + * data. This should equal 0 when specifying the register address within the + * Slave device to/from which the ensuing data transaction will take place. + * + * @param num Slave number (0-3) + * @return Current write mode for specified slave (0 = register address + data, 1 = data only) + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +bool MPU6050::getSlaveWriteMode(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, buffer); + return buffer[0]; +} +/** Set write mode for the specified slave (0-3). + * @param num Slave number (0-3) + * @param mode New write mode for specified slave (0 = register address + data, 1 = data only) + * @see getSlaveWriteMode() + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +void MPU6050::setSlaveWriteMode(uint8_t num, bool mode) { + if (num > 3) return; + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, mode); +} +/** Get word pair grouping order offset for the specified slave (0-3). + * This sets specifies the grouping order of word pairs received from registers. + * When cleared to 0, bytes from register addresses 0 and 1, 2 and 3, etc (even, + * then odd register addresses) are paired to form a word. When set to 1, bytes + * from register addresses are paired 1 and 2, 3 and 4, etc. (odd, then even + * register addresses) are paired to form a word. + * + * @param num Slave number (0-3) + * @return Current word pair grouping order offset for specified slave + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +bool MPU6050::getSlaveWordGroupOffset(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, buffer); + return buffer[0]; +} +/** Set word pair grouping order offset for the specified slave (0-3). + * @param num Slave number (0-3) + * @param enabled New word pair grouping order offset for specified slave + * @see getSlaveWordGroupOffset() + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled) { + if (num > 3) return; + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, enabled); +} +/** Get number of bytes to read for the specified slave (0-3). + * Specifies the number of bytes transferred to and from Slave 0. Clearing this + * bit to 0 is equivalent to disabling the register by writing 0 to I2C_SLV0_EN. + * @param num Slave number (0-3) + * @return Number of bytes to read for specified slave + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +uint8_t MPU6050::getSlaveDataLength(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, buffer); + return buffer[0]; +} +/** Set number of bytes to read for the specified slave (0-3). + * @param num Slave number (0-3) + * @param length Number of bytes to read for specified slave + * @see getSlaveDataLength() + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length) { + if (num > 3) return; + I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, length); +} + +// I2C_SLV* registers (Slave 4) + +/** Get the I2C address of Slave 4. + * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read + * operation, and if it is cleared, then it's a write operation. The remaining + * bits (6-0) are the 7-bit device address of the slave device. + * + * @return Current address for Slave 4 + * @see getSlaveAddress() + * @see MPU6050_RA_I2C_SLV4_ADDR + */ +uint8_t MPU6050::getSlave4Address() { + I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, buffer); + return buffer[0]; +} +/** Set the I2C address of Slave 4. + * @param address New address for Slave 4 + * @see getSlave4Address() + * @see MPU6050_RA_I2C_SLV4_ADDR + */ +void MPU6050::setSlave4Address(uint8_t address) { + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, address); +} +/** Get the active internal register for the Slave 4. + * Read/write operations for this slave will be done to whatever internal + * register address is stored in this MPU register. + * + * @return Current active register for Slave 4 + * @see MPU6050_RA_I2C_SLV4_REG + */ +uint8_t MPU6050::getSlave4Register() { + I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_REG, buffer); + return buffer[0]; +} +/** Set the active internal register for Slave 4. + * @param reg New active register for Slave 4 + * @see getSlave4Register() + * @see MPU6050_RA_I2C_SLV4_REG + */ +void MPU6050::setSlave4Register(uint8_t reg) { + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_REG, reg); +} +/** Set new byte to write to Slave 4. + * This register stores the data to be written into the Slave 4. If I2C_SLV4_RW + * is set 1 (set to read), this register has no effect. + * @param data New byte to write to Slave 4 + * @see MPU6050_RA_I2C_SLV4_DO + */ +void MPU6050::setSlave4OutputByte(uint8_t data) { + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_DO, data); +} +/** Get the enabled value for the Slave 4. + * When set to 1, this bit enables Slave 4 for data transfer operations. When + * cleared to 0, this bit disables Slave 4 from data transfer operations. + * @return Current enabled value for Slave 4 + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +bool MPU6050::getSlave4Enabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, buffer); + return buffer[0]; +} +/** Set the enabled value for Slave 4. + * @param enabled New enabled value for Slave 4 + * @see getSlave4Enabled() + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +void MPU6050::setSlave4Enabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, enabled); +} +/** Get the enabled value for Slave 4 transaction interrupts. + * When set to 1, this bit enables the generation of an interrupt signal upon + * completion of a Slave 4 transaction. When cleared to 0, this bit disables the + * generation of an interrupt signal upon completion of a Slave 4 transaction. + * The interrupt status can be observed in Register 54. + * + * @return Current enabled value for Slave 4 transaction interrupts. + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +bool MPU6050::getSlave4InterruptEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, buffer); + return buffer[0]; +} +/** Set the enabled value for Slave 4 transaction interrupts. + * @param enabled New enabled value for Slave 4 transaction interrupts. + * @see getSlave4InterruptEnabled() + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +void MPU6050::setSlave4InterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, enabled); +} +/** Get write mode for Slave 4. + * When set to 1, the transaction will read or write data only. When cleared to + * 0, the transaction will write a register address prior to reading or writing + * data. This should equal 0 when specifying the register address within the + * Slave device to/from which the ensuing data transaction will take place. + * + * @return Current write mode for Slave 4 (0 = register address + data, 1 = data only) + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +bool MPU6050::getSlave4WriteMode() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, buffer); + return buffer[0]; +} +/** Set write mode for the Slave 4. + * @param mode New write mode for Slave 4 (0 = register address + data, 1 = data only) + * @see getSlave4WriteMode() + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +void MPU6050::setSlave4WriteMode(bool mode) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mode); +} +/** Get Slave 4 master delay value. + * This configures the reduced access rate of I2C slaves relative to the Sample + * Rate. When a slave's access rate is decreased relative to the Sample Rate, + * the slave is accessed every: + * + * 1 / (1 + I2C_MST_DLY) samples + * + * This base Sample Rate in turn is determined by SMPLRT_DIV (register 25) and + * DLPF_CFG (register 26). Whether a slave's access rate is reduced relative to + * the Sample Rate is determined by I2C_MST_DELAY_CTRL (register 103). For + * further information regarding the Sample Rate, please refer to register 25. + * + * @return Current Slave 4 master delay value + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +uint8_t MPU6050::getSlave4MasterDelay() { + I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, buffer); + return buffer[0]; +} +/** Set Slave 4 master delay value. + * @param delay New Slave 4 master delay value + * @see getSlave4MasterDelay() + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +void MPU6050::setSlave4MasterDelay(uint8_t delay) { + I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay); +} +/** Get last available byte read from Slave 4. + * This register stores the data read from Slave 4. This field is populated + * after a read transaction. + * @return Last available byte read from to Slave 4 + * @see MPU6050_RA_I2C_SLV4_DI + */ +uint8_t MPU6050::getSlate4InputByte() { + I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_DI, buffer); + return buffer[0]; +} + +// I2C_MST_STATUS register + +/** Get FSYNC interrupt status. + * This bit reflects the status of the FSYNC interrupt from an external device + * into the MPU-60X0. This is used as a way to pass an external interrupt + * through the MPU-60X0 to the host application processor. When set to 1, this + * bit will cause an interrupt if FSYNC_INT_EN is asserted in INT_PIN_CFG + * (Register 55). + * @return FSYNC interrupt status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getPassthroughStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, buffer); + return buffer[0]; +} +/** Get Slave 4 transaction done status. + * Automatically sets to 1 when a Slave 4 transaction has completed. This + * triggers an interrupt if the I2C_MST_INT_EN bit in the INT_ENABLE register + * (Register 56) is asserted and if the SLV_4_DONE_INT bit is asserted in the + * I2C_SLV4_CTRL register (Register 52). + * @return Slave 4 transaction done status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getSlave4IsDone() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, buffer); + return buffer[0]; +} +/** Get master arbitration lost status. + * This bit automatically sets to 1 when the I2C Master has lost arbitration of + * the auxiliary I2C bus (an error condition). This triggers an interrupt if the + * I2C_MST_INT_EN bit in the INT_ENABLE register (Register 56) is asserted. + * @return Master arbitration lost status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getLostArbitration() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, buffer); + return buffer[0]; +} +/** Get Slave 4 NACK status. + * This bit automatically sets to 1 when the I2C Master receives a NACK in a + * transaction with Slave 4. This triggers an interrupt if the I2C_MST_INT_EN + * bit in the INT_ENABLE register (Register 56) is asserted. + * @return Slave 4 NACK interrupt status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getSlave4Nack() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, buffer); + return buffer[0]; +} +/** Get Slave 3 NACK status. + * This bit automatically sets to 1 when the I2C Master receives a NACK in a + * transaction with Slave 3. This triggers an interrupt if the I2C_MST_INT_EN + * bit in the INT_ENABLE register (Register 56) is asserted. + * @return Slave 3 NACK interrupt status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getSlave3Nack() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, buffer); + return buffer[0]; +} +/** Get Slave 2 NACK status. + * This bit automatically sets to 1 when the I2C Master receives a NACK in a + * transaction with Slave 2. This triggers an interrupt if the I2C_MST_INT_EN + * bit in the INT_ENABLE register (Register 56) is asserted. + * @return Slave 2 NACK interrupt status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getSlave2Nack() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, buffer); + return buffer[0]; +} +/** Get Slave 1 NACK status. + * This bit automatically sets to 1 when the I2C Master receives a NACK in a + * transaction with Slave 1. This triggers an interrupt if the I2C_MST_INT_EN + * bit in the INT_ENABLE register (Register 56) is asserted. + * @return Slave 1 NACK interrupt status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getSlave1Nack() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, buffer); + return buffer[0]; +} +/** Get Slave 0 NACK status. + * This bit automatically sets to 1 when the I2C Master receives a NACK in a + * transaction with Slave 0. This triggers an interrupt if the I2C_MST_INT_EN + * bit in the INT_ENABLE register (Register 56) is asserted. + * @return Slave 0 NACK interrupt status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getSlave0Nack() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, buffer); + return buffer[0]; +} + +// INT_PIN_CFG register + +/** Get interrupt logic level mode. + * Will be set 0 for active-high, 1 for active-low. + * @return Current interrupt mode (0=active-high, 1=active-low) + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_INT_LEVEL_BIT + */ +bool MPU6050::getInterruptMode() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, buffer); + return buffer[0]; +} +/** Set interrupt logic level mode. + * @param mode New interrupt mode (0=active-high, 1=active-low) + * @see getInterruptMode() + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_INT_LEVEL_BIT + */ +void MPU6050::setInterruptMode(bool mode) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mode); +} +/** Get interrupt drive mode. + * Will be set 0 for push-pull, 1 for open-drain. + * @return Current interrupt drive mode (0=push-pull, 1=open-drain) + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_INT_OPEN_BIT + */ +bool MPU6050::getInterruptDrive() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, buffer); + return buffer[0]; +} +/** Set interrupt drive mode. + * @param drive New interrupt drive mode (0=push-pull, 1=open-drain) + * @see getInterruptDrive() + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_INT_OPEN_BIT + */ +void MPU6050::setInterruptDrive(bool drive) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, drive); +} +/** Get interrupt latch mode. + * Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared. + * @return Current latch mode (0=50us-pulse, 1=latch-until-int-cleared) + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_LATCH_INT_EN_BIT + */ +bool MPU6050::getInterruptLatch() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, buffer); + return buffer[0]; +} +/** Set interrupt latch mode. + * @param latch New latch mode (0=50us-pulse, 1=latch-until-int-cleared) + * @see getInterruptLatch() + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_LATCH_INT_EN_BIT + */ +void MPU6050::setInterruptLatch(bool latch) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, latch); +} +/** Get interrupt latch clear mode. + * Will be set 0 for status-read-only, 1 for any-register-read. + * @return Current latch clear mode (0=status-read-only, 1=any-register-read) + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT + */ +bool MPU6050::getInterruptLatchClear() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, buffer); + return buffer[0]; +} +/** Set interrupt latch clear mode. + * @param clear New latch clear mode (0=status-read-only, 1=any-register-read) + * @see getInterruptLatchClear() + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT + */ +void MPU6050::setInterruptLatchClear(bool clear) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear); +} +/** Get FSYNC interrupt logic level mode. + * @return Current FSYNC interrupt mode (0=active-high, 1=active-low) + * @see getFSyncInterruptMode() + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT + */ +bool MPU6050::getFSyncInterruptLevel() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, buffer); + return buffer[0]; +} +/** Set FSYNC interrupt logic level mode. + * @param mode New FSYNC interrupt mode (0=active-high, 1=active-low) + * @see getFSyncInterruptMode() + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT + */ +void MPU6050::setFSyncInterruptLevel(bool level) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level); +} +/** Get FSYNC pin interrupt enabled setting. + * Will be set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled setting + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT + */ +bool MPU6050::getFSyncInterruptEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, buffer); + return buffer[0]; +} +/** Set FSYNC pin interrupt enabled setting. + * @param enabled New FSYNC pin interrupt enabled setting + * @see getFSyncInterruptEnabled() + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT + */ +void MPU6050::setFSyncInterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled); +} +/** Get I2C bypass enabled status. + * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to + * 0, the host application processor will be able to directly access the + * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host + * application processor will not be able to directly access the auxiliary I2C + * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106 + * bit[5]). + * @return Current I2C bypass enabled status + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT + */ +bool MPU6050::getI2CBypassEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, buffer); + return buffer[0]; +} +/** Set I2C bypass enabled status. + * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to + * 0, the host application processor will be able to directly access the + * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host + * application processor will not be able to directly access the auxiliary I2C + * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106 + * bit[5]). + * @param enabled New I2C bypass enabled status + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT + */ +void MPU6050::setI2CBypassEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled); +} +/** Get reference clock output enabled status. + * When this bit is equal to 1, a reference clock output is provided at the + * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For + * further information regarding CLKOUT, please refer to the MPU-60X0 Product + * Specification document. + * @return Current reference clock output enabled status + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_CLKOUT_EN_BIT + */ +bool MPU6050::getClockOutputEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, buffer); + return buffer[0]; +} +/** Set reference clock output enabled status. + * When this bit is equal to 1, a reference clock output is provided at the + * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For + * further information regarding CLKOUT, please refer to the MPU-60X0 Product + * Specification document. + * @param enabled New reference clock output enabled status + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_CLKOUT_EN_BIT + */ +void MPU6050::setClockOutputEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled); +} + +// INT_ENABLE register + +/** Get full interrupt enabled status. + * Full register byte for all interrupts, for quick reading. Each bit will be + * set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled status + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_FF_BIT + **/ +uint8_t MPU6050::getIntEnabled() { + I2Cdev::readByte(devAddr, MPU6050_RA_INT_ENABLE, buffer); + return buffer[0]; +} +/** Set full interrupt enabled status. + * Full register byte for all interrupts, for quick reading. Each bit should be + * set 0 for disabled, 1 for enabled. + * @param enabled New interrupt enabled status + * @see getIntFreefallEnabled() + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_FF_BIT + **/ +void MPU6050::setIntEnabled(uint8_t enabled) { + I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, enabled); +} +/** Get Free Fall interrupt enabled status. + * Will be set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled status + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_FF_BIT + **/ +bool MPU6050::getIntFreefallEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, buffer); + return buffer[0]; +} +/** Set Free Fall interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntFreefallEnabled() + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_FF_BIT + **/ +void MPU6050::setIntFreefallEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, enabled); +} +/** Get Motion Detection interrupt enabled status. + * Will be set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled status + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_MOT_BIT + **/ +bool MPU6050::getIntMotionEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, buffer); + return buffer[0]; +} +/** Set Motion Detection interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntMotionEnabled() + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_MOT_BIT + **/ +void MPU6050::setIntMotionEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, enabled); +} +/** Get Zero Motion Detection interrupt enabled status. + * Will be set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled status + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_ZMOT_BIT + **/ +bool MPU6050::getIntZeroMotionEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, buffer); + return buffer[0]; +} +/** Set Zero Motion Detection interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntZeroMotionEnabled() + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_ZMOT_BIT + **/ +void MPU6050::setIntZeroMotionEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, enabled); +} +/** Get FIFO Buffer Overflow interrupt enabled status. + * Will be set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled status + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT + **/ +bool MPU6050::getIntFIFOBufferOverflowEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer); + return buffer[0]; +} +/** Set FIFO Buffer Overflow interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntFIFOBufferOverflowEnabled() + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT + **/ +void MPU6050::setIntFIFOBufferOverflowEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled); +} +/** Get I2C Master interrupt enabled status. + * This enables any of the I2C Master interrupt sources to generate an + * interrupt. Will be set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled status + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT + **/ +bool MPU6050::getIntI2CMasterEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer); + return buffer[0]; +} +/** Set I2C Master interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntI2CMasterEnabled() + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT + **/ +void MPU6050::setIntI2CMasterEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, enabled); +} +/** Get Data Ready interrupt enabled setting. + * This event occurs each time a write operation to all of the sensor registers + * has been completed. Will be set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled status + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_DATA_RDY_BIT + */ +bool MPU6050::getIntDataReadyEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer); + return buffer[0]; +} +/** Set Data Ready interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntDataReadyEnabled() + * @see MPU6050_RA_INT_CFG + * @see MPU6050_INTERRUPT_DATA_RDY_BIT + */ +void MPU6050::setIntDataReadyEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, enabled); +} + +// INT_STATUS register + +/** Get full set of interrupt status bits. + * These bits clear to 0 after the register has been read. Very useful + * for getting multiple INT statuses, since each single bit read clears + * all of them because it has to read the whole byte. + * @return Current interrupt status + * @see MPU6050_RA_INT_STATUS + */ +uint8_t MPU6050::getIntStatus() { + I2Cdev::readByte(devAddr, MPU6050_RA_INT_STATUS, buffer); + return buffer[0]; +} +/** Get Free Fall interrupt status. + * This bit automatically sets to 1 when a Free Fall interrupt has been + * generated. The bit clears to 0 after the register has been read. + * @return Current interrupt status + * @see MPU6050_RA_INT_STATUS + * @see MPU6050_INTERRUPT_FF_BIT + */ +bool MPU6050::getIntFreefallStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, buffer); + return buffer[0]; +} +/** Get Motion Detection interrupt status. + * This bit automatically sets to 1 when a Motion Detection interrupt has been + * generated. The bit clears to 0 after the register has been read. + * @return Current interrupt status + * @see MPU6050_RA_INT_STATUS + * @see MPU6050_INTERRUPT_MOT_BIT + */ +bool MPU6050::getIntMotionStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, buffer); + return buffer[0]; +} +/** Get Zero Motion Detection interrupt status. + * This bit automatically sets to 1 when a Zero Motion Detection interrupt has + * been generated. The bit clears to 0 after the register has been read. + * @return Current interrupt status + * @see MPU6050_RA_INT_STATUS + * @see MPU6050_INTERRUPT_ZMOT_BIT + */ +bool MPU6050::getIntZeroMotionStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, buffer); + return buffer[0]; +} +/** Get FIFO Buffer Overflow interrupt status. + * This bit automatically sets to 1 when a Free Fall interrupt has been + * generated. The bit clears to 0 after the register has been read. + * @return Current interrupt status + * @see MPU6050_RA_INT_STATUS + * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT + */ +bool MPU6050::getIntFIFOBufferOverflowStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer); + return buffer[0]; +} +/** Get I2C Master interrupt status. + * This bit automatically sets to 1 when an I2C Master interrupt has been + * generated. For a list of I2C Master interrupts, please refer to Register 54. + * The bit clears to 0 after the register has been read. + * @return Current interrupt status + * @see MPU6050_RA_INT_STATUS + * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT + */ +bool MPU6050::getIntI2CMasterStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer); + return buffer[0]; +} +/** Get Data Ready interrupt status. + * This bit automatically sets to 1 when a Data Ready interrupt has been + * generated. The bit clears to 0 after the register has been read. + * @return Current interrupt status + * @see MPU6050_RA_INT_STATUS + * @see MPU6050_INTERRUPT_DATA_RDY_BIT + */ +bool MPU6050::getIntDataReadyStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer); + return buffer[0]; +} + +// ACCEL_*OUT_* registers + +/** Get raw 9-axis motion sensor readings (accel/gyro/compass). + * FUNCTION NOT FULLY IMPLEMENTED YET. + * @param ax 16-bit signed integer container for accelerometer X-axis value + * @param ay 16-bit signed integer container for accelerometer Y-axis value + * @param az 16-bit signed integer container for accelerometer Z-axis value + * @param gx 16-bit signed integer container for gyroscope X-axis value + * @param gy 16-bit signed integer container for gyroscope Y-axis value + * @param gz 16-bit signed integer container for gyroscope Z-axis value + * @param mx 16-bit signed integer container for magnetometer X-axis value + * @param my 16-bit signed integer container for magnetometer Y-axis value + * @param mz 16-bit signed integer container for magnetometer Z-axis value + * @see getMotion6() + * @see getAcceleration() + * @see getRotation() + * @see MPU6050_RA_ACCEL_XOUT_H + */ +void MPU6050::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) { + getMotion6(ax, ay, az, gx, gy, gz); + // TODO: magnetometer integration +} +/** Get raw 6-axis motion sensor readings (accel/gyro). + * Retrieves all currently available motion sensor values. + * @param ax 16-bit signed integer container for accelerometer X-axis value + * @param ay 16-bit signed integer container for accelerometer Y-axis value + * @param az 16-bit signed integer container for accelerometer Z-axis value + * @param gx 16-bit signed integer container for gyroscope X-axis value + * @param gy 16-bit signed integer container for gyroscope Y-axis value + * @param gz 16-bit signed integer container for gyroscope Z-axis value + * @see getAcceleration() + * @see getRotation() + * @see MPU6050_RA_ACCEL_XOUT_H + */ +void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { + I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer); + *ax = (((int16_t)buffer[0]) << 8) | buffer[1]; + *ay = (((int16_t)buffer[2]) << 8) | buffer[3]; + *az = (((int16_t)buffer[4]) << 8) | buffer[5]; + *gx = (((int16_t)buffer[8]) << 8) | buffer[9]; + *gy = (((int16_t)buffer[10]) << 8) | buffer[11]; + *gz = (((int16_t)buffer[12]) << 8) | buffer[13]; +} +/** Get 3-axis accelerometer readings. + * These registers store the most recent accelerometer measurements. + * Accelerometer measurements are written to these registers at the Sample Rate + * as defined in Register 25. + * + * The accelerometer measurement registers, along with the temperature + * measurement registers, gyroscope measurement registers, and external sensor + * data registers, are composed of two sets of registers: an internal register + * set and a user-facing read register set. + * + * The data within the accelerometer sensors' internal register set is always + * updated at the Sample Rate. Meanwhile, the user-facing read register set + * duplicates the internal register set's data values whenever the serial + * interface is idle. This guarantees that a burst read of sensor registers will + * read measurements from the same sampling instant. Note that if burst reads + * are not used, the user is responsible for ensuring a set of single byte reads + * correspond to a single sampling instant by checking the Data Ready interrupt. + * + * Each 16-bit accelerometer measurement has a full scale defined in ACCEL_FS + * (Register 28). For each full scale setting, the accelerometers' sensitivity + * per LSB in ACCEL_xOUT is shown in the table below: + * + * 
+ * AFS_SEL | Full Scale Range | LSB Sensitivity
+ * --------+------------------+----------------
+ * 0       | +/- 2g           | 8192 LSB/mg
+ * 1       | +/- 4g           | 4096 LSB/mg
+ * 2       | +/- 8g           | 2048 LSB/mg
+ * 3       | +/- 16g          | 1024 LSB/mg
+ *
+ * + * @param x 16-bit signed integer container for X-axis acceleration + * @param y 16-bit signed integer container for Y-axis acceleration + * @param z 16-bit signed integer container for Z-axis acceleration + * @see MPU6050_RA_GYRO_XOUT_H + */ +void MPU6050::getAcceleration(int16_t* x, int16_t* y, int16_t* z) { + I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buffer); + *x = (((int16_t)buffer[0]) << 8) | buffer[1]; + *y = (((int16_t)buffer[2]) << 8) | buffer[3]; + *z = (((int16_t)buffer[4]) << 8) | buffer[5]; +} +/** Get X-axis accelerometer reading. + * @return X-axis acceleration measurement in 16-bit 2's complement format + * @see getMotion6() + * @see MPU6050_RA_ACCEL_XOUT_H + */ +int16_t MPU6050::getAccelerationX() { + I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +/** Get Y-axis accelerometer reading. + * @return Y-axis acceleration measurement in 16-bit 2's complement format + * @see getMotion6() + * @see MPU6050_RA_ACCEL_YOUT_H + */ +int16_t MPU6050::getAccelerationY() { + I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +/** Get Z-axis accelerometer reading. + * @return Z-axis acceleration measurement in 16-bit 2's complement format + * @see getMotion6() + * @see MPU6050_RA_ACCEL_ZOUT_H + */ +int16_t MPU6050::getAccelerationZ() { + I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} + +// TEMP_OUT_* registers + +/** Get current internal temperature. + * @return Temperature reading in 16-bit 2's complement format + * @see MPU6050_RA_TEMP_OUT_H + */ +int16_t MPU6050::getTemperature() { + I2Cdev::readBytes(devAddr, MPU6050_RA_TEMP_OUT_H, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} + +// GYRO_*OUT_* registers + +/** Get 3-axis gyroscope readings. + * These gyroscope measurement registers, along with the accelerometer + * measurement registers, temperature measurement registers, and external sensor + * data registers, are composed of two sets of registers: an internal register + * set and a user-facing read register set. + * The data within the gyroscope sensors' internal register set is always + * updated at the Sample Rate. Meanwhile, the user-facing read register set + * duplicates the internal register set's data values whenever the serial + * interface is idle. This guarantees that a burst read of sensor registers will + * read measurements from the same sampling instant. Note that if burst reads + * are not used, the user is responsible for ensuring a set of single byte reads + * correspond to a single sampling instant by checking the Data Ready interrupt. + * + * Each 16-bit gyroscope measurement has a full scale defined in FS_SEL + * (Register 27). For each full scale setting, the gyroscopes' sensitivity per + * LSB in GYRO_xOUT is shown in the table below: + * + * 
+ * FS_SEL | Full Scale Range   | LSB Sensitivity
+ * -------+--------------------+----------------
+ * 0      | +/- 250 degrees/s  | 131 LSB/deg/s
+ * 1      | +/- 500 degrees/s  | 65.5 LSB/deg/s
+ * 2      | +/- 1000 degrees/s | 32.8 LSB/deg/s
+ * 3      | +/- 2000 degrees/s | 16.4 LSB/deg/s
+ *
+ * + * @param x 16-bit signed integer container for X-axis rotation + * @param y 16-bit signed integer container for Y-axis rotation + * @param z 16-bit signed integer container for Z-axis rotation + * @see getMotion6() + * @see MPU6050_RA_GYRO_XOUT_H + */ +void MPU6050::getRotation(int16_t* x, int16_t* y, int16_t* z) { + I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer); + *x = (((int16_t)buffer[0]) << 8) | buffer[1]; + *y = (((int16_t)buffer[2]) << 8) | buffer[3]; + *z = (((int16_t)buffer[4]) << 8) | buffer[5]; +} +/** Get X-axis gyroscope reading. + * @return X-axis rotation measurement in 16-bit 2's complement format + * @see getMotion6() + * @see MPU6050_RA_GYRO_XOUT_H + */ +int16_t MPU6050::getRotationX() { + I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +/** Get Y-axis gyroscope reading. + * @return Y-axis rotation measurement in 16-bit 2's complement format + * @see getMotion6() + * @see MPU6050_RA_GYRO_YOUT_H + */ +int16_t MPU6050::getRotationY() { + I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_YOUT_H, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +/** Get Z-axis gyroscope reading. + * @return Z-axis rotation measurement in 16-bit 2's complement format + * @see getMotion6() + * @see MPU6050_RA_GYRO_ZOUT_H + */ +int16_t MPU6050::getRotationZ() { + I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} + +// EXT_SENS_DATA_* registers + +/** Read single byte from external sensor data register. + * These registers store data read from external sensors by the Slave 0, 1, 2, + * and 3 on the auxiliary I2C interface. Data read by Slave 4 is stored in + * I2C_SLV4_DI (Register 53). + * + * External sensor data is written to these registers at the Sample Rate as + * defined in Register 25. This access rate can be reduced by using the Slave + * Delay Enable registers (Register 103). + * + * External sensor data registers, along with the gyroscope measurement + * registers, accelerometer measurement registers, and temperature measurement + * registers, are composed of two sets of registers: an internal register set + * and a user-facing read register set. + * + * The data within the external sensors' internal register set is always updated + * at the Sample Rate (or the reduced access rate) whenever the serial interface + * is idle. This guarantees that a burst read of sensor registers will read + * measurements from the same sampling instant. Note that if burst reads are not + * used, the user is responsible for ensuring a set of single byte reads + * correspond to a single sampling instant by checking the Data Ready interrupt. + * + * Data is placed in these external sensor data registers according to + * I2C_SLV0_CTRL, I2C_SLV1_CTRL, I2C_SLV2_CTRL, and I2C_SLV3_CTRL (Registers 39, + * 42, 45, and 48). When more than zero bytes are read (I2C_SLVx_LEN > 0) from + * an enabled slave (I2C_SLVx_EN = 1), the slave is read at the Sample Rate (as + * defined in Register 25) or delayed rate (if specified in Register 52 and + * 103). During each Sample cycle, slave reads are performed in order of Slave + * number. If all slaves are enabled with more than zero bytes to be read, the + * order will be Slave 0, followed by Slave 1, Slave 2, and Slave 3. + * + * Each enabled slave will have EXT_SENS_DATA registers associated with it by + * number of bytes read (I2C_SLVx_LEN) in order of slave number, starting from + * EXT_SENS_DATA_00. Note that this means enabling or disabling a slave may + * change the higher numbered slaves' associated registers. Furthermore, if + * fewer total bytes are being read from the external sensors as a result of + * such a change, then the data remaining in the registers which no longer have + * an associated slave device (i.e. high numbered registers) will remain in + * these previously allocated registers unless reset. + * + * If the sum of the read lengths of all SLVx transactions exceed the number of + * available EXT_SENS_DATA registers, the excess bytes will be dropped. There + * are 24 EXT_SENS_DATA registers and hence the total read lengths between all + * the slaves cannot be greater than 24 or some bytes will be lost. + * + * Note: Slave 4's behavior is distinct from that of Slaves 0-3. For further + * information regarding the characteristics of Slave 4, please refer to + * Registers 49 to 53. + * + * EXAMPLE: + * Suppose that Slave 0 is enabled with 4 bytes to be read (I2C_SLV0_EN = 1 and + * I2C_SLV0_LEN = 4) while Slave 1 is enabled with 2 bytes to be read so that + * I2C_SLV1_EN = 1 and I2C_SLV1_LEN = 2. In such a situation, EXT_SENS_DATA _00 + * through _03 will be associated with Slave 0, while EXT_SENS_DATA _04 and 05 + * will be associated with Slave 1. If Slave 2 is enabled as well, registers + * starting from EXT_SENS_DATA_06 will be allocated to Slave 2. + * + * If Slave 2 is disabled while Slave 3 is enabled in this same situation, then + * registers starting from EXT_SENS_DATA_06 will be allocated to Slave 3 + * instead. + * + * REGISTER ALLOCATION FOR DYNAMIC DISABLE VS. NORMAL DISABLE: + * If a slave is disabled at any time, the space initially allocated to the + * slave in the EXT_SENS_DATA register, will remain associated with that slave. + * This is to avoid dynamic adjustment of the register allocation. + * + * The allocation of the EXT_SENS_DATA registers is recomputed only when (1) all + * slaves are disabled, or (2) the I2C_MST_RST bit is set (Register 106). + * + * This above is also true if one of the slaves gets NACKed and stops + * functioning. + * + * @param position Starting position (0-23) + * @return Byte read from register + */ +uint8_t MPU6050::getExternalSensorByte(int position) { + I2Cdev::readByte(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, buffer); + return buffer[0]; +} +/** Read word (2 bytes) from external sensor data registers. + * @param position Starting position (0-21) + * @return Word read from register + * @see getExternalSensorByte() + */ +uint16_t MPU6050::getExternalSensorWord(int position) { + I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, buffer); + return (((uint16_t)buffer[0]) << 8) | buffer[1]; +} +/** Read double word (4 bytes) from external sensor data registers. + * @param position Starting position (0-20) + * @return Double word read from registers + * @see getExternalSensorByte() + */ +uint32_t MPU6050::getExternalSensorDWord(int position) { + I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, buffer); + return (((uint32_t)buffer[0]) << 24) | (((uint32_t)buffer[1]) << 16) | (((uint16_t)buffer[2]) << 8) | buffer[3]; +} + +// MOT_DETECT_STATUS register + +/** Get full motion detection status register content (all bits). + * @return Motion detection status byte + * @see MPU6050_RA_MOT_DETECT_STATUS + */ +uint8_t MPU6050::getMotionStatus() { + I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DETECT_STATUS, buffer); + return buffer[0]; +} +/** Get X-axis negative motion detection interrupt status. + * @return Motion detection status + * @see MPU6050_RA_MOT_DETECT_STATUS + * @see MPU6050_MOTION_MOT_XNEG_BIT + */ +bool MPU6050::getXNegMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, buffer); + return buffer[0]; +} +/** Get X-axis positive motion detection interrupt status. + * @return Motion detection status + * @see MPU6050_RA_MOT_DETECT_STATUS + * @see MPU6050_MOTION_MOT_XPOS_BIT + */ +bool MPU6050::getXPosMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, buffer); + return buffer[0]; +} +/** Get Y-axis negative motion detection interrupt status. + * @return Motion detection status + * @see MPU6050_RA_MOT_DETECT_STATUS + * @see MPU6050_MOTION_MOT_YNEG_BIT + */ +bool MPU6050::getYNegMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, buffer); + return buffer[0]; +} +/** Get Y-axis positive motion detection interrupt status. + * @return Motion detection status + * @see MPU6050_RA_MOT_DETECT_STATUS + * @see MPU6050_MOTION_MOT_YPOS_BIT + */ +bool MPU6050::getYPosMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, buffer); + return buffer[0]; +} +/** Get Z-axis negative motion detection interrupt status. + * @return Motion detection status + * @see MPU6050_RA_MOT_DETECT_STATUS + * @see MPU6050_MOTION_MOT_ZNEG_BIT + */ +bool MPU6050::getZNegMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, buffer); + return buffer[0]; +} +/** Get Z-axis positive motion detection interrupt status. + * @return Motion detection status + * @see MPU6050_RA_MOT_DETECT_STATUS + * @see MPU6050_MOTION_MOT_ZPOS_BIT + */ +bool MPU6050::getZPosMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, buffer); + return buffer[0]; +} +/** Get zero motion detection interrupt status. + * @return Motion detection status + * @see MPU6050_RA_MOT_DETECT_STATUS + * @see MPU6050_MOTION_MOT_ZRMOT_BIT + */ +bool MPU6050::getZeroMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, buffer); + return buffer[0]; +} + +// I2C_SLV*_DO register + +/** Write byte to Data Output container for specified slave. + * This register holds the output data written into Slave when Slave is set to + * write mode. For further information regarding Slave control, please + * refer to Registers 37 to 39 and immediately following. + * @param num Slave number (0-3) + * @param data Byte to write + * @see MPU6050_RA_I2C_SLV0_DO + */ +void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data) { + if (num > 3) return; + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + num, data); +} + +// I2C_MST_DELAY_CTRL register + +/** Get external data shadow delay enabled status. + * This register is used to specify the timing of external sensor data + * shadowing. When DELAY_ES_SHADOW is set to 1, shadowing of external + * sensor data is delayed until all data has been received. + * @return Current external data shadow delay enabled status. + * @see MPU6050_RA_I2C_MST_DELAY_CTRL + * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT + */ +bool MPU6050::getExternalShadowDelayEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, buffer); + return buffer[0]; +} +/** Set external data shadow delay enabled status. + * @param enabled New external data shadow delay enabled status. + * @see getExternalShadowDelayEnabled() + * @see MPU6050_RA_I2C_MST_DELAY_CTRL + * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT + */ +void MPU6050::setExternalShadowDelayEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled); +} +/** Get slave delay enabled status. + * When a particular slave delay is enabled, the rate of access for the that + * slave device is reduced. When a slave's access rate is decreased relative to + * the Sample Rate, the slave is accessed every: + * + * 1 / (1 + I2C_MST_DLY) Samples + * + * This base Sample Rate in turn is determined by SMPLRT_DIV (register * 25) + * and DLPF_CFG (register 26). + * + * For further information regarding I2C_MST_DLY, please refer to register 52. + * For further information regarding the Sample Rate, please refer to register 25. + * + * @param num Slave number (0-4) + * @return Current slave delay enabled status. + * @see MPU6050_RA_I2C_MST_DELAY_CTRL + * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT + */ +bool MPU6050::getSlaveDelayEnabled(uint8_t num) { + // MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc. + if (num > 4) return 0; + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, buffer); + return buffer[0]; +} +/** Set slave delay enabled status. + * @param num Slave number (0-4) + * @param enabled New slave delay enabled status. + * @see MPU6050_RA_I2C_MST_DELAY_CTRL + * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT + */ +void MPU6050::setSlaveDelayEnabled(uint8_t num, bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled); +} + +// SIGNAL_PATH_RESET register + +/** Reset gyroscope signal path. + * The reset will revert the signal path analog to digital converters and + * filters to their power up configurations. + * @see MPU6050_RA_SIGNAL_PATH_RESET + * @see MPU6050_PATHRESET_GYRO_RESET_BIT + */ +void MPU6050::resetGyroscopePath() { + I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, true); +} +/** Reset accelerometer signal path. + * The reset will revert the signal path analog to digital converters and + * filters to their power up configurations. + * @see MPU6050_RA_SIGNAL_PATH_RESET + * @see MPU6050_PATHRESET_ACCEL_RESET_BIT + */ +void MPU6050::resetAccelerometerPath() { + I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, true); +} +/** Reset temperature sensor signal path. + * The reset will revert the signal path analog to digital converters and + * filters to their power up configurations. + * @see MPU6050_RA_SIGNAL_PATH_RESET + * @see MPU6050_PATHRESET_TEMP_RESET_BIT + */ +void MPU6050::resetTemperaturePath() { + I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, true); +} + +// MOT_DETECT_CTRL register + +/** Get accelerometer power-on delay. + * The accelerometer data path provides samples to the sensor registers, Motion + * detection, Zero Motion detection, and Free Fall detection modules. The + * signal path contains filters which must be flushed on wake-up with new + * samples before the detection modules begin operations. The default wake-up + * delay, of 4ms can be lengthened by up to 3ms. This additional delay is + * specified in ACCEL_ON_DELAY in units of 1 LSB = 1 ms. The user may select + * any value above zero unless instructed otherwise by InvenSense. Please refer + * to Section 8 of the MPU-6000/MPU-6050 Product Specification document for + * further information regarding the detection modules. + * @return Current accelerometer power-on delay + * @see MPU6050_RA_MOT_DETECT_CTRL + * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT + */ +uint8_t MPU6050::getAccelerometerPowerOnDelay() { + I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, buffer); + return buffer[0]; +} +/** Set accelerometer power-on delay. + * @param delay New accelerometer power-on delay (0-3) + * @see getAccelerometerPowerOnDelay() + * @see MPU6050_RA_MOT_DETECT_CTRL + * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT + */ +void MPU6050::setAccelerometerPowerOnDelay(uint8_t delay) { + I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay); +} +/** Get Free Fall detection counter decrement configuration. + * Detection is registered by the Free Fall detection module after accelerometer + * measurements meet their respective threshold conditions over a specified + * number of samples. When the threshold conditions are met, the corresponding + * detection counter increments by 1. The user may control the rate at which the + * detection counter decrements when the threshold condition is not met by + * configuring FF_COUNT. The decrement rate can be set according to the + * following table: + * + * 
+ * FF_COUNT | Counter Decrement
+ * ---------+------------------
+ * 0        | Reset
+ * 1        | 1
+ * 2        | 2
+ * 3        | 4
+ *
+ * + * When FF_COUNT is configured to 0 (reset), any non-qualifying sample will + * reset the counter to 0. For further information on Free Fall detection, + * please refer to Registers 29 to 32. + * + * @return Current decrement configuration + * @see MPU6050_RA_MOT_DETECT_CTRL + * @see MPU6050_DETECT_FF_COUNT_BIT + */ +uint8_t MPU6050::getFreefallDetectionCounterDecrement() { + I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, buffer); + return buffer[0]; +} +/** Set Free Fall detection counter decrement configuration. + * @param decrement New decrement configuration value + * @see getFreefallDetectionCounterDecrement() + * @see MPU6050_RA_MOT_DETECT_CTRL + * @see MPU6050_DETECT_FF_COUNT_BIT + */ +void MPU6050::setFreefallDetectionCounterDecrement(uint8_t decrement) { + I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, decrement); +} +/** Get Motion detection counter decrement configuration. + * Detection is registered by the Motion detection module after accelerometer + * measurements meet their respective threshold conditions over a specified + * number of samples. When the threshold conditions are met, the corresponding + * detection counter increments by 1. The user may control the rate at which the + * detection counter decrements when the threshold condition is not met by + * configuring MOT_COUNT. The decrement rate can be set according to the + * following table: + * + * 
+ * MOT_COUNT | Counter Decrement
+ * ----------+------------------
+ * 0         | Reset
+ * 1         | 1
+ * 2         | 2
+ * 3         | 4
+ *
+ * + * When MOT_COUNT is configured to 0 (reset), any non-qualifying sample will + * reset the counter to 0. For further information on Motion detection, + * please refer to Registers 29 to 32. + * + */ +uint8_t MPU6050::getMotionDetectionCounterDecrement() { + I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, buffer); + return buffer[0]; +} +/** Set Motion detection counter decrement configuration. + * @param decrement New decrement configuration value + * @see getMotionDetectionCounterDecrement() + * @see MPU6050_RA_MOT_DETECT_CTRL + * @see MPU6050_DETECT_MOT_COUNT_BIT + */ +void MPU6050::setMotionDetectionCounterDecrement(uint8_t decrement) { + I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, decrement); +} + +// USER_CTRL register + +/** Get FIFO enabled status. + * When this bit is set to 0, the FIFO buffer is disabled. The FIFO buffer + * cannot be written to or read from while disabled. The FIFO buffer's state + * does not change unless the MPU-60X0 is power cycled. + * @return Current FIFO enabled status + * @see MPU6050_RA_USER_CTRL + * @see MPU6050_USERCTRL_FIFO_EN_BIT + */ +bool MPU6050::getFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set FIFO enabled status. + * @param enabled New FIFO enabled status + * @see getFIFOEnabled() + * @see MPU6050_RA_USER_CTRL + * @see MPU6050_USERCTRL_FIFO_EN_BIT + */ +void MPU6050::setFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled); +} +/** Get I2C Master Mode enabled status. + * When this mode is enabled, the MPU-60X0 acts as the I2C Master to the + * external sensor slave devices on the auxiliary I2C bus. When this bit is + * cleared to 0, the auxiliary I2C bus lines (AUX_DA and AUX_CL) are logically + * driven by the primary I2C bus (SDA and SCL). This is a precondition to + * enabling Bypass Mode. For further information regarding Bypass Mode, please + * refer to Register 55. + * @return Current I2C Master Mode enabled status + * @see MPU6050_RA_USER_CTRL + * @see MPU6050_USERCTRL_I2C_MST_EN_BIT + */ +bool MPU6050::getI2CMasterModeEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, buffer); + return buffer[0]; +} +/** Set I2C Master Mode enabled status. + * @param enabled New I2C Master Mode enabled status + * @see getI2CMasterModeEnabled() + * @see MPU6050_RA_USER_CTRL + * @see MPU6050_USERCTRL_I2C_MST_EN_BIT + */ +void MPU6050::setI2CMasterModeEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled); +} +/** Switch from I2C to SPI mode (MPU-6000 only) + * If this is set, the primary SPI interface will be enabled in place of the + * disabled primary I2C interface. + */ +void MPU6050::switchSPIEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled); +} +/** Reset the FIFO. + * This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This + * bit automatically clears to 0 after the reset has been triggered. + * @see MPU6050_RA_USER_CTRL + * @see MPU6050_USERCTRL_FIFO_RESET_BIT + */ +void MPU6050::resetFIFO() { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, true); +} +/** Reset the I2C Master. + * This bit resets the I2C Master when set to 1 while I2C_MST_EN equals 0. + * This bit automatically clears to 0 after the reset has been triggered. + * @see MPU6050_RA_USER_CTRL + * @see MPU6050_USERCTRL_I2C_MST_RESET_BIT + */ +void MPU6050::resetI2CMaster() { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, true); +} +/** Reset all sensor registers and signal paths. + * When set to 1, this bit resets the signal paths for all sensors (gyroscopes, + * accelerometers, and temperature sensor). This operation will also clear the + * sensor registers. This bit automatically clears to 0 after the reset has been + * triggered. + * + * When resetting only the signal path (and not the sensor registers), please + * use Register 104, SIGNAL_PATH_RESET. + * + * @see MPU6050_RA_USER_CTRL + * @see MPU6050_USERCTRL_SIG_COND_RESET_BIT + */ +void MPU6050::resetSensors() { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, true); +} + +// PWR_MGMT_1 register + +/** Trigger a full device reset. + * A small delay of ~50ms may be desirable after triggering a reset. + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_DEVICE_RESET_BIT + */ +void MPU6050::reset() { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true); +} +/** Get sleep mode status. + * Setting the SLEEP bit in the register puts the device into very low power + * sleep mode. In this mode, only the serial interface and internal registers + * remain active, allowing for a very low standby current. Clearing this bit + * puts the device back into normal mode. To save power, the individual standby + * selections for each of the gyros should be used if any gyro axis is not used + * by the application. + * @return Current sleep mode enabled status + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_SLEEP_BIT + */ +bool MPU6050::getSleepEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, buffer); + return buffer[0]; +} +/** Set sleep mode status. + * @param enabled New sleep mode enabled status + * @see getSleepEnabled() + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_SLEEP_BIT + */ +void MPU6050::setSleepEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled); +} +/** Get wake cycle enabled status. + * When this bit is set to 1 and SLEEP is disabled, the MPU-60X0 will cycle + * between sleep mode and waking up to take a single sample of data from active + * sensors at a rate determined by LP_WAKE_CTRL (register 108). + * @return Current sleep mode enabled status + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_CYCLE_BIT + */ +bool MPU6050::getWakeCycleEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, buffer); + return buffer[0]; +} +/** Set wake cycle enabled status. + * @param enabled New sleep mode enabled status + * @see getWakeCycleEnabled() + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_CYCLE_BIT + */ +void MPU6050::setWakeCycleEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled); +} +/** Get temperature sensor enabled status. + * Control the usage of the internal temperature sensor. + * + * Note: this register stores the *disabled* value, but for consistency with the + * rest of the code, the function is named and used with standard true/false + * values to indicate whether the sensor is enabled or disabled, respectively. + * + * @return Current temperature sensor enabled status + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_TEMP_DIS_BIT + */ +bool MPU6050::getTempSensorEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, buffer); + return buffer[0] == 0; // 1 is actually disabled here +} +/** Set temperature sensor enabled status. + * Note: this register stores the *disabled* value, but for consistency with the + * rest of the code, the function is named and used with standard true/false + * values to indicate whether the sensor is enabled or disabled, respectively. + * + * @param enabled New temperature sensor enabled status + * @see getTempSensorEnabled() + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_TEMP_DIS_BIT + */ +void MPU6050::setTempSensorEnabled(bool enabled) { + // 1 is actually disabled here + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled); +} +/** Get clock source setting. + * @return Current clock source setting + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_CLKSEL_BIT + * @see MPU6050_PWR1_CLKSEL_LENGTH + */ +uint8_t MPU6050::getClockSource() { + I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, buffer); + return buffer[0]; +} +/** Set clock source setting. + * An internal 8MHz oscillator, gyroscope based clock, or external sources can + * be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillator + * or an external source is chosen as the clock source, the MPU-60X0 can operate + * in low power modes with the gyroscopes disabled. + * + * Upon power up, the MPU-60X0 clock source defaults to the internal oscillator. + * However, it is highly recommended that the device be configured to use one of + * the gyroscopes (or an external clock source) as the clock reference for + * improved stability. The clock source can be selected according to the following table: + * + * 
+ * CLK_SEL | Clock Source
+ * --------+--------------------------------------
+ * 0       | Internal oscillator
+ * 1       | PLL with X Gyro reference
+ * 2       | PLL with Y Gyro reference
+ * 3       | PLL with Z Gyro reference
+ * 4       | PLL with external 32.768kHz reference
+ * 5       | PLL with external 19.2MHz reference
+ * 6       | Reserved
+ * 7       | Stops the clock and keeps the timing generator in reset
+ *
+ * + * @param source New clock source setting + * @see getClockSource() + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_CLKSEL_BIT + * @see MPU6050_PWR1_CLKSEL_LENGTH + */ +void MPU6050::setClockSource(uint8_t source) { + I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source); +} + +// PWR_MGMT_2 register + +/** Get wake frequency in Accel-Only Low Power Mode. + * The MPU-60X0 can be put into Accerlerometer Only Low Power Mode by setting + * PWRSEL to 1 in the Power Management 1 register (Register 107). In this mode, + * the device will power off all devices except for the primary I2C interface, + * waking only the accelerometer at fixed intervals to take a single + * measurement. The frequency of wake-ups can be configured with LP_WAKE_CTRL + * as shown below: + * + * 
+ * LP_WAKE_CTRL | Wake-up Frequency
+ * -------------+------------------
+ * 0            | 1.25 Hz
+ * 1            | 2.5 Hz
+ * 2            | 5 Hz
+ * 3            | 10 Hz
+ *
+ * + * For further information regarding the MPU-60X0's power modes, please refer to + * Register 107. + * + * @return Current wake frequency + * @see MPU6050_RA_PWR_MGMT_2 + */ +uint8_t MPU6050::getWakeFrequency() { + I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, buffer); + return buffer[0]; +} +/** Set wake frequency in Accel-Only Low Power Mode. + * @param frequency New wake frequency + * @see MPU6050_RA_PWR_MGMT_2 + */ +void MPU6050::setWakeFrequency(uint8_t frequency) { + I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency); +} + +/** Get X-axis accelerometer standby enabled status. + * If enabled, the X-axis will not gather or report data (or use power). + * @return Current X-axis standby enabled status + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_XA_BIT + */ +bool MPU6050::getStandbyXAccelEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, buffer); + return buffer[0]; +} +/** Set X-axis accelerometer standby enabled status. + * @param New X-axis standby enabled status + * @see getStandbyXAccelEnabled() + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_XA_BIT + */ +void MPU6050::setStandbyXAccelEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, enabled); +} +/** Get Y-axis accelerometer standby enabled status. + * If enabled, the Y-axis will not gather or report data (or use power). + * @return Current Y-axis standby enabled status + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_YA_BIT + */ +bool MPU6050::getStandbyYAccelEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, buffer); + return buffer[0]; +} +/** Set Y-axis accelerometer standby enabled status. + * @param New Y-axis standby enabled status + * @see getStandbyYAccelEnabled() + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_YA_BIT + */ +void MPU6050::setStandbyYAccelEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, enabled); +} +/** Get Z-axis accelerometer standby enabled status. + * If enabled, the Z-axis will not gather or report data (or use power). + * @return Current Z-axis standby enabled status + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_ZA_BIT + */ +bool MPU6050::getStandbyZAccelEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, buffer); + return buffer[0]; +} +/** Set Z-axis accelerometer standby enabled status. + * @param New Z-axis standby enabled status + * @see getStandbyZAccelEnabled() + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_ZA_BIT + */ +void MPU6050::setStandbyZAccelEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, enabled); +} +/** Get X-axis gyroscope standby enabled status. + * If enabled, the X-axis will not gather or report data (or use power). + * @return Current X-axis standby enabled status + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_XG_BIT + */ +bool MPU6050::getStandbyXGyroEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, buffer); + return buffer[0]; +} +/** Set X-axis gyroscope standby enabled status. + * @param New X-axis standby enabled status + * @see getStandbyXGyroEnabled() + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_XG_BIT + */ +void MPU6050::setStandbyXGyroEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, enabled); +} +/** Get Y-axis gyroscope standby enabled status. + * If enabled, the Y-axis will not gather or report data (or use power). + * @return Current Y-axis standby enabled status + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_YG_BIT + */ +bool MPU6050::getStandbyYGyroEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, buffer); + return buffer[0]; +} +/** Set Y-axis gyroscope standby enabled status. + * @param New Y-axis standby enabled status + * @see getStandbyYGyroEnabled() + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_YG_BIT + */ +void MPU6050::setStandbyYGyroEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, enabled); +} +/** Get Z-axis gyroscope standby enabled status. + * If enabled, the Z-axis will not gather or report data (or use power). + * @return Current Z-axis standby enabled status + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_ZG_BIT + */ +bool MPU6050::getStandbyZGyroEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, buffer); + return buffer[0]; +} +/** Set Z-axis gyroscope standby enabled status. + * @param New Z-axis standby enabled status + * @see getStandbyZGyroEnabled() + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_ZG_BIT + */ +void MPU6050::setStandbyZGyroEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled); +} + +// FIFO_COUNT* registers + +/** Get current FIFO buffer size. + * This value indicates the number of bytes stored in the FIFO buffer. This + * number is in turn the number of bytes that can be read from the FIFO buffer + * and it is directly proportional to the number of samples available given the + * set of sensor data bound to be stored in the FIFO (register 35 and 36). + * @return Current FIFO buffer size + */ +uint16_t MPU6050::getFIFOCount() { + I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer); + return (((uint16_t)buffer[0]) << 8) | buffer[1]; +} + +// FIFO_R_W register + +/** Get byte from FIFO buffer. + * This register is used to read and write data from the FIFO buffer. Data is + * written to the FIFO in order of register number (from lowest to highest). If + * all the FIFO enable flags (see below) are enabled and all External Sensor + * Data registers (Registers 73 to 96) are associated with a Slave device, the + * contents of registers 59 through 96 will be written in order at the Sample + * Rate. + * + * The contents of the sensor data registers (Registers 59 to 96) are written + * into the FIFO buffer when their corresponding FIFO enable flags are set to 1 + * in FIFO_EN (Register 35). An additional flag for the sensor data registers + * associated with I2C Slave 3 can be found in I2C_MST_CTRL (Register 36). + * + * If the FIFO buffer has overflowed, the status bit FIFO_OFLOW_INT is + * automatically set to 1. This bit is located in INT_STATUS (Register 58). + * When the FIFO buffer has overflowed, the oldest data will be lost and new + * data will be written to the FIFO. + * + * If the FIFO buffer is empty, reading this register will return the last byte + * that was previously read from the FIFO until new data is available. The user + * should check FIFO_COUNT to ensure that the FIFO buffer is not read when + * empty. + * + * @return Byte from FIFO buffer + */ +uint8_t MPU6050::getFIFOByte() { + I2Cdev::readByte(devAddr, MPU6050_RA_FIFO_R_W, buffer); + return buffer[0]; +} +void MPU6050::getFIFOBytes(uint8_t *data, uint8_t length) { + if(length > 0){ + I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data); + } else { + *data = 0; + } +} +/** Write byte to FIFO buffer. + * @see getFIFOByte() + * @see MPU6050_RA_FIFO_R_W + */ +void MPU6050::setFIFOByte(uint8_t data) { + I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data); +} + +// WHO_AM_I register + +/** Get Device ID. + * This register is used to verify the identity of the device (0b110100, 0x34). + * @return Device ID (6 bits only! should be 0x34) + * @see MPU6050_RA_WHO_AM_I + * @see MPU6050_WHO_AM_I_BIT + * @see MPU6050_WHO_AM_I_LENGTH + */ +uint8_t MPU6050::getDeviceID() { + I2Cdev::readBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer); + return buffer[0]; +} +/** Set Device ID. + * Write a new ID into the WHO_AM_I register (no idea why this should ever be + * necessary though). + * @param id New device ID to set. + * @see getDeviceID() + * @see MPU6050_RA_WHO_AM_I + * @see MPU6050_WHO_AM_I_BIT + * @see MPU6050_WHO_AM_I_LENGTH + */ +void MPU6050::setDeviceID(uint8_t id) { + I2Cdev::writeBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, id); +} + +// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== + +// XG_OFFS_TC register + +uint8_t MPU6050::getOTPBankValid() { + I2Cdev::readBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer); + return buffer[0]; +} +void MPU6050::setOTPBankValid(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled); +} +int8_t MPU6050::getXGyroOffsetTC() { + I2Cdev::readBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer); + return buffer[0]; +} +void MPU6050::setXGyroOffsetTC(int8_t offset) { + I2Cdev::writeBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); +} + +// YG_OFFS_TC register + +int8_t MPU6050::getYGyroOffsetTC() { + I2Cdev::readBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer); + return buffer[0]; +} +void MPU6050::setYGyroOffsetTC(int8_t offset) { + I2Cdev::writeBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); +} + +// ZG_OFFS_TC register + +int8_t MPU6050::getZGyroOffsetTC() { + I2Cdev::readBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer); + return buffer[0]; +} +void MPU6050::setZGyroOffsetTC(int8_t offset) { + I2Cdev::writeBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); +} + +// X_FINE_GAIN register + +int8_t MPU6050::getXFineGain() { + I2Cdev::readByte(devAddr, MPU6050_RA_X_FINE_GAIN, buffer); + return buffer[0]; +} +void MPU6050::setXFineGain(int8_t gain) { + I2Cdev::writeByte(devAddr, MPU6050_RA_X_FINE_GAIN, gain); +} + +// Y_FINE_GAIN register + +int8_t MPU6050::getYFineGain() { + I2Cdev::readByte(devAddr, MPU6050_RA_Y_FINE_GAIN, buffer); + return buffer[0]; +} +void MPU6050::setYFineGain(int8_t gain) { + I2Cdev::writeByte(devAddr, MPU6050_RA_Y_FINE_GAIN, gain); +} + +// Z_FINE_GAIN register + +int8_t MPU6050::getZFineGain() { + I2Cdev::readByte(devAddr, MPU6050_RA_Z_FINE_GAIN, buffer); + return buffer[0]; +} +void MPU6050::setZFineGain(int8_t gain) { + I2Cdev::writeByte(devAddr, MPU6050_RA_Z_FINE_GAIN, gain); +} + +// XA_OFFS_* registers + +int16_t MPU6050::getXAccelOffset() { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +void MPU6050::setXAccelOffset(int16_t offset) { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::writeWord(devAddr, SaveAddress, offset); +} + +// YA_OFFS_* register + +int16_t MPU6050::getYAccelOffset() { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_YA_OFFS_H:0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +void MPU6050::setYAccelOffset(int16_t offset) { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_YA_OFFS_H:0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::writeWord(devAddr, SaveAddress, offset); +} + +// ZA_OFFS_* register + +int16_t MPU6050::getZAccelOffset() { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_ZA_OFFS_H:0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +void MPU6050::setZAccelOffset(int16_t offset) { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_ZA_OFFS_H:0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::writeWord(devAddr, SaveAddress, offset); +} + +// XG_OFFS_USR* registers + +int16_t MPU6050::getXGyroOffset() { + I2Cdev::readBytes(devAddr, MPU6050_RA_XG_OFFS_USRH, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +void MPU6050::setXGyroOffset(int16_t offset) { + I2Cdev::writeWord(devAddr, MPU6050_RA_XG_OFFS_USRH, offset); +} + +// YG_OFFS_USR* register + +int16_t MPU6050::getYGyroOffset() { + I2Cdev::readBytes(devAddr, MPU6050_RA_YG_OFFS_USRH, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +void MPU6050::setYGyroOffset(int16_t offset) { + I2Cdev::writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset); +} + +// ZG_OFFS_USR* register + +int16_t MPU6050::getZGyroOffset() { + I2Cdev::readBytes(devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +void MPU6050::setZGyroOffset(int16_t offset) { + I2Cdev::writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset); +} + +// INT_ENABLE register (DMP functions) + +bool MPU6050::getIntPLLReadyEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer); + return buffer[0]; +} +void MPU6050::setIntPLLReadyEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled); +} +bool MPU6050::getIntDMPEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, buffer); + return buffer[0]; +} +void MPU6050::setIntDMPEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled); +} + +// DMP_INT_STATUS + +bool MPU6050::getDMPInt5Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer); + return buffer[0]; +} +bool MPU6050::getDMPInt4Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer); + return buffer[0]; +} +bool MPU6050::getDMPInt3Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, buffer); + return buffer[0]; +} +bool MPU6050::getDMPInt2Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer); + return buffer[0]; +} +bool MPU6050::getDMPInt1Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, buffer); + return buffer[0]; +} +bool MPU6050::getDMPInt0Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, buffer); + return buffer[0]; +} + +// INT_STATUS register (DMP functions) + +bool MPU6050::getIntPLLReadyStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer); + return buffer[0]; +} +bool MPU6050::getIntDMPStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, buffer); + return buffer[0]; +} + +// USER_CTRL register (DMP functions) + +bool MPU6050::getDMPEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer); + return buffer[0]; +} +void MPU6050::setDMPEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled); +} +void MPU6050::resetDMP() { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true); +} + +// BANK_SEL register + +void MPU6050::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) { + bank &= 0x1F; + if (userBank) bank |= 0x20; + if (prefetchEnabled) bank |= 0x40; + I2Cdev::writeByte(devAddr, MPU6050_RA_BANK_SEL, bank); +} + +// MEM_START_ADDR register + +void MPU6050::setMemoryStartAddress(uint8_t address) { + I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address); +} + +// MEM_R_W register + +uint8_t MPU6050::readMemoryByte() { + I2Cdev::readByte(devAddr, MPU6050_RA_MEM_R_W, buffer); + return buffer[0]; +} +void MPU6050::writeMemoryByte(uint8_t data) { + I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_R_W, data); +} +void MPU6050::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) { + setMemoryBank(bank); + setMemoryStartAddress(address); + uint8_t chunkSize; + for (uint16_t i = 0; i < dataSize;) { + // determine correct chunk size according to bank position and data size + chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE; + + // make sure we don't go past the data size + if (i + chunkSize > dataSize) chunkSize = dataSize - i; + + // make sure this chunk doesn't go past the bank boundary (256 bytes) + if (chunkSize > 256 - address) chunkSize = 256 - address; + + // read the chunk of data as specified + I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i); + + // increase byte index by [chunkSize] + i += chunkSize; + + // uint8_t automatically wraps to 0 at 256 + address += chunkSize; + + // if we aren't done, update bank (if necessary) and address + if (i < dataSize) { + if (address == 0) bank++; + setMemoryBank(bank); + setMemoryStartAddress(address); + } + } +} +bool MPU6050::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) { + setMemoryBank(bank); + setMemoryStartAddress(address); + uint8_t chunkSize; + uint8_t *verifyBuffer=0; + uint8_t *progBuffer=0; + uint16_t i; + uint8_t j; + if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); + if (useProgMem) progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); + for (i = 0; i < dataSize;) { + // determine correct chunk size according to bank position and data size + chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE; + + // make sure we don't go past the data size + if (i + chunkSize > dataSize) chunkSize = dataSize - i; + + // make sure this chunk doesn't go past the bank boundary (256 bytes) + if (chunkSize > 256 - address) chunkSize = 256 - address; + + if (useProgMem) { + // write the chunk of data as specified + for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j); + } else { + // write the chunk of data as specified + progBuffer = (uint8_t *)data + i; + } + + I2Cdev::writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer); + + // verify data if needed + if (verify && verifyBuffer) { + setMemoryBank(bank); + setMemoryStartAddress(address); + I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer); + if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) { + /*Serial.print("Block write verification error, bank "); + Serial.print(bank, DEC); + Serial.print(", address "); + Serial.print(address, DEC); + Serial.print("!\nExpected:"); + for (j = 0; j < chunkSize; j++) { + Serial.print(" 0x"); + if (progBuffer[j] < 16) Serial.print("0"); + Serial.print(progBuffer[j], HEX); + } + Serial.print("\nReceived:"); + for (uint8_t j = 0; j < chunkSize; j++) { + Serial.print(" 0x"); + if (verifyBuffer[i + j] < 16) Serial.print("0"); + Serial.print(verifyBuffer[i + j], HEX); + } + Serial.print("\n");*/ + free(verifyBuffer); + if (useProgMem) free(progBuffer); + return false; // uh oh. + } + } + + // increase byte index by [chunkSize] + i += chunkSize; + + // uint8_t automatically wraps to 0 at 256 + address += chunkSize; + + // if we aren't done, update bank (if necessary) and address + if (i < dataSize) { + if (address == 0) bank++; + setMemoryBank(bank); + setMemoryStartAddress(address); + } + } + if (verify) free(verifyBuffer); + if (useProgMem) free(progBuffer); + return true; +} +bool MPU6050::writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) { + return writeMemoryBlock(data, dataSize, bank, address, verify, true); +} +bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) { + uint8_t *progBuffer = 0; + uint8_t success, special; + uint16_t i, j; + if (useProgMem) { + progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary + } + + // config set data is a long string of blocks with the following structure: + // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]] + uint8_t bank, offset, length; + for (i = 0; i < dataSize;) { + if (useProgMem) { + bank = pgm_read_byte(data + i++); + offset = pgm_read_byte(data + i++); + length = pgm_read_byte(data + i++); + } else { + bank = data[i++]; + offset = data[i++]; + length = data[i++]; + } + + // write data or perform special action + if (length > 0) { + // regular block of data to write + /*Serial.print("Writing config block to bank "); + Serial.print(bank); + Serial.print(", offset "); + Serial.print(offset); + Serial.print(", length="); + Serial.println(length);*/ + if (useProgMem) { + if (sizeof(progBuffer) < length) progBuffer = (uint8_t *)realloc(progBuffer, length); + for (j = 0; j < length; j++) progBuffer[j] = pgm_read_byte(data + i + j); + } else { + progBuffer = (uint8_t *)data + i; + } + success = writeMemoryBlock(progBuffer, length, bank, offset, true); + i += length; + } else { + // special instruction + // NOTE: this kind of behavior (what and when to do certain things) + // is totally undocumented. This code is in here based on observed + // behavior only, and exactly why (or even whether) it has to be here + // is anybody's guess for now. + if (useProgMem) { + special = pgm_read_byte(data + i++); + } else { + special = data[i++]; + } + /*Serial.print("Special command code "); + Serial.print(special, HEX); + Serial.println(" found...");*/ + if (special == 0x01) { + // enable DMP-related interrupts + + //setIntZeroMotionEnabled(true); + //setIntFIFOBufferOverflowEnabled(true); + //setIntDMPEnabled(true); + I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, 0x32); // single operation + + success = true; + } else { + // unknown special command + success = false; + } + } + + if (!success) { + if (useProgMem) free(progBuffer); + return false; // uh oh + } + } + if (useProgMem) free(progBuffer); + return true; +} +bool MPU6050::writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) { + return writeDMPConfigurationSet(data, dataSize, true); +} + +// DMP_CFG_1 register + +uint8_t MPU6050::getDMPConfig1() { + I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_1, buffer); + return buffer[0]; +} +void MPU6050::setDMPConfig1(uint8_t config) { + I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config); +} + +// DMP_CFG_2 register + +uint8_t MPU6050::getDMPConfig2() { + I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_2, buffer); + return buffer[0]; +} +void MPU6050::setDMPConfig2(uint8_t config) { + I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config); +} + + +//*************************************************************************************** +//********************** Calibration Routines ********************** +//*************************************************************************************** +/** + @brief Fully calibrate Gyro from ZERO in about 6-7 Loops 600-700 readings +*/ +void MPU6050::CalibrateGyro(uint8_t Loops ) { + double kP = 0.3; + double kI = 90; + float x; + x = (100 - map(Loops, 1, 5, 20, 0)) * .01; + kP *= x; + kI *= x; + + PID( 0x43, kP, kI, Loops); +} + +/** + @brief Fully calibrate Accel from ZERO in about 6-7 Loops 600-700 readings +*/ +void MPU6050::CalibrateAccel(uint8_t Loops ) { + + float kP = 0.3; + float kI = 20; + float x; + x = (100 - map(Loops, 1, 5, 20, 0)) * .01; + kP *= x; + kI *= x; + PID( 0x3B, kP, kI, Loops); +} + +void MPU6050::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){ + uint8_t SaveAddress = (ReadAddress == 0x3B)?((getDeviceID() < 0x38 )? 0x06:0x77):0x13; + + int16_t Data; + float Reading; + int16_t BitZero[3]; + uint8_t shift =(SaveAddress == 0x77)?3:2; + float Error, PTerm, ITerm[3]; + int16_t eSample; + uint32_t eSum ; + Serial.write('>'); + for (int i = 0; i < 3; i++) { + I2Cdev::readWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word) + Reading = Data; + if(SaveAddress != 0x13){ + BitZero[i] = Data & 1; // Capture Bit Zero to properly handle Accelerometer calibration + ITerm[i] = ((float)Reading) * 8; + } else { + ITerm[i] = Reading * 4; + } + } + for (int L = 0; L < Loops; L++) { + eSample = 0; + for (int c = 0; c < 100; c++) {// 100 PI Calculations + eSum = 0; + for (int i = 0; i < 3; i++) { + I2Cdev::readWords(devAddr, ReadAddress + (i * 2), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word) + Reading = Data; + if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384; //remove Gravity + Error = -Reading; + eSum += abs(Reading); + PTerm = kP * Error; + ITerm[i] += (Error * 0.001) * kI; // Integral term 1000 Calculations a second = 0.001 + if(SaveAddress != 0x13){ + Data = round((PTerm + ITerm[i] ) / 8); //Compute PID Output + Data = ((Data)&0xFFFE) |BitZero[i]; // Insert Bit0 Saved at beginning + } else Data = round((PTerm + ITerm[i] ) / 4); //Compute PID Output + I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data); + } + if((c == 99) && eSum > 1000){ // Error is still to great to continue + c = 0; + Serial.write('*'); + } + if((eSum * ((ReadAddress == 0x3B)?.05: 1)) < 5) eSample++; // Successfully found offsets prepare to advance + if((eSum < 100) && (c > 10) && (eSample >= 10)) break; // Advance to next Loop + delay(1); + } + Serial.write('.'); + kP *= .75; + kI *= .75; + for (int i = 0; i < 3; i++){ + if(SaveAddress != 0x13) { + Data = round((ITerm[i] ) / 8); //Compute PID Output + Data = ((Data)&0xFFFE) |BitZero[i]; // Insert Bit0 Saved at beginning + } else Data = round((ITerm[i]) / 4); + I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data); + } + } + resetFIFO(); + resetDMP(); +} + +#define printfloatx(Name,Variable,Spaces,Precision,EndTxt) { Serial.print(F(Name)); {char S[(Spaces + Precision + 3)];Serial.print(F(" ")); Serial.print(dtostrf((float)Variable,Spaces,Precision ,S));}Serial.print(F(EndTxt)); }//Name,Variable,Spaces,Precision,EndTxt +void MPU6050::PrintActiveOffsets() { + uint8_t AOffsetRegister = (getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77; + int16_t Data[3]; + //Serial.print(F("Offset Register 0x")); + //Serial.print(AOffsetRegister>>4,HEX);Serial.print(AOffsetRegister&0x0F,HEX); + Serial.print(F("\n// X Accel Y Accel Z Accel X Gyro Y Gyro Z Gyro\n//OFFSETS ")); + if(AOffsetRegister == 0x06) I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)Data); + else { + I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)Data); + I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)Data+1); + I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)Data+2); + } + // A_OFFSET_H_READ_A_OFFS(Data); + printfloatx("", Data[0], 5, 0, ", "); + printfloatx("", Data[1], 5, 0, ", "); + printfloatx("", Data[2], 5, 0, ", "); + I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)Data); + // XG_OFFSET_H_READ_OFFS_USR(Data); + printfloatx("", Data[0], 5, 0, ", "); + printfloatx("", Data[1], 5, 0, ", "); + printfloatx("", Data[2], 5, 0, "\n"); +} diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050.h b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050.h new file mode 100644 index 00000000..871f423a --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050.h @@ -0,0 +1,1041 @@ +// I2Cdev library collection - MPU6050 I2C device class +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 10/3/2011 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// ... - ongoing debug release + +// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE +// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF +// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING. + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _MPU6050_H_ +#define _MPU6050_H_ + +#include "I2Cdev.h" + +// supporting link: http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517 +// also: http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s + +#ifdef __AVR__ +#include +#elif defined(ARDUINO_ARCH_SAMD) +#include +#else +//#define PROGMEM /* empty */ +//#define pgm_read_byte(x) (*(x)) +//#define pgm_read_word(x) (*(x)) +//#define pgm_read_float(x) (*(x)) +//#define PSTR(STR) STR +#endif + + +#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board +#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) +#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW + +#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN +#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN +#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN +#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS +#define MPU6050_RA_XA_OFFS_L_TC 0x07 +#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS +#define MPU6050_RA_YA_OFFS_L_TC 0x09 +#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS +#define MPU6050_RA_ZA_OFFS_L_TC 0x0B +#define MPU6050_RA_SELF_TEST_X 0x0D //[7:5] XA_TEST[4-2], [4:0] XG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_Y 0x0E //[7:5] YA_TEST[4-2], [4:0] YG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_Z 0x0F //[7:5] ZA_TEST[4-2], [4:0] ZG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_A 0x10 //[5:4] XA_TEST[1-0], [3:2] YA_TEST[1-0], [1:0] ZA_TEST[1-0] +#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR +#define MPU6050_RA_XG_OFFS_USRL 0x14 +#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR +#define MPU6050_RA_YG_OFFS_USRL 0x16 +#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR +#define MPU6050_RA_ZG_OFFS_USRL 0x18 +#define MPU6050_RA_SMPLRT_DIV 0x19 +#define MPU6050_RA_CONFIG 0x1A +#define MPU6050_RA_GYRO_CONFIG 0x1B +#define MPU6050_RA_ACCEL_CONFIG 0x1C +#define MPU6050_RA_FF_THR 0x1D +#define MPU6050_RA_FF_DUR 0x1E +#define MPU6050_RA_MOT_THR 0x1F +#define MPU6050_RA_MOT_DUR 0x20 +#define MPU6050_RA_ZRMOT_THR 0x21 +#define MPU6050_RA_ZRMOT_DUR 0x22 +#define MPU6050_RA_FIFO_EN 0x23 +#define MPU6050_RA_I2C_MST_CTRL 0x24 +#define MPU6050_RA_I2C_SLV0_ADDR 0x25 +#define MPU6050_RA_I2C_SLV0_REG 0x26 +#define MPU6050_RA_I2C_SLV0_CTRL 0x27 +#define MPU6050_RA_I2C_SLV1_ADDR 0x28 +#define MPU6050_RA_I2C_SLV1_REG 0x29 +#define MPU6050_RA_I2C_SLV1_CTRL 0x2A +#define MPU6050_RA_I2C_SLV2_ADDR 0x2B +#define MPU6050_RA_I2C_SLV2_REG 0x2C +#define MPU6050_RA_I2C_SLV2_CTRL 0x2D +#define MPU6050_RA_I2C_SLV3_ADDR 0x2E +#define MPU6050_RA_I2C_SLV3_REG 0x2F +#define MPU6050_RA_I2C_SLV3_CTRL 0x30 +#define MPU6050_RA_I2C_SLV4_ADDR 0x31 +#define MPU6050_RA_I2C_SLV4_REG 0x32 +#define MPU6050_RA_I2C_SLV4_DO 0x33 +#define MPU6050_RA_I2C_SLV4_CTRL 0x34 +#define MPU6050_RA_I2C_SLV4_DI 0x35 +#define MPU6050_RA_I2C_MST_STATUS 0x36 +#define MPU6050_RA_INT_PIN_CFG 0x37 +#define MPU6050_RA_INT_ENABLE 0x38 +#define MPU6050_RA_DMP_INT_STATUS 0x39 +#define MPU6050_RA_INT_STATUS 0x3A +#define MPU6050_RA_ACCEL_XOUT_H 0x3B +#define MPU6050_RA_ACCEL_XOUT_L 0x3C +#define MPU6050_RA_ACCEL_YOUT_H 0x3D +#define MPU6050_RA_ACCEL_YOUT_L 0x3E +#define MPU6050_RA_ACCEL_ZOUT_H 0x3F +#define MPU6050_RA_ACCEL_ZOUT_L 0x40 +#define MPU6050_RA_TEMP_OUT_H 0x41 +#define MPU6050_RA_TEMP_OUT_L 0x42 +#define MPU6050_RA_GYRO_XOUT_H 0x43 +#define MPU6050_RA_GYRO_XOUT_L 0x44 +#define MPU6050_RA_GYRO_YOUT_H 0x45 +#define MPU6050_RA_GYRO_YOUT_L 0x46 +#define MPU6050_RA_GYRO_ZOUT_H 0x47 +#define MPU6050_RA_GYRO_ZOUT_L 0x48 +#define MPU6050_RA_EXT_SENS_DATA_00 0x49 +#define MPU6050_RA_EXT_SENS_DATA_01 0x4A +#define MPU6050_RA_EXT_SENS_DATA_02 0x4B +#define MPU6050_RA_EXT_SENS_DATA_03 0x4C +#define MPU6050_RA_EXT_SENS_DATA_04 0x4D +#define MPU6050_RA_EXT_SENS_DATA_05 0x4E +#define MPU6050_RA_EXT_SENS_DATA_06 0x4F +#define MPU6050_RA_EXT_SENS_DATA_07 0x50 +#define MPU6050_RA_EXT_SENS_DATA_08 0x51 +#define MPU6050_RA_EXT_SENS_DATA_09 0x52 +#define MPU6050_RA_EXT_SENS_DATA_10 0x53 +#define MPU6050_RA_EXT_SENS_DATA_11 0x54 +#define MPU6050_RA_EXT_SENS_DATA_12 0x55 +#define MPU6050_RA_EXT_SENS_DATA_13 0x56 +#define MPU6050_RA_EXT_SENS_DATA_14 0x57 +#define MPU6050_RA_EXT_SENS_DATA_15 0x58 +#define MPU6050_RA_EXT_SENS_DATA_16 0x59 +#define MPU6050_RA_EXT_SENS_DATA_17 0x5A +#define MPU6050_RA_EXT_SENS_DATA_18 0x5B +#define MPU6050_RA_EXT_SENS_DATA_19 0x5C +#define MPU6050_RA_EXT_SENS_DATA_20 0x5D +#define MPU6050_RA_EXT_SENS_DATA_21 0x5E +#define MPU6050_RA_EXT_SENS_DATA_22 0x5F +#define MPU6050_RA_EXT_SENS_DATA_23 0x60 +#define MPU6050_RA_MOT_DETECT_STATUS 0x61 +#define MPU6050_RA_I2C_SLV0_DO 0x63 +#define MPU6050_RA_I2C_SLV1_DO 0x64 +#define MPU6050_RA_I2C_SLV2_DO 0x65 +#define MPU6050_RA_I2C_SLV3_DO 0x66 +#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 +#define MPU6050_RA_SIGNAL_PATH_RESET 0x68 +#define MPU6050_RA_MOT_DETECT_CTRL 0x69 +#define MPU6050_RA_USER_CTRL 0x6A +#define MPU6050_RA_PWR_MGMT_1 0x6B +#define MPU6050_RA_PWR_MGMT_2 0x6C +#define MPU6050_RA_BANK_SEL 0x6D +#define MPU6050_RA_MEM_START_ADDR 0x6E +#define MPU6050_RA_MEM_R_W 0x6F +#define MPU6050_RA_DMP_CFG_1 0x70 +#define MPU6050_RA_DMP_CFG_2 0x71 +#define MPU6050_RA_FIFO_COUNTH 0x72 +#define MPU6050_RA_FIFO_COUNTL 0x73 +#define MPU6050_RA_FIFO_R_W 0x74 +#define MPU6050_RA_WHO_AM_I 0x75 + +#define MPU6050_SELF_TEST_XA_1_BIT 0x07 +#define MPU6050_SELF_TEST_XA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_XA_2_BIT 0x05 +#define MPU6050_SELF_TEST_XA_2_LENGTH 0x02 +#define MPU6050_SELF_TEST_YA_1_BIT 0x07 +#define MPU6050_SELF_TEST_YA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_YA_2_BIT 0x03 +#define MPU6050_SELF_TEST_YA_2_LENGTH 0x02 +#define MPU6050_SELF_TEST_ZA_1_BIT 0x07 +#define MPU6050_SELF_TEST_ZA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_ZA_2_BIT 0x01 +#define MPU6050_SELF_TEST_ZA_2_LENGTH 0x02 + +#define MPU6050_SELF_TEST_XG_1_BIT 0x04 +#define MPU6050_SELF_TEST_XG_1_LENGTH 0x05 +#define MPU6050_SELF_TEST_YG_1_BIT 0x04 +#define MPU6050_SELF_TEST_YG_1_LENGTH 0x05 +#define MPU6050_SELF_TEST_ZG_1_BIT 0x04 +#define MPU6050_SELF_TEST_ZG_1_LENGTH 0x05 + +#define MPU6050_TC_PWR_MODE_BIT 7 +#define MPU6050_TC_OFFSET_BIT 6 +#define MPU6050_TC_OFFSET_LENGTH 6 +#define MPU6050_TC_OTP_BNK_VLD_BIT 0 + +#define MPU6050_VDDIO_LEVEL_VLOGIC 0 +#define MPU6050_VDDIO_LEVEL_VDD 1 + +#define MPU6050_CFG_EXT_SYNC_SET_BIT 5 +#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 +#define MPU6050_CFG_DLPF_CFG_BIT 2 +#define MPU6050_CFG_DLPF_CFG_LENGTH 3 + +#define MPU6050_EXT_SYNC_DISABLED 0x0 +#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 +#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 +#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 +#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 +#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 +#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 +#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 + +#define MPU6050_DLPF_BW_256 0x00 +#define MPU6050_DLPF_BW_188 0x01 +#define MPU6050_DLPF_BW_98 0x02 +#define MPU6050_DLPF_BW_42 0x03 +#define MPU6050_DLPF_BW_20 0x04 +#define MPU6050_DLPF_BW_10 0x05 +#define MPU6050_DLPF_BW_5 0x06 + +#define MPU6050_GCONFIG_FS_SEL_BIT 4 +#define MPU6050_GCONFIG_FS_SEL_LENGTH 2 + +#define MPU6050_GYRO_FS_250 0x00 +#define MPU6050_GYRO_FS_500 0x01 +#define MPU6050_GYRO_FS_1000 0x02 +#define MPU6050_GYRO_FS_2000 0x03 + +#define MPU6050_ACONFIG_XA_ST_BIT 7 +#define MPU6050_ACONFIG_YA_ST_BIT 6 +#define MPU6050_ACONFIG_ZA_ST_BIT 5 +#define MPU6050_ACONFIG_AFS_SEL_BIT 4 +#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 +#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 +#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 + +#define MPU6050_ACCEL_FS_2 0x00 +#define MPU6050_ACCEL_FS_4 0x01 +#define MPU6050_ACCEL_FS_8 0x02 +#define MPU6050_ACCEL_FS_16 0x03 + +#define MPU6050_DHPF_RESET 0x00 +#define MPU6050_DHPF_5 0x01 +#define MPU6050_DHPF_2P5 0x02 +#define MPU6050_DHPF_1P25 0x03 +#define MPU6050_DHPF_0P63 0x04 +#define MPU6050_DHPF_HOLD 0x07 + +#define MPU6050_TEMP_FIFO_EN_BIT 7 +#define MPU6050_XG_FIFO_EN_BIT 6 +#define MPU6050_YG_FIFO_EN_BIT 5 +#define MPU6050_ZG_FIFO_EN_BIT 4 +#define MPU6050_ACCEL_FIFO_EN_BIT 3 +#define MPU6050_SLV2_FIFO_EN_BIT 2 +#define MPU6050_SLV1_FIFO_EN_BIT 1 +#define MPU6050_SLV0_FIFO_EN_BIT 0 + +#define MPU6050_MULT_MST_EN_BIT 7 +#define MPU6050_WAIT_FOR_ES_BIT 6 +#define MPU6050_SLV_3_FIFO_EN_BIT 5 +#define MPU6050_I2C_MST_P_NSR_BIT 4 +#define MPU6050_I2C_MST_CLK_BIT 3 +#define MPU6050_I2C_MST_CLK_LENGTH 4 + +#define MPU6050_CLOCK_DIV_348 0x0 +#define MPU6050_CLOCK_DIV_333 0x1 +#define MPU6050_CLOCK_DIV_320 0x2 +#define MPU6050_CLOCK_DIV_308 0x3 +#define MPU6050_CLOCK_DIV_296 0x4 +#define MPU6050_CLOCK_DIV_286 0x5 +#define MPU6050_CLOCK_DIV_276 0x6 +#define MPU6050_CLOCK_DIV_267 0x7 +#define MPU6050_CLOCK_DIV_258 0x8 +#define MPU6050_CLOCK_DIV_500 0x9 +#define MPU6050_CLOCK_DIV_471 0xA +#define MPU6050_CLOCK_DIV_444 0xB +#define MPU6050_CLOCK_DIV_421 0xC +#define MPU6050_CLOCK_DIV_400 0xD +#define MPU6050_CLOCK_DIV_381 0xE +#define MPU6050_CLOCK_DIV_364 0xF + +#define MPU6050_I2C_SLV_RW_BIT 7 +#define MPU6050_I2C_SLV_ADDR_BIT 6 +#define MPU6050_I2C_SLV_ADDR_LENGTH 7 +#define MPU6050_I2C_SLV_EN_BIT 7 +#define MPU6050_I2C_SLV_BYTE_SW_BIT 6 +#define MPU6050_I2C_SLV_REG_DIS_BIT 5 +#define MPU6050_I2C_SLV_GRP_BIT 4 +#define MPU6050_I2C_SLV_LEN_BIT 3 +#define MPU6050_I2C_SLV_LEN_LENGTH 4 + +#define MPU6050_I2C_SLV4_RW_BIT 7 +#define MPU6050_I2C_SLV4_ADDR_BIT 6 +#define MPU6050_I2C_SLV4_ADDR_LENGTH 7 +#define MPU6050_I2C_SLV4_EN_BIT 7 +#define MPU6050_I2C_SLV4_INT_EN_BIT 6 +#define MPU6050_I2C_SLV4_REG_DIS_BIT 5 +#define MPU6050_I2C_SLV4_MST_DLY_BIT 4 +#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 + +#define MPU6050_MST_PASS_THROUGH_BIT 7 +#define MPU6050_MST_I2C_SLV4_DONE_BIT 6 +#define MPU6050_MST_I2C_LOST_ARB_BIT 5 +#define MPU6050_MST_I2C_SLV4_NACK_BIT 4 +#define MPU6050_MST_I2C_SLV3_NACK_BIT 3 +#define MPU6050_MST_I2C_SLV2_NACK_BIT 2 +#define MPU6050_MST_I2C_SLV1_NACK_BIT 1 +#define MPU6050_MST_I2C_SLV0_NACK_BIT 0 + +#define MPU6050_INTCFG_INT_LEVEL_BIT 7 +#define MPU6050_INTCFG_INT_OPEN_BIT 6 +#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 +#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 +#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 +#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 +#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 +#define MPU6050_INTCFG_CLKOUT_EN_BIT 0 + +#define MPU6050_INTMODE_ACTIVEHIGH 0x00 +#define MPU6050_INTMODE_ACTIVELOW 0x01 + +#define MPU6050_INTDRV_PUSHPULL 0x00 +#define MPU6050_INTDRV_OPENDRAIN 0x01 + +#define MPU6050_INTLATCH_50USPULSE 0x00 +#define MPU6050_INTLATCH_WAITCLEAR 0x01 + +#define MPU6050_INTCLEAR_STATUSREAD 0x00 +#define MPU6050_INTCLEAR_ANYREAD 0x01 + +#define MPU6050_INTERRUPT_FF_BIT 7 +#define MPU6050_INTERRUPT_MOT_BIT 6 +#define MPU6050_INTERRUPT_ZMOT_BIT 5 +#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 +#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 +#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 +#define MPU6050_INTERRUPT_DMP_INT_BIT 1 +#define MPU6050_INTERRUPT_DATA_RDY_BIT 0 + +// TODO: figure out what these actually do +// UMPL source code is not very obivous +#define MPU6050_DMPINT_5_BIT 5 +#define MPU6050_DMPINT_4_BIT 4 +#define MPU6050_DMPINT_3_BIT 3 +#define MPU6050_DMPINT_2_BIT 2 +#define MPU6050_DMPINT_1_BIT 1 +#define MPU6050_DMPINT_0_BIT 0 + +#define MPU6050_MOTION_MOT_XNEG_BIT 7 +#define MPU6050_MOTION_MOT_XPOS_BIT 6 +#define MPU6050_MOTION_MOT_YNEG_BIT 5 +#define MPU6050_MOTION_MOT_YPOS_BIT 4 +#define MPU6050_MOTION_MOT_ZNEG_BIT 3 +#define MPU6050_MOTION_MOT_ZPOS_BIT 2 +#define MPU6050_MOTION_MOT_ZRMOT_BIT 0 + +#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 +#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 +#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 +#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 +#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 +#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 + +#define MPU6050_PATHRESET_GYRO_RESET_BIT 2 +#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 +#define MPU6050_PATHRESET_TEMP_RESET_BIT 0 + +#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 +#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 +#define MPU6050_DETECT_FF_COUNT_BIT 3 +#define MPU6050_DETECT_FF_COUNT_LENGTH 2 +#define MPU6050_DETECT_MOT_COUNT_BIT 1 +#define MPU6050_DETECT_MOT_COUNT_LENGTH 2 + +#define MPU6050_DETECT_DECREMENT_RESET 0x0 +#define MPU6050_DETECT_DECREMENT_1 0x1 +#define MPU6050_DETECT_DECREMENT_2 0x2 +#define MPU6050_DETECT_DECREMENT_4 0x3 + +#define MPU6050_USERCTRL_DMP_EN_BIT 7 +#define MPU6050_USERCTRL_FIFO_EN_BIT 6 +#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 +#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 +#define MPU6050_USERCTRL_DMP_RESET_BIT 3 +#define MPU6050_USERCTRL_FIFO_RESET_BIT 2 +#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 +#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 + +#define MPU6050_PWR1_DEVICE_RESET_BIT 7 +#define MPU6050_PWR1_SLEEP_BIT 6 +#define MPU6050_PWR1_CYCLE_BIT 5 +#define MPU6050_PWR1_TEMP_DIS_BIT 3 +#define MPU6050_PWR1_CLKSEL_BIT 2 +#define MPU6050_PWR1_CLKSEL_LENGTH 3 + +#define MPU6050_CLOCK_INTERNAL 0x00 +#define MPU6050_CLOCK_PLL_XGYRO 0x01 +#define MPU6050_CLOCK_PLL_YGYRO 0x02 +#define MPU6050_CLOCK_PLL_ZGYRO 0x03 +#define MPU6050_CLOCK_PLL_EXT32K 0x04 +#define MPU6050_CLOCK_PLL_EXT19M 0x05 +#define MPU6050_CLOCK_KEEP_RESET 0x07 + +#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 +#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 +#define MPU6050_PWR2_STBY_XA_BIT 5 +#define MPU6050_PWR2_STBY_YA_BIT 4 +#define MPU6050_PWR2_STBY_ZA_BIT 3 +#define MPU6050_PWR2_STBY_XG_BIT 2 +#define MPU6050_PWR2_STBY_YG_BIT 1 +#define MPU6050_PWR2_STBY_ZG_BIT 0 + +#define MPU6050_WAKE_FREQ_1P25 0x0 +#define MPU6050_WAKE_FREQ_2P5 0x1 +#define MPU6050_WAKE_FREQ_5 0x2 +#define MPU6050_WAKE_FREQ_10 0x3 + +#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 +#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 +#define MPU6050_BANKSEL_MEM_SEL_BIT 4 +#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 + +#define MPU6050_WHO_AM_I_BIT 6 +#define MPU6050_WHO_AM_I_LENGTH 6 + +#define MPU6050_DMP_MEMORY_BANKS 8 +#define MPU6050_DMP_MEMORY_BANK_SIZE 256 +#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 + +// note: DMP code memory blocks defined at end of header file + +class MPU6050 { + public: + MPU6050(uint8_t address=MPU6050_DEFAULT_ADDRESS); + + void initialize(); + bool testConnection(); + + // AUX_VDDIO register + uint8_t getAuxVDDIOLevel(); + void setAuxVDDIOLevel(uint8_t level); + + // SMPLRT_DIV register + uint8_t getRate(); + void setRate(uint8_t rate); + + // CONFIG register + uint8_t getExternalFrameSync(); + void setExternalFrameSync(uint8_t sync); + uint8_t getDLPFMode(); + void setDLPFMode(uint8_t bandwidth); + + // GYRO_CONFIG register + uint8_t getFullScaleGyroRange(); + void setFullScaleGyroRange(uint8_t range); + + // SELF_TEST registers + uint8_t getAccelXSelfTestFactoryTrim(); + uint8_t getAccelYSelfTestFactoryTrim(); + uint8_t getAccelZSelfTestFactoryTrim(); + + uint8_t getGyroXSelfTestFactoryTrim(); + uint8_t getGyroYSelfTestFactoryTrim(); + uint8_t getGyroZSelfTestFactoryTrim(); + + // ACCEL_CONFIG register + bool getAccelXSelfTest(); + void setAccelXSelfTest(bool enabled); + bool getAccelYSelfTest(); + void setAccelYSelfTest(bool enabled); + bool getAccelZSelfTest(); + void setAccelZSelfTest(bool enabled); + uint8_t getFullScaleAccelRange(); + void setFullScaleAccelRange(uint8_t range); + uint8_t getDHPFMode(); + void setDHPFMode(uint8_t mode); + + // FF_THR register + uint8_t getFreefallDetectionThreshold(); + void setFreefallDetectionThreshold(uint8_t threshold); + + // FF_DUR register + uint8_t getFreefallDetectionDuration(); + void setFreefallDetectionDuration(uint8_t duration); + + // MOT_THR register + uint8_t getMotionDetectionThreshold(); + void setMotionDetectionThreshold(uint8_t threshold); + + // MOT_DUR register + uint8_t getMotionDetectionDuration(); + void setMotionDetectionDuration(uint8_t duration); + + // ZRMOT_THR register + uint8_t getZeroMotionDetectionThreshold(); + void setZeroMotionDetectionThreshold(uint8_t threshold); + + // ZRMOT_DUR register + uint8_t getZeroMotionDetectionDuration(); + void setZeroMotionDetectionDuration(uint8_t duration); + + // FIFO_EN register + bool getTempFIFOEnabled(); + void setTempFIFOEnabled(bool enabled); + bool getXGyroFIFOEnabled(); + void setXGyroFIFOEnabled(bool enabled); + bool getYGyroFIFOEnabled(); + void setYGyroFIFOEnabled(bool enabled); + bool getZGyroFIFOEnabled(); + void setZGyroFIFOEnabled(bool enabled); + bool getAccelFIFOEnabled(); + void setAccelFIFOEnabled(bool enabled); + bool getSlave2FIFOEnabled(); + void setSlave2FIFOEnabled(bool enabled); + bool getSlave1FIFOEnabled(); + void setSlave1FIFOEnabled(bool enabled); + bool getSlave0FIFOEnabled(); + void setSlave0FIFOEnabled(bool enabled); + + // I2C_MST_CTRL register + bool getMultiMasterEnabled(); + void setMultiMasterEnabled(bool enabled); + bool getWaitForExternalSensorEnabled(); + void setWaitForExternalSensorEnabled(bool enabled); + bool getSlave3FIFOEnabled(); + void setSlave3FIFOEnabled(bool enabled); + bool getSlaveReadWriteTransitionEnabled(); + void setSlaveReadWriteTransitionEnabled(bool enabled); + uint8_t getMasterClockSpeed(); + void setMasterClockSpeed(uint8_t speed); + + // I2C_SLV* registers (Slave 0-3) + uint8_t getSlaveAddress(uint8_t num); + void setSlaveAddress(uint8_t num, uint8_t address); + uint8_t getSlaveRegister(uint8_t num); + void setSlaveRegister(uint8_t num, uint8_t reg); + bool getSlaveEnabled(uint8_t num); + void setSlaveEnabled(uint8_t num, bool enabled); + bool getSlaveWordByteSwap(uint8_t num); + void setSlaveWordByteSwap(uint8_t num, bool enabled); + bool getSlaveWriteMode(uint8_t num); + void setSlaveWriteMode(uint8_t num, bool mode); + bool getSlaveWordGroupOffset(uint8_t num); + void setSlaveWordGroupOffset(uint8_t num, bool enabled); + uint8_t getSlaveDataLength(uint8_t num); + void setSlaveDataLength(uint8_t num, uint8_t length); + + // I2C_SLV* registers (Slave 4) + uint8_t getSlave4Address(); + void setSlave4Address(uint8_t address); + uint8_t getSlave4Register(); + void setSlave4Register(uint8_t reg); + void setSlave4OutputByte(uint8_t data); + bool getSlave4Enabled(); + void setSlave4Enabled(bool enabled); + bool getSlave4InterruptEnabled(); + void setSlave4InterruptEnabled(bool enabled); + bool getSlave4WriteMode(); + void setSlave4WriteMode(bool mode); + uint8_t getSlave4MasterDelay(); + void setSlave4MasterDelay(uint8_t delay); + uint8_t getSlate4InputByte(); + + // I2C_MST_STATUS register + bool getPassthroughStatus(); + bool getSlave4IsDone(); + bool getLostArbitration(); + bool getSlave4Nack(); + bool getSlave3Nack(); + bool getSlave2Nack(); + bool getSlave1Nack(); + bool getSlave0Nack(); + + // INT_PIN_CFG register + bool getInterruptMode(); + void setInterruptMode(bool mode); + bool getInterruptDrive(); + void setInterruptDrive(bool drive); + bool getInterruptLatch(); + void setInterruptLatch(bool latch); + bool getInterruptLatchClear(); + void setInterruptLatchClear(bool clear); + bool getFSyncInterruptLevel(); + void setFSyncInterruptLevel(bool level); + bool getFSyncInterruptEnabled(); + void setFSyncInterruptEnabled(bool enabled); + bool getI2CBypassEnabled(); + void setI2CBypassEnabled(bool enabled); + bool getClockOutputEnabled(); + void setClockOutputEnabled(bool enabled); + + // INT_ENABLE register + uint8_t getIntEnabled(); + void setIntEnabled(uint8_t enabled); + bool getIntFreefallEnabled(); + void setIntFreefallEnabled(bool enabled); + bool getIntMotionEnabled(); + void setIntMotionEnabled(bool enabled); + bool getIntZeroMotionEnabled(); + void setIntZeroMotionEnabled(bool enabled); + bool getIntFIFOBufferOverflowEnabled(); + void setIntFIFOBufferOverflowEnabled(bool enabled); + bool getIntI2CMasterEnabled(); + void setIntI2CMasterEnabled(bool enabled); + bool getIntDataReadyEnabled(); + void setIntDataReadyEnabled(bool enabled); + + // INT_STATUS register + uint8_t getIntStatus(); + bool getIntFreefallStatus(); + bool getIntMotionStatus(); + bool getIntZeroMotionStatus(); + bool getIntFIFOBufferOverflowStatus(); + bool getIntI2CMasterStatus(); + bool getIntDataReadyStatus(); + + // ACCEL_*OUT_* registers + void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz); + void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); + void getAcceleration(int16_t* x, int16_t* y, int16_t* z); + int16_t getAccelerationX(); + int16_t getAccelerationY(); + int16_t getAccelerationZ(); + + // TEMP_OUT_* registers + int16_t getTemperature(); + + // GYRO_*OUT_* registers + void getRotation(int16_t* x, int16_t* y, int16_t* z); + int16_t getRotationX(); + int16_t getRotationY(); + int16_t getRotationZ(); + + // EXT_SENS_DATA_* registers + uint8_t getExternalSensorByte(int position); + uint16_t getExternalSensorWord(int position); + uint32_t getExternalSensorDWord(int position); + + // MOT_DETECT_STATUS register + uint8_t getMotionStatus(); + bool getXNegMotionDetected(); + bool getXPosMotionDetected(); + bool getYNegMotionDetected(); + bool getYPosMotionDetected(); + bool getZNegMotionDetected(); + bool getZPosMotionDetected(); + bool getZeroMotionDetected(); + + // I2C_SLV*_DO register + void setSlaveOutputByte(uint8_t num, uint8_t data); + + // I2C_MST_DELAY_CTRL register + bool getExternalShadowDelayEnabled(); + void setExternalShadowDelayEnabled(bool enabled); + bool getSlaveDelayEnabled(uint8_t num); + void setSlaveDelayEnabled(uint8_t num, bool enabled); + + // SIGNAL_PATH_RESET register + void resetGyroscopePath(); + void resetAccelerometerPath(); + void resetTemperaturePath(); + + // MOT_DETECT_CTRL register + uint8_t getAccelerometerPowerOnDelay(); + void setAccelerometerPowerOnDelay(uint8_t delay); + uint8_t getFreefallDetectionCounterDecrement(); + void setFreefallDetectionCounterDecrement(uint8_t decrement); + uint8_t getMotionDetectionCounterDecrement(); + void setMotionDetectionCounterDecrement(uint8_t decrement); + + // USER_CTRL register + bool getFIFOEnabled(); + void setFIFOEnabled(bool enabled); + bool getI2CMasterModeEnabled(); + void setI2CMasterModeEnabled(bool enabled); + void switchSPIEnabled(bool enabled); + void resetFIFO(); + void resetI2CMaster(); + void resetSensors(); + + // PWR_MGMT_1 register + void reset(); + bool getSleepEnabled(); + void setSleepEnabled(bool enabled); + bool getWakeCycleEnabled(); + void setWakeCycleEnabled(bool enabled); + bool getTempSensorEnabled(); + void setTempSensorEnabled(bool enabled); + uint8_t getClockSource(); + void setClockSource(uint8_t source); + + // PWR_MGMT_2 register + uint8_t getWakeFrequency(); + void setWakeFrequency(uint8_t frequency); + bool getStandbyXAccelEnabled(); + void setStandbyXAccelEnabled(bool enabled); + bool getStandbyYAccelEnabled(); + void setStandbyYAccelEnabled(bool enabled); + bool getStandbyZAccelEnabled(); + void setStandbyZAccelEnabled(bool enabled); + bool getStandbyXGyroEnabled(); + void setStandbyXGyroEnabled(bool enabled); + bool getStandbyYGyroEnabled(); + void setStandbyYGyroEnabled(bool enabled); + bool getStandbyZGyroEnabled(); + void setStandbyZGyroEnabled(bool enabled); + + // FIFO_COUNT_* registers + uint16_t getFIFOCount(); + + // FIFO_R_W register + uint8_t getFIFOByte(); + void setFIFOByte(uint8_t data); + void getFIFOBytes(uint8_t *data, uint8_t length); + + // WHO_AM_I register + uint8_t getDeviceID(); + void setDeviceID(uint8_t id); + + // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== + + // XG_OFFS_TC register + uint8_t getOTPBankValid(); + void setOTPBankValid(bool enabled); + int8_t getXGyroOffsetTC(); + void setXGyroOffsetTC(int8_t offset); + + // YG_OFFS_TC register + int8_t getYGyroOffsetTC(); + void setYGyroOffsetTC(int8_t offset); + + // ZG_OFFS_TC register + int8_t getZGyroOffsetTC(); + void setZGyroOffsetTC(int8_t offset); + + // X_FINE_GAIN register + int8_t getXFineGain(); + void setXFineGain(int8_t gain); + + // Y_FINE_GAIN register + int8_t getYFineGain(); + void setYFineGain(int8_t gain); + + // Z_FINE_GAIN register + int8_t getZFineGain(); + void setZFineGain(int8_t gain); + + // XA_OFFS_* registers + int16_t getXAccelOffset(); + void setXAccelOffset(int16_t offset); + + // YA_OFFS_* register + int16_t getYAccelOffset(); + void setYAccelOffset(int16_t offset); + + // ZA_OFFS_* register + int16_t getZAccelOffset(); + void setZAccelOffset(int16_t offset); + + // XG_OFFS_USR* registers + int16_t getXGyroOffset(); + void setXGyroOffset(int16_t offset); + + // YG_OFFS_USR* register + int16_t getYGyroOffset(); + void setYGyroOffset(int16_t offset); + + // ZG_OFFS_USR* register + int16_t getZGyroOffset(); + void setZGyroOffset(int16_t offset); + + // INT_ENABLE register (DMP functions) + bool getIntPLLReadyEnabled(); + void setIntPLLReadyEnabled(bool enabled); + bool getIntDMPEnabled(); + void setIntDMPEnabled(bool enabled); + + // DMP_INT_STATUS + bool getDMPInt5Status(); + bool getDMPInt4Status(); + bool getDMPInt3Status(); + bool getDMPInt2Status(); + bool getDMPInt1Status(); + bool getDMPInt0Status(); + + // INT_STATUS register (DMP functions) + bool getIntPLLReadyStatus(); + bool getIntDMPStatus(); + + // USER_CTRL register (DMP functions) + bool getDMPEnabled(); + void setDMPEnabled(bool enabled); + void resetDMP(); + + // BANK_SEL register + void setMemoryBank(uint8_t bank, bool prefetchEnabled=false, bool userBank=false); + + // MEM_START_ADDR register + void setMemoryStartAddress(uint8_t address); + + // MEM_R_W register + uint8_t readMemoryByte(); + void writeMemoryByte(uint8_t data); + void readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0); + bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true, bool useProgMem=false); + bool writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true); + + bool writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem=false); + bool writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize); + + // DMP_CFG_1 register + uint8_t getDMPConfig1(); + void setDMPConfig1(uint8_t config); + + // DMP_CFG_2 register + uint8_t getDMPConfig2(); + void setDMPConfig2(uint8_t config); + + // Calibration Routines + void CalibrateGyro(uint8_t Loops = 15); // Fine tune after setting offsets with less Loops. + void CalibrateAccel(uint8_t Loops = 15);// Fine tune after setting offsets with less Loops. + void PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops); // Does the math + void PrintActiveOffsets(); // See the results of the Calibration + + + + // special methods for MotionApps 2.0 implementation + #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20 + + uint8_t dmpInitialize(); + bool dmpPacketAvailable(); + + uint8_t dmpSetFIFORate(uint8_t fifoRate); + uint8_t dmpGetFIFORate(); + uint8_t dmpGetSampleStepSizeMS(); + uint8_t dmpGetSampleFrequency(); + int32_t dmpDecodeTemperature(int8_t tempReg); + + // Register callbacks after a packet of FIFO data is processed + //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); + //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); + uint8_t dmpRunFIFORateProcesses(); + + // Setup FIFO for various output + uint8_t dmpSendQuaternion(uint_fast16_t accuracy); + uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); + uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + + // Get Fixed Point data from FIFO + uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpSetLinearAccelFilterCoefficient(float coef); + uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); + uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); + uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); + uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); + uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); + + uint8_t dmpGetEuler(float *data, Quaternion *q); + uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); + + // Get Floating Point data from FIFO + uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); + + uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); + uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); + + uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); + + uint8_t dmpInitFIFOParam(); + uint8_t dmpCloseFIFO(); + uint8_t dmpSetGyroDataSource(uint8_t source); + uint8_t dmpDecodeQuantizedAccel(); + uint32_t dmpGetGyroSumOfSquare(); + uint32_t dmpGetAccelSumOfSquare(); + void dmpOverrideQuaternion(long *q); + uint16_t dmpGetFIFOPacketSize(); + #endif + + // special methods for MotionApps 4.1 implementation + #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41 + + uint8_t dmpInitialize(); + bool dmpPacketAvailable(); + + uint8_t dmpSetFIFORate(uint8_t fifoRate); + uint8_t dmpGetFIFORate(); + uint8_t dmpGetSampleStepSizeMS(); + uint8_t dmpGetSampleFrequency(); + int32_t dmpDecodeTemperature(int8_t tempReg); + + // Register callbacks after a packet of FIFO data is processed + //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); + //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); + uint8_t dmpRunFIFORateProcesses(); + + // Setup FIFO for various output + uint8_t dmpSendQuaternion(uint_fast16_t accuracy); + uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); + uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + + // Get Fixed Point data from FIFO + uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0); + uint8_t dmpSetLinearAccelFilterCoefficient(float coef); + uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); + uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); + uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); + uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); + uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); + + uint8_t dmpGetEuler(float *data, Quaternion *q); + uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); + + // Get Floating Point data from FIFO + uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); + + uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); + uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); + + uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); + + uint8_t dmpInitFIFOParam(); + uint8_t dmpCloseFIFO(); + uint8_t dmpSetGyroDataSource(uint8_t source); + uint8_t dmpDecodeQuantizedAccel(); + uint32_t dmpGetGyroSumOfSquare(); + uint32_t dmpGetAccelSumOfSquare(); + void dmpOverrideQuaternion(long *q); + uint16_t dmpGetFIFOPacketSize(); + #endif + + private: + uint8_t devAddr; + uint8_t buffer[14]; + #if defined(MPU6050_INCLUDE_DMP_MOTIONAPPS20) or defined(MPU6050_INCLUDE_DMP_MOTIONAPPS41) + uint8_t *dmpPacketBuffer; + uint16_t dmpPacketSize; + #endif +}; + +#endif /* _MPU6050_H_ */ diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050_6Axis_MotionApps20.h b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050_6Axis_MotionApps20.h new file mode 100644 index 00000000..d6a1c137 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050_6Axis_MotionApps20.h @@ -0,0 +1,617 @@ +// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 5/20/2013 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2019/07/08 - merged all DMP Firmware configuration items into the dmpMemory array +// - Simplified dmpInitialize() to accomidate the dmpmemory array alterations +// ... - ongoing debug release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ +#define _MPU6050_6AXIS_MOTIONAPPS20_H_ + +#include "I2Cdev.h" +#include "helper_3dmath.h" + +// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board +#define MPU6050_INCLUDE_DMP_MOTIONAPPS20 + +#include "MPU6050.h" + +// Tom Carpenter's conditional PROGMEM code +// http://forum.arduino.cc/index.php?topic=129407.0 +#ifdef __AVR__ + #include +#elif defined(ESP8266) || defined(ESP32) + #include +#else + // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen + #ifndef __PGMSPACE_H_ + #define __PGMSPACE_H_ 1 + #include + + #define PROGMEM + #define PGM_P const char * + #define PSTR(str) (str) + #define F(x) x + + typedef void prog_void; + typedef char prog_char; + typedef unsigned char prog_uchar; + typedef int8_t prog_int8_t; + typedef uint8_t prog_uint8_t; + typedef int16_t prog_int16_t; + typedef uint16_t prog_uint16_t; + typedef int32_t prog_int32_t; + typedef uint32_t prog_uint32_t; + + #define strcpy_P(dest, src) strcpy((dest), (src)) + #define strcat_P(dest, src) strcat((dest), (src)) + #define strcmp_P(a, b) strcmp((a), (b)) + + #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) + #define pgm_read_word(addr) (*(const unsigned short *)(addr)) + #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) + #define pgm_read_float(addr) (*(const float *)(addr)) + + #define pgm_read_byte_near(addr) pgm_read_byte(addr) + #define pgm_read_word_near(addr) pgm_read_word(addr) + #define pgm_read_dword_near(addr) pgm_read_dword(addr) + #define pgm_read_float_near(addr) pgm_read_float(addr) + #define pgm_read_byte_far(addr) pgm_read_byte(addr) + #define pgm_read_word_far(addr) pgm_read_word(addr) + #define pgm_read_dword_far(addr) pgm_read_dword(addr) + #define pgm_read_float_far(addr) pgm_read_float(addr) + #endif +#endif + +/* Source is from the InvenSense MotionApps v2 demo code. Original source is + * unavailable, unless you happen to be amazing as decompiling binary by + * hand (in which case, please contact me, and I'm totally serious). + * + * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the + * DMP reverse-engineering he did to help make this bit of wizardry + * possible. + */ + +// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. +// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) +// after moving string constants to flash memory storage using the F() +// compiler macro (Arduino IDE 1.0+ required). + +//#define DEBUG +#ifdef DEBUG + #define DEBUG_PRINT(x) Serial.print(x) + #define DEBUG_PRINTF(x, y) Serial.print(x, y) + #define DEBUG_PRINTLN(x) Serial.println(x) + #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) +#else + #define DEBUG_PRINT(x) + #define DEBUG_PRINTF(x, y) + #define DEBUG_PRINTLN(x) + #define DEBUG_PRINTLNF(x, y) +#endif + +#define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] +#define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] +#define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] + +/* ================================================================================================ * + | Default MotionApps v2.0 42-byte FIFO packet structure: | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | + | | + | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | + | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | + * ================================================================================================ */ + +// this block of memory gets written to the MPU on start-up, and it seems +// to be volatile memory, so it has to be done each time (it only takes ~1 +// second though) + +// I Only Changed this by applying all the configuration data and capturing it before startup: +// *** this is a capture of the DMP Firmware after all the messy changes were made so we can just load it +const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { + /* bank # 0 */ + 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x40, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCB, 0x47, 0xA2, 0x20, 0x00, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, + 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, + 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, + 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, + /* bank # 1 */ + 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, + 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, + 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x09, 0x23, 0xA1, 0x35, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, + 0x80, 0x00, 0xFF, 0xFF, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, + 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, + /* bank # 2 */ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0x8B, 0xC1, 0x00, 0x00, 0x01, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + /* bank # 3 */ + 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, + 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, + 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, + 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, + 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, + 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, + 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, + 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C, + 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, + 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, + 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, + 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, + 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, + 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, + 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, + 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, + /* bank # 4 */ + 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, + 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, + 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, + 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, + 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, + 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, + 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, + 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, + 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, + 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, + 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, + 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, + /* bank # 5 */ + 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, + 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, + 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, + 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, + 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, + 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, + 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, + 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, + 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, + 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, + 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, + 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, + 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, + 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, + 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, + 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, + /* bank # 6 */ + 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, + 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, + 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, + 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, + 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, + 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, + 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, + 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, + 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, + 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, + 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, + 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, + 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, + 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, + 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, + 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, + /* bank # 7 */ + 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, + 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, + 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, + 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, + 0xDD, 0xF1, 0x20, 0x28, 0x30, 0x38, 0x9A, 0xF1, 0x28, 0x30, 0x38, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, + 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, + 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0x28, 0x30, 0x38, + 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0x30, 0xDC, + 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xFE, 0xD8, 0xFF, + +}; + +#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR +#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default +#endif + +// I Simplified this: +uint8_t MPU6050::dmpInitialize() { + // reset device + DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); + reset(); + delay(30); // wait after reset + + // enable sleep mode and wake cycle + /*Serial.println(F("Enabling sleep mode...")); + setSleepEnabled(true); + Serial.println(F("Enabling wake cycle...")); + setWakeCycleEnabled(true);*/ + + // disable sleep mode + setSleepEnabled(false); + + // get MPU hardware revision + setMemoryBank(0x10, true, true); + setMemoryStartAddress(0x06); + Serial.println(F("Checking hardware revision...")); + Serial.print(F("Revision @ user[16][6] = ")); + Serial.println(readMemoryByte(), HEX); + Serial.println(F("Resetting memory bank selection to 0...")); + setMemoryBank(0, false, false); + + // check OTP bank valid + DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); + DEBUG_PRINT(F("OTP bank is ")); + DEBUG_PRINTLN(getOTPBankValid() ? F("valid!") : F("invalid!")); + + // setup weird slave stuff (?) + DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); + setSlaveAddress(0, 0x7F); + DEBUG_PRINTLN(F("Disabling I2C Master mode...")); + setI2CMasterModeEnabled(false); + DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); + setSlaveAddress(0, 0x68); + DEBUG_PRINTLN(F("Resetting I2C Master control...")); + resetI2CMaster(); + delay(20); + DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); + setClockSource(MPU6050_CLOCK_PLL_ZGYRO); + + DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); + setIntEnabled(1<= dmpGetFIFOPacketSize(); +} + +// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); +// uint8_t MPU6050::dmpGetFIFORate(); +// uint8_t MPU6050::dmpGetSampleStepSizeMS(); +// uint8_t MPU6050::dmpGetSampleFrequency(); +// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); + +//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); +//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); +//uint8_t MPU6050::dmpRunFIFORateProcesses(); + +// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + +uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]); + data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]); + data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]); + return 0; +} +uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[28] << 8) | packet[29]; + data[1] = (packet[32] << 8) | packet[33]; + data[2] = (packet[36] << 8) | packet[37]; + return 0; +} +uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[28] << 8) | packet[29]; + v -> y = (packet[32] << 8) | packet[33]; + v -> z = (packet[36] << 8) | packet[37]; + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); + data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); + data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); + data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[0] << 8) | packet[1]); + data[1] = ((packet[4] << 8) | packet[5]); + data[2] = ((packet[8] << 8) | packet[9]); + data[3] = ((packet[12] << 8) | packet[13]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + if (status == 0) { + q -> w = (float)qI[0] / 16384.0f; + q -> x = (float)qI[1] / 16384.0f; + q -> y = (float)qI[2] / 16384.0f; + q -> z = (float)qI[3] / 16384.0f; + return 0; + } + return status; // int16 return value, indicates error if this line is reached +} +// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); + data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); + data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); + return 0; +} +uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[16] << 8) | packet[17]; + data[1] = (packet[20] << 8) | packet[21]; + data[2] = (packet[24] << 8) | packet[25]; + return 0; +} +uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[16] << 8) | packet[17]; + v -> y = (packet[20] << 8) | packet[21]; + v -> z = (packet[24] << 8) | packet[25]; + return 0; +} +// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); +// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { + // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) + v -> x = vRaw -> x - gravity -> x*8192; + v -> y = vRaw -> y - gravity -> y*8192; + v -> z = vRaw -> z - gravity -> z*8192; + return 0; +} +// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { + // rotate measured 3D acceleration vector into original state + // frame of reference based on orientation quaternion + memcpy(v, vReal, sizeof(VectorInt16)); + v -> rotate(q); + return 0; +} +// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); + return status; +} + +uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { + v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); + v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); + v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; + return 0; +} +// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); +// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); + +uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi + data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta + data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi + return 0; +} + +#ifdef USE_OLD_DMPGETYAWPITCHROLL +uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); + return 0; +} +#else +uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan2(gravity -> y , gravity -> z); + if (gravity -> z < 0) { + if(data[1] > 0) { + data[1] = PI - data[1]; + } else { + data[1] = -PI - data[1]; + } + } + return 0; +} +#endif + +// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); + +uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { + /*for (uint8_t k = 0; k < dmpPacketSize; k++) { + if (dmpData[k] < 0x10) Serial.print("0"); + Serial.print(dmpData[k], HEX); + Serial.print(" "); + } + Serial.print("\n");*/ + //Serial.println((uint16_t)dmpPacketBuffer); + return 0; +} +uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { + uint8_t status; + uint8_t buf[dmpPacketSize]; + for (uint8_t i = 0; i < numPackets; i++) { + // read packet from FIFO + getFIFOBytes(buf, dmpPacketSize); + + // process packet + if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; + + // increment external process count variable, if supplied + if (processed != 0) (*processed)++; + } + return 0; +} + +// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); + +// uint8_t MPU6050::dmpInitFIFOParam(); +// uint8_t MPU6050::dmpCloseFIFO(); +// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); +// uint8_t MPU6050::dmpDecodeQuantizedAccel(); +// uint32_t MPU6050::dmpGetGyroSumOfSquare(); +// uint32_t MPU6050::dmpGetAccelSumOfSquare(); +// void MPU6050::dmpOverrideQuaternion(long *q); +uint16_t MPU6050::dmpGetFIFOPacketSize() { + return dmpPacketSize; +} + +#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h new file mode 100644 index 00000000..34a4ccc5 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h @@ -0,0 +1,617 @@ +// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 6.12 implementation +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 5/20/2013 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2019/7/10 - I incorporated DMP Firmware Version 6.12 Latest as of today with many features and bug fixes. +// - MPU6050 Registers have not changed just the DMP Image so that full backwards compatibility is present +// - Run-time calibration routine is enabled which calibrates after no motion state is detected +// - once no motion state is detected Calibration completes within 0.5 seconds +// - The Drawback is that the firmware image is larger. +// ... - ongoing debug release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ +#define _MPU6050_6AXIS_MOTIONAPPS20_H_ + +#include "I2Cdev.h" +#include "helper_3dmath.h" + +// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board +#define MPU6050_INCLUDE_DMP_MOTIONAPPS20 // same definitions Should work with V6.12 + +#include "MPU6050.h" + +// Tom Carpenter's conditional PROGMEM code +// http://forum.arduino.cc/index.php?topic=129407.0 +#ifdef __AVR__ + #include +#elif defined(ESP8266) || defined(ESP32) + #include +#else + // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen + #ifndef __PGMSPACE_H_ + #define __PGMSPACE_H_ 1 + #include + + #define PROGMEM + #define PGM_P const char * + #define PSTR(str) (str) + #define F(x) x + + typedef void prog_void; + typedef char prog_char; + typedef unsigned char prog_uchar; + typedef int8_t prog_int8_t; + typedef uint8_t prog_uint8_t; + typedef int16_t prog_int16_t; + typedef uint16_t prog_uint16_t; + typedef int32_t prog_int32_t; + typedef uint32_t prog_uint32_t; + + #define strcpy_P(dest, src) strcpy((dest), (src)) + #define strcat_P(dest, src) strcat((dest), (src)) + #define strcmp_P(a, b) strcmp((a), (b)) + + #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) + #define pgm_read_word(addr) (*(const unsigned short *)(addr)) + #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) + #define pgm_read_float(addr) (*(const float *)(addr)) + + #define pgm_read_byte_near(addr) pgm_read_byte(addr) + #define pgm_read_word_near(addr) pgm_read_word(addr) + #define pgm_read_dword_near(addr) pgm_read_dword(addr) + #define pgm_read_float_near(addr) pgm_read_float(addr) + #define pgm_read_byte_far(addr) pgm_read_byte(addr) + #define pgm_read_word_far(addr) pgm_read_word(addr) + #define pgm_read_dword_far(addr) pgm_read_dword(addr) + #define pgm_read_float_far(addr) pgm_read_float(addr) + #endif +#endif + +/* Source is from the InvenSense MotionApps v2 demo code. Original source is + * unavailable, unless you happen to be amazing as decompiling binary by + * hand (in which case, please contact me, and I'm totally serious). + * + * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the + * DMP reverse-engineering he did to help make this bit of wizardry + * possible. + */ + +// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. +// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) +// after moving string constants to flash memory storage using the F() +// compiler macro (Arduino IDE 1.0+ required). + +//#define DEBUG +#ifdef DEBUG + #define DEBUG_PRINT(x) Serial.print(x) + #define DEBUG_PRINTF(x, y) Serial.print(x, y) + #define DEBUG_PRINTLN(x) Serial.println(x) + #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) +#else + #define DEBUG_PRINT(x) + #define DEBUG_PRINTF(x, y) + #define DEBUG_PRINTLN(x) + #define DEBUG_PRINTLNF(x, y) +#endif + +#define MPU6050_DMP_CODE_SIZE 3062 // dmpMemory[] + +/* ================================================================ * + | Default MotionApps v6.12 28-byte FIFO packet structure: | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 | + | | + | [GYRO X][GYRO Y][GYRO Z][ACC X ][ACC Y ][ACC Z ] | + | 16 17 18 19 20 21 22 23 24 25 26 27 | + * ================================================================ */ + +// this block of memory gets written to the MPU on start-up, and it seems +// to be volatile memory, so it has to be done each time (it only takes ~1 +// second though) + +// *** this is a capture of the DMP Firmware V6.1.2 after all the messy changes were made so we can just load it +const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { +/* bank # 0 */ +0x00, 0xF8, 0xF6, 0x2A, 0x3F, 0x68, 0xF5, 0x7A, 0x00, 0x06, 0xFF, 0xFE, 0x00, 0x03, 0x00, 0x00, +0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, +0x03, 0x0C, 0x30, 0xC3, 0x0A, 0x74, 0x56, 0x2D, 0x0D, 0x62, 0xDB, 0xC7, 0x16, 0xF4, 0xBA, 0x02, +0x38, 0x83, 0xF8, 0x83, 0x30, 0x00, 0xF8, 0x83, 0x25, 0x8E, 0xF8, 0x83, 0x30, 0x00, 0xF8, 0x83, +0xFF, 0xFF, 0xFF, 0xFF, 0x0C, 0xBD, 0xD8, 0x11, 0x24, 0x00, 0x04, 0x00, 0x1A, 0x82, 0x79, 0xA1, +0x00, 0x36, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6F, 0xA2, +0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, +0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, +0x1F, 0xA4, 0xE8, 0xE4, 0xFF, 0xF5, 0xDC, 0xB9, 0x00, 0x5B, 0x79, 0xCF, 0x1F, 0x3F, 0x78, 0x76, +0x00, 0x86, 0x7C, 0x5A, 0x00, 0x86, 0x23, 0x47, 0xFA, 0xB9, 0x86, 0x31, 0x00, 0x74, 0x87, 0x8A, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x43, 0x05, 0xFF, 0xFF, 0xE9, 0xA8, 0x00, 0x00, 0x21, 0x82, +0xFA, 0xB8, 0x4D, 0x46, 0xFF, 0xFA, 0xDF, 0x3D, 0xFF, 0xFF, 0xB2, 0xB3, 0x00, 0x00, 0x00, 0x00, +0x3F, 0xFF, 0xBA, 0x98, 0x00, 0x5D, 0xAC, 0x08, 0x00, 0x0A, 0x63, 0x78, 0x00, 0x01, 0x46, 0x21, +0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x42, 0xB5, 0x00, 0x06, 0x00, 0x64, 0x00, 0x64, 0x00, 0x06, +0x14, 0x06, 0x02, 0x9F, 0x0F, 0x47, 0x91, 0x32, 0xD9, 0x0E, 0x9F, 0xC9, 0x1D, 0xCF, 0x4C, 0x34, +0x3B, 0xB6, 0x7A, 0xE8, 0x00, 0x64, 0x00, 0x06, 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFE, +/* bank # 1 */ +0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x07, 0x00, 0x00, 0xFF, 0xF1, 0x00, 0x00, 0xFA, 0x46, 0x00, 0x00, 0xA2, 0xB8, 0x00, 0x00, +0x10, 0x00, 0x00, 0x00, 0x04, 0xD6, 0x00, 0x00, 0x04, 0xCC, 0x00, 0x00, 0x04, 0xCC, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, +0x00, 0x00, 0x00, 0x32, 0xF8, 0x98, 0x00, 0x00, 0xFF, 0x65, 0x00, 0x00, 0x83, 0x0F, 0x00, 0x00, +0x00, 0x06, 0x00, 0x00, 0xFF, 0xF1, 0x00, 0x00, 0xFA, 0x46, 0x00, 0x00, 0xA2, 0xB8, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, +0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0D, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x02, 0x00, 0x00, +0x00, 0x01, 0xFB, 0x83, 0x00, 0x7C, 0x00, 0x00, 0xFB, 0x15, 0xFC, 0x00, 0x1F, 0xB4, 0xFF, 0x83, +0x00, 0x00, 0x00, 0x01, 0x00, 0x65, 0x00, 0x07, 0x00, 0x64, 0x03, 0xE8, 0x00, 0x64, 0x00, 0x28, +0x00, 0x00, 0xFF, 0xFD, 0x00, 0x00, 0x00, 0x00, 0x16, 0xA0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x10, 0x00, 0x00, 0x2F, 0x00, 0x00, 0x00, 0x00, 0x01, 0xF4, 0x00, 0x00, 0x10, 0x00, +/* bank # 2 */ +0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0xBA, 0xC6, 0x00, 0x47, 0x78, 0xA2, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, +0x00, 0x00, 0x23, 0xBB, 0x00, 0x2E, 0xA2, 0x5B, 0x00, 0x00, 0x05, 0x68, 0x00, 0x0B, 0xCF, 0x49, +0x00, 0x04, 0xFF, 0xFD, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, +0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x64, 0x00, 0x07, 0x00, 0x08, 0x00, 0x06, 0x00, 0x06, 0xFF, 0xFE, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x2E, 0xA2, 0x5B, 0x00, 0x00, 0x05, 0x68, 0x00, 0x0B, 0xCF, 0x49, 0x00, 0x00, 0x00, 0x00, +0x00, 0xF8, 0xF6, 0x2A, 0x3F, 0x68, 0xF5, 0x7A, 0x00, 0x04, 0xFF, 0xFD, 0x00, 0x02, 0x00, 0x00, +0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x0E, +0xFF, 0xFF, 0xFF, 0xCF, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xFF, 0xFF, 0xFF, 0x9C, +0x00, 0x00, 0x43, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, +0xFF, 0xE5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +/* bank # 3 */ +0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xD3, +0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3C, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1F, 0x9E, 0x65, 0x5D, +0x0C, 0x0A, 0x4E, 0x68, 0xCD, 0xCF, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xC6, 0x19, 0xCE, 0x82, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x71, 0x1C, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xD7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, +0x00, 0x11, 0xDC, 0x47, 0x03, 0x00, 0x00, 0x00, 0xC7, 0x93, 0x8F, 0x9D, 0x1E, 0x1B, 0x1C, 0x19, +0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0E, 0xDF, 0xA4, 0x38, 0x1F, 0x9E, 0x65, 0x5D, +0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x71, 0x1C, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x3F, 0xFF, 0xFF, 0xFD, 0xFF, 0xFF, 0xF4, 0xC9, 0xFF, 0xFF, 0xBC, 0xF0, 0x00, 0x01, 0x0C, 0x0F, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0xF5, 0xB7, 0xBA, 0xB3, 0x67, 0x7D, 0xDF, 0x7E, 0x72, 0x90, 0x2E, 0x55, 0x4C, 0xF6, 0xE6, 0x88, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +/* bank # 4 */ +0xD8, 0xDC, 0xB4, 0xB8, 0xB0, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xB3, 0xB7, 0xBB, 0x8E, 0x9E, +0xAE, 0xF1, 0x32, 0xF5, 0x1B, 0xF1, 0xB4, 0xB8, 0xB0, 0x80, 0x97, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, +0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0xF1, 0xA9, +0x89, 0x26, 0x46, 0x66, 0xB2, 0x89, 0x99, 0xA9, 0x2D, 0x55, 0x7D, 0xB0, 0xB0, 0x8A, 0xA8, 0x96, +0x36, 0x56, 0x76, 0xF1, 0xBA, 0xA3, 0xB4, 0xB2, 0x80, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB2, 0x83, +0x98, 0xBA, 0xA3, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xB2, 0xB9, 0xB4, 0x98, 0x83, 0xF1, +0xA3, 0x29, 0x55, 0x7D, 0xBA, 0xB5, 0xB1, 0xA3, 0x83, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xB2, +0xB6, 0xAA, 0x83, 0x93, 0x28, 0x54, 0x7C, 0xF1, 0xB9, 0xA3, 0x82, 0x93, 0x61, 0xBA, 0xA2, 0xDA, +0xDE, 0xDF, 0xDB, 0x81, 0x9A, 0xB9, 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xF1, 0xDA, 0xBA, 0xA2, 0xDF, +0xD9, 0xBA, 0xA2, 0xFA, 0xB9, 0xA3, 0x82, 0x92, 0xDB, 0x31, 0xBA, 0xA2, 0xD9, 0xBA, 0xA2, 0xF8, +0xDF, 0x85, 0xA4, 0xD0, 0xC1, 0xBB, 0xAD, 0x83, 0xC2, 0xC5, 0xC7, 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, +0xBA, 0xA0, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xAA, 0xB3, 0x8D, 0xB4, 0x98, 0x0D, 0x35, +0x5D, 0xB2, 0xB6, 0xBA, 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, +0xB8, 0xAA, 0x87, 0x2C, 0x54, 0x7C, 0xBA, 0xA4, 0xB0, 0x8A, 0xB6, 0x91, 0x32, 0x56, 0x76, 0xB2, +0x84, 0x94, 0xA4, 0xC8, 0x08, 0xCD, 0xD8, 0xB8, 0xB4, 0xB0, 0xF1, 0x99, 0x82, 0xA8, 0x2D, 0x55, +0x7D, 0x98, 0xA8, 0x0E, 0x16, 0x1E, 0xA2, 0x2C, 0x54, 0x7C, 0x92, 0xA4, 0xF0, 0x2C, 0x50, 0x78, +/* bank # 5 */ +0xF1, 0x84, 0xA8, 0x98, 0xC4, 0xCD, 0xFC, 0xD8, 0x0D, 0xDB, 0xA8, 0xFC, 0x2D, 0xF3, 0xD9, 0xBA, +0xA6, 0xF8, 0xDA, 0xBA, 0xA6, 0xDE, 0xD8, 0xBA, 0xB2, 0xB6, 0x86, 0x96, 0xA6, 0xD0, 0xF3, 0xC8, +0x41, 0xDA, 0xA6, 0xC8, 0xF8, 0xD8, 0xB0, 0xB4, 0xB8, 0x82, 0xA8, 0x92, 0xF5, 0x2C, 0x54, 0x88, +0x98, 0xF1, 0x35, 0xD9, 0xF4, 0x18, 0xD8, 0xF1, 0xA2, 0xD0, 0xF8, 0xF9, 0xA8, 0x84, 0xD9, 0xC7, +0xDF, 0xF8, 0xF8, 0x83, 0xC5, 0xDA, 0xDF, 0x69, 0xDF, 0x83, 0xC1, 0xD8, 0xF4, 0x01, 0x14, 0xF1, +0xA8, 0x82, 0x4E, 0xA8, 0x84, 0xF3, 0x11, 0xD1, 0x82, 0xF5, 0xD9, 0x92, 0x28, 0x97, 0x88, 0xF1, +0x09, 0xF4, 0x1C, 0x1C, 0xD8, 0x84, 0xA8, 0xF3, 0xC0, 0xF9, 0xD1, 0xD9, 0x97, 0x82, 0xF1, 0x29, +0xF4, 0x0D, 0xD8, 0xF3, 0xF9, 0xF9, 0xD1, 0xD9, 0x82, 0xF4, 0xC2, 0x03, 0xD8, 0xDE, 0xDF, 0x1A, +0xD8, 0xF1, 0xA2, 0xFA, 0xF9, 0xA8, 0x84, 0x98, 0xD9, 0xC7, 0xDF, 0xF8, 0xF8, 0xF8, 0x83, 0xC7, +0xDA, 0xDF, 0x69, 0xDF, 0xF8, 0x83, 0xC3, 0xD8, 0xF4, 0x01, 0x14, 0xF1, 0x98, 0xA8, 0x82, 0x2E, +0xA8, 0x84, 0xF3, 0x11, 0xD1, 0x82, 0xF5, 0xD9, 0x92, 0x50, 0x97, 0x88, 0xF1, 0x09, 0xF4, 0x1C, +0xD8, 0x84, 0xA8, 0xF3, 0xC0, 0xF8, 0xF9, 0xD1, 0xD9, 0x97, 0x82, 0xF1, 0x49, 0xF4, 0x0D, 0xD8, +0xF3, 0xF9, 0xF9, 0xD1, 0xD9, 0x82, 0xF4, 0xC4, 0x03, 0xD8, 0xDE, 0xDF, 0xD8, 0xF1, 0xAD, 0x88, +0x98, 0xCC, 0xA8, 0x09, 0xF9, 0xD9, 0x82, 0x92, 0xA8, 0xF5, 0x7C, 0xF1, 0x88, 0x3A, 0xCF, 0x94, +0x4A, 0x6E, 0x98, 0xDB, 0x69, 0x31, 0xDA, 0xAD, 0xF2, 0xDE, 0xF9, 0xD8, 0x87, 0x95, 0xA8, 0xF2, +0x21, 0xD1, 0xDA, 0xA5, 0xF9, 0xF4, 0x17, 0xD9, 0xF1, 0xAE, 0x8E, 0xD0, 0xC0, 0xC3, 0xAE, 0x82, +/* bank # 6 */ +0xC6, 0x84, 0xC3, 0xA8, 0x85, 0x95, 0xC8, 0xA5, 0x88, 0xF2, 0xC0, 0xF1, 0xF4, 0x01, 0x0E, 0xF1, +0x8E, 0x9E, 0xA8, 0xC6, 0x3E, 0x56, 0xF5, 0x54, 0xF1, 0x88, 0x72, 0xF4, 0x01, 0x15, 0xF1, 0x98, +0x45, 0x85, 0x6E, 0xF5, 0x8E, 0x9E, 0x04, 0x88, 0xF1, 0x42, 0x98, 0x5A, 0x8E, 0x9E, 0x06, 0x88, +0x69, 0xF4, 0x01, 0x1C, 0xF1, 0x98, 0x1E, 0x11, 0x08, 0xD0, 0xF5, 0x04, 0xF1, 0x1E, 0x97, 0x02, +0x02, 0x98, 0x36, 0x25, 0xDB, 0xF9, 0xD9, 0x85, 0xA5, 0xF3, 0xC1, 0xDA, 0x85, 0xA5, 0xF3, 0xDF, +0xD8, 0x85, 0x95, 0xA8, 0xF3, 0x09, 0xDA, 0xA5, 0xFA, 0xD8, 0x82, 0x92, 0xA8, 0xF5, 0x78, 0xF1, +0x88, 0x1A, 0x84, 0x9F, 0x26, 0x88, 0x98, 0x21, 0xDA, 0xF4, 0x1D, 0xF3, 0xD8, 0x87, 0x9F, 0x39, +0xD1, 0xAF, 0xD9, 0xDF, 0xDF, 0xFB, 0xF9, 0xF4, 0x0C, 0xF3, 0xD8, 0xFA, 0xD0, 0xF8, 0xDA, 0xF9, +0xF9, 0xD0, 0xDF, 0xD9, 0xF9, 0xD8, 0xF4, 0x0B, 0xD8, 0xF3, 0x87, 0x9F, 0x39, 0xD1, 0xAF, 0xD9, +0xDF, 0xDF, 0xF4, 0x1D, 0xF3, 0xD8, 0xFA, 0xFC, 0xA8, 0x69, 0xF9, 0xF9, 0xAF, 0xD0, 0xDA, 0xDE, +0xFA, 0xD9, 0xF8, 0x8F, 0x9F, 0xA8, 0xF1, 0xCC, 0xF3, 0x98, 0xDB, 0x45, 0xD9, 0xAF, 0xDF, 0xD0, +0xF8, 0xD8, 0xF1, 0x8F, 0x9F, 0xA8, 0xCA, 0xF3, 0x88, 0x09, 0xDA, 0xAF, 0x8F, 0xCB, 0xF8, 0xD8, +0xF2, 0xAD, 0x97, 0x8D, 0x0C, 0xD9, 0xA5, 0xDF, 0xF9, 0xBA, 0xA6, 0xF3, 0xFA, 0xF4, 0x12, 0xF2, +0xD8, 0x95, 0x0D, 0xD1, 0xD9, 0xBA, 0xA6, 0xF3, 0xFA, 0xDA, 0xA5, 0xF2, 0xC1, 0xBA, 0xA6, 0xF3, +0xDF, 0xD8, 0xF1, 0xBA, 0xB2, 0xB6, 0x86, 0x96, 0xA6, 0xD0, 0xCA, 0xF3, 0x49, 0xDA, 0xA6, 0xCB, +0xF8, 0xD8, 0xB0, 0xB4, 0xB8, 0xD8, 0xAD, 0x84, 0xF2, 0xC0, 0xDF, 0xF1, 0x8F, 0xCB, 0xC3, 0xA8, +/* bank # 7 */ +0xB2, 0xB6, 0x86, 0x96, 0xC8, 0xC1, 0xCB, 0xC3, 0xF3, 0xB0, 0xB4, 0x88, 0x98, 0xA8, 0x21, 0xDB, +0x71, 0x8D, 0x9D, 0x71, 0x85, 0x95, 0x21, 0xD9, 0xAD, 0xF2, 0xFA, 0xD8, 0x85, 0x97, 0xA8, 0x28, +0xD9, 0xF4, 0x08, 0xD8, 0xF2, 0x8D, 0x29, 0xDA, 0xF4, 0x05, 0xD9, 0xF2, 0x85, 0xA4, 0xC2, 0xF2, +0xD8, 0xA8, 0x8D, 0x94, 0x01, 0xD1, 0xD9, 0xF4, 0x11, 0xF2, 0xD8, 0x87, 0x21, 0xD8, 0xF4, 0x0A, +0xD8, 0xF2, 0x84, 0x98, 0xA8, 0xC8, 0x01, 0xD1, 0xD9, 0xF4, 0x11, 0xD8, 0xF3, 0xA4, 0xC8, 0xBB, +0xAF, 0xD0, 0xF2, 0xDE, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xD8, 0xF1, 0xB8, 0xF6, +0xB5, 0xB9, 0xB0, 0x8A, 0x95, 0xA3, 0xDE, 0x3C, 0xA3, 0xD9, 0xF8, 0xD8, 0x5C, 0xA3, 0xD9, 0xF8, +0xD8, 0x7C, 0xA3, 0xD9, 0xF8, 0xD8, 0xF8, 0xF9, 0xD1, 0xA5, 0xD9, 0xDF, 0xDA, 0xFA, 0xD8, 0xB1, +0x85, 0x30, 0xF7, 0xD9, 0xDE, 0xD8, 0xF8, 0x30, 0xAD, 0xDA, 0xDE, 0xD8, 0xF2, 0xB4, 0x8C, 0x99, +0xA3, 0x2D, 0x55, 0x7D, 0xA0, 0x83, 0xDF, 0xDF, 0xDF, 0xB5, 0x91, 0xA0, 0xF6, 0x29, 0xD9, 0xFB, +0xD8, 0xA0, 0xFC, 0x29, 0xD9, 0xFA, 0xD8, 0xA0, 0xD0, 0x51, 0xD9, 0xF8, 0xD8, 0xFC, 0x51, 0xD9, +0xF9, 0xD8, 0x79, 0xD9, 0xFB, 0xD8, 0xA0, 0xD0, 0xFC, 0x79, 0xD9, 0xFA, 0xD8, 0xA1, 0xF9, 0xF9, +0xF9, 0xF9, 0xF9, 0xA0, 0xDA, 0xDF, 0xDF, 0xDF, 0xD8, 0xA1, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xAC, +0xDE, 0xF8, 0xAD, 0xDE, 0x83, 0x93, 0xAC, 0x2C, 0x54, 0x7C, 0xF1, 0xA8, 0xDF, 0xDF, 0xDF, 0xF6, +0x9D, 0x2C, 0xDA, 0xA0, 0xDF, 0xD9, 0xFA, 0xDB, 0x2D, 0xF8, 0xD8, 0xA8, 0x50, 0xDA, 0xA0, 0xD0, +0xDE, 0xD9, 0xD0, 0xF8, 0xF8, 0xF8, 0xDB, 0x55, 0xF8, 0xD8, 0xA8, 0x78, 0xDA, 0xA0, 0xD0, 0xDF, +/* bank # 8 */ +0xD9, 0xD0, 0xFA, 0xF8, 0xF8, 0xF8, 0xF8, 0xDB, 0x7D, 0xF8, 0xD8, 0x9C, 0xA8, 0x8C, 0xF5, 0x30, +0xDB, 0x38, 0xD9, 0xD0, 0xDE, 0xDF, 0xA0, 0xD0, 0xDE, 0xDF, 0xD8, 0xA8, 0x48, 0xDB, 0x58, 0xD9, +0xDF, 0xD0, 0xDE, 0xA0, 0xDF, 0xD0, 0xDE, 0xD8, 0xA8, 0x68, 0xDB, 0x70, 0xD9, 0xDF, 0xDF, 0xA0, +0xDF, 0xDF, 0xD8, 0xF1, 0xA8, 0x88, 0x90, 0x2C, 0x54, 0x7C, 0x98, 0xA8, 0xD0, 0x5C, 0x38, 0xD1, +0xDA, 0xF2, 0xAE, 0x8C, 0xDF, 0xF9, 0xD8, 0xB0, 0x87, 0xA8, 0xC1, 0xC1, 0xB1, 0x88, 0xA8, 0xC6, +0xF9, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, +0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xF7, 0x8D, 0x9D, 0xAD, 0xF8, 0x18, 0xDA, +0xF2, 0xAE, 0xDF, 0xD8, 0xF7, 0xAD, 0xFA, 0x30, 0xD9, 0xA4, 0xDE, 0xF9, 0xD8, 0xF2, 0xAE, 0xDE, +0xFA, 0xF9, 0x83, 0xA7, 0xD9, 0xC3, 0xC5, 0xC7, 0xF1, 0x88, 0x9B, 0xA7, 0x7A, 0xAD, 0xF7, 0xDE, +0xDF, 0xA4, 0xF8, 0x84, 0x94, 0x08, 0xA7, 0x97, 0xF3, 0x00, 0xAE, 0xF2, 0x98, 0x19, 0xA4, 0x88, +0xC6, 0xA3, 0x94, 0x88, 0xF6, 0x32, 0xDF, 0xF2, 0x83, 0x93, 0xDB, 0x09, 0xD9, 0xF2, 0xAA, 0xDF, +0xD8, 0xD8, 0xAE, 0xF8, 0xF9, 0xD1, 0xDA, 0xF3, 0xA4, 0xDE, 0xA7, 0xF1, 0x88, 0x9B, 0x7A, 0xD8, +0xF3, 0x84, 0x94, 0xAE, 0x19, 0xF9, 0xDA, 0xAA, 0xF1, 0xDF, 0xD8, 0xA8, 0x81, 0xC0, 0xC3, 0xC5, +0xC7, 0xA3, 0x92, 0x83, 0xF6, 0x28, 0xAD, 0xDE, 0xD9, 0xF8, 0xD8, 0xA3, 0x50, 0xAD, 0xD9, 0xF8, +0xD8, 0xA3, 0x78, 0xAD, 0xD9, 0xF8, 0xD8, 0xF8, 0xF9, 0xD1, 0xA1, 0xDA, 0xDE, 0xC3, 0xC5, 0xC7, +0xD8, 0xA1, 0x81, 0x94, 0xF8, 0x18, 0xF2, 0xB0, 0x89, 0xAC, 0xC3, 0xC5, 0xC7, 0xF1, 0xD8, 0xB8, +/* bank # 9 */ +0xB4, 0xB0, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, +0x0C, 0x20, 0x14, 0x40, 0xB0, 0xB4, 0xB8, 0xF0, 0xA8, 0x8A, 0x9A, 0x28, 0x50, 0x78, 0xB7, 0x9B, +0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xF1, 0xBB, 0xAB, +0x88, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0xB3, 0x8B, 0xB8, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0xB0, +0x88, 0xB4, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xBB, 0xAB, 0xB3, 0x8B, 0x02, 0x26, 0x46, 0x66, 0xB0, +0xB8, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 0x8A, 0x24, 0x70, 0x59, +0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 0x8A, 0x64, 0x48, 0x31, +0x8B, 0x30, 0x49, 0x60, 0x88, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 0x28, +0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 0xF0, +0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xA9, +0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 0x8C, +0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 0x7E, +0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB5, 0xB9, 0xA3, 0xDF, 0xDF, 0xDF, 0xAE, 0xD0, +0xDF, 0xAA, 0xD0, 0xDE, 0xF2, 0xAB, 0xF8, 0xF9, 0xD9, 0xB0, 0x87, 0xC4, 0xAA, 0xF1, 0xDF, 0xDF, +0xBB, 0xAF, 0xDF, 0xDF, 0xB9, 0xD8, 0xB1, 0xF1, 0xA3, 0x97, 0x8E, 0x60, 0xDF, 0xB0, 0x84, 0xF2, +0xC8, 0xF8, 0xF9, 0xD9, 0xDE, 0xD8, 0x93, 0x85, 0xF1, 0x4A, 0xB1, 0x83, 0xA3, 0x08, 0xB5, 0x83, +/* bank # 10 */ +0x9A, 0x08, 0x10, 0xB7, 0x9F, 0x10, 0xD8, 0xF1, 0xB0, 0xBA, 0xAE, 0xB0, 0x8A, 0xC2, 0xB2, 0xB6, +0x8E, 0x9E, 0xF1, 0xFB, 0xD9, 0xF4, 0x1D, 0xD8, 0xF9, 0xD9, 0x0C, 0xF1, 0xD8, 0xF8, 0xF8, 0xAD, +0x61, 0xD9, 0xAE, 0xFB, 0xD8, 0xF4, 0x0C, 0xF1, 0xD8, 0xF8, 0xF8, 0xAD, 0x19, 0xD9, 0xAE, 0xFB, +0xDF, 0xD8, 0xF4, 0x16, 0xF1, 0xD8, 0xF8, 0xAD, 0x8D, 0x61, 0xD9, 0xF4, 0xF4, 0xAC, 0xF5, 0x9C, +0x9C, 0x8D, 0xDF, 0x2B, 0xBA, 0xB6, 0xAE, 0xFA, 0xF8, 0xF4, 0x0B, 0xD8, 0xF1, 0xAE, 0xD0, 0xF8, +0xAD, 0x51, 0xDA, 0xAE, 0xFA, 0xF8, 0xF1, 0xD8, 0xB9, 0xB1, 0xB6, 0xA3, 0x83, 0x9C, 0x08, 0xB9, +0xB1, 0x83, 0x9A, 0xB5, 0xAA, 0xC0, 0xFD, 0x30, 0x83, 0xB7, 0x9F, 0x10, 0xB5, 0x8B, 0x93, 0xF2, +0x02, 0x02, 0xD1, 0xAB, 0xDA, 0xDE, 0xD8, 0xF1, 0xB0, 0x80, 0xBA, 0xAB, 0xC0, 0xC3, 0xB2, 0x84, +0xC1, 0xC3, 0xD8, 0xB1, 0xB9, 0xF3, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, +0x87, 0x9C, 0xB9, 0xA3, 0xDD, 0xF1, 0xB3, 0x8B, 0x8B, 0x8B, 0x8B, 0x8B, 0xB0, 0x87, 0x20, 0x28, +0x30, 0x38, 0xB2, 0x8B, 0xB6, 0x9B, 0xF2, 0xA3, 0xC0, 0xC8, 0xC2, 0xC4, 0xCC, 0xC6, 0xA3, 0xA3, +0xA3, 0xF1, 0xB0, 0x87, 0xB5, 0x9A, 0xD8, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 0xBA, 0xAC, 0xDF, 0xB9, +0xA3, 0xFE, 0xF2, 0xAB, 0xC4, 0xAA, 0xF1, 0xDF, 0xDF, 0xBB, 0xAF, 0xDF, 0xDF, 0xA3, 0xA3, 0xA3, +0xD8, 0xD8, 0xD8, 0xBB, 0xB3, 0xB7, 0xF1, 0xAA, 0xF9, 0xDA, 0xFF, 0xD9, 0x80, 0x9A, 0xAA, 0x28, +0xB4, 0x80, 0x98, 0xA7, 0x20, 0xB7, 0x97, 0x87, 0xA8, 0x66, 0x88, 0xF0, 0x79, 0x51, 0xF1, 0x90, +0x2C, 0x87, 0x0C, 0xA7, 0x81, 0x97, 0x62, 0x93, 0xF0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, +/* bank # 11 */ +0x51, 0x79, 0x90, 0xA5, 0xF1, 0x28, 0x4C, 0x6C, 0x87, 0x0C, 0x95, 0x18, 0x85, 0x78, 0xA3, 0x83, +0x90, 0x28, 0x4C, 0x6C, 0x88, 0x6C, 0xD8, 0xF3, 0xA2, 0x82, 0x00, 0xF2, 0x10, 0xA8, 0x92, 0x19, +0x80, 0xA2, 0xF2, 0xD9, 0x26, 0xD8, 0xF1, 0x88, 0xA8, 0x4D, 0xD9, 0x48, 0xD8, 0x96, 0xA8, 0x39, +0x80, 0xD9, 0x3C, 0xD8, 0x95, 0x80, 0xA8, 0x39, 0xA6, 0x86, 0x98, 0xD9, 0x2C, 0xDA, 0x87, 0xA7, +0x2C, 0xD8, 0xA8, 0x89, 0x95, 0x19, 0xA9, 0x80, 0xD9, 0x38, 0xD8, 0xA8, 0x89, 0x39, 0xA9, 0x80, +0xDA, 0x3C, 0xD8, 0xA8, 0x2E, 0xA8, 0x39, 0x90, 0xD9, 0x0C, 0xD8, 0xA8, 0x95, 0x31, 0x98, 0xD9, +0x0C, 0xD8, 0xA8, 0x09, 0xD9, 0xFF, 0xD8, 0x01, 0xDA, 0xFF, 0xD8, 0x95, 0x39, 0xA9, 0xDA, 0x26, +0xFF, 0xD8, 0x90, 0xA8, 0x0D, 0x89, 0x99, 0xA8, 0x10, 0x80, 0x98, 0x21, 0xDA, 0x2E, 0xD8, 0x89, +0x99, 0xA8, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, 0x86, 0x96, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, +0x87, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, 0x82, 0x92, 0xF3, 0x41, 0x80, 0xF1, 0xD9, 0x2E, 0xD8, +0xA8, 0x82, 0xF3, 0x19, 0x80, 0xF1, 0xD9, 0x2E, 0xD8, 0x82, 0xAC, 0xF3, 0xC0, 0xA2, 0x80, 0x22, +0xF1, 0xA6, 0x2E, 0xA7, 0x2E, 0xA9, 0x22, 0x98, 0xA8, 0x29, 0xDA, 0xAC, 0xDE, 0xFF, 0xD8, 0xA2, +0xF2, 0x2A, 0xF1, 0xA9, 0x2E, 0x82, 0x92, 0xA8, 0xF2, 0x31, 0x80, 0xA6, 0x96, 0xF1, 0xD9, 0x00, +0xAC, 0x8C, 0x9C, 0x0C, 0x30, 0xAC, 0xDE, 0xD0, 0xDE, 0xFF, 0xD8, 0x8C, 0x9C, 0xAC, 0xD0, 0x10, +0xAC, 0xDE, 0x80, 0x92, 0xA2, 0xF2, 0x4C, 0x82, 0xA8, 0xF1, 0xCA, 0xF2, 0x35, 0xF1, 0x96, 0x88, +0xA6, 0xD9, 0x00, 0xD8, 0xF1, 0xFF, +}; + +// this divisor is pre configured into the above image and can't be modified at this time. +#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR +#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default +#endif + +// this is the most basic initialization I can create. with the intent that we access the register bytes as few times as needed to get the job done. +// for detailed descriptins of all registers and there purpose google "MPU-6000/MPU-6050 Register Map and Descriptions" +uint8_t MPU6050::dmpInitialize() { // Lets get it over with fast Write everything once and set it up necely + uint8_t val; + uint16_t ival; + // Reset procedure per instructions in the "MPU-6000/MPU-6050 Register Map and Descriptions" page 41 + I2Cdev::writeBit(devAddr,0x6B, 7, (val = 1)); //PWR_MGMT_1: reset with 100ms delay + delay(100); + I2Cdev::writeBits(devAddr,0x6A, 2, 3, (val = 0b111)); // full SIGNAL_PATH_RESET: with another 100ms delay + delay(100); + I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 1000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro + I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x00)); // 0000 0000 INT_ENABLE: no Interrupt + I2Cdev::writeBytes(devAddr,0x23, 1, &(val = 0x00)); // 0000 0000 MPU FIFO_EN: (all off) Using DMP's FIFO instead + I2Cdev::writeBytes(devAddr,0x1C, 1, &(val = 0x00)); // 0000 0000 ACCEL_CONFIG: 0 = Accel Full Scale Select: 2g + I2Cdev::writeBytes(devAddr,0x37, 1, &(val = 0x80)); // 1001 0000 INT_PIN_CFG: ACTL The logic level for int pin is active low. and interrupt status bits are cleared on any read + I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 0000 0001 PWR_MGMT_1: Clock Source Select PLL_X_gyro + I2Cdev::writeBytes(devAddr,0x19, 1, &(val = 0x04)); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)) + I2Cdev::writeBytes(devAddr,0x1A, 1, &(val = 0x01)); // 0000 0001 CONFIG: Digital Low Pass Filter (DLPF) Configuration 188HZ //Im betting this will be the beat + if (!writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) return 1; // Loads the DMP image into the MPU6050 Memory // Should Never Fail + I2Cdev::writeWords(devAddr, 0x70, 1, &(ival = 0x0400)); // DMP Program Start Address + I2Cdev::writeBytes(devAddr,0x1B, 1, &(val = 0x18)); // 0001 1000 GYRO_CONFIG: 3 = +2000 Deg/sec + I2Cdev::writeBytes(devAddr,0x6A, 1, &(val = 0xC0)); // 1100 1100 USER_CTRL: Enable Fifo and Reset Fifo + I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x02)); // 0000 0010 INT_ENABLE: RAW_DMP_INT_EN on + I2Cdev::writeBit(devAddr,0x6A, 2, 1); // Reset FIFO one last time just for kicks. (MPUi2cWrite reads 0x6A first and only alters 1 bit and then saves the byte) + + setDMPEnabled(false); // disable DMP for compatibility with the MPU6050 library +/* + dmpPacketSize += 16;//DMP_FEATURE_6X_LP_QUAT + dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_ACCEL + dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_GYRO +*/ + dmpPacketSize = 28; + return 0; +} + +bool MPU6050::dmpPacketAvailable() { + return getFIFOCount() >= dmpGetFIFOPacketSize(); +} + +// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); +// uint8_t MPU6050::dmpGetFIFORate(); +// uint8_t MPU6050::dmpGetSampleStepSizeMS(); +// uint8_t MPU6050::dmpGetSampleFrequency(); +// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); + +//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); +//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); +//uint8_t MPU6050::dmpRunFIFORateProcesses(); + +// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + +uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[16] << 8) | packet[17]); + data[1] = (((uint32_t)packet[18] << 8) | packet[19]); + data[2] = (((uint32_t)packet[20] << 8) | packet[21]); + return 0; +} +uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[16] << 8) | packet[17]; + data[1] = (packet[18] << 8) | packet[19]; + data[2] = (packet[20] << 8) | packet[21]; + return 0; +} +uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[16] << 8) | packet[17]; + v -> y = (packet[18] << 8) | packet[19]; + v -> z = (packet[20] << 8) | packet[21]; + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); + data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); + data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); + data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[0] << 8) | packet[1]); + data[1] = ((packet[4] << 8) | packet[5]); + data[2] = ((packet[8] << 8) | packet[9]); + data[3] = ((packet[12] << 8) | packet[13]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + if (status == 0) { + q -> w = (float)qI[0] / 16384.0f; + q -> x = (float)qI[1] / 16384.0f; + q -> y = (float)qI[2] / 16384.0f; + q -> z = (float)qI[3] / 16384.0f; + return 0; + } + return status; // int16 return value, indicates error if this line is reached +} +// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[22] << 8) | packet[23]); + data[1] = (((uint32_t)packet[24] << 8) | packet[25]); + data[2] = (((uint32_t)packet[26] << 8) | packet[27]); + return 0; +} +uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[22] << 8) | packet[23]; + data[1] = (packet[24] << 8) | packet[25]; + data[2] = (packet[26] << 8) | packet[27]; + return 0; +} +uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[22] << 8) | packet[23]; + v -> y = (packet[24] << 8) | packet[25]; + v -> z = (packet[26] << 8) | packet[27]; + return 0; +} +// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); +// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { + // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) + v -> x = vRaw -> x - gravity -> x*8192; + v -> y = vRaw -> y - gravity -> y*8192; + v -> z = vRaw -> z - gravity -> z*8192; + return 0; +} +// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { + // rotate measured 3D acceleration vector into original state + // frame of reference based on orientation quaternion + memcpy(v, vReal, sizeof(VectorInt16)); + v -> rotate(q); + return 0; +} +// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); + return status; +} + +uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { + v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); + v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); + v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; + return 0; +} +// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); +// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); + +uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi + data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta + data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi + return 0; +} + +#ifdef USE_OLD_DMPGETYAWPITCHROLL +uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); + return 0; +} +#else +uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan2(gravity -> y , gravity -> z); + if (gravity -> z < 0) { + if(data[1] > 0) { + data[1] = PI - data[1]; + } else { + data[1] = -PI - data[1]; + } + } + return 0; +} +#endif + +// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); + +uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { + /*for (uint8_t k = 0; k < dmpPacketSize; k++) { + if (dmpData[k] < 0x10) Serial.print("0"); + Serial.print(dmpData[k], HEX); + Serial.print(" "); + } + Serial.print("\n");*/ + //Serial.println((uint16_t)dmpPacketBuffer); + return 0; +} +uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { + uint8_t status; + uint8_t buf[dmpPacketSize]; + for (uint8_t i = 0; i < numPackets; i++) { + // read packet from FIFO + getFIFOBytes(buf, dmpPacketSize); + + // process packet + if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; + + // increment external process count variable, if supplied + if (processed != 0) (*processed)++; + } + return 0; +} + +// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); + +// uint8_t MPU6050::dmpInitFIFOParam(); +// uint8_t MPU6050::dmpCloseFIFO(); +// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); +// uint8_t MPU6050::dmpDecodeQuantizedAccel(); +// uint32_t MPU6050::dmpGetGyroSumOfSquare(); +// uint32_t MPU6050::dmpGetAccelSumOfSquare(); +// void MPU6050::dmpOverrideQuaternion(long *q); +uint16_t MPU6050::dmpGetFIFOPacketSize() { + return dmpPacketSize; +} + +#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050_9Axis_MotionApps41.h b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050_9Axis_MotionApps41.h new file mode 100644 index 00000000..763a49b9 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/MPU6050_9Axis_MotionApps41.h @@ -0,0 +1,887 @@ +// I2Cdev library collection - MPU6050 I2C device class, 9-axis MotionApps 4.1 implementation +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 6/18/2012 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// ... - ongoing debug release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _MPU6050_9AXIS_MOTIONAPPS41_H_ +#define _MPU6050_9AXIS_MOTIONAPPS41_H_ + +#include "I2Cdev.h" +#include "helper_3dmath.h" + +// MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board +#define MPU6050_INCLUDE_DMP_MOTIONAPPS41 + +#include "MPU6050.h" + +// Tom Carpenter's conditional PROGMEM code +// http://forum.arduino.cc/index.php?topic=129407.0 +#ifdef __AVR__ + #include +#else + // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen + #ifndef __PGMSPACE_H_ + #define __PGMSPACE_H_ 1 + #include + + #define PROGMEM + #define PGM_P const char * + #define PSTR(str) (str) + #define F(x) x + + typedef void prog_void; + typedef char prog_char; + //typedef unsigned char prog_uchar; + typedef int8_t prog_int8_t; + typedef uint8_t prog_uint8_t; + typedef int16_t prog_int16_t; + typedef uint16_t prog_uint16_t; + typedef int32_t prog_int32_t; + typedef uint32_t prog_uint32_t; + + #define strcpy_P(dest, src) strcpy((dest), (src)) + #define strcat_P(dest, src) strcat((dest), (src)) + #define strcmp_P(a, b) strcmp((a), (b)) + + #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) + #define pgm_read_word(addr) (*(const unsigned short *)(addr)) + #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) + #define pgm_read_float(addr) (*(const float *)(addr)) + + #define pgm_read_byte_near(addr) pgm_read_byte(addr) + #define pgm_read_word_near(addr) pgm_read_word(addr) + #define pgm_read_dword_near(addr) pgm_read_dword(addr) + #define pgm_read_float_near(addr) pgm_read_float(addr) + #define pgm_read_byte_far(addr) pgm_read_byte(addr) + #define pgm_read_word_far(addr) pgm_read_word(addr) + #define pgm_read_dword_far(addr) pgm_read_dword(addr) + #define pgm_read_float_far(addr) pgm_read_float(addr) + #endif +#endif + +// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. +// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) +// after moving string constants to flash memory storage using the F() +// compiler macro (Arduino IDE 1.0+ required). + +//#define DEBUG +#ifdef DEBUG + #define DEBUG_PRINT(x) Serial.print(x) + #define DEBUG_PRINTF(x, y) Serial.print(x, y) + #define DEBUG_PRINTLN(x) Serial.println(x) + #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) +#else + #define DEBUG_PRINT(x) + #define DEBUG_PRINTF(x, y) + #define DEBUG_PRINTLN(x) + #define DEBUG_PRINTLNF(x, y) +#endif + +#define MPU6050_DMP_CODE_SIZE 1962 // dmpMemory[] +#define MPU6050_DMP_CONFIG_SIZE 232 // dmpConfig[] +#define MPU6050_DMP_UPDATES_SIZE 140 // dmpUpdates[] + +/* ================================================================================================ * + | Default MotionApps v4.1 48-byte FIFO packet structure: | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | + | | + | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | + | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | + * ================================================================================================ */ + +// this block of memory gets written to the MPU on start-up, and it seems +// to be volatile memory, so it has to be done each time (it only takes ~1 +// second though) +const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { + // bank 0, 256 bytes + 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, + 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, + 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, + 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, + + // bank 1, 256 bytes + 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, + 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, + 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, + 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, + + // bank 2, 256 bytes + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + + // bank 3, 256 bytes + 0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4, + 0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, + 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, + 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, + 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, + 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, + 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, + 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, + 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, + 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, + 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, + 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, + 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, + 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54, + 0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D, + 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86, + + // bank 4, 256 bytes + 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, + 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, + 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, + 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, + 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, + 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, + 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, + 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, + 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, + 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, + 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, + 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, + + // bank 5, 256 bytes + 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, + 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, + 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, + 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, + 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, + 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, + 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, + 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, + 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, + 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, + 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86, + 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, + 0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9, + 0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30, + 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0, + 0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24, + + // bank 6, 256 bytes + 0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40, + 0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, + 0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, + 0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, + 0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, + 0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, + 0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, + 0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, + 0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, + 0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, + 0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, + 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, + 0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, + 0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, + 0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, + 0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, + + // bank 7, 170 bytes (remainder) + 0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, + 0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, + 0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, + 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, + 0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, + 0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9, + 0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, + 0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, + 0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, + 0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, + 0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF +}; + +#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR +#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x03 +#endif + +const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { +// BANK OFFSET LENGTH [DATA] + 0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ? + 0x03, 0x82, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration + 0x03, 0xB2, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration + 0x00, 0x68, 0x04, 0x02, 0xCA, 0xE3, 0x09, // D_0_104 inv_set_gyro_calibration + 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration + 0x03, 0x86, 0x03, 0x0C, 0xC9, 0x2C, // FCFG_2 inv_set_accel_calibration + 0x03, 0x90, 0x03, 0x26, 0x46, 0x66, // (continued)...FCFG_2 inv_set_accel_calibration + 0x00, 0x6C, 0x02, 0x40, 0x00, // D_0_108 inv_set_accel_calibration + + 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration + 0x02, 0x44, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_01 + 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 + 0x02, 0x4C, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_10 + 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 + 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 + 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 + 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 + 0x02, 0xBC, 0x04, 0xC0, 0x00, 0x00, 0x00, // CPASS_MTX_22 + + 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel + 0x03, 0x86, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors + 0x04, 0x22, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion + 0x00, 0xA3, 0x01, 0x00, // ? + 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update + 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion + 0x07, 0x9F, 0x01, 0x30, // CFG_16 inv_set_footer + 0x07, 0x67, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro + 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo + 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? + 0x02, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // ? + 0x07, 0x83, 0x06, 0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ? + // SPECIAL 0x01 = enable interrupts + 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION + 0x07, 0xA7, 0x01, 0xFE, // ? + 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? + 0x07, 0x67, 0x01, 0x9A, // ? + 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo + 0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo + 0x02, 0x16, 0x02, 0x00, MPU6050_DMP_FIFO_RATE_DIVISOR // D_0_22 inv_set_fifo_rate + + // This very last 0x03 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, + // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. + // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) + + // It is important to make sure the host processor can keep up with reading and processing + // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. +}; + +const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { + 0x01, 0xB2, 0x02, 0xFF, 0xF5, + 0x01, 0x90, 0x04, 0x0A, 0x0D, 0x97, 0xC0, + 0x00, 0xA3, 0x01, 0x00, + 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, + 0x01, 0x6A, 0x02, 0x06, 0x00, + 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, + 0x02, 0x60, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x01, 0x08, 0x02, 0x01, 0x20, + 0x01, 0x0A, 0x02, 0x00, 0x4E, + 0x01, 0x02, 0x02, 0xFE, 0xB3, + 0x02, 0x6C, 0x04, 0x00, 0x00, 0x00, 0x00, // READ + 0x02, 0x6C, 0x04, 0xFA, 0xFE, 0x00, 0x00, + 0x02, 0x60, 0x0C, 0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C, + 0x02, 0xF4, 0x04, 0x00, 0x00, 0x00, 0x00, + 0x02, 0xF8, 0x04, 0x00, 0x00, 0x00, 0x00, + 0x02, 0xFC, 0x04, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 +}; + +uint8_t MPU6050::dmpInitialize() { + // reset device + DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); + reset(); + delay(30); // wait after reset + + // disable sleep mode + DEBUG_PRINTLN(F("Disabling sleep mode...")); + setSleepEnabled(false); + + // get MPU product ID + DEBUG_PRINTLN(F("Getting product ID...")); + //uint8_t productID = 0; //getProductID(); + DEBUG_PRINT(F("Product ID = ")); + DEBUG_PRINT(productID); + + // get MPU hardware revision + DEBUG_PRINTLN(F("Selecting user bank 16...")); + setMemoryBank(0x10, true, true); + DEBUG_PRINTLN(F("Selecting memory byte 6...")); + setMemoryStartAddress(0x06); + DEBUG_PRINTLN(F("Checking hardware revision...")); + uint8_t hwRevision = readMemoryByte(); + DEBUG_PRINT(F("Revision @ user[16][6] = ")); + DEBUG_PRINTLNF(hwRevision, HEX); + DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); + setMemoryBank(0, false, false); + + // check OTP bank valid + DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); + uint8_t otpValid = getOTPBankValid(); + DEBUG_PRINT(F("OTP bank is ")); + DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); + + // get X/Y/Z gyro offsets + DEBUG_PRINTLN(F("Reading gyro offset values...")); + int8_t xgOffset = getXGyroOffset(); + int8_t ygOffset = getYGyroOffset(); + int8_t zgOffset = getZGyroOffset(); + DEBUG_PRINT(F("X gyro offset = ")); + DEBUG_PRINTLN(xgOffset); + DEBUG_PRINT(F("Y gyro offset = ")); + DEBUG_PRINTLN(ygOffset); + DEBUG_PRINT(F("Z gyro offset = ")); + DEBUG_PRINTLN(zgOffset); + + I2Cdev::readByte(devAddr, MPU6050_RA_USER_CTRL, buffer); // ? + + DEBUG_PRINTLN(F("Enabling interrupt latch, clear on any read, AUX bypass enabled")); + I2Cdev::writeByte(devAddr, MPU6050_RA_INT_PIN_CFG, 0x32); + + // enable MPU AUX I2C bypass mode + //DEBUG_PRINTLN(F("Enabling AUX I2C bypass mode...")); + //setI2CBypassEnabled(true); + + DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); + //mag -> setMode(0); + I2Cdev::writeByte(0x0E, 0x0A, 0x00); + + DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access...")); + //mag -> setMode(0x0F); + I2Cdev::writeByte(0x0E, 0x0A, 0x0F); + + DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration...")); + int8_t asax, asay, asaz; + //mag -> getAdjustment(&asax, &asay, &asaz); + I2Cdev::readBytes(0x0E, 0x10, 3, buffer); + asax = (int8_t)buffer[0]; + asay = (int8_t)buffer[1]; + asaz = (int8_t)buffer[2]; + DEBUG_PRINT(F("Adjustment X/Y/Z = ")); + DEBUG_PRINT(asax); + DEBUG_PRINT(F(" / ")); + DEBUG_PRINT(asay); + DEBUG_PRINT(F(" / ")); + DEBUG_PRINTLN(asaz); + + DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); + //mag -> setMode(0); + I2Cdev::writeByte(0x0E, 0x0A, 0x00); + + // load DMP code into memory banks + DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); + DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); + DEBUG_PRINTLN(F(" bytes)")); + if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { + DEBUG_PRINTLN(F("Success! DMP code written and verified.")); + + DEBUG_PRINTLN(F("Configuring DMP and related settings...")); + + // write DMP configuration + DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); + DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); + DEBUG_PRINTLN(F(" bytes in config def)")); + if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { + DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); + + DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); + setIntEnabled(1< setMode(1); + I2Cdev::writeByte(0x0E, 0x0A, 0x01); + + // setup AK8975 (0x0E) as Slave 0 in read mode + DEBUG_PRINTLN(F("Setting up AK8975 read slave 0...")); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_ADDR, 0x8E); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_REG, 0x01); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_CTRL, 0xDA); + + // setup AK8975 (0x0E) as Slave 2 in write mode + DEBUG_PRINTLN(F("Setting up AK8975 write slave 2...")); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_ADDR, 0x0E); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_REG, 0x0A); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_CTRL, 0x81); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_DO, 0x01); + + // setup I2C timing/delay control + DEBUG_PRINTLN(F("Setting up slave access delay...")); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV4_CTRL, 0x18); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05); + + // enable interrupts + DEBUG_PRINTLN(F("Enabling default interrupt behavior/no bypass...")); + I2Cdev::writeByte(0x68, MPU6050_RA_INT_PIN_CFG, 0x00); + + // enable I2C master mode and reset DMP/FIFO + DEBUG_PRINTLN(F("Enabling I2C master mode...")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20); + DEBUG_PRINTLN(F("Resetting FIFO...")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x24); + DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20); + DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO...")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0xE8); + + DEBUG_PRINTLN(F("Writing final memory update 5/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 6/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 7/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 8/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 9/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 10/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 11/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Reading final memory update 12/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + #ifdef DEBUG + DEBUG_PRINT(F("Read bytes: ")); + for (j = 0; j < 4; j++) { + DEBUG_PRINTF(dmpUpdate[3 + j], HEX); + DEBUG_PRINT(" "); + } + DEBUG_PRINTLN(""); + #endif + + DEBUG_PRINTLN(F("Writing final memory update 13/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 14/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 15/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 16/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 17/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Waiting for FIRO count >= 46...")); + while ((fifoCount = getFIFOCount()) < 46); + DEBUG_PRINTLN(F("Reading FIFO...")); + getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes + DEBUG_PRINTLN(F("Reading interrupt status...")); + getIntStatus(); + + DEBUG_PRINTLN(F("Writing final memory update 18/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); + while ((fifoCount = getFIFOCount()) < 48); + DEBUG_PRINTLN(F("Reading FIFO...")); + getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes + DEBUG_PRINTLN(F("Reading interrupt status...")); + getIntStatus(); + DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); + while ((fifoCount = getFIFOCount()) < 48); + DEBUG_PRINTLN(F("Reading FIFO...")); + getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes + DEBUG_PRINTLN(F("Reading interrupt status...")); + getIntStatus(); + + DEBUG_PRINTLN(F("Writing final memory update 19/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); + setDMPEnabled(false); + + DEBUG_PRINTLN(F("Setting up internal 48-byte (default) DMP packet buffer...")); + dmpPacketSize = 48; + /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { + return 3; // TODO: proper error code for no memory + }*/ + + DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); + resetFIFO(); + getIntStatus(); + } else { + DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); + return 2; // configuration block loading failed + } + } else { + DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); + return 1; // main binary block loading failed + } + return 0; // success +} + +bool MPU6050::dmpPacketAvailable() { + return getFIFOCount() >= dmpGetFIFOPacketSize(); +} + +// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); +// uint8_t MPU6050::dmpGetFIFORate(); +// uint8_t MPU6050::dmpGetSampleStepSizeMS(); +// uint8_t MPU6050::dmpGetSampleFrequency(); +// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); + +//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); +//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); +//uint8_t MPU6050::dmpRunFIFORateProcesses(); + +// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + +uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[34] << 24) | ((uint32_t)packet[35] << 16) | ((uint32_t)packet[36] << 8) | packet[37]); + data[1] = (((uint32_t)packet[38] << 24) | ((uint32_t)packet[39] << 16) | ((uint32_t)packet[40] << 8) | packet[41]); + data[2] = (((uint32_t)packet[42] << 24) | ((uint32_t)packet[43] << 16) | ((uint32_t)packet[44] << 8) | packet[45]); + return 0; +} +uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[34] << 8) | packet[35]; + data[1] = (packet[38] << 8) | packet[39]; + data[2] = (packet[42] << 8) | packet[43]; + return 0; +} +uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[34] << 8) | packet[35]; + v -> y = (packet[38] << 8) | packet[39]; + v -> z = (packet[42] << 8) | packet[43]; + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); + data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); + data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); + data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[0] << 8) | packet[1]); + data[1] = ((packet[4] << 8) | packet[5]); + data[2] = ((packet[8] << 8) | packet[9]); + data[3] = ((packet[12] << 8) | packet[13]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + if (status == 0) { + q -> w = (float)qI[0] / 16384.0f; + q -> x = (float)qI[1] / 16384.0f; + q -> y = (float)qI[2] / 16384.0f; + q -> z = (float)qI[3] / 16384.0f; + return 0; + } + return status; // int16 return value, indicates error if this line is reached +} +// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); + data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); + data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); + return 0; +} +uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[16] << 8) | packet[17]; + data[1] = (packet[20] << 8) | packet[21]; + data[2] = (packet[24] << 8) | packet[25]; + return 0; +} +uint8_t MPU6050::dmpGetMag(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[28] << 8) | packet[29]; + data[1] = (packet[30] << 8) | packet[31]; + data[2] = (packet[32] << 8) | packet[33]; + return 0; +} +// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); +// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { + // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) + v -> x = vRaw -> x - gravity -> x*4096; + v -> y = vRaw -> y - gravity -> y*4096; + v -> z = vRaw -> z - gravity -> z*4096; + return 0; +} +// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { + // rotate measured 3D acceleration vector into original state + // frame of reference based on orientation quaternion + memcpy(v, vReal, sizeof(VectorInt16)); + v -> rotate(q); + return 0; +} +// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); + return status; +} + +uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { + v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); + v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); + v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; + return 0; +} +// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); +// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); + +uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi + data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta + data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi + return 0; +} + +#ifdef USE_OLD_DMPGETYAWPITCHROLL +uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); + return 0; +} +#else +uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan2(gravity -> y , gravity -> z); + if(gravity->z<0) { + if(data[1]>0) { + data[1] = PI - data[1]; + } else { + data[1] = -PI - data[1]; + } + } + return 0; +} +#endif + +// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); + +uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { + /*for (uint8_t k = 0; k < dmpPacketSize; k++) { + if (dmpData[k] < 0x10) Serial.print("0"); + Serial.print(dmpData[k], HEX); + Serial.print(" "); + } + Serial.print("\n");*/ + //Serial.println((uint16_t)dmpPacketBuffer); + return 0; +} +uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { + uint8_t status; + uint8_t buf[dmpPacketSize]; + for (uint8_t i = 0; i < numPackets; i++) { + // read packet from FIFO + getFIFOBytes(buf, dmpPacketSize); + + // process packet + if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; + + // increment external process count variable, if supplied + if (processed != 0) *processed++; + } + return 0; +} + +// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); + +// uint8_t MPU6050::dmpInitFIFOParam(); +// uint8_t MPU6050::dmpCloseFIFO(); +// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); +// uint8_t MPU6050::dmpDecodeQuantizedAccel(); +// uint32_t MPU6050::dmpGetGyroSumOfSquare(); +// uint32_t MPU6050::dmpGetAccelSumOfSquare(); +// void MPU6050::dmpOverrideQuaternion(long *q); +uint16_t MPU6050::dmpGetFIFOPacketSize() { + return dmpPacketSize; +} + +#endif /* _MPU6050_9AXIS_MOTIONAPPS41_H_ */ diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/helper_3dmath.h b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/helper_3dmath.h new file mode 100644 index 00000000..9ed260ec --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU6050/helper_3dmath.h @@ -0,0 +1,216 @@ +// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper +// 6/5/2012 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2012-06-05 - add 3D math helper file to DMP6 example sketch + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _HELPER_3DMATH_H_ +#define _HELPER_3DMATH_H_ + +class Quaternion { + public: + float w; + float x; + float y; + float z; + + Quaternion() { + w = 1.0f; + x = 0.0f; + y = 0.0f; + z = 0.0f; + } + + Quaternion(float nw, float nx, float ny, float nz) { + w = nw; + x = nx; + y = ny; + z = nz; + } + + Quaternion getProduct(Quaternion q) { + // Quaternion multiplication is defined by: + // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2) + // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2) + // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2) + // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2 + return Quaternion( + w*q.w - x*q.x - y*q.y - z*q.z, // new w + w*q.x + x*q.w + y*q.z - z*q.y, // new x + w*q.y - x*q.z + y*q.w + z*q.x, // new y + w*q.z + x*q.y - y*q.x + z*q.w); // new z + } + + Quaternion getConjugate() { + return Quaternion(w, -x, -y, -z); + } + + float getMagnitude() { + return sqrt(w*w + x*x + y*y + z*z); + } + + void normalize() { + float m = getMagnitude(); + w /= m; + x /= m; + y /= m; + z /= m; + } + + Quaternion getNormalized() { + Quaternion r(w, x, y, z); + r.normalize(); + return r; + } +}; + +class VectorInt16 { + public: + int16_t x; + int16_t y; + int16_t z; + + VectorInt16() { + x = 0; + y = 0; + z = 0; + } + + VectorInt16(int16_t nx, int16_t ny, int16_t nz) { + x = nx; + y = ny; + z = nz; + } + + float getMagnitude() { + return sqrt(x*x + y*y + z*z); + } + + void normalize() { + float m = getMagnitude(); + x /= m; + y /= m; + z /= m; + } + + VectorInt16 getNormalized() { + VectorInt16 r(x, y, z); + r.normalize(); + return r; + } + + void rotate(Quaternion *q) { + // http://www.cprogramming.com/tutorial/3d/quaternions.html + // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm + // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation + // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1 + + // P_out = q * P_in * conj(q) + // - P_out is the output vector + // - q is the orientation quaternion + // - P_in is the input vector (a*aReal) + // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z]) + Quaternion p(0, x, y, z); + + // quaternion multiplication: q * p, stored back in p + p = q -> getProduct(p); + + // quaternion multiplication: p * conj(q), stored back in p + p = p.getProduct(q -> getConjugate()); + + // p quaternion is now [0, x', y', z'] + x = p.x; + y = p.y; + z = p.z; + } + + VectorInt16 getRotated(Quaternion *q) { + VectorInt16 r(x, y, z); + r.rotate(q); + return r; + } +}; + +class VectorFloat { + public: + float x; + float y; + float z; + + VectorFloat() { + x = 0; + y = 0; + z = 0; + } + + VectorFloat(float nx, float ny, float nz) { + x = nx; + y = ny; + z = nz; + } + + float getMagnitude() { + return sqrt(x*x + y*y + z*z); + } + + void normalize() { + float m = getMagnitude(); + x /= m; + y /= m; + z /= m; + } + + VectorFloat getNormalized() { + VectorFloat r(x, y, z); + r.normalize(); + return r; + } + + void rotate(Quaternion *q) { + Quaternion p(0, x, y, z); + + // quaternion multiplication: q * p, stored back in p + p = q -> getProduct(p); + + // quaternion multiplication: p * conj(q), stored back in p + p = p.getProduct(q -> getConjugate()); + + // p quaternion is now [0, x', y', z'] + x = p.x; + y = p.y; + z = p.z; + } + + VectorFloat getRotated(Quaternion *q) { + VectorFloat r(x, y, z); + r.rotate(q); + return r; + } +}; + +#endif /* _HELPER_3DMATH_H_ */ \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU9250/MPU9250.cpp b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU9250/MPU9250.cpp new file mode 100644 index 00000000..7d2722a8 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU9250/MPU9250.cpp @@ -0,0 +1,1202 @@ +/* +MPU9250.cpp +Brian R Taylor +brian.taylor@bolderflight.com +Copyright (c) 2017 Bolder Flight Systems +Permission is hereby granted, free of charge, to any person obtaining a copy of this software +and associated documentation files (the "Software"), to deal in the Software without restriction, +including without limitation the rights to use, copy, modify, merge, publish, distribute, +sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in all copies or +substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING +BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +#include "Arduino.h" +#include "MPU9250.h" + +/* MPU9250 object, input the I2C bus and address */ +MPU9250::MPU9250(TwoWire &bus,uint8_t address){ + _i2c = &bus; // I2C bus + _address = address; // I2C address + _useSPI = false; // set to use I2C +} + +/* MPU9250 object, input the SPI bus and chip select pin */ +MPU9250::MPU9250(SPIClass &bus,uint8_t csPin){ + _spi = &bus; // SPI bus + _csPin = csPin; // chip select pin + _useSPI = true; // set to use SPI +} + +/* starts communication with the MPU-9250 */ +int MPU9250::begin(){ + if( _useSPI ) { // using SPI for communication + // use low speed SPI for register setting + _useSPIHS = false; + // setting CS pin to output + pinMode(_csPin,OUTPUT); + // setting CS pin high + digitalWrite(_csPin,HIGH); + // begin SPI communication + _spi->begin(); + } else { // using I2C for communication + // starting the I2C bus + _i2c->begin(); + // setting the I2C clock + _i2c->setClock(_i2cRate); + } + // select clock source to gyro + if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){ + return -1; + } + // enable I2C master mode + if(writeRegister(USER_CTRL,I2C_MST_EN) < 0){ + return -2; + } + // set the I2C bus speed to 400 kHz + if(writeRegister(I2C_MST_CTRL,I2C_MST_CLK) < 0){ + return -3; + } + // set AK8963 to Power Down + writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN); + // reset the MPU9250 + writeRegister(PWR_MGMNT_1,PWR_RESET); + // wait for MPU-9250 to come back up + delay(1); + // reset the AK8963 + writeAK8963Register(AK8963_CNTL2,AK8963_RESET); + // select clock source to gyro + if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){ + return -4; + } + // check the WHO AM I byte, expected value is 0x71 (decimal 113) or 0x73 (decimal 115) + if((whoAmI() != 113)&&(whoAmI() != 115)){ + return -5; + } + // enable accelerometer and gyro + if(writeRegister(PWR_MGMNT_2,SEN_ENABLE) < 0){ + return -6; + } + // setting accel range to 16G as default + if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_16G) < 0){ + return -7; + } + _accelScale = G * 16.0f/32767.5f; // setting the accel scale to 16G + _accelRange = ACCEL_RANGE_16G; + // setting the gyro range to 2000DPS as default + if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_2000DPS) < 0){ + return -8; + } + _gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS + _gyroRange = GYRO_RANGE_2000DPS; + // setting bandwidth to 184Hz as default + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0){ + return -9; + } + if(writeRegister(CONFIG,GYRO_DLPF_184) < 0){ // setting gyro bandwidth to 184Hz + return -10; + } + _bandwidth = DLPF_BANDWIDTH_184HZ; + // setting the sample rate divider to 0 as default + if(writeRegister(SMPDIV,0x00) < 0){ + return -11; + } + _srd = 0; + // enable I2C master mode + if(writeRegister(USER_CTRL,I2C_MST_EN) < 0){ + return -12; + } + // set the I2C bus speed to 400 kHz + if( writeRegister(I2C_MST_CTRL,I2C_MST_CLK) < 0){ + return -13; + } + // check AK8963 WHO AM I register, expected value is 0x48 (decimal 72) + if( whoAmIAK8963() != 72 ){ + return -14; + } + /* get the magnetometer calibration */ + // set AK8963 to Power Down + if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ + return -15; + } + delay(100); // long wait between AK8963 mode changes + // set AK8963 to FUSE ROM access + if(writeAK8963Register(AK8963_CNTL1,AK8963_FUSE_ROM) < 0){ + return -16; + } + delay(100); // long wait between AK8963 mode changes + // read the AK8963 ASA registers and compute magnetometer scale factors + readAK8963Registers(AK8963_ASA,3,_buffer); + _magScaleX = ((((float)_buffer[0]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla + _magScaleY = ((((float)_buffer[1]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla + _magScaleZ = ((((float)_buffer[2]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla + // set AK8963 to Power Down + if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ + return -17; + } + delay(100); // long wait between AK8963 mode changes + // set AK8963 to 16 bit resolution, 100 Hz update rate + if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS2) < 0){ + return -18; + } + delay(100); // long wait between AK8963 mode changes + // select clock source to gyro + if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){ + return -19; + } + // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate + readAK8963Registers(AK8963_HXL,7,_buffer); + // estimate gyro bias + if (calibrateGyro() < 0) { + return -20; + } + // successful init, return 1 + return 1; +} + +/* sets the accelerometer full scale range to values other than default */ +int MPU9250::setAccelRange(AccelRange range) { + // use low speed SPI for register setting + _useSPIHS = false; + switch(range) { + case ACCEL_RANGE_2G: { + // setting the accel range to 2G + if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_2G) < 0){ + return -1; + } + _accelScale = G * 2.0f/32767.5f; // setting the accel scale to 2G + break; + } + case ACCEL_RANGE_4G: { + // setting the accel range to 4G + if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_4G) < 0){ + return -1; + } + _accelScale = G * 4.0f/32767.5f; // setting the accel scale to 4G + break; + } + case ACCEL_RANGE_8G: { + // setting the accel range to 8G + if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_8G) < 0){ + return -1; + } + _accelScale = G * 8.0f/32767.5f; // setting the accel scale to 8G + break; + } + case ACCEL_RANGE_16G: { + // setting the accel range to 16G + if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_16G) < 0){ + return -1; + } + _accelScale = G * 16.0f/32767.5f; // setting the accel scale to 16G + break; + } + } + _accelRange = range; + return 1; +} + +/* sets the gyro full scale range to values other than default */ +int MPU9250::setGyroRange(GyroRange range) { + // use low speed SPI for register setting + _useSPIHS = false; + switch(range) { + case GYRO_RANGE_250DPS: { + // setting the gyro range to 250DPS + if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_250DPS) < 0){ + return -1; + } + _gyroScale = 250.0f/32767.5f * _d2r; // setting the gyro scale to 250DPS + break; + } + case GYRO_RANGE_500DPS: { + // setting the gyro range to 500DPS + if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_500DPS) < 0){ + return -1; + } + _gyroScale = 500.0f/32767.5f * _d2r; // setting the gyro scale to 500DPS + break; + } + case GYRO_RANGE_1000DPS: { + // setting the gyro range to 1000DPS + if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_1000DPS) < 0){ + return -1; + } + _gyroScale = 1000.0f/32767.5f * _d2r; // setting the gyro scale to 1000DPS + break; + } + case GYRO_RANGE_2000DPS: { + // setting the gyro range to 2000DPS + if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_2000DPS) < 0){ + return -1; + } + _gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS + break; + } + } + _gyroRange = range; + return 1; +} + +/* sets the DLPF bandwidth to values other than default */ +int MPU9250::setDlpfBandwidth(DlpfBandwidth bandwidth) { + // use low speed SPI for register setting + _useSPIHS = false; + switch(bandwidth) { + case DLPF_BANDWIDTH_184HZ: { + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0){ // setting accel bandwidth to 184Hz + return -1; + } + if(writeRegister(CONFIG,GYRO_DLPF_184) < 0){ // setting gyro bandwidth to 184Hz + return -2; + } + break; + } + case DLPF_BANDWIDTH_92HZ: { + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_92) < 0){ // setting accel bandwidth to 92Hz + return -1; + } + if(writeRegister(CONFIG,GYRO_DLPF_92) < 0){ // setting gyro bandwidth to 92Hz + return -2; + } + break; + } + case DLPF_BANDWIDTH_41HZ: { + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_41) < 0){ // setting accel bandwidth to 41Hz + return -1; + } + if(writeRegister(CONFIG,GYRO_DLPF_41) < 0){ // setting gyro bandwidth to 41Hz + return -2; + } + break; + } + case DLPF_BANDWIDTH_20HZ: { + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_20) < 0){ // setting accel bandwidth to 20Hz + return -1; + } + if(writeRegister(CONFIG,GYRO_DLPF_20) < 0){ // setting gyro bandwidth to 20Hz + return -2; + } + break; + } + case DLPF_BANDWIDTH_10HZ: { + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_10) < 0){ // setting accel bandwidth to 10Hz + return -1; + } + if(writeRegister(CONFIG,GYRO_DLPF_10) < 0){ // setting gyro bandwidth to 10Hz + return -2; + } + break; + } + case DLPF_BANDWIDTH_5HZ: { + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_5) < 0){ // setting accel bandwidth to 5Hz + return -1; + } + if(writeRegister(CONFIG,GYRO_DLPF_5) < 0){ // setting gyro bandwidth to 5Hz + return -2; + } + break; + } + } + _bandwidth = bandwidth; + return 1; +} + +/* sets the sample rate divider to values other than default */ +int MPU9250::setSrd(uint8_t srd) { + // use low speed SPI for register setting + _useSPIHS = false; + /* setting the sample rate divider to 19 to facilitate setting up magnetometer */ + if(writeRegister(SMPDIV,19) < 0){ // setting the sample rate divider + return -1; + } + if(srd > 9){ + // set AK8963 to Power Down + if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ + return -2; + } + delay(100); // long wait between AK8963 mode changes + // set AK8963 to 16 bit resolution, 8 Hz update rate + if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS1) < 0){ + return -3; + } + delay(100); // long wait between AK8963 mode changes + // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate + readAK8963Registers(AK8963_HXL,7,_buffer); + } else { + // set AK8963 to Power Down + if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ + return -2; + } + delay(100); // long wait between AK8963 mode changes + // set AK8963 to 16 bit resolution, 100 Hz update rate + if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS2) < 0){ + return -3; + } + delay(100); // long wait between AK8963 mode changes + // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate + readAK8963Registers(AK8963_HXL,7,_buffer); + } + /* setting the sample rate divider */ + if(writeRegister(SMPDIV,srd) < 0){ // setting the sample rate divider + return -4; + } + _srd = srd; + return 1; +} + +/* enables the data ready interrupt */ +int MPU9250::enableDataReadyInterrupt() { + // use low speed SPI for register setting + _useSPIHS = false; + /* setting the interrupt */ + if (writeRegister(INT_PIN_CFG,INT_PULSE_50US) < 0){ // setup interrupt, 50 us pulse + return -1; + } + if (writeRegister(INT_ENABLE,INT_RAW_RDY_EN) < 0){ // set to data ready + return -2; + } + return 1; +} + +/* disables the data ready interrupt */ +int MPU9250::disableDataReadyInterrupt() { + // use low speed SPI for register setting + _useSPIHS = false; + if(writeRegister(INT_ENABLE,INT_DISABLE) < 0){ // disable interrupt + return -1; + } + return 1; +} + +/* configures and enables wake on motion, low power mode */ +int MPU9250::enableWakeOnMotion(float womThresh_mg,LpAccelOdr odr) { + // use low speed SPI for register setting + _useSPIHS = false; + // set AK8963 to Power Down + writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN); + // reset the MPU9250 + writeRegister(PWR_MGMNT_1,PWR_RESET); + // wait for MPU-9250 to come back up + delay(1); + if(writeRegister(PWR_MGMNT_1,0x00) < 0){ // cycle 0, sleep 0, standby 0 + return -1; + } + if(writeRegister(PWR_MGMNT_2,DIS_GYRO) < 0){ // disable gyro measurements + return -2; + } + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0){ // setting accel bandwidth to 184Hz + return -3; + } + if(writeRegister(INT_ENABLE,INT_WOM_EN) < 0){ // enabling interrupt to wake on motion + return -4; + } + if(writeRegister(MOT_DETECT_CTRL,(ACCEL_INTEL_EN | ACCEL_INTEL_MODE)) < 0){ // enabling accel hardware intelligence + return -5; + } + _womThreshold = map(womThresh_mg, 0, 1020, 0, 255); + if(writeRegister(WOM_THR,_womThreshold) < 0){ // setting wake on motion threshold + return -6; + } + if(writeRegister(LP_ACCEL_ODR,(uint8_t)odr) < 0){ // set frequency of wakeup + return -7; + } + if(writeRegister(PWR_MGMNT_1,PWR_CYCLE) < 0){ // switch to accel low power mode + return -8; + } + return 1; +} + +/* configures and enables the FIFO buffer */ +int MPU9250FIFO::enableFifo(bool accel,bool gyro,bool mag,bool temp) { + // use low speed SPI for register setting + _useSPIHS = false; + if(writeRegister(USER_CTRL, (0x40 | I2C_MST_EN)) < 0){ + return -1; + } + if(writeRegister(FIFO_EN,(accel*FIFO_ACCEL)|(gyro*FIFO_GYRO)|(mag*FIFO_MAG)|(temp*FIFO_TEMP)) < 0){ + return -2; + } + _enFifoAccel = accel; + _enFifoGyro = gyro; + _enFifoMag = mag; + _enFifoTemp = temp; + _fifoFrameSize = accel*6 + gyro*6 + mag*7 + temp*2; + return 1; +} + +/* reads the most current data from MPU9250 and stores in buffer */ +int MPU9250::readSensor() { + _useSPIHS = true; // use the high speed SPI for data readout + // grab the data from the MPU9250 + if (readRegisters(ACCEL_OUT, 21, _buffer) < 0) { + return -1; + } + // combine into 16 bit values + _axcounts = (((int16_t)_buffer[0]) << 8) | _buffer[1]; + _aycounts = (((int16_t)_buffer[2]) << 8) | _buffer[3]; + _azcounts = (((int16_t)_buffer[4]) << 8) | _buffer[5]; + _tcounts = (((int16_t)_buffer[6]) << 8) | _buffer[7]; + _gxcounts = (((int16_t)_buffer[8]) << 8) | _buffer[9]; + _gycounts = (((int16_t)_buffer[10]) << 8) | _buffer[11]; + _gzcounts = (((int16_t)_buffer[12]) << 8) | _buffer[13]; + _hxcounts = (((int16_t)_buffer[15]) << 8) | _buffer[14]; + _hycounts = (((int16_t)_buffer[17]) << 8) | _buffer[16]; + _hzcounts = (((int16_t)_buffer[19]) << 8) | _buffer[18]; + // transform and convert to float values + _ax = (((float)(tX[0]*_axcounts + tX[1]*_aycounts + tX[2]*_azcounts) * _accelScale) - _axb)*_axs; + _ay = (((float)(tY[0]*_axcounts + tY[1]*_aycounts + tY[2]*_azcounts) * _accelScale) - _ayb)*_ays; + _az = (((float)(tZ[0]*_axcounts + tZ[1]*_aycounts + tZ[2]*_azcounts) * _accelScale) - _azb)*_azs; + _gx = ((float)(tX[0]*_gxcounts + tX[1]*_gycounts + tX[2]*_gzcounts) * _gyroScale) - _gxb; + _gy = ((float)(tY[0]*_gxcounts + tY[1]*_gycounts + tY[2]*_gzcounts) * _gyroScale) - _gyb; + _gz = ((float)(tZ[0]*_gxcounts + tZ[1]*_gycounts + tZ[2]*_gzcounts) * _gyroScale) - _gzb; + _hx = (((float)(_hxcounts) * _magScaleX) - _hxb)*_hxs; + _hy = (((float)(_hycounts) * _magScaleY) - _hyb)*_hys; + _hz = (((float)(_hzcounts) * _magScaleZ) - _hzb)*_hzs; + _t = ((((float) _tcounts) - _tempOffset)/_tempScale) + _tempOffset; + return 1; +} + +/* returns the accelerometer measurement in the x direction, m/s/s */ +float MPU9250::getAccelX_mss() { + return _ax; +} + +/* returns the accelerometer measurement in the y direction, m/s/s */ +float MPU9250::getAccelY_mss() { + return _ay; +} + +/* returns the accelerometer measurement in the z direction, m/s/s */ +float MPU9250::getAccelZ_mss() { + return _az; +} + +/* returns the gyroscope measurement in the x direction, rad/s */ +float MPU9250::getGyroX_rads() { + return _gx; +} + +/* returns the gyroscope measurement in the y direction, rad/s */ +float MPU9250::getGyroY_rads() { + return _gy; +} + +/* returns the gyroscope measurement in the z direction, rad/s */ +float MPU9250::getGyroZ_rads() { + return _gz; +} + +/* returns the magnetometer measurement in the x direction, uT */ +float MPU9250::getMagX_uT() { + return _hx; +} + +/* returns the magnetometer measurement in the y direction, uT */ +float MPU9250::getMagY_uT() { + return _hy; +} + +/* returns the magnetometer measurement in the z direction, uT */ +float MPU9250::getMagZ_uT() { + return _hz; +} + +/* returns the die temperature, C */ +float MPU9250::getTemperature_C() { + return _t; +} + +/* reads data from the MPU9250 FIFO and stores in buffer */ +int MPU9250FIFO::readFifo() { + _useSPIHS = true; // use the high speed SPI for data readout + // get the fifo size + readRegisters(FIFO_COUNT, 2, _buffer); + _fifoSize = (((uint16_t) (_buffer[0]&0x0F)) <<8) + (((uint16_t) _buffer[1])); + // read and parse the buffer + for (size_t i=0; i < _fifoSize/_fifoFrameSize; i++) { + // grab the data from the MPU9250 + if (readRegisters(FIFO_READ,_fifoFrameSize,_buffer) < 0) { + return -1; + } + if (_enFifoAccel) { + // combine into 16 bit values + _axcounts = (((int16_t)_buffer[0]) << 8) | _buffer[1]; + _aycounts = (((int16_t)_buffer[2]) << 8) | _buffer[3]; + _azcounts = (((int16_t)_buffer[4]) << 8) | _buffer[5]; + // transform and convert to float values + _axFifo[i] = (((float)(tX[0]*_axcounts + tX[1]*_aycounts + tX[2]*_azcounts) * _accelScale)-_axb)*_axs; + _ayFifo[i] = (((float)(tY[0]*_axcounts + tY[1]*_aycounts + tY[2]*_azcounts) * _accelScale)-_ayb)*_ays; + _azFifo[i] = (((float)(tZ[0]*_axcounts + tZ[1]*_aycounts + tZ[2]*_azcounts) * _accelScale)-_azb)*_azs; + _aSize = _fifoSize/_fifoFrameSize; + } + if (_enFifoTemp) { + // combine into 16 bit values + _tcounts = (((int16_t)_buffer[0 + _enFifoAccel*6]) << 8) | _buffer[1 + _enFifoAccel*6]; + // transform and convert to float values + _tFifo[i] = ((((float) _tcounts) - _tempOffset)/_tempScale) + _tempOffset; + _tSize = _fifoSize/_fifoFrameSize; + } + if (_enFifoGyro) { + // combine into 16 bit values + _gxcounts = (((int16_t)_buffer[0 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[1 + _enFifoAccel*6 + _enFifoTemp*2]; + _gycounts = (((int16_t)_buffer[2 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[3 + _enFifoAccel*6 + _enFifoTemp*2]; + _gzcounts = (((int16_t)_buffer[4 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[5 + _enFifoAccel*6 + _enFifoTemp*2]; + // transform and convert to float values + _gxFifo[i] = ((float)(tX[0]*_gxcounts + tX[1]*_gycounts + tX[2]*_gzcounts) * _gyroScale) - _gxb; + _gyFifo[i] = ((float)(tY[0]*_gxcounts + tY[1]*_gycounts + tY[2]*_gzcounts) * _gyroScale) - _gyb; + _gzFifo[i] = ((float)(tZ[0]*_gxcounts + tZ[1]*_gycounts + tZ[2]*_gzcounts) * _gyroScale) - _gzb; + _gSize = _fifoSize/_fifoFrameSize; + } + if (_enFifoMag) { + // combine into 16 bit values + _hxcounts = (((int16_t)_buffer[1 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]) << 8) | _buffer[0 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]; + _hycounts = (((int16_t)_buffer[3 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]) << 8) | _buffer[2 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]; + _hzcounts = (((int16_t)_buffer[5 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]) << 8) | _buffer[4 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]; + // transform and convert to float values + _hxFifo[i] = (((float)(_hxcounts) * _magScaleX) - _hxb)*_hxs; + _hyFifo[i] = (((float)(_hycounts) * _magScaleY) - _hyb)*_hys; + _hzFifo[i] = (((float)(_hzcounts) * _magScaleZ) - _hzb)*_hzs; + _hSize = _fifoSize/_fifoFrameSize; + } + } + return 1; +} + +/* returns the accelerometer FIFO size and data in the x direction, m/s/s */ +void MPU9250FIFO::getFifoAccelX_mss(size_t *size,float* data) { + *size = _aSize; + memcpy(data,_axFifo,_aSize*sizeof(float)); +} + +/* returns the accelerometer FIFO size and data in the y direction, m/s/s */ +void MPU9250FIFO::getFifoAccelY_mss(size_t *size,float* data) { + *size = _aSize; + memcpy(data,_ayFifo,_aSize*sizeof(float)); +} + +/* returns the accelerometer FIFO size and data in the z direction, m/s/s */ +void MPU9250FIFO::getFifoAccelZ_mss(size_t *size,float* data) { + *size = _aSize; + memcpy(data,_azFifo,_aSize*sizeof(float)); +} + +/* returns the gyroscope FIFO size and data in the x direction, rad/s */ +void MPU9250FIFO::getFifoGyroX_rads(size_t *size,float* data) { + *size = _gSize; + memcpy(data,_gxFifo,_gSize*sizeof(float)); +} + +/* returns the gyroscope FIFO size and data in the y direction, rad/s */ +void MPU9250FIFO::getFifoGyroY_rads(size_t *size,float* data) { + *size = _gSize; + memcpy(data,_gyFifo,_gSize*sizeof(float)); +} + +/* returns the gyroscope FIFO size and data in the z direction, rad/s */ +void MPU9250FIFO::getFifoGyroZ_rads(size_t *size,float* data) { + *size = _gSize; + memcpy(data,_gzFifo,_gSize*sizeof(float)); +} + +/* returns the magnetometer FIFO size and data in the x direction, uT */ +void MPU9250FIFO::getFifoMagX_uT(size_t *size,float* data) { + *size = _hSize; + memcpy(data,_hxFifo,_hSize*sizeof(float)); +} + +/* returns the magnetometer FIFO size and data in the y direction, uT */ +void MPU9250FIFO::getFifoMagY_uT(size_t *size,float* data) { + *size = _hSize; + memcpy(data,_hyFifo,_hSize*sizeof(float)); +} + +/* returns the magnetometer FIFO size and data in the z direction, uT */ +void MPU9250FIFO::getFifoMagZ_uT(size_t *size,float* data) { + *size = _hSize; + memcpy(data,_hzFifo,_hSize*sizeof(float)); +} + +/* returns the die temperature FIFO size and data, C */ +void MPU9250FIFO::getFifoTemperature_C(size_t *size,float* data) { + *size = _tSize; + memcpy(data,_tFifo,_tSize*sizeof(float)); +} + +/* estimates the gyro biases */ +int MPU9250::calibrateGyro() { + // set the range, bandwidth, and srd + if (setGyroRange(GYRO_RANGE_250DPS) < 0) { + return -1; + } + if (setDlpfBandwidth(DLPF_BANDWIDTH_20HZ) < 0) { + return -2; + } + if (setSrd(19) < 0) { + return -3; + } + + // take samples and find bias + _gxbD = 0; + _gybD = 0; + _gzbD = 0; + for (size_t i=0; i < _numSamples; i++) { + readSensor(); + _gxbD += (getGyroX_rads() + _gxb)/((double)_numSamples); + _gybD += (getGyroY_rads() + _gyb)/((double)_numSamples); + _gzbD += (getGyroZ_rads() + _gzb)/((double)_numSamples); + delay(20); + } + _gxb = (float)_gxbD; + _gyb = (float)_gybD; + _gzb = (float)_gzbD; + + // set the range, bandwidth, and srd back to what they were + if (setGyroRange(_gyroRange) < 0) { + return -4; + } + if (setDlpfBandwidth(_bandwidth) < 0) { + return -5; + } + if (setSrd(_srd) < 0) { + return -6; + } + return 1; +} + +/* returns the gyro bias in the X direction, rad/s */ +float MPU9250::getGyroBiasX_rads() { + return _gxb; +} + +/* returns the gyro bias in the Y direction, rad/s */ +float MPU9250::getGyroBiasY_rads() { + return _gyb; +} + +/* returns the gyro bias in the Z direction, rad/s */ +float MPU9250::getGyroBiasZ_rads() { + return _gzb; +} + +/* sets the gyro bias in the X direction to bias, rad/s */ +void MPU9250::setGyroBiasX_rads(float bias) { + _gxb = bias; +} + +/* sets the gyro bias in the Y direction to bias, rad/s */ +void MPU9250::setGyroBiasY_rads(float bias) { + _gyb = bias; +} + +/* sets the gyro bias in the Z direction to bias, rad/s */ +void MPU9250::setGyroBiasZ_rads(float bias) { + _gzb = bias; +} + +/* finds bias and scale factor calibration for the accelerometer, +this should be run for each axis in each direction (6 total) to find +the min and max values along each */ +int MPU9250::calibrateAccel() { + // set the range, bandwidth, and srd + if (setAccelRange(ACCEL_RANGE_2G) < 0) { + return -1; + } + if (setDlpfBandwidth(DLPF_BANDWIDTH_20HZ) < 0) { + return -2; + } + if (setSrd(19) < 0) { + return -3; + } + + // take samples and find min / max + _axbD = 0; + _aybD = 0; + _azbD = 0; + for (size_t i=0; i < _numSamples; i++) { + readSensor(); + _axbD += (getAccelX_mss()/_axs + _axb)/((double)_numSamples); + _aybD += (getAccelY_mss()/_ays + _ayb)/((double)_numSamples); + _azbD += (getAccelZ_mss()/_azs + _azb)/((double)_numSamples); + delay(20); + } + if (_axbD > 9.0f) { + _axmax = (float)_axbD; + } + if (_aybD > 9.0f) { + _aymax = (float)_aybD; + } + if (_azbD > 9.0f) { + _azmax = (float)_azbD; + } + if (_axbD < -9.0f) { + _axmin = (float)_axbD; + } + if (_aybD < -9.0f) { + _aymin = (float)_aybD; + } + if (_azbD < -9.0f) { + _azmin = (float)_azbD; + } + + // find bias and scale factor + if ((abs(_axmin) > 9.0f) && (abs(_axmax) > 9.0f)) { + _axb = (_axmin + _axmax) / 2.0f; + _axs = G/((abs(_axmin) + abs(_axmax)) / 2.0f); + } + if ((abs(_aymin) > 9.0f) && (abs(_aymax) > 9.0f)) { + _ayb = (_aymin + _aymax) / 2.0f; + _ays = G/((abs(_aymin) + abs(_aymax)) / 2.0f); + } + if ((abs(_azmin) > 9.0f) && (abs(_azmax) > 9.0f)) { + _azb = (_azmin + _azmax) / 2.0f; + _azs = G/((abs(_azmin) + abs(_azmax)) / 2.0f); + } + + // set the range, bandwidth, and srd back to what they were + if (setAccelRange(_accelRange) < 0) { + return -4; + } + if (setDlpfBandwidth(_bandwidth) < 0) { + return -5; + } + if (setSrd(_srd) < 0) { + return -6; + } + return 1; +} + +/* returns the accelerometer bias in the X direction, m/s/s */ +float MPU9250::getAccelBiasX_mss() { + return _axb; +} + +/* returns the accelerometer scale factor in the X direction */ +float MPU9250::getAccelScaleFactorX() { + return _axs; +} + +/* returns the accelerometer bias in the Y direction, m/s/s */ +float MPU9250::getAccelBiasY_mss() { + return _ayb; +} + +/* returns the accelerometer scale factor in the Y direction */ +float MPU9250::getAccelScaleFactorY() { + return _ays; +} + +/* returns the accelerometer bias in the Z direction, m/s/s */ +float MPU9250::getAccelBiasZ_mss() { + return _azb; +} + +/* returns the accelerometer scale factor in the Z direction */ +float MPU9250::getAccelScaleFactorZ() { + return _azs; +} + +/* sets the accelerometer bias (m/s/s) and scale factor in the X direction */ +void MPU9250::setAccelCalX(float bias,float scaleFactor) { + _axb = bias; + _axs = scaleFactor; +} + +/* sets the accelerometer bias (m/s/s) and scale factor in the Y direction */ +void MPU9250::setAccelCalY(float bias,float scaleFactor) { + _ayb = bias; + _ays = scaleFactor; +} + +/* sets the accelerometer bias (m/s/s) and scale factor in the Z direction */ +void MPU9250::setAccelCalZ(float bias,float scaleFactor) { + _azb = bias; + _azs = scaleFactor; +} + +/* finds bias and scale factor calibration for the magnetometer, +the sensor should be rotated in a figure 8 motion until complete */ +int MPU9250::calibrateMag() { + // set the srd + if (setSrd(19) < 0) { + return -1; + } + + // get a starting set of data + readSensor(); + _hxmax = getMagX_uT(); + _hxmin = getMagX_uT(); + _hymax = getMagY_uT(); + _hymin = getMagY_uT(); + _hzmax = getMagZ_uT(); + _hzmin = getMagZ_uT(); + + // collect data to find max / min in each channel + _counter = 0; + while (_counter < _maxCounts) { + _delta = 0.0f; + _framedelta = 0.0f; + readSensor(); + _hxfilt = (_hxfilt*((float)_coeff-1)+(getMagX_uT()/_hxs+_hxb))/((float)_coeff); + _hyfilt = (_hyfilt*((float)_coeff-1)+(getMagY_uT()/_hys+_hyb))/((float)_coeff); + _hzfilt = (_hzfilt*((float)_coeff-1)+(getMagZ_uT()/_hzs+_hzb))/((float)_coeff); + if (_hxfilt > _hxmax) { + _delta = _hxfilt - _hxmax; + _hxmax = _hxfilt; + } + if (_delta > _framedelta) { + _framedelta = _delta; + } + if (_hyfilt > _hymax) { + _delta = _hyfilt - _hymax; + _hymax = _hyfilt; + } + if (_delta > _framedelta) { + _framedelta = _delta; + } + if (_hzfilt > _hzmax) { + _delta = _hzfilt - _hzmax; + _hzmax = _hzfilt; + } + if (_delta > _framedelta) { + _framedelta = _delta; + } + if (_hxfilt < _hxmin) { + _delta = abs(_hxfilt - _hxmin); + _hxmin = _hxfilt; + } + if (_delta > _framedelta) { + _framedelta = _delta; + } + if (_hyfilt < _hymin) { + _delta = abs(_hyfilt - _hymin); + _hymin = _hyfilt; + } + if (_delta > _framedelta) { + _framedelta = _delta; + } + if (_hzfilt < _hzmin) { + _delta = abs(_hzfilt - _hzmin); + _hzmin = _hzfilt; + } + if (_delta > _framedelta) { + _framedelta = _delta; + } + if (_framedelta > _deltaThresh) { + _counter = 0; + } else { + _counter++; + } + delay(20); + } + + // find the magnetometer bias + _hxb = (_hxmax + _hxmin) / 2.0f; + _hyb = (_hymax + _hymin) / 2.0f; + _hzb = (_hzmax + _hzmin) / 2.0f; + + // find the magnetometer scale factor + _hxs = (_hxmax - _hxmin) / 2.0f; + _hys = (_hymax - _hymin) / 2.0f; + _hzs = (_hzmax - _hzmin) / 2.0f; + _avgs = (_hxs + _hys + _hzs) / 3.0f; + _hxs = _avgs/_hxs; + _hys = _avgs/_hys; + _hzs = _avgs/_hzs; + + // set the srd back to what it was + if (setSrd(_srd) < 0) { + return -2; + } + return 1; +} + +/* returns the magnetometer bias in the X direction, uT */ +float MPU9250::getMagBiasX_uT() { + return _hxb; +} + +/* returns the magnetometer scale factor in the X direction */ +float MPU9250::getMagScaleFactorX() { + return _hxs; +} + +/* returns the magnetometer bias in the Y direction, uT */ +float MPU9250::getMagBiasY_uT() { + return _hyb; +} + +/* returns the magnetometer scale factor in the Y direction */ +float MPU9250::getMagScaleFactorY() { + return _hys; +} + +/* returns the magnetometer bias in the Z direction, uT */ +float MPU9250::getMagBiasZ_uT() { + return _hzb; +} + +/* returns the magnetometer scale factor in the Z direction */ +float MPU9250::getMagScaleFactorZ() { + return _hzs; +} + +/* sets the magnetometer bias (uT) and scale factor in the X direction */ +void MPU9250::setMagCalX(float bias,float scaleFactor) { + _hxb = bias; + _hxs = scaleFactor; +} + +/* sets the magnetometer bias (uT) and scale factor in the Y direction */ +void MPU9250::setMagCalY(float bias,float scaleFactor) { + _hyb = bias; + _hys = scaleFactor; +} + +/* sets the magnetometer bias (uT) and scale factor in the Z direction */ +void MPU9250::setMagCalZ(float bias,float scaleFactor) { + _hzb = bias; + _hzs = scaleFactor; +} + +#if defined(__IMXRT1062__) +/* writes a byte to MPU9250 register given a register address and data */ +int MPU9250::writeRegister(uint8_t subAddress, uint8_t data){ + /* write data to device */ + if( _useSPI ){ + _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction + digitalWriteFast(_csPin,LOW); // select the MPU9250 chip + delayNanoseconds(200); + _spi->transfer(subAddress); // write the register address + _spi->transfer(data); // write the data + digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip + delayNanoseconds(200); + _spi->endTransaction(); // end the transaction + } + else{ + _i2c->beginTransmission(_address); // open the device + _i2c->write(subAddress); // write the register address + _i2c->write(data); // write the data + _i2c->endTransmission(); + } + + delay(10); + + /* read back the register */ + readRegisters(subAddress,1,_buffer); + /* check the read back register against the written register */ + if(_buffer[0] == data) { + return 1; + } + else{ + return -1; + } +} + +/* reads registers from MPU9250 given a starting register address, number of bytes, and a pointer to store data */ +int MPU9250::readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest){ + if( _useSPI ){ + // begin the transaction + if(_useSPIHS){ + _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3)); + } + else{ + _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); + } + digitalWriteFast(_csPin,LOW); // select the MPU9250 chip + delayNanoseconds(200); + _spi->transfer(subAddress | SPI_READ); // specify the starting register address + for(uint8_t i = 0; i < count; i++){ + dest[i] = _spi->transfer(0x00); // read the data + } + digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip + delayNanoseconds(200); + _spi->endTransaction(); // end the transaction + return 1; + } + else{ + _i2c->beginTransmission(_address); // open the device + _i2c->write(subAddress); // specify the starting register address + _i2c->endTransmission(false); + _numBytes = _i2c->requestFrom(_address, count); // specify the number of bytes to receive + if (_numBytes == count) { + for(uint8_t i = 0; i < count; i++){ + dest[i] = _i2c->read(); + } + return 1; + } else { + return -1; + } + } +} +#else +/* writes a byte to MPU9250 register given a register address and data */ +int MPU9250::writeRegister(uint8_t subAddress, uint8_t data){ + /* write data to device */ + if( _useSPI ){ + _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction + digitalWrite(_csPin,LOW); // select the MPU9250 chip + _spi->transfer(subAddress); // write the register address + _spi->transfer(data); // write the data + digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip + _spi->endTransaction(); // end the transaction + } + else{ + _i2c->beginTransmission(_address); // open the device + _i2c->write(subAddress); // write the register address + _i2c->write(data); // write the data + _i2c->endTransmission(); + } + + delay(10); + + /* read back the register */ + readRegisters(subAddress,1,_buffer); + /* check the read back register against the written register */ + if(_buffer[0] == data) { + return 1; + } + else{ + return -1; + } +} + +/* reads registers from MPU9250 given a starting register address, number of bytes, and a pointer to store data */ +int MPU9250::readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest){ + if( _useSPI ){ + // begin the transaction + if(_useSPIHS){ + _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3)); + } + else{ + _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); + } + digitalWrite(_csPin,LOW); // select the MPU9250 chip + _spi->transfer(subAddress | SPI_READ); // specify the starting register address + for(uint8_t i = 0; i < count; i++){ + dest[i] = _spi->transfer(0x00); // read the data + } + digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip + _spi->endTransaction(); // end the transaction + return 1; + } + else{ + _i2c->beginTransmission(_address); // open the device + _i2c->write(subAddress); // specify the starting register address + _i2c->endTransmission(false); + _numBytes = _i2c->requestFrom(_address, count); // specify the number of bytes to receive + if (_numBytes == count) { + for(uint8_t i = 0; i < count; i++){ + dest[i] = _i2c->read(); + } + return 1; + } else { + return -1; + } + } +} +#endif + +/* writes a register to the AK8963 given a register address and data */ +int MPU9250::writeAK8963Register(uint8_t subAddress, uint8_t data){ + // set slave 0 to the AK8963 and set for write + if (writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR) < 0) { + return -1; + } + // set the register to the desired AK8963 sub address + if (writeRegister(I2C_SLV0_REG,subAddress) < 0) { + return -2; + } + // store the data for write + if (writeRegister(I2C_SLV0_DO,data) < 0) { + return -3; + } + // enable I2C and send 1 byte + if (writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1) < 0) { + return -4; + } + // read the register and confirm + if (readAK8963Registers(subAddress,1,_buffer) < 0) { + return -5; + } + if(_buffer[0] == data) { + return 1; + } else{ + return -6; + } +} + +/* reads registers from the AK8963 */ +int MPU9250::readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest){ + // set slave 0 to the AK8963 and set for read + if (writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) < 0) { + return -1; + } + // set the register to the desired AK8963 sub address + if (writeRegister(I2C_SLV0_REG,subAddress) < 0) { + return -2; + } + // enable I2C and request the bytes + if (writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | count) < 0) { + return -3; + } + delay(1); // takes some time for these registers to fill + // read the bytes off the MPU9250 EXT_SENS_DATA registers + _status = readRegisters(EXT_SENS_DATA_00,count,dest); + return _status; +} + +/* gets the MPU9250 WHO_AM_I register value, expected to be 0x71 */ +int MPU9250::whoAmI(){ + // read the WHO AM I register + if (readRegisters(WHO_AM_I,1,_buffer) < 0) { + return -1; + } + // return the register value + return _buffer[0]; +} + +/* gets the AK8963 WHO_AM_I register value, expected to be 0x48 */ +int MPU9250::whoAmIAK8963(){ + // read the WHO AM I register + if (readAK8963Registers(AK8963_WHO_AM_I,1,_buffer) < 0) { + return -1; + } + // return the register value + return _buffer[0]; +} + +// jihlein additions start + +void MPU9250::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { + uint8_t buffer[14]; + readRegisters(ACCEL_OUT, 14, buffer); + *ax = (((int16_t)buffer[0]) << 8) | buffer[1]; + *ay = (((int16_t)buffer[2]) << 8) | buffer[3]; + *az = (((int16_t)buffer[4]) << 8) | buffer[5]; + *gx = (((int16_t)buffer[8]) << 8) | buffer[9]; + *gy = (((int16_t)buffer[10]) << 8) | buffer[11]; + *gz = (((int16_t)buffer[12]) << 8) | buffer[13]; +} + +void MPU9250::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) { + uint8_t buffer[20]; + readRegisters(ACCEL_OUT, 20, buffer); + *ax = (((int16_t)buffer[0]) << 8) | buffer[1]; + *ay = (((int16_t)buffer[2]) << 8) | buffer[3]; + *az = (((int16_t)buffer[4]) << 8) | buffer[5]; + *gx = (((int16_t)buffer[8]) << 8) | buffer[9]; + *gy = (((int16_t)buffer[10]) << 8) | buffer[11]; + *gz = (((int16_t)buffer[12]) << 8) | buffer[13]; + *mx = (((int16_t)buffer[15]) << 8) | buffer[14]; + *my = (((int16_t)buffer[17]) << 8) | buffer[16]; + *mz = (((int16_t)buffer[19]) << 8) | buffer[18]; +} + +// jihlein additions end \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU9250/MPU9250.h b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU9250/MPU9250.h new file mode 100644 index 00000000..bbbe2c5a --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/MPU9250/MPU9250.h @@ -0,0 +1,313 @@ +/* + MPU9250.h + Brian R Taylor + brian.taylor@bolderflight.com + + Copyright (c) 2017 Bolder Flight Systems + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#ifndef MPU9250_h +#define MPU9250_h + +#include "Arduino.h" +#include "Wire.h" // I2C library +#include "SPI.h" // SPI library + +class MPU9250{ + public: + enum GyroRange + { + GYRO_RANGE_250DPS, + GYRO_RANGE_500DPS, + GYRO_RANGE_1000DPS, + GYRO_RANGE_2000DPS + }; + enum AccelRange + { + ACCEL_RANGE_2G, + ACCEL_RANGE_4G, + ACCEL_RANGE_8G, + ACCEL_RANGE_16G + }; + enum DlpfBandwidth + { + DLPF_BANDWIDTH_184HZ, + DLPF_BANDWIDTH_92HZ, + DLPF_BANDWIDTH_41HZ, + DLPF_BANDWIDTH_20HZ, + DLPF_BANDWIDTH_10HZ, + DLPF_BANDWIDTH_5HZ + }; + enum LpAccelOdr + { + LP_ACCEL_ODR_0_24HZ = 0, + LP_ACCEL_ODR_0_49HZ = 1, + LP_ACCEL_ODR_0_98HZ = 2, + LP_ACCEL_ODR_1_95HZ = 3, + LP_ACCEL_ODR_3_91HZ = 4, + LP_ACCEL_ODR_7_81HZ = 5, + LP_ACCEL_ODR_15_63HZ = 6, + LP_ACCEL_ODR_31_25HZ = 7, + LP_ACCEL_ODR_62_50HZ = 8, + LP_ACCEL_ODR_125HZ = 9, + LP_ACCEL_ODR_250HZ = 10, + LP_ACCEL_ODR_500HZ = 11 + }; + MPU9250(TwoWire &bus,uint8_t address); + MPU9250(SPIClass &bus,uint8_t csPin); + int begin(); + int setAccelRange(AccelRange range); + int setGyroRange(GyroRange range); + int setDlpfBandwidth(DlpfBandwidth bandwidth); + int setSrd(uint8_t srd); + int enableDataReadyInterrupt(); + int disableDataReadyInterrupt(); + int enableWakeOnMotion(float womThresh_mg,LpAccelOdr odr); + int readSensor(); + float getAccelX_mss(); + float getAccelY_mss(); + float getAccelZ_mss(); + float getGyroX_rads(); + float getGyroY_rads(); + float getGyroZ_rads(); + float getMagX_uT(); + float getMagY_uT(); + float getMagZ_uT(); + float getTemperature_C(); + + int calibrateGyro(); + float getGyroBiasX_rads(); + float getGyroBiasY_rads(); + float getGyroBiasZ_rads(); + void setGyroBiasX_rads(float bias); + void setGyroBiasY_rads(float bias); + void setGyroBiasZ_rads(float bias); + int calibrateAccel(); + float getAccelBiasX_mss(); + float getAccelScaleFactorX(); + float getAccelBiasY_mss(); + float getAccelScaleFactorY(); + float getAccelBiasZ_mss(); + float getAccelScaleFactorZ(); + void setAccelCalX(float bias,float scaleFactor); + void setAccelCalY(float bias,float scaleFactor); + void setAccelCalZ(float bias,float scaleFactor); + int calibrateMag(); + float getMagBiasX_uT(); + float getMagScaleFactorX(); + float getMagBiasY_uT(); + float getMagScaleFactorY(); + float getMagBiasZ_uT(); + float getMagScaleFactorZ(); + void setMagCalX(float bias,float scaleFactor); + void setMagCalY(float bias,float scaleFactor); + void setMagCalZ(float bias,float scaleFactor); + // jihlein additions start + void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz); + void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); + // jihlein additions end + protected: + // i2c + uint8_t _address; + TwoWire *_i2c; + const uint32_t _i2cRate = 400000; // 400 kHz + size_t _numBytes; // number of bytes received from I2C + // spi + SPIClass *_spi; + uint8_t _csPin; + bool _useSPI; + bool _useSPIHS; + const uint8_t SPI_READ = 0x80; + const uint32_t SPI_LS_CLOCK = 1000000; // 1 MHz + const uint32_t SPI_HS_CLOCK = 15000000; // 15 MHz + // track success of interacting with sensor + int _status; + // buffer for reading from sensor + uint8_t _buffer[21]; + // data counts + int16_t _axcounts,_aycounts,_azcounts; + int16_t _gxcounts,_gycounts,_gzcounts; + int16_t _hxcounts,_hycounts,_hzcounts; + int16_t _tcounts; + // data buffer + float _ax, _ay, _az; + float _gx, _gy, _gz; + float _hx, _hy, _hz; + float _t; + // wake on motion + uint8_t _womThreshold; + // scale factors + float _accelScale; + float _gyroScale; + float _magScaleX, _magScaleY, _magScaleZ; + const float _tempScale = 333.87f; + const float _tempOffset = 21.0f; + // configuration + AccelRange _accelRange; + GyroRange _gyroRange; + DlpfBandwidth _bandwidth; + uint8_t _srd; + // gyro bias estimation + size_t _numSamples = 100; + double _gxbD, _gybD, _gzbD; + float _gxb, _gyb, _gzb; + // accel bias and scale factor estimation + double _axbD, _aybD, _azbD; + float _axmax, _aymax, _azmax; + float _axmin, _aymin, _azmin; + float _axb, _ayb, _azb; + float _axs = 1.0f; + float _ays = 1.0f; + float _azs = 1.0f; + // magnetometer bias and scale factor estimation + uint16_t _maxCounts = 1000; + float _deltaThresh = 0.3f; + uint8_t _coeff = 8; + uint16_t _counter; + float _framedelta, _delta; + float _hxfilt, _hyfilt, _hzfilt; + float _hxmax, _hymax, _hzmax; + float _hxmin, _hymin, _hzmin; + float _hxb, _hyb, _hzb; + float _hxs = 1.0f; + float _hys = 1.0f; + float _hzs = 1.0f; + float _avgs; + // transformation matrix + /* transform the accel and gyro axes to match the magnetometer axes */ + const int16_t tX[3] = {0, 1, 0}; + const int16_t tY[3] = {1, 0, 0}; + const int16_t tZ[3] = {0, 0, -1}; + // constants + const float G = 9.807f; + const float _d2r = 3.14159265359f/180.0f; + // MPU9250 registers + const uint8_t ACCEL_OUT = 0x3B; + const uint8_t GYRO_OUT = 0x43; + const uint8_t TEMP_OUT = 0x41; + const uint8_t EXT_SENS_DATA_00 = 0x49; + const uint8_t ACCEL_CONFIG = 0x1C; + const uint8_t ACCEL_FS_SEL_2G = 0x00; + const uint8_t ACCEL_FS_SEL_4G = 0x08; + const uint8_t ACCEL_FS_SEL_8G = 0x10; + const uint8_t ACCEL_FS_SEL_16G = 0x18; + const uint8_t GYRO_CONFIG = 0x1B; + const uint8_t GYRO_FS_SEL_250DPS = 0x00; + const uint8_t GYRO_FS_SEL_500DPS = 0x08; + const uint8_t GYRO_FS_SEL_1000DPS = 0x10; + const uint8_t GYRO_FS_SEL_2000DPS = 0x18; + const uint8_t ACCEL_CONFIG2 = 0x1D; + const uint8_t ACCEL_DLPF_184 = 0x01; + const uint8_t ACCEL_DLPF_92 = 0x02; + const uint8_t ACCEL_DLPF_41 = 0x03; + const uint8_t ACCEL_DLPF_20 = 0x04; + const uint8_t ACCEL_DLPF_10 = 0x05; + const uint8_t ACCEL_DLPF_5 = 0x06; + const uint8_t CONFIG = 0x1A; + const uint8_t GYRO_DLPF_184 = 0x01; + const uint8_t GYRO_DLPF_92 = 0x02; + const uint8_t GYRO_DLPF_41 = 0x03; + const uint8_t GYRO_DLPF_20 = 0x04; + const uint8_t GYRO_DLPF_10 = 0x05; + const uint8_t GYRO_DLPF_5 = 0x06; + const uint8_t SMPDIV = 0x19; + const uint8_t INT_PIN_CFG = 0x37; + const uint8_t INT_ENABLE = 0x38; + const uint8_t INT_DISABLE = 0x00; + const uint8_t INT_PULSE_50US = 0x00; + const uint8_t INT_WOM_EN = 0x40; + const uint8_t INT_RAW_RDY_EN = 0x01; + const uint8_t PWR_MGMNT_1 = 0x6B; + const uint8_t PWR_CYCLE = 0x20; + const uint8_t PWR_RESET = 0x80; + const uint8_t CLOCK_SEL_PLL = 0x01; + const uint8_t PWR_MGMNT_2 = 0x6C; + const uint8_t SEN_ENABLE = 0x00; + const uint8_t DIS_GYRO = 0x07; + const uint8_t USER_CTRL = 0x6A; + const uint8_t I2C_MST_EN = 0x20; + const uint8_t I2C_MST_CLK = 0x0D; + const uint8_t I2C_MST_CTRL = 0x24; + const uint8_t I2C_SLV0_ADDR = 0x25; + const uint8_t I2C_SLV0_REG = 0x26; + const uint8_t I2C_SLV0_DO = 0x63; + const uint8_t I2C_SLV0_CTRL = 0x27; + const uint8_t I2C_SLV0_EN = 0x80; + const uint8_t I2C_READ_FLAG = 0x80; + const uint8_t MOT_DETECT_CTRL = 0x69; + const uint8_t ACCEL_INTEL_EN = 0x80; + const uint8_t ACCEL_INTEL_MODE = 0x40; + const uint8_t LP_ACCEL_ODR = 0x1E; + const uint8_t WOM_THR = 0x1F; + const uint8_t WHO_AM_I = 0x75; + const uint8_t FIFO_EN = 0x23; + const uint8_t FIFO_TEMP = 0x80; + const uint8_t FIFO_GYRO = 0x70; + const uint8_t FIFO_ACCEL = 0x08; + const uint8_t FIFO_MAG = 0x01; + const uint8_t FIFO_COUNT = 0x72; + const uint8_t FIFO_READ = 0x74; + // AK8963 registers + const uint8_t AK8963_I2C_ADDR = 0x0C; + const uint8_t AK8963_HXL = 0x03; + const uint8_t AK8963_CNTL1 = 0x0A; + const uint8_t AK8963_PWR_DOWN = 0x00; + const uint8_t AK8963_CNT_MEAS1 = 0x12; + const uint8_t AK8963_CNT_MEAS2 = 0x16; + const uint8_t AK8963_FUSE_ROM = 0x0F; + const uint8_t AK8963_CNTL2 = 0x0B; + const uint8_t AK8963_RESET = 0x01; + const uint8_t AK8963_ASA = 0x10; + const uint8_t AK8963_WHO_AM_I = 0x00; + // private functions + int writeRegister(uint8_t subAddress, uint8_t data); + int readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest); + int writeAK8963Register(uint8_t subAddress, uint8_t data); + int readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest); + int whoAmI(); + int whoAmIAK8963(); +}; + +class MPU9250FIFO: public MPU9250 { + public: + using MPU9250::MPU9250; + int enableFifo(bool accel,bool gyro,bool mag,bool temp); + int readFifo(); + void getFifoAccelX_mss(size_t *size,float* data); + void getFifoAccelY_mss(size_t *size,float* data); + void getFifoAccelZ_mss(size_t *size,float* data); + void getFifoGyroX_rads(size_t *size,float* data); + void getFifoGyroY_rads(size_t *size,float* data); + void getFifoGyroZ_rads(size_t *size,float* data); + void getFifoMagX_uT(size_t *size,float* data); + void getFifoMagY_uT(size_t *size,float* data); + void getFifoMagZ_uT(size_t *size,float* data); + void getFifoTemperature_C(size_t *size,float* data); + protected: + // fifo + bool _enFifoAccel,_enFifoGyro,_enFifoMag,_enFifoTemp; + size_t _fifoSize,_fifoFrameSize; + float _axFifo[85], _ayFifo[85], _azFifo[85]; + size_t _aSize; + float _gxFifo[85], _gyFifo[85], _gzFifo[85]; + size_t _gSize; + float _hxFifo[73], _hyFifo[73], _hzFifo[73]; + size_t _hSize; + float _tFifo[256]; + size_t _tSize; +}; + +#endif diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/SBUS/SBUS.cpp b/Versions/dRehmFlight_Teensy_BETA_1.4/src/SBUS/SBUS.cpp new file mode 100644 index 00000000..07085420 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/SBUS/SBUS.cpp @@ -0,0 +1,376 @@ +/* + SBUS.cpp + Brian R Taylor + brian.taylor@bolderflight.com + + Copyright (c) 2016 Bolder Flight Systems + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#include "SBUS.h" + +// SEE: https://learn.adafruit.com/adafruit-feather-m0-basic-proto/adapting-sketches-to-m0 +#if defined(ARDUINO_SAMD_ZERO) && defined(SERIAL_PORT_USBVIRTUAL) + // Required for Serial on Zero based boards + #define Serial SERIAL_PORT_USBVIRTUAL +#endif + +#if defined(__MK20DX128__) || defined(__MK20DX256__) + // globals needed for emulating two stop bytes on Teensy 3.0 and 3.1/3.2 + IntervalTimer serialTimer; + HardwareSerial* SERIALPORT; + uint8_t PACKET[25]; + volatile int SENDINDEX; + void sendByte(); +#endif +/* SBUS object, input the serial bus */ +SBUS::SBUS(HardwareSerial& bus) +{ + _bus = &bus; +} + +/* starts the serial communication */ +void SBUS::begin() +{ + // initialize parsing state + _parserState = 0; + // initialize default scale factors and biases + for (uint8_t i = 0; i < _numChannels; i++) { + setEndPoints(i,_defaultMin,_defaultMax); + } + // begin the serial port for SBUS + #if defined(__MK20DX128__) || defined(__MK20DX256__) // Teensy 3.0 || Teensy 3.1/3.2 + _bus->begin(_sbusBaud,SERIAL_8E1_RXINV_TXINV); + SERIALPORT = _bus; + #elif defined(__IMXRT1062__) || defined(__IMXRT1052__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) || defined(__MKL26Z64__) // Teensy 4.0 || Teensy 4.0 Beta || Teensy 3.5 || Teensy 3.6 || Teensy LC + _bus->begin(_sbusBaud,SERIAL_8E2_RXINV_TXINV); + #elif defined(STM32L496xx) || defined(STM32L476xx) || defined(STM32L433xx) || defined(STM32L432xx) // STM32L4 + _bus->begin(_sbusBaud,SERIAL_SBUS); + #elif defined(_BOARD_MAPLE_MINI_H_) // Maple Mini + _bus->begin(_sbusBaud,SERIAL_8E2); + #elif defined(ESP32) // ESP32 + _bus->begin(_sbusBaud,SERIAL_8E2); + #elif defined(__AVR_ATmega2560__) || defined(__AVR_ATmega328P__) || defined(__AVR_ATmega32U4__) // Arduino Mega 2560, 328P or 32u4 + _bus->begin(_sbusBaud, SERIAL_8E2); + #elif defined(ARDUINO_SAMD_ZERO) // Adafruit Feather M0 + _bus->begin(_sbusBaud, SERIAL_8E2); + #else + #error unsupported device + #endif +} + +/* read the SBUS data */ +bool SBUS::read(uint16_t* channels, bool* failsafe, bool* lostFrame) +{ + // parse the SBUS packet + if (parse()) { + if (channels) { + // 16 channels of 11 bit data + channels[0] = (uint16_t) ((_payload[0] |_payload[1] <<8) & 0x07FF); + channels[1] = (uint16_t) ((_payload[1]>>3 |_payload[2] <<5) & 0x07FF); + channels[2] = (uint16_t) ((_payload[2]>>6 |_payload[3] <<2 |_payload[4]<<10) & 0x07FF); + channels[3] = (uint16_t) ((_payload[4]>>1 |_payload[5] <<7) & 0x07FF); + channels[4] = (uint16_t) ((_payload[5]>>4 |_payload[6] <<4) & 0x07FF); + channels[5] = (uint16_t) ((_payload[6]>>7 |_payload[7] <<1 |_payload[8]<<9) & 0x07FF); + channels[6] = (uint16_t) ((_payload[8]>>2 |_payload[9] <<6) & 0x07FF); + channels[7] = (uint16_t) ((_payload[9]>>5 |_payload[10]<<3) & 0x07FF); + channels[8] = (uint16_t) ((_payload[11] |_payload[12]<<8) & 0x07FF); + channels[9] = (uint16_t) ((_payload[12]>>3|_payload[13]<<5) & 0x07FF); + channels[10] = (uint16_t) ((_payload[13]>>6|_payload[14]<<2 |_payload[15]<<10) & 0x07FF); + channels[11] = (uint16_t) ((_payload[15]>>1|_payload[16]<<7) & 0x07FF); + channels[12] = (uint16_t) ((_payload[16]>>4|_payload[17]<<4) & 0x07FF); + channels[13] = (uint16_t) ((_payload[17]>>7|_payload[18]<<1 |_payload[19]<<9) & 0x07FF); + channels[14] = (uint16_t) ((_payload[19]>>2|_payload[20]<<6) & 0x07FF); + channels[15] = (uint16_t) ((_payload[20]>>5|_payload[21]<<3) & 0x07FF); + } + if (lostFrame) { + // count lost frames + if (_payload[22] & _sbusLostFrame) { + *lostFrame = true; + } else { + *lostFrame = false; + } + } + if (failsafe) { + // failsafe state + if (_payload[22] & _sbusFailSafe) { + *failsafe = true; + } + else{ + *failsafe = false; + } + } + // return true on receiving a full packet + return true; + } else { + // return false if a full packet is not received + return false; + } +} + +/* read the SBUS data and calibrate it to +/- 1 */ +bool SBUS::readCal(float* calChannels, bool* failsafe, bool* lostFrame) +{ + uint16_t channels[_numChannels]; + // read the SBUS data + if(read(&channels[0],failsafe,lostFrame)) { + // linear calibration + for (uint8_t i = 0; i < _numChannels; i++) { + calChannels[i] = channels[i] * _sbusScale[i] + _sbusBias[i]; + if (_useReadCoeff[i]) { + calChannels[i] = PolyVal(_readLen[i],_readCoeff[i],calChannels[i]); + } + } + // return true on receiving a full packet + return true; + } else { + // return false if a full packet is not received + return false; + } +} + +/* write SBUS packets */ +void SBUS::write(uint16_t* channels) +{ + static uint8_t packet[25]; + /* assemble the SBUS packet */ + // SBUS header + packet[0] = _sbusHeader; + // 16 channels of 11 bit data + if (channels) { + packet[1] = (uint8_t) ((channels[0] & 0x07FF)); + packet[2] = (uint8_t) ((channels[0] & 0x07FF)>>8 | (channels[1] & 0x07FF)<<3); + packet[3] = (uint8_t) ((channels[1] & 0x07FF)>>5 | (channels[2] & 0x07FF)<<6); + packet[4] = (uint8_t) ((channels[2] & 0x07FF)>>2); + packet[5] = (uint8_t) ((channels[2] & 0x07FF)>>10 | (channels[3] & 0x07FF)<<1); + packet[6] = (uint8_t) ((channels[3] & 0x07FF)>>7 | (channels[4] & 0x07FF)<<4); + packet[7] = (uint8_t) ((channels[4] & 0x07FF)>>4 | (channels[5] & 0x07FF)<<7); + packet[8] = (uint8_t) ((channels[5] & 0x07FF)>>1); + packet[9] = (uint8_t) ((channels[5] & 0x07FF)>>9 | (channels[6] & 0x07FF)<<2); + packet[10] = (uint8_t) ((channels[6] & 0x07FF)>>6 | (channels[7] & 0x07FF)<<5); + packet[11] = (uint8_t) ((channels[7] & 0x07FF)>>3); + packet[12] = (uint8_t) ((channels[8] & 0x07FF)); + packet[13] = (uint8_t) ((channels[8] & 0x07FF)>>8 | (channels[9] & 0x07FF)<<3); + packet[14] = (uint8_t) ((channels[9] & 0x07FF)>>5 | (channels[10] & 0x07FF)<<6); + packet[15] = (uint8_t) ((channels[10] & 0x07FF)>>2); + packet[16] = (uint8_t) ((channels[10] & 0x07FF)>>10 | (channels[11] & 0x07FF)<<1); + packet[17] = (uint8_t) ((channels[11] & 0x07FF)>>7 | (channels[12] & 0x07FF)<<4); + packet[18] = (uint8_t) ((channels[12] & 0x07FF)>>4 | (channels[13] & 0x07FF)<<7); + packet[19] = (uint8_t) ((channels[13] & 0x07FF)>>1); + packet[20] = (uint8_t) ((channels[13] & 0x07FF)>>9 | (channels[14] & 0x07FF)<<2); + packet[21] = (uint8_t) ((channels[14] & 0x07FF)>>6 | (channels[15] & 0x07FF)<<5); + packet[22] = (uint8_t) ((channels[15] & 0x07FF)>>3); + } + // flags + packet[23] = 0x00; + // footer + packet[24] = _sbusFooter; + #if defined(__MK20DX128__) || defined(__MK20DX256__) // Teensy 3.0 || Teensy 3.1/3.2 + // use ISR to send byte at a time, + // 130 us between bytes to emulate 2 stop bits + noInterrupts(); + memcpy(&PACKET,&packet,sizeof(packet)); + interrupts(); + serialTimer.priority(255); + serialTimer.begin(sendByte,130); + #else + // write packet + _bus->write(packet,25); + #endif +} + +/* write SBUS packets from calibrated inputs */ +void SBUS::writeCal(float* calChannels) +{ + uint16_t channels[_numChannels] = {0}; + // linear calibration + if (calChannels) { + for (uint8_t i = 0; i < _numChannels; i++) { + if (_useWriteCoeff[i]) { + calChannels[i] = PolyVal(_writeLen[i],_writeCoeff[i],calChannels[i]); + } + channels[i] = (calChannels[i] - _sbusBias[i]) / _sbusScale[i]; + } + } + write(channels); +} + +void SBUS::setEndPoints(uint8_t channel,uint16_t min,uint16_t max) +{ + _sbusMin[channel] = min; + _sbusMax[channel] = max; + scaleBias(channel); +} + +void SBUS::getEndPoints(uint8_t channel,uint16_t *min,uint16_t *max) +{ + if (min&&max) { + *min = _sbusMin[channel]; + *max = _sbusMax[channel]; + } +} + +void SBUS::setReadCal(uint8_t channel,float *coeff,uint8_t len) +{ + if (coeff) { + if (!_readCoeff) { + _readCoeff = (float**) malloc(sizeof(float*)*_numChannels); + } + if (!_readCoeff[channel]) { + _readCoeff[channel] = (float*) malloc(sizeof(float)*len); + } else { + free(_readCoeff[channel]); + _readCoeff[channel] = (float*) malloc(sizeof(float)*len); + } + for (uint8_t i = 0; i < len; i++) { + _readCoeff[channel][i] = coeff[i]; + } + _readLen[channel] = len; + _useReadCoeff[channel] = true; + } +} + +void SBUS::getReadCal(uint8_t channel,float *coeff,uint8_t len) +{ + if (coeff) { + for (uint8_t i = 0; (i < _readLen[channel]) && (i < len); i++) { + coeff[i] = _readCoeff[channel][i]; + } + } +} + +void SBUS::setWriteCal(uint8_t channel,float *coeff,uint8_t len) +{ + if (coeff) { + if (!_writeCoeff) { + _writeCoeff = (float**) malloc(sizeof(float*)*_numChannels); + } + if (!_writeCoeff[channel]) { + _writeCoeff[channel] = (float*) malloc(sizeof(float)*len); + } else { + free(_writeCoeff[channel]); + _writeCoeff[channel] = (float*) malloc(sizeof(float)*len); + } + for (uint8_t i = 0; i < len; i++) { + _writeCoeff[channel][i] = coeff[i]; + } + _writeLen[channel] = len; + _useWriteCoeff[channel] = true; + } +} + +void SBUS::getWriteCal(uint8_t channel,float *coeff,uint8_t len) +{ + if (coeff) { + for (uint8_t i = 0; (i < _writeLen[channel]) && (i < len); i++) { + coeff[i] = _writeCoeff[channel][i]; + } + } +} + +/* destructor, free dynamically allocated memory */ +SBUS::~SBUS() +{ + if (_readCoeff) { + for (uint8_t i = 0; i < _numChannels; i++) { + if (_readCoeff[i]) { + free(_readCoeff[i]); + } + } + free(_readCoeff); + } + if (_writeCoeff) { + for (uint8_t i = 0; i < _numChannels; i++) { + if (_writeCoeff[i]) { + free(_writeCoeff[i]); + } + } + free(_writeCoeff); + } +} + +/* parse the SBUS data */ +bool SBUS::parse() +{ + // reset the parser state if too much time has passed + static elapsedMicros _sbusTime = 0; + if (_sbusTime > SBUS_TIMEOUT_US) {_parserState = 0;} + // see if serial data is available + while (_bus->available() > 0) { + _sbusTime = 0; + _curByte = _bus->read(); + // find the header + if (_parserState == 0) { + if ((_curByte == _sbusHeader) && ((_prevByte == _sbusFooter) || ((_prevByte & _sbus2Mask) == _sbus2Footer))) { + _parserState++; + } else { + _parserState = 0; + } + } else { + // strip off the data + if ((_parserState-1) < _payloadSize) { + _payload[_parserState-1] = _curByte; + _parserState++; + } + // check the end byte + if ((_parserState-1) == _payloadSize) { + if ((_curByte == _sbusFooter) || ((_curByte & _sbus2Mask) == _sbus2Footer)) { + _parserState = 0; + return true; + } else { + _parserState = 0; + return false; + } + } + } + _prevByte = _curByte; + } + // return false if a partial packet + return false; +} + +/* compute scale factor and bias from end points */ +void SBUS::scaleBias(uint8_t channel) +{ + _sbusScale[channel] = 2.0f / ((float)_sbusMax[channel] - (float)_sbusMin[channel]); + _sbusBias[channel] = -1.0f*((float)_sbusMin[channel] + ((float)_sbusMax[channel] - (float)_sbusMin[channel]) / 2.0f) * _sbusScale[channel]; +} + +float SBUS::PolyVal(size_t PolySize, float *Coefficients, float X) { + if (Coefficients) { + float Y = Coefficients[0]; + for (uint8_t i = 1; i < PolySize; i++) { + Y = Y*X + Coefficients[i]; + } + return(Y); + } else { + return 0; + } +} + +// function to send byte at a time with +// ISR to emulate 2 stop bits on Teensy 3.0 and 3.1/3.2 +#if defined(__MK20DX128__) || defined(__MK20DX256__) // Teensy 3.0 || Teensy 3.1/3.2 + void sendByte() + { + if (SENDINDEX < 25) { + SERIALPORT->write(PACKET[SENDINDEX]); + SENDINDEX++; + } else { + serialTimer.end(); + SENDINDEX = 0; + } + } +#endif diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/SBUS/SBUS.h b/Versions/dRehmFlight_Teensy_BETA_1.4/src/SBUS/SBUS.h new file mode 100644 index 00000000..215cb9b7 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/SBUS/SBUS.h @@ -0,0 +1,82 @@ +/* + SBUS.h + Brian R Taylor + brian.taylor@bolderflight.com + + Copyright (c) 2016 Bolder Flight Systems + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#ifndef SBUS_h +#define SBUS_h + +#include "Arduino.h" +#include "elapsedMillis.h" + +/* +* Hardware Serial Supported: +* Teensy 3.0 || Teensy 3.1/3.2 || Teensy 3.5 || Teensy 3.6 || Teensy LC || STM32L4 || Maple Mini || Arduino Mega 2560 || ESP32 +*/ +#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) \ + || defined(__MK66FX1M0__) || defined(__MKL26Z64__) || defined(__IMXRT1052__) \ + || defined(STM32L496xx) || defined(STM32L476xx) || defined(STM32L433xx) \ + || defined(STM32L432xx) || defined(_BOARD_MAPLE_MINI_H_) \ + || defined(__AVR_ATmega2560__) || defined(ESP32) +#endif + +class SBUS{ + public: + SBUS(HardwareSerial& bus); + void begin(); + bool read(uint16_t* channels, bool* failsafe, bool* lostFrame); + bool readCal(float* calChannels, bool* failsafe, bool* lostFrame); + void write(uint16_t* channels); + void writeCal(float *channels); + void setEndPoints(uint8_t channel,uint16_t min,uint16_t max); + void getEndPoints(uint8_t channel,uint16_t *min,uint16_t *max); + void setReadCal(uint8_t channel,float *coeff,uint8_t len); + void getReadCal(uint8_t channel,float *coeff,uint8_t len); + void setWriteCal(uint8_t channel,float *coeff,uint8_t len); + void getWriteCal(uint8_t channel,float *coeff,uint8_t len); + ~SBUS(); + private: + const uint32_t _sbusBaud = 100000; + static const uint8_t _numChannels = 16; + const uint8_t _sbusHeader = 0x0F; + const uint8_t _sbusFooter = 0x00; + const uint8_t _sbus2Footer = 0x04; + const uint8_t _sbus2Mask = 0x0F; + const uint32_t SBUS_TIMEOUT_US = 7000; + uint8_t _parserState, _prevByte = _sbusFooter, _curByte; + static const uint8_t _payloadSize = 24; + uint8_t _payload[_payloadSize]; + const uint8_t _sbusLostFrame = 0x04; + const uint8_t _sbusFailSafe = 0x08; + const uint16_t _defaultMin = 172; + const uint16_t _defaultMax = 1811; + uint16_t _sbusMin[_numChannels]; + uint16_t _sbusMax[_numChannels]; + float _sbusScale[_numChannels]; + float _sbusBias[_numChannels]; + float **_readCoeff, **_writeCoeff; + uint8_t _readLen[_numChannels],_writeLen[_numChannels]; + bool _useReadCoeff[_numChannels], _useWriteCoeff[_numChannels]; + HardwareSerial* _bus; + bool parse(); + void scaleBias(uint8_t channel); + float PolyVal(size_t PolySize, float *Coefficients, float X); +}; + +#endif diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/SBUS/elapsedMillis.h b/Versions/dRehmFlight_Teensy_BETA_1.4/src/SBUS/elapsedMillis.h new file mode 100644 index 00000000..48cff11d --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/SBUS/elapsedMillis.h @@ -0,0 +1,81 @@ +/* Elapsed time types - for easy-to-use measurements of elapsed time + * http://www.pjrc.com/teensy/ + * Copyright (c) 2017 PJRC.COM, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifndef elapsedMillis_h +#define elapsedMillis_h +#ifdef __cplusplus + +#if ARDUINO >= 100 +#include "Arduino.h" +#else +#include "WProgram.h" +#endif + +class elapsedMillis +{ +private: + unsigned long ms; +public: + elapsedMillis(void) { ms = millis(); } + elapsedMillis(unsigned long val) { ms = millis() - val; } + elapsedMillis(const elapsedMillis &orig) { ms = orig.ms; } + operator unsigned long () const { return millis() - ms; } + elapsedMillis & operator = (const elapsedMillis &rhs) { ms = rhs.ms; return *this; } + elapsedMillis & operator = (unsigned long val) { ms = millis() - val; return *this; } + elapsedMillis & operator -= (unsigned long val) { ms += val ; return *this; } + elapsedMillis & operator += (unsigned long val) { ms -= val ; return *this; } + elapsedMillis operator - (int val) const { elapsedMillis r(*this); r.ms += val; return r; } + elapsedMillis operator - (unsigned int val) const { elapsedMillis r(*this); r.ms += val; return r; } + elapsedMillis operator - (long val) const { elapsedMillis r(*this); r.ms += val; return r; } + elapsedMillis operator - (unsigned long val) const { elapsedMillis r(*this); r.ms += val; return r; } + elapsedMillis operator + (int val) const { elapsedMillis r(*this); r.ms -= val; return r; } + elapsedMillis operator + (unsigned int val) const { elapsedMillis r(*this); r.ms -= val; return r; } + elapsedMillis operator + (long val) const { elapsedMillis r(*this); r.ms -= val; return r; } + elapsedMillis operator + (unsigned long val) const { elapsedMillis r(*this); r.ms -= val; return r; } +}; + +class elapsedMicros +{ +private: + unsigned long us; +public: + elapsedMicros(void) { us = micros(); } + elapsedMicros(unsigned long val) { us = micros() - val; } + elapsedMicros(const elapsedMicros &orig) { us = orig.us; } + operator unsigned long () const { return micros() - us; } + elapsedMicros & operator = (const elapsedMicros &rhs) { us = rhs.us; return *this; } + elapsedMicros & operator = (unsigned long val) { us = micros() - val; return *this; } + elapsedMicros & operator -= (unsigned long val) { us += val ; return *this; } + elapsedMicros & operator += (unsigned long val) { us -= val ; return *this; } + elapsedMicros operator - (int val) const { elapsedMicros r(*this); r.us += val; return r; } + elapsedMicros operator - (unsigned int val) const { elapsedMicros r(*this); r.us += val; return r; } + elapsedMicros operator - (long val) const { elapsedMicros r(*this); r.us += val; return r; } + elapsedMicros operator - (unsigned long val) const { elapsedMicros r(*this); r.us += val; return r; } + elapsedMicros operator + (int val) const { elapsedMicros r(*this); r.us -= val; return r; } + elapsedMicros operator + (unsigned int val) const { elapsedMicros r(*this); r.us -= val; return r; } + elapsedMicros operator + (long val) const { elapsedMicros r(*this); r.us -= val; return r; } + elapsedMicros operator + (unsigned long val) const { elapsedMicros r(*this); r.us -= val; return r; } +}; + +#endif // __cplusplus +#endif // elapsedMillis_h diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/TFMPlus/TFMPlus.cpp b/Versions/dRehmFlight_Teensy_BETA_1.4/src/TFMPlus/TFMPlus.cpp new file mode 100644 index 00000000..8a4a63a3 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/TFMPlus/TFMPlus.cpp @@ -0,0 +1,385 @@ +/* File Name: TFMPlus.cpp + * Version: 1.5.0 + * Described: Arduino Library for the Benewake TFMini-Plus Lidar sensor + * The TFMini-Plus is a unique product, and the various + * TFMini Libraries are not compatible with the Plus. + * Developer: Bud Ryerson + * Inception: v0.2.0 - 31 JAN 2019 + * v1.0.0 - 25FEB19 - Initial release + * v1.0.1 - 09MAR19 - 'build()' function always returned TRUE. + Corrected to return FALSE if serial data is not available. + And other minor corrections to textual descriptions. + * v1.1.0 - 13MAR19 - To simplify, all interface functions now + return boolean. Status code is still set and can be read. + 'testSum()' is deleted and 'makeSum()' is used instead. + Example code is updated. + * v1.1.1 - 14MAR19 - Two commands: RESTORE_FACTORY_SETTINGS + and SAVE_SETTINGS were not defined correctly. + * v1.2.1 - 02APR19 - Rewrote 'getData()' function to make it faster + and more robust when serial read skips a byte or fails entirely. + * v1.3.1 - 08APR19 - Redefined commands to include response length + ********************** IMPORTANT ************************ + **** Changed name of 'buildCommand()' to 'sendCommand()'. **** + **************************************************************** + * v.1.3.2 - Added a line to getData() to flush the serial buffer + of all but last frame of data before reading. This does not + effect usage, but will ensure that the latest data is read. + * v.1.3.3 - 20MAY19 - Changed 'sendCommand()' to add a second byte, + to the HEADER recognition routine, the reply length byte. + This makes recognition of the command reply more robust. + Zeroed out 'data frame' snd 'command reply' buffer arrays + completely before reading from device. Added but did not + implement some I2C command codes. + * v.1.3.4 - 07JUN19 - Added 'TFMP_' to all error status defines. + The ubiquitous 'Arduino.h' also contains a 'SERIAL' define. + * v.1.3.5 - 25OCT19 - Added missing underscore to parameter + in header file TFMPlus.h: + Line 138 #define BAUD_14400 0x003840 + * v.1.3.6 - 27APR20 - a little cleanup in 'getData()' + * v.1.4.0 - 15JUN20 - Changed all data variables from unsigned + to signed integers. Defined abnormal data codes + as per TFMini-S Producut Manual + - - - - - - - - - - - - - - - + Dist | Strength | Comment + -1 Other value Strength ≤ 100 + -2 -1 Signal strength saturation + -4 Other value Ambient light saturation + - - - - - - - - - - - - - - - + * v.1.4.1 - 22JUL20 - Fixed bug in sendCommand() checksum calculation + - Changed two printf()s to Serial.print()s + - Fixed printReply() to show data from 'reply' rather than 'frame' + * v.1.4.2 - 19MAY21 - Changed command paramter 'FRAME_5' to correct value. + It was set to 0x0003. Now it's set to 0x0005 + * v.1.5.0 - 06SEP21 - Corrected (reversed) Enable/Disable commands in 'TFMPlus.h' + Changed three command names + OBTAIN_FIRMWARE_VERSION is now GET_FIRMWARE_VERSION + RESTORE_FACTORY_SETTINGS is now HARD_RESET + SYSTEM_RESET is now SOFT_RESET + * + * Default settings for the TFMini-Plus are a 115200 serial baud rate + * and a 100Hz measurement frame rate. The device will begin returning + * measurement data immediately on power up. + * + * 'begin()' function passes a serial stream to library and + * returns TRUE/FALSE whether serial data is available. + * Function also sets a public one byte status code. + * Status codes are defined within the library. + * + * 'getData( dist, flux, temp)' passes back measurement data + * • dist = distance in centimeters, + * • flux = signal strength in arbitrary units, and + * • temp = an encoded number in degrees centigrade + * Function returns TRUE/FALSE whether completed without error. + * Error, if any, is saved as a one byte 'status' code. + * + * 'sendCommand( cmnd, param)' sends a 32bit command code (cmnd) + * and a 32bit parameter value (param). Returns TRUE/FALSE and + * sets a one byte status code. + * Commands are selected from the library's list of defined commands. + * Parameter values can be entered directly (115200, 250, etc) but + * for safety, they should be chosen from the Library's defined lists. + * An incorrect value can render the device uncommunicative. + * + */ + +#include "TFMPlus.h" +//#include // Future I2C Implementation + +// Constructor +TFMPlus::TFMPlus(){} +TFMPlus::~TFMPlus(){} + +// Return TRUE/FALSE whether receiving serial data from +// device, and set system status to provide more information. +bool TFMPlus::begin(Stream *streamPtr) +{ + pStream = streamPtr; // Save reference to stream/serial object. + delay( 10); // Delay for device data in serial buffer. + if( (*pStream).available()) // If data present... + { + status = TFMP_READY; // set status to READY + return true; // and return TRUE. + } + else // Otherwise... + { + status = TFMP_SERIAL; // set status to SERIAL error + return false; // and return false. + } +} + +bool TFMPlus::getData( int16_t &dist, int16_t &flux, int16_t &temp) +{ + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 1 - Get data from the device. + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Set timer to one second timeout if HEADER never appears + // or serial data never becomes available. + uint32_t serialTimeout = millis() + 1000; + + if( !(*pStream).available()) + { + return false; + } + + // Flush all but last frame of data from the serial buffer. + while( (*pStream).available() > TFMP_FRAME_SIZE) (*pStream).read(); + + // Zero out the entire frame data buffer. + memset( frame, 0, sizeof( frame)); + + // Read one byte from the serial buffer into the last byte of + // the frame buffer, then left shift the whole array one byte. + // Repeat until the two HEADER bytes show up as the first + // two bytes in the array. + while( ( frame[ 0] != 0x59) || ( frame[ 1] != 0x59)) + { + if( (*pStream).available()) + { + // Read one byte into the framebuffer's + // last plus one position. + frame[ TFMP_FRAME_SIZE] = (*pStream).read(); + // Shift the last nine bytes one byte left. + memcpy( frame, frame + 1, TFMP_FRAME_SIZE); + } + // If HEADER or serial data are not available + // after more than one second... + if( millis() > serialTimeout) + { + status = TFMP_HEADER; // then set error... + return false; // and return "false". + } + } + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 2 - Perform a checksum test. + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Clear the 'chkSum' variable declared in 'TFMPlus.h' + chkSum = 0; + // Add together all bytes but the last. + for( uint8_t i = 0; i < ( TFMP_FRAME_SIZE - 1); i++) chkSum += frame[ i]; + // If the low order byte does not equal the last byte... + if( ( uint8_t)chkSum != frame[ TFMP_FRAME_SIZE - 1]) + { + status = TFMP_CHECKSUM; // then set error... + return false; // and return "false." + } + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 3 - Interpret the frame data + // and if okay, then go home + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + dist = frame[ 2] + ( frame[ 3] << 8); + flux = frame[ 4] + ( frame[ 5] << 8); + temp = frame[ 6] + ( frame[ 7] << 8); + // Convert temp code to degrees Celsius. + temp = ( temp >> 3) - 256; + // Convert Celsius to degrees Farenheit + // temp = uint8_t( temp * 9 / 5) + 32; + + // - - Evaluate Abnormal Data Values - - + // Values are from the TFMini-S Product Manual + // Signal strength <= 100 + if( dist == -1) status = TFMP_WEAK; + // Signal Strength saturation + else if( flux == -1) status = TFMP_STRONG; + // Ambient Light saturation + else if( dist == -4) status = TFMP_FLOOD; + // Data is apparently okay + else status = TFMP_READY; + + if( status != TFMP_READY) return false; + else return true; +} + +// Pass back only the distance data +bool TFMPlus::getData( int16_t &dist) +{ + static int16_t flux, temp; + return getData( dist, flux, temp); +} + +// = = = = = SEND A COMMAND TO THE DEVICE = = = = = = = = = =0 +// +// Create a proper command byte array, send the command, +// get a repsonse, and return the status +bool TFMPlus::sendCommand( uint32_t cmnd, uint32_t param) +{ + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 1 - Build the command data to send to the device + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + static uint8_t cmndLen; // Length of command + static uint8_t replyLen; // Length of command reply data + static uint8_t cmndData[ 8]; // 8 byte send command array + + memset( cmndData, 0, 8); // Clear the send command array. + memcpy( &cmndData[ 0], &cmnd, 4); // Copy 4 bytes of data: reply length, + // command length, command number and + // a one byte parameter, all encoded as + // a 32 bit unsigned integer. + + replyLen = cmndData[ 0]; // Save the first byte as reply length. + cmndLen = cmndData[ 1]; // Save the second byte as command length. + cmndData[ 0] = 0x5A; // Set the first byte to HEADER code. + + if( cmnd == SET_FRAME_RATE) // If the command is Set FrameRate... + { + memcpy( &cmndData[ 3], ¶m, 2); // add the 2 byte FrameRate parameter. + } + else if( cmnd == SET_BAUD_RATE) // If the command is Set BaudRate... + { + memcpy( &cmndData[ 3], ¶m, 4); // add the 3 byte BaudRate parameter. + } + + // Create a checksum byte for the command data array. + chkSum = 0; + // Add together all bytes but the last... + for( uint8_t i = 0; i < ( cmndLen - 1); i++) chkSum += cmndData[ i]; + // and save it as the last byte of command data. + cmndData[ cmndLen - 1] = (uint8_t)chkSum; + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 2 - Send the command data array to the device + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + while( (*pStream).available()) (*pStream).read(); // flush input buffer + (*pStream).flush(); // flush output buffer + for( uint8_t i = 0; i < cmndLen; i++) (*pStream).write( cmndData[ i]); + + + // + + + + + + + + + + + + + + + + + + + + + + + + + + // If the command does not expect a reply, then we're + // finished here. Call the getData() function instead. + if( replyLen == 0) return true; + // + + + + + + + + + + + + + + + + + + + + + + + + + + + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 3 - Get command reply data back from the device. + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Set a one second timer to timeout if HEADER never appears + // or serial data never becomes available + uint32_t serialTimeout = millis() + 1000; + // Clear out the entire command reply data buffer + memset( reply, 0, sizeof( reply)); + // Read one byte from the serial buffer into the end of + // the reply buffer and then left shift the whole array. + // Repeat until the HEADER byte and reply length byte + // show up as the first two bytes in the array. + while( ( reply[ 0] != 0x5A) || ( reply[ 1] != replyLen)) + { + if( (*pStream).available()) + { + // Read one byte into the reply buffer's + // last-plus-one position. + reply[ replyLen] = (*pStream).read(); + // Shift the last nine bytes one byte left. + memcpy( reply, reply+1, TFMP_REPLY_SIZE); + } + // If HEADER pattern or Serial data are not available + // after more than one second... + if( millis() > serialTimeout) + { + status = TFMP_TIMEOUT; // then set error... + return false; // and return "false". + } + } + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 4 - Perform a checksum test. + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Clear the 'chkSum' variable declared in 'TFMPlus.h' + chkSum = 0; + // Add together all bytes but the last... + for( uint8_t i = 0; i < ( replyLen - 1); i++) chkSum += reply[ i]; + // If the low order byte of the Sum does not equal the last byte... + if( reply[ replyLen - 1] != (uint8_t)chkSum) + { + status = TFMP_CHECKSUM; // then set error... + return false; // and return "false." + } + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 5 - Interpret different command responses. + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + if( cmnd == GET_FIRMWARE_VERSION) + { + version[ 0] = reply[5]; // set firmware version. + version[ 1] = reply[4]; + version[ 2] = reply[3]; + } + else + { + if( cmnd == SOFT_RESET || + cmnd == HARD_RESET || + cmnd == SAVE_SETTINGS ) + { + if( reply[ 3] == 1) // If PASS/FAIL byte not zero ... + { + status = TFMP_FAIL; // set status 'FAIL'... + return false; // and return 'false'. + } + } + } + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 6 - Set READY status and go home + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + status = TFMP_READY; + return true; +} + +// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +// - - - - - The following is for testing purposes - - - - +// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + +// Called by either 'printFrame()' or 'printReply()' +// Print status condition either 'READY' or error type +void TFMPlus::printStatus() +{ + Serial.print("Status: "); + if( status == TFMP_READY) Serial.print( "READY"); + else if( status == TFMP_SERIAL) Serial.print( "SERIAL"); + else if( status == TFMP_HEADER) Serial.print( "HEADER"); + else if( status == TFMP_CHECKSUM) Serial.print( "CHECKSUM"); + else if( status == TFMP_TIMEOUT) Serial.print( "TIMEOUT"); + else if( status == TFMP_PASS) Serial.print( "PASS"); + else if( status == TFMP_FAIL) Serial.print( "FAIL"); + else if( status == TFMP_I2CREAD) Serial.print( "I2C-READ"); + else if( status == TFMP_I2CWRITE) Serial.print( "I2C-WRITE"); + else if( status == TFMP_I2CLENGTH) Serial.print( "I2C-LENGTH"); + else if( status == TFMP_WEAK) Serial.print( "Signal weak"); + else if( status == TFMP_STRONG) Serial.print( "Signal saturation"); + else if( status == TFMP_FLOOD) Serial.print( "Ambient light saturation"); + else Serial.print( "OTHER"); + Serial.println(); +} + +// Print error type and HEX values +// of each byte in the data frame +void TFMPlus::printFrame() +{ + printStatus(); + // Print the Hex value of each byte of data + Serial.print("Data:"); + for( uint8_t i = 0; i < TFMP_FRAME_SIZE; i++) + { + Serial.print(" "); + Serial.print( frame[ i] < 16 ? "0" : ""); + Serial.print( frame[ i], HEX); + } + Serial.println(); +} + +// Print error type and HEX values of +// each byte in the command response frame. +void TFMPlus::printReply() +{ + printStatus(); + // Print the Hex value of each byte + for( uint8_t i = 0; i < TFMP_REPLY_SIZE; i++) + { + Serial.print(" "); + Serial.print( reply[ i] < 16 ? "0" : ""); + Serial.print( reply[ i], HEX); + } + Serial.println(); +} diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/src/TFMPlus/TFMPlus.h b/Versions/dRehmFlight_Teensy_BETA_1.4/src/TFMPlus/TFMPlus.h new file mode 100644 index 00000000..64721987 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/src/TFMPlus/TFMPlus.h @@ -0,0 +1,227 @@ +/* File Name: TFMPlus.h + * Version: 1.5.0 + * Described: Arduino Library for the Benewake TFMini-Plus Lidar sensor + * The TFMini-Plus is a unique product, and the various + * TFMini Libraries are not compatible with the Plus. + * Developer: Bud Ryerson + * Inception: v0.2.0 - 31 JAN 2019 + * v1.0.0 - 25FEB19 - Initial release + * v1.0.1 - 09MAR19 - 'build()' function always returned TRUE. + Corrected to return FALSE if serial data is not available. + And other minor corrections to textual descriptions. + * v1.1.0 - 13MAR19 - To simplify, all interface functions now + return boolean. Status code is still set and can be read. + 'testSum()' is deleted and 'makeSum()' is used instead. + Example code is updated. + * v1.1.1 - 14MAR19 - Two commands: RESTORE_FACTORY_SETTINGS + and SAVE_SETTINGS were not defined correctly. + * v1.2.1 - 02APR19 - Rewrote 'getData()' function to make it faster + and more robust when serial read skips a byte or fails entirely. + * v1.3.1 - 08APR19 - Redefined commands to include response length + ********************** IMPORTANT ************************ + **** Changed name of 'buildCommand()' to 'sendCommand()'. **** + **************************************************************** + * v.1.3.2 - Added a line to getData() to flush the serial buffer + of all but last frame of data before reading. This does not + effect usage, but will ensure that the latest data is read. + * v.1.3.3 - 20MAY19 - Changed 'sendCommand()' to add a second byte, + to the HEADER recognition routine, the reply length byte. + Zeroed out 'data frame' snd 'command reply' buffer arrays + completely before reading from device. Added but did not + implement some I2C command codes. + * v.1.3.4 - 07JUN19 - Added 'TFMP_' to all error status defines. + The ubiquitous 'Arduino.h' also contains a 'SERIAL' define. + * v.1.3.5 - 25OCT19 - Added missing underscore to paramter: + #define BAUD_14400 0x003840 + * v.1.3.6 - 27APR20 - a little cleanup in 'getData()' + * v.1.4.0 - 15JUN20 + 1. Changed all data variables from unsigned to signed integers. + 2. Defined abnormal data codes as per TFMini-S Producut Manual + - - - - - - - - - - - - - - - - + Distance | Strength | Comment + -1 Other value Strength ≤ 100 + -2 -1 Signal strength saturation + -4 Other value Ambient light saturation + - - - - - - - - - - - - - - - - + * v.1.4.1 - 22JUL20 - Fixed bugs in TFMPlus.cpp + * v.1.5.0 - 06SEP21 - Corrected (reversed) Enable/Disable commands + Changed three command names + OBTAIN_FIRMWARE_VERSION is now GET_FIRMWARE_VERSION + RESTORE_FACTORY_SETTINGS is now HARD_RESET + SYSTEM_RESET is now SOFT_RESET + * + * Default settings for the TFMini-Plus are a 115200 serial baud rate + * and a 100Hz measurement frame rate. The device will begin returning + * measurement data immediately on power up. + * + * 'begin()' function passes a serial stream to library and + * returns TRUE/FALSE whether serial data is available. + * Function also sets a public one byte status code. + * Status codes are defined within the library. + * + * 'getData( dist, flux, temp)' passes back measurement data + * • dist = distance in centimeters, + * • flux = signal strength in arbitrary units, and + * • temp = an encoded number in degrees centigrade + * Function returns TRUE/FALSE whether completed without error. + * Error, if any, is saved as a one byte 'status' code. + * + * 'sendCommand( cmnd, param)' sends a 32bit command code (cmnd) + * and a 32bit parameter value (param). Returns TRUE/FALSE and + * sets a one byte status code. + * Commands are selected from the library's list of defined commands. + * Parameter values can be entered directly (115200, 250, etc) but + * for safety, they should be chosen from the Library's defined lists. + * An incorrect value can render the device uncommunicative. + * + */ + +#ifndef TFMPLUS_H // Guard to compile only once +#define TFMPLUS_H + +#include // Always include this. It's important. + +// Buffer sizes +#define TFMP_FRAME_SIZE 9 // Size of data frame = 9 bytes +#define TFMP_REPLY_SIZE 8 // Longest command reply = 8 bytes +#define TFMP_COMMAND_MAX 8 // Longest command = 8 bytes + +// Timeout Limits for various functions +#define TFMP_MAX_READS 20 // readData() sets SERIAL error +#define MAX_BYTES_BEFORE_HEADER 20 // getData() sets HEADER error +#define MAX_ATTEMPTS_TO_MEASURE 20 + +#define TFMP_DEFAULT_ADDRESS 0x10 // default I2C slave address + // as hexidecimal integer +// System Error Status Condition +#define TFMP_READY 0 // no error +#define TFMP_SERIAL 1 // serial timeout +#define TFMP_HEADER 2 // no header found +#define TFMP_CHECKSUM 3 // checksum doesn't match +#define TFMP_TIMEOUT 4 // I2C timeout +#define TFMP_PASS 5 // reply from some system commands +#define TFMP_FAIL 6 // " +#define TFMP_I2CREAD 7 +#define TFMP_I2CWRITE 8 +#define TFMP_I2CLENGTH 9 +#define TFMP_WEAK 10 // Signal Strength ≤ 100 +#define TFMP_STRONG 11 // Signal Strength saturation +#define TFMP_FLOOD 12 // Ambient Light saturation +#define TFMP_MEASURE 13 + + +/* - - - - - - - - - TFMini Plus - - - - - - - - - + Data Frame format: + Byte0 Byte1 Byte2 Byte3 Byte4 Byte5 Byte6 Byte7 Byte8 + 0x59 0x59 Dist_L Dist_H Flux_L Flux_H Temp_L Temp_H CheckSum_ + Data Frame Header character: Hex 0x59, Decimal 89, or "Y" + + Command format: + Byte0 Byte1 Byte2 Byte3 to Len-2 Byte Len-1 + 0x5A Length Cmd ID Payload if any Checksum + - - - - - - - - - - - - - - - - - - - - - - - - - */ + +// The library 'sendCommand( cmnd, param)' function +// defines a command (cmnd) in the the following format: +// 0x 00 00 00 00 +// one byte command command reply +// payload number length length +#define GET_FIRMWARE_VERSION 0x00010407 // returns 3 byte firmware version +#define TRIGGER_DETECTION 0x00040400 // must have set frame rate to zero + // returns a 9 byte data frame +#define SOFT_RESET 0x00020405 // returns a 1 byte pass/fail (0/1) +#define HARD_RESET 0x00100405 // " +#define SAVE_SETTINGS 0x00110405 // This must follow every command + // that modifies volatile parameters. + // Returns a 1 byte pass/fail (0/1) + +#define SET_FRAME_RATE 0x00030606 // These commands each return +#define SET_BAUD_RATE 0x00060808 // an echo of the command +#define STANDARD_FORMAT_CM 0x01050505 // " +#define PIXHAWK_FORMAT 0x02050505 // " +#define STANDARD_FORMAT_MM 0x06050505 // " +#define ENABLE_OUTPUT 0x01070505 // " +#define DISABLE_OUTPUT 0x00070505 // " +#define SET_I2C_ADDRESS 0x100B0505 // " + +#define SET_SERIAL_MODE 0x000A0500 // default is Serial (UART) +#define SET_I2C_MODE 0x010A0500 // set device as I2C slave + +#define I2C_FORMAT_CM 0x01000500 // returns a 9 byte data frame +#define I2C_FORMAT_MM 0x06000500 // " + +// * * * * * * * Description of I/O Mode * * * * * * * +// Normally, device Pin 3 is either Serial transmit (TX) or I2C clock (SCL). +// When 'I/O Mode' is set other than 'Standard,' Pin 3 becomes a simple HI/LO +// (near/far) binary output. Thereafter, only Pin 2, the Serial RX line, is +// functional, and only Serial data sent to the device is possible. +//#define SET_IO_MODE_STANDARD 0x003B0900 // 'Standard' is default mode +//#define SET_IO_MODE_HILO 0x013B0900 // I/O, near high and far low +//#define SET_IO_MODE_LOHI 0x023B0900 // I/O, near low and far high +// * * * This library does not support the I/O Mode interface * * * + +// Command Parameters +#define BAUD_9600 0x002580 // UART serial baud rate +#define BAUD_14400 0x003840 // expressed in hexidecimal +#define BAUD_19200 0x004B00 +#define BAUD_56000 0x00DAC0 +#define BAUD_115200 0x01C200 +#define BAUD_460800 0x070800 +#define BAUD_921600 0x0E1000 + +#define FRAME_0 0x0000 // internal measurement rate +#define FRAME_1 0x0001 // expressed in hexidecimal +#define FRAME_2 0x0002 +#define FRAME_5 0x0005 // incorrectly set to 3 in prior versions +#define FRAME_10 0x000A +#define FRAME_20 0x0014 +#define FRAME_25 0x0019 +#define FRAME_50 0x0032 +#define FRAME_100 0x0064 +#define FRAME_125 0x007D +#define FRAME_200 0x00C8 +#define FRAME_250 0x00FA +#define FRAME_500 0x01F4 +#define FRAME_1000 0x03E8 + +// Object Class Definitions +class TFMPlus +{ + public: + TFMPlus(); + ~TFMPlus(); + + uint8_t version[ 3]; // to save firmware version + uint8_t status; // to save library error status + + // Return T/F whether serial data available, set error status if not. + bool begin( Stream *streamPtr); + // Read device data and pass back three values + bool getData( int16_t &dist, int16_t &flux, int16_t &temp); + // Short version, passes back distance data only + bool getData( int16_t &dist); + // Build and send a command, and check response + bool sendCommand( uint32_t cmnd, uint32_t param); + + // For testing purposes: print frame or reply data and status + // as a string of HEX characters + void printFrame(); + void printReply(); + bool getResponse(); + + private: + Stream* pStream; // pointer to the device serial stream + // The data buffers are one byte longer than necessary + // because we read one byte into the last position, then + // shift the whole array left by one byte after each read. + uint8_t frame[ TFMP_FRAME_SIZE + 1]; + uint8_t reply[ TFMP_REPLY_SIZE + 1]; + + uint16_t chkSum; // to calculate the check sum byte. + + // for testing - called by 'printFrame()' or 'printReply()' + void printStatus(); + +}; + +#endif From a2bb359d2ea5064965c36283b59016a51019fd4c Mon Sep 17 00:00:00 2001 From: Brian Jones Date: Sat, 9 Aug 2025 15:18:22 -0500 Subject: [PATCH 2/6] Working on a 2.0 version that can support different IMUs and possibly boards as well as drastically cleaning up and modularizing the code. --- .../dRehmFlight_Teensy_BETA_1.3.ino | 31 +- .../dRehmFlight_Teensy_BETA_1.4.ino | 93 +- .../dRehmFlight_Teensy_BETA_2.0/COPYING.txt | 674 ++++ .../dRehmFlight_Teensy_BETA_2.0.ino | 1021 +++++ .../dRehmFlight_Teensy_BETA_2.0/radioComm.ino | 198 + .../src/DSMRX/DSMRX.cpp | 116 + .../src/DSMRX/DSMRX.h | 85 + .../src/IMU_PID/IMU_PID.cpp | 1015 +++++ .../src/IMU_PID/IMU_PID.h | 196 + .../src/IMU_PID/keywords.txt | 34 + .../src/MPU6050/I2Cdev.cpp | 1468 ++++++++ .../src/MPU6050/I2Cdev.h | 283 ++ .../src/MPU6050/MPU6050.cpp | 3330 +++++++++++++++++ .../src/MPU6050/MPU6050.h | 1041 ++++++ .../src/MPU6050/MPU6050_6Axis_MotionApps20.h | 617 +++ .../MPU6050/MPU6050_6Axis_MotionApps_V6_12.h | 617 +++ .../src/MPU6050/MPU6050_9Axis_MotionApps41.h | 887 +++++ .../src/MPU6050/helper_3dmath.h | 216 ++ .../src/MPU9250/MPU9250.cpp | 1202 ++++++ .../src/MPU9250/MPU9250.h | 313 ++ .../src/SBUS/SBUS.cpp | 376 ++ .../src/SBUS/SBUS.h | 82 + .../src/SBUS/elapsedMillis.h | 81 + .../src/TFMPlus/TFMPlus.cpp | 385 ++ .../src/TFMPlus/TFMPlus.h | 227 ++ .../src/dRhemFlight_hardware.h | 78 + 26 files changed, 14617 insertions(+), 49 deletions(-) create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/COPYING.txt create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/radioComm.ino create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/DSMRX/DSMRX.cpp create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/DSMRX/DSMRX.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/IMU_PID.cpp create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/IMU_PID.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/keywords.txt create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/I2Cdev.cpp create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/I2Cdev.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050.cpp create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050_6Axis_MotionApps20.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050_9Axis_MotionApps41.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/helper_3dmath.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU9250/MPU9250.cpp create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU9250/MPU9250.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/SBUS/SBUS.cpp create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/SBUS/SBUS.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/SBUS/elapsedMillis.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/TFMPlus/TFMPlus.cpp create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/TFMPlus/TFMPlus.h create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/dRhemFlight_hardware.h diff --git a/Versions/dRehmFlight_Teensy_BETA_1.3/dRehmFlight_Teensy_BETA_1.3.ino b/Versions/dRehmFlight_Teensy_BETA_1.3/dRehmFlight_Teensy_BETA_1.3.ino index 9248fd6d..c2f390ef 100644 --- a/Versions/dRehmFlight_Teensy_BETA_1.3/dRehmFlight_Teensy_BETA_1.3.ino +++ b/Versions/dRehmFlight_Teensy_BETA_1.3/dRehmFlight_Teensy_BETA_1.3.ino @@ -168,12 +168,13 @@ float MagScaleY = 1.0; float MagScaleZ = 1.0; //IMU calibration parameters - calibrate IMU using calculate_IMU_error() in the void setup() to get these values, then comment out calculate_IMU_error() -float AccErrorX = 0.0; -float AccErrorY = 0.0; -float AccErrorZ = 0.0; -float GyroErrorX = 0.0; -float GyroErrorY= 0.0; -float GyroErrorZ = 0.0; +float AccErrorX = 0.00; +float AccErrorY = 0.01; +float AccErrorZ = -0.05; +float GyroErrorX = -0.40; +float GyroErrorY = 0.55; +float GyroErrorZ = -0.58; + //Controller parameters (take note of defaults before modifying!): float i_limit = 25.0; //Integrator saturation level, mostly for safety (default 25.0) @@ -400,7 +401,7 @@ void loop() { //printGyroData(); //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest) //printAccelData(); //Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level) //printMagData(); //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300) - //printRollPitchYaw(); //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level) + printRollPitchYaw(); //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level) //printPIDoutput(); //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1) //printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250) //printServoCommands(); //Prints the values being written to the servos (expected: 0 to 180) @@ -1605,7 +1606,9 @@ void printDesiredState() { void printGyroData() { if (current_time - print_counter > 10000) { print_counter = micros(); - Serial.print(F("GyroX:")); + Serial.print(F("LOW:-300.0")); + Serial.print(F(" HIGH:300.0")); + Serial.print(F(" GyroX:")); Serial.print(GyroX); Serial.print(F(" GyroY:")); Serial.print(GyroY); @@ -1617,7 +1620,9 @@ void printGyroData() { void printAccelData() { if (current_time - print_counter > 10000) { print_counter = micros(); - Serial.print(F("AccX:")); + Serial.print(F("LOW:-2.0")); + Serial.print(F(" HIGH:2.0")); + Serial.print(F(" AccX:")); Serial.print(AccX); Serial.print(F(" AccY:")); Serial.print(AccY); @@ -1641,7 +1646,9 @@ void printMagData() { void printRollPitchYaw() { if (current_time - print_counter > 10000) { print_counter = micros(); - Serial.print(F("roll:")); + Serial.print(F("LOW:-180.0")); + Serial.print(F(" HIGH:180.0")); + Serial.print(F(" roll:")); Serial.print(roll_IMU); Serial.print(F(" pitch:")); Serial.print(pitch_IMU); @@ -1653,7 +1660,9 @@ void printRollPitchYaw() { void printPIDoutput() { if (current_time - print_counter > 10000) { print_counter = micros(); - Serial.print(F("roll_PID:")); + Serial.print(F("LOW:-1.0")); + Serial.print(F(" HIGH:1.0")); + Serial.print(F(" roll_PID:")); Serial.print(roll_PID); Serial.print(F(" pitch_PID:")); Serial.print(pitch_PID); diff --git a/Versions/dRehmFlight_Teensy_BETA_1.4/dRehmFlight_Teensy_BETA_1.4.ino b/Versions/dRehmFlight_Teensy_BETA_1.4/dRehmFlight_Teensy_BETA_1.4.ino index 8ca9024e..0ab27e1c 100644 --- a/Versions/dRehmFlight_Teensy_BETA_1.4/dRehmFlight_Teensy_BETA_1.4.ino +++ b/Versions/dRehmFlight_Teensy_BETA_1.4/dRehmFlight_Teensy_BETA_1.4.ino @@ -2,7 +2,7 @@ //Author: Nicholas Rehm //Project Start: 1/6/2020 //Last Updated: 7/29/2022 -//Version: Beta 1.3 +//Version: Beta 1.4 //========================================================================================================================// @@ -35,16 +35,16 @@ Everyone that sends me pictures and videos of your flying creations! -Nick //========================================================================================================================// //Uncomment only one receiver type -#define USE_PWM_RX +//#define USE_PWM_RX //#define USE_PPM_RX -//#define USE_SBUS_RX +#define USE_SBUS_RX //#define USE_DSM_RX static const uint8_t num_DSM_channels = 6; //If using DSM RX, change this to match the number of transmitter channels you have //Uncomment only one IMU -#define USE_MPU6050_I2C //Default +//#define USE_MPU6050_I2C //Default //#define USE_MPU9250_SPI -//#define USE_LSM6DSOX_SPI +#define USE_LSM6DSOX_SPI //Uncomment only one full scale gyro range (deg/sec) #define GYRO_250DPS //Default @@ -88,11 +88,11 @@ static const uint8_t num_DSM_channels = 6; //If using DSM RX, change this to mat #include Adafruit_LSM6DSOX lsm6dsox; - #define LSM_CS 36 - #define LSM_SCK 37 - #define LSM_MISO 34 - #define LSM_MOSI 35 - + #define LSM_MISO 34 // MISO2/DO--red + #define LSM_MOSI 35 // MOSI2/SDA--black + #define LSM_CS 36 // CS2--green-yellow + #define LSM_SCK 37 // SCK2(SCL)--white + sensors_event_t accel; sensors_event_t gyro; sensors_event_t temp; //unused, but could be interesting @@ -100,6 +100,15 @@ static const uint8_t num_DSM_channels = 6; //If using DSM RX, change this to mat #error No MPU defined... #endif +// BJ - testing defined values +#define DEG2RAD 0.0174533f +#define RAD2DEG 57.29575496f +#define G2MS 9.80665f +#define MS2G 0.10197162129779283f + + + + //========================================================================================================================// @@ -194,12 +203,13 @@ float MagScaleY = 1.0; float MagScaleZ = 1.0; //IMU calibration parameters - calibrate IMU using calculate_IMU_error() in the void setup() to get these values, then comment out calculate_IMU_error() -float AccErrorX = 0.0; -float AccErrorY = 0.0; -float AccErrorZ = 0.0; -float GyroErrorX = 0.0; -float GyroErrorY= 0.0; -float GyroErrorZ = 0.0; +float AccErrorX = -0.01; +float AccErrorY = 0.01; +float AccErrorZ = 0.04; +float GyroErrorX = 0.41; +float GyroErrorY = 0.17; +float GyroErrorZ = -0.67; + //Controller parameters (take note of defaults before modifying!): float i_limit = 25.0; //Integrator saturation level, mostly for safety (default 25.0) @@ -427,7 +437,7 @@ void loop() { //printAccelData(); //Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level) //printMagData(); //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300) //printRollPitchYaw(); //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level) - //printPIDoutput(); //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1) + printPIDoutput(); //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1) //printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250) //printServoCommands(); //Prints the values being written to the servos (expected: 0 to 180) //printLoopRate(); //Prints the time between loops in microseconds (expected: microseconds between loop iterations) @@ -621,12 +631,16 @@ void getIMUdata() { mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); #elif defined USE_LSM6DSOX_SPI lsm6dsox.getEvent(&accel, &gyro, &temp); - AccX = accel.acceleration.x / 9.80665; // Convert m/s to G-factor - AccY = accel.acceleration.y / 9.80665; - AccZ = accel.acceleration.z / 9.80665; - GyroX = gyro.gyro.x * 57.29578; // convert rad/s back to deg/sec - GyroY = gyro.gyro.y * 57.29578; - GyroZ = gyro.gyro.z * 57.29578; + + // THe LSM6DSOX has the X and Y reversed from the MPU6050 as well has having the X/Y accelerometers reversed and Y gyro reversed + AccX = accel.acceleration.y * MS2G; // Convert m/s to G-factor + AccY = accel.acceleration.x * -MS2G; + AccZ = accel.acceleration.z * MS2G; + GyroX = gyro.gyro.y * RAD2DEG; // convert rad/s back to deg/sec + GyroY = gyro.gyro.x * -RAD2DEG; + GyroZ = gyro.gyro.z * RAD2DEG; + + #endif // The MPU6050 and MPU9250 IMUs have a different scale factor from the LSM6DSOX @@ -715,12 +729,15 @@ void calculate_IMU_error() { mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); #elif defined USE_LSM6DSOX_SPI lsm6dsox.getEvent(&accel, &gyro, &temp); - AccX = accel.acceleration.x / 9.80665; // Convert m/s to G-factor - AccY = accel.acceleration.y / 9.80665; - AccZ = accel.acceleration.z / 9.80665; - GyroX = gyro.gyro.x * 57.29578; // convert rad/s back to deg/sec - GyroY = gyro.gyro.y * 57.29578; - GyroZ = gyro.gyro.z * 57.29578; + + // THe LSM6DSOX has the X and Y reversed from the MPU6050 as well has having the X/Y accelerometers reversed and Y gyro reversed + AccX = accel.acceleration.y * MS2G; // Convert m/s to G-factor + AccY = accel.acceleration.x * -MS2G; + AccZ = accel.acceleration.z * MS2G; + GyroX = gyro.gyro.y * RAD2DEG; // convert rad/s back to deg/sec + GyroY = gyro.gyro.x * -RAD2DEG; + GyroZ = gyro.gyro.z * RAD2DEG; + #endif // The MPU6050 and MPU9250 IMUs have a different scale factor from the LSM6DSOX @@ -823,9 +840,9 @@ void Madgwick(float gx, float gy, float gz, float ax, float ay, float az, float } //Convert gyroscope degrees/sec to radians/sec - gx *= 0.0174533f; - gy *= 0.0174533f; - gz *= 0.0174533f; + gx *= DEG2RAD; + gy *= DEG2RAD; + gz *= DEG2RAD; //Rate of change of quaternion from gyroscope qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); @@ -927,9 +944,9 @@ void Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, fl float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; //Convert gyroscope degrees/sec to radians/sec - gx *= 0.0174533f; - gy *= 0.0174533f; - gz *= 0.0174533f; + gx *= DEG2RAD; + gy *= DEG2RAD; + gz *= DEG2RAD; //Rate of change of quaternion from gyroscope qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); @@ -1349,7 +1366,7 @@ void failSafe() { //If any failures, set to default failsafe values #ifdef USE_SBUS_RX if (sbusFailSafe) { - #elif + #else if ((check1 + check2 + check3 + check4 + check5 + check6) > 0) { #endif channel_1_pwm = channel_1_fs; @@ -1773,8 +1790,8 @@ void printRollPitchYaw() { void printPIDoutput() { if (current_time - print_counter > 10000) { print_counter = micros(); - Serial.print(F("LOW:-2.0")); - Serial.print(F(" HIGH:2.0")); + Serial.print(F("LOW:-1.0")); + Serial.print(F(" HIGH:1.0")); Serial.print(F(" roll_PID:")); Serial.print(roll_PID); Serial.print(F(" pitch_PID:")); diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/COPYING.txt b/Versions/dRehmFlight_Teensy_BETA_2.0/COPYING.txt new file mode 100644 index 00000000..80e659dc --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/COPYING.txt @@ -0,0 +1,674 @@ +GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino b/Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino new file mode 100644 index 00000000..294d411b --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino @@ -0,0 +1,1021 @@ +//Arduino/Teensy Flight Controller - dRehmFlight +//Author: Nicholas Rehm +//Project Start: 1/6/2020 +//Last Updated: 7/29/2022 +//Version: Beta 2.0 + +//========================================================================================================================// + +//CREDITS + SPECIAL THANKS +/* +Some elements inspired by: +http://www.brokking.net/ymfc-32_main.html + +Madgwick filter function adapted from: +https://github.com/arduino-libraries/MadgwickAHRS + +MPU9250 implementation based on MPU9250 library by: +brian.taylor@bolderflight.com +http://www.bolderflight.com + +Adafruit LSM6DSOX implementation added by: +bjones@aggiejones.com +http://nvrtd.design + +Thank you to: +RcGroups 'jihlein' - IMU implementation overhaul + SBUS implementation. +Everyone that sends me pictures and videos of your flying creations! -Nick + +*/ + + +//REQUIRED LIBRARIES (included with download in main sketch folder) +// #include "src/dRhemFlight_hardware.h" +// #include //I2c communication +// #include //SPI communication +#include //Commanding any extra actuators, installed with teensyduino installer +#include "src/IMU_PID/IMU_PID.h" + + + + + + + + + + + + + + + +//========================================================================================================================// +// USER-SPECIFIED VARIABLES // +//========================================================================================================================// + +//Radio failsafe values for every channel in the event that bad reciever data is detected. Recommended defaults: +unsigned long channel_1_fs = 1000; //thro +unsigned long channel_2_fs = 1500; //ail +unsigned long channel_3_fs = 1500; //elev +unsigned long channel_4_fs = 1500; //rudd +unsigned long channel_5_fs = 2000; //gear, greater than 1500 = throttle cut +unsigned long channel_6_fs = 2000; //aux1 + + +//Controller parameters (take note of defaults before modifying!): +float maxRoll = 30.0; //Max roll angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode +float maxPitch = 30.0; //Max pitch angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode +float maxYaw = 160.0; //Max yaw rate in deg/sec + + +//========================================================================================================================// +// DECLARE PINS // +//========================================================================================================================// + +//NOTE: Pin 13 is reserved for onboard LED, pins 18 and 19 are reserved for the MPU6050 IMU for default setup +//Radio: +//Note: If using SBUS, connect to pin 21 (RX5), if using DSM, connect to pin 15 (RX3) +const int ch1Pin = 15; //throttle +const int ch2Pin = 16; //ail +const int ch3Pin = 17; //ele +const int ch4Pin = 20; //rudd +const int ch5Pin = 21; //gear (throttle cut) +const int ch6Pin = 22; //aux1 (free aux channel) +const int PPM_Pin = 23; +//OneShot125 ESC pin outputs: +const int m1Pin = 0; +const int m2Pin = 1; +const int m3Pin = 2; +const int m4Pin = 3; +const int m5Pin = 4; +const int m6Pin = 5; +//PWM servo or ESC outputs: +const int servo1Pin = 6; +const int servo2Pin = 7; +const int servo3Pin = 8; +const int servo4Pin = 9; +// const int servo5Pin = 10; +// const int servo6Pin = 11; +// const int servo7Pin = 12; +PWMServo servo1; //Create servo objects to control a servo or ESC with PWM +PWMServo servo2; +PWMServo servo3; +PWMServo servo4; +// PWMServo servo5; +// PWMServo servo6; +// PWMServo servo7; + + + +//========================================================================================================================// + +// declare IMU and pins + +IMU myIMU(MPU6050_I2C); +// values are MPU6050_I2C (Default), MPU9250_SPI, LSM6DSOX_SPI + +/* +IMU Pin locations for IMU: + +defaults for Teensy 4.0: + default pins for MPU6050 using I2C: + SDA -> pin 18 + SCL -> pin 19 + + default pins for MPU9250 using SPI: + MISO -> pin 34 + MOSI -> pin 35 + CS -> pin 36 + SCK -> pin 37 + + default pins for Adafruit LSM6DSOX: + MISO -> pin 34 + MOSI -> pin 35 + CS -> pin 36 + SCK -> pin 37 +*/ + + + + +//DECLARE GLOBAL VARIABLES + +//General stuff +float dt; +unsigned long current_time, prev_time; +unsigned long print_counter, serial_counter; +unsigned long blink_counter, blink_delay; +bool blinkAlternate; + +//Radio communication: +unsigned long channel_1_pwm, channel_2_pwm, channel_3_pwm, channel_4_pwm, channel_5_pwm, channel_6_pwm; +unsigned long channel_1_pwm_prev, channel_2_pwm_prev, channel_3_pwm_prev, channel_4_pwm_prev; + + + + +//Uncomment only one receiver type +//#define USE_PWM_RX +//#define USE_PPM_RX +#define USE_SBUS_RX +//#define USE_DSM_RX +static const uint8_t num_DSM_channels = 6; //If using DSM RX, change this to match the number of transmitter channels you have + +// Other defines that depend on the above +#if defined USE_SBUS_RX + #include "src/SBUS/SBUS.h" //sBus interface +#endif + +#if defined USE_DSM_RX + #include "src/DSMRX/DSMRX.h" +#endif + +#if defined USE_SBUS_RX + SBUS sbus(Serial5); + uint16_t sbusChannels[16]; + bool sbusFailSafe; + bool sbusLostFrame; +#endif +#if defined USE_DSM_RX + DSM1024 DSM; +#endif + + + +//Normalized desired state: +float thro_des, roll_des, pitch_des, yaw_des; +float roll_passthru, pitch_passthru, yaw_passthru; + + +//Mixer +float m1_command_scaled, m2_command_scaled, m3_command_scaled, m4_command_scaled, m5_command_scaled, m6_command_scaled; +int m1_command_PWM, m2_command_PWM, m3_command_PWM, m4_command_PWM, m5_command_PWM, m6_command_PWM; +float s1_command_scaled, s2_command_scaled, s3_command_scaled, s4_command_scaled, s5_command_scaled, s6_command_scaled, s7_command_scaled; +int s1_command_PWM, s2_command_PWM, s3_command_PWM, s4_command_PWM, s5_command_PWM, s6_command_PWM, s7_command_PWM; + +//Flight status +bool armedFly = false; + +//========================================================================================================================// +// VOID SETUP // +//========================================================================================================================// + +void setup() { + Serial.begin(500000); //USB serial + delay(500); + + //Initialize all pins + pinMode(13, OUTPUT); //Pin 13 LED blinker on board, do not modify + pinMode(m1Pin, OUTPUT); + pinMode(m2Pin, OUTPUT); + pinMode(m3Pin, OUTPUT); + pinMode(m4Pin, OUTPUT); + pinMode(m5Pin, OUTPUT); + pinMode(m6Pin, OUTPUT); + servo1.attach(servo1Pin, 900, 2100); //Pin, min PWM value, max PWM value + servo2.attach(servo2Pin, 900, 2100); + servo3.attach(servo3Pin, 900, 2100); + servo4.attach(servo4Pin, 900, 2100); + // servo5.attach(servo5Pin, 900, 2100); + // servo6.attach(servo6Pin, 900, 2100); + // servo7.attach(servo7Pin, 900, 2100); + + //Set built in LED to turn on to signal startup + digitalWrite(13, HIGH); + + delay(5); + + //Initialize radio communication + radioSetup(); + + //Set radio channels to default (safe) values before entering main loop + channel_1_pwm = channel_1_fs; + channel_2_pwm = channel_2_fs; + channel_3_pwm = channel_3_fs; + channel_4_pwm = channel_4_fs; + channel_5_pwm = channel_5_fs; + channel_6_pwm = channel_6_fs; + + //Initialize IMU communication + if (!myIMU.begin()) { + Serial.println("IMU init failed!"); + while (1); + } + delay(5); + + //Get IMU error to zero accelerometer and gyro readings, assuming vehicle is level when powered up + //myIMU.calculate_IMU_error(); //Calibration parameters printed to serial monitor. Paste these in the user specified variables section, then comment this out forever. + + //Arm servo channels + servo1.write(0); //Command servo angle from 0-180 degrees (1000 to 2000 PWM) + servo2.write(0); //Set these to 90 for servos if you do not want them to briefly max out on startup + servo3.write(0); //Keep these at 0 if you are using servo outputs for motors + servo4.write(0); + // servo5.write(0); + // servo6.write(0); + // servo7.write(0); + + delay(5); + + //calibrateESCs(); //PROPS OFF. Uncomment this to calibrate your ESCs by setting throttle stick to max, powering on, and lowering throttle to zero after the beeps + //Code will not proceed past here if this function is uncommented! + + //Arm OneShot125 motors + m1_command_PWM = 125; //Command OneShot125 ESC from 125 to 250us pulse length + m2_command_PWM = 125; + m3_command_PWM = 125; + m4_command_PWM = 125; + m5_command_PWM = 125; + m6_command_PWM = 125; + armMotors(); //Loop over commandMotors() until ESCs happily arm + + //Indicate entering main loop with 3 quick blinks + setupBlink(3,160,70); //numBlinks, upTime (ms), downTime (ms) + + //If using MPU9250 IMU, uncomment for one-time magnetometer calibration (may need to repeat for new locations) + //myIMU.calibrateMagnetometer(); //Generates magentometer error and scale factors to be pasted in user-specified variables section + +} + + + +//========================================================================================================================// +// MAIN LOOP // +//========================================================================================================================// + +void loop() { + //Keep track of what time it is and how much time has elapsed since the last loop + prev_time = current_time; + current_time = micros(); + dt = (current_time - prev_time)/1000000.0; + + loopBlink(); //Indicate we are in main loop with short blink every 1.5 seconds + + //Print data at 100hz (uncomment one at a time for troubleshooting) - SELECT ONE: + //printRadioData(); //Prints radio pwm values (expected: 1000 to 2000) + //printDesiredState(); //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle) + printGyroData(); //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest) + //printAccelData(); //Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level) + //printMagData(); //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300) + //printRollPitchYaw(); //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level) + //printPIDoutput(); //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1) + //printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250) + //printServoCommands(); //Prints the values being written to the servos (expected: 0 to 180) + //printLoopRate(); //Prints the time between loops in microseconds (expected: microseconds between loop iterations) + + // Get arming status + armedStatus(); //Check if the throttle cut is off and throttle is low. + + //Get vehicle state + myIMU.getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise + // myIMU.Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU angle estimates (degrees + myIMU.Madgwick(dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU angle estimates (degrees) + + //Compute desired state + getDesState(); //Convert raw commands to normalized values based on saturated control limits + + //PID Controller - SELECT ONE: + myIMU.controlANGLE(roll_des, pitch_des, yaw_des, dt, channel_1_pwm); //Stabilize on angle setpoint + //myIMU.controlANGLE2(roll_des, pitch_des, yaw_des, dt, channel_1_pwm); //Stabilize on angle setpoint using cascaded method. Rate controller must be tuned well first! + //myIMU.controlRATE(roll_des, pitch_des, yaw_des, dt, channel_1_pwm); //Stabilize on rate setpoint + + //Actuator mixing and scaling to PWM values + controlMixer(); //Mixes PID outputs to scaled actuator commands -- custom mixing assignments done here + scaleCommands(); //Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) + + //Throttle cut check + throttleCut(); //Directly sets motor commands to low based on state of ch5 + + //Command actuators + commandMotors(); //Sends command pulses to each motor pin using OneShot125 protocol + servo1.write(s1_command_PWM); //Writes PWM value to servo object + servo2.write(s2_command_PWM); + servo3.write(s3_command_PWM); + servo4.write(s4_command_PWM); + // servo5.write(s5_command_PWM); + // servo6.write(s6_command_PWM); + // servo7.write(s7_command_PWM); + + //Get vehicle commands for next loop iteration + getCommands(); //Pulls current available radio commands + failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup + + //Regulate loop rate + loopRate(2000); //Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default +} + + + +//========================================================================================================================// +// FUNCTIONS // +//========================================================================================================================// + + + +void controlMixer() { + //DESCRIPTION: Mixes scaled commands from PID controller to actuator outputs based on vehicle configuration + /* + * Takes roll_PID, pitch_PID, and yaw_PID computed from the PID controller and appropriately mixes them for the desired + * vehicle configuration. For example on a quadcopter, the left two motors should have +roll_PID while the right two motors + * should have -roll_PID. Front two should have -pitch_PID and the back two should have +pitch_PID etc... every motor has + * normalized (0 to 1) thro_des command for throttle control. Can also apply direct unstabilized commands from the transmitter with + * roll_passthru, pitch_passthru, and yaw_passthu. mX_command_scaled and sX_command scaled variables are used in scaleCommands() + * in preparation to be sent to the motor ESCs and servos. + * + *Relevant variables: + *thro_des - direct thottle control + *roll_PID, pitch_PID, yaw_PID - stabilized axis variables + *roll_passthru, pitch_passthru, yaw_passthru - direct unstabilized command passthrough + *channel_6_pwm - free auxillary channel, can be used to toggle things with an 'if' statement + */ + + //Quad mixing - EXAMPLE + m1_command_scaled = thro_des - myIMU.pitch_PID + myIMU.roll_PID + myIMU.yaw_PID; //Front Left + m2_command_scaled = thro_des - myIMU.pitch_PID - myIMU.roll_PID - myIMU.yaw_PID; //Front Right + m3_command_scaled = thro_des + myIMU.pitch_PID - myIMU.roll_PID + myIMU.yaw_PID; //Back Right + m4_command_scaled = thro_des + myIMU.pitch_PID + myIMU.roll_PID - myIMU.yaw_PID; //Back Left + m5_command_scaled = 0; + m6_command_scaled = 0; + + //0.5 is centered servo, 0.0 is zero throttle if connecting to ESC for conventional PWM, 1.0 is max throttle + s1_command_scaled = 0; + s2_command_scaled = 0; + s3_command_scaled = 0; + s4_command_scaled = 0; + s5_command_scaled = 0; + s6_command_scaled = 0; + s7_command_scaled = 0; + +} + +void armedStatus() { + //DESCRIPTION: Check if the throttle cut is off and the throttle input is low to prepare for flight. + if ((channel_5_pwm < 1500) && (channel_1_pwm < 1050)) { + armedFly = true; + } +} + +void calibrateAttitude() { + //DESCRIPTION: Used to warm up the main loop to allow the madwick filter to converge before commands can be sent to the actuators + //Assuming vehicle is powered up on level surface! + /* + * This function is used on startup to warm up the attitude estimation and is what causes startup to take a few seconds + * to boot. + */ + //Warm up IMU and madgwick filter in simulated main loop + for (int i = 0; i <= 10000; i++) { + prev_time = current_time; + current_time = micros(); + dt = (current_time - prev_time)/1000000.0; + myIMU.getIMUdata(); + myIMU.Madgwick(dt); + loopRate(2000); //do not exceed 2000Hz + } +} + +void getDesState() { + //DESCRIPTION: Normalizes desired control values to appropriate values + /* + * Updates the desired state variables thro_des, roll_des, pitch_des, and yaw_des. These are computed by using the raw + * RC pwm commands and scaling them to be within our limits defined in setup. thro_des stays within 0 to 1 range. + * roll_des and pitch_des are scaled to be within max roll/pitch amount in either degrees (angle mode) or degrees/sec + * (rate mode). yaw_des is scaled to be within max yaw in degrees/sec. Also creates roll_passthru, pitch_passthru, and + * yaw_passthru variables, to be used in commanding motors/servos with direct unstabilized commands in controlMixer(). + */ + thro_des = (channel_1_pwm - 1000.0)/1000.0; //Between 0 and 1 + roll_des = (channel_2_pwm - 1500.0)/500.0; //Between -1 and 1 + pitch_des = (channel_3_pwm - 1500.0)/500.0; //Between -1 and 1 + yaw_des = (channel_4_pwm - 1500.0)/500.0; //Between -1 and 1 + roll_passthru = roll_des/2.0; //Between -0.5 and 0.5 + pitch_passthru = pitch_des/2.0; //Between -0.5 and 0.5 + yaw_passthru = yaw_des/2.0; //Between -0.5 and 0.5 + + //Constrain within normalized bounds + thro_des = constrain(thro_des, 0.0, 1.0); //Between 0 and 1 + roll_des = constrain(roll_des, -1.0, 1.0)*maxRoll; //Between -maxRoll and +maxRoll + pitch_des = constrain(pitch_des, -1.0, 1.0)*maxPitch; //Between -maxPitch and +maxPitch + yaw_des = constrain(yaw_des, -1.0, 1.0)*maxYaw; //Between -maxYaw and +maxYaw + roll_passthru = constrain(roll_passthru, -0.5, 0.5); + pitch_passthru = constrain(pitch_passthru, -0.5, 0.5); + yaw_passthru = constrain(yaw_passthru, -0.5, 0.5); +} + + +void scaleCommands() { + //DESCRIPTION: Scale normalized actuator commands to values for ESC/Servo protocol + /* + * mX_command_scaled variables from the mixer function are scaled to 125-250us for OneShot125 protocol. sX_command_scaled variables from + * the mixer function are scaled to 0-180 for the servo library using standard PWM. + * mX_command_PWM are updated here which are used to command the motors in commandMotors(). sX_command_PWM are updated + * which are used to command the servos. + */ + //Scaled to 125us - 250us for oneshot125 protocol + m1_command_PWM = m1_command_scaled*125 + 125; + m2_command_PWM = m2_command_scaled*125 + 125; + m3_command_PWM = m3_command_scaled*125 + 125; + m4_command_PWM = m4_command_scaled*125 + 125; + m5_command_PWM = m5_command_scaled*125 + 125; + m6_command_PWM = m6_command_scaled*125 + 125; + //Constrain commands to motors within oneshot125 bounds + m1_command_PWM = constrain(m1_command_PWM, 125, 250); + m2_command_PWM = constrain(m2_command_PWM, 125, 250); + m3_command_PWM = constrain(m3_command_PWM, 125, 250); + m4_command_PWM = constrain(m4_command_PWM, 125, 250); + m5_command_PWM = constrain(m5_command_PWM, 125, 250); + m6_command_PWM = constrain(m6_command_PWM, 125, 250); + + //Scaled to 0-180 for servo library + s1_command_PWM = s1_command_scaled*180; + s2_command_PWM = s2_command_scaled*180; + s3_command_PWM = s3_command_scaled*180; + s4_command_PWM = s4_command_scaled*180; + s5_command_PWM = s5_command_scaled*180; + s6_command_PWM = s6_command_scaled*180; + s7_command_PWM = s7_command_scaled*180; + //Constrain commands to servos within servo library bounds + s1_command_PWM = constrain(s1_command_PWM, 0, 180); + s2_command_PWM = constrain(s2_command_PWM, 0, 180); + s3_command_PWM = constrain(s3_command_PWM, 0, 180); + s4_command_PWM = constrain(s4_command_PWM, 0, 180); + s5_command_PWM = constrain(s5_command_PWM, 0, 180); + s6_command_PWM = constrain(s6_command_PWM, 0, 180); + s7_command_PWM = constrain(s7_command_PWM, 0, 180); + +} + +void getCommands() { + //DESCRIPTION: Get raw PWM values for every channel from the radio + /* + * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of + * the loop. If using a PWM or PPM receiver, the radio commands are retrieved from a function in the readPWM file separate from this one which + * is running a bunch of interrupts to continuously update the radio readings. If using an SBUS receiver, the alues are pulled from the SBUS library directly. + * The raw radio commands are filtered with a first order low-pass filter to eliminate any really high frequency noise. + */ + + #if defined USE_PPM_RX || defined USE_PWM_RX + channel_1_pwm = getRadioPWM(1); + channel_2_pwm = getRadioPWM(2); + channel_3_pwm = getRadioPWM(3); + channel_4_pwm = getRadioPWM(4); + channel_5_pwm = getRadioPWM(5); + channel_6_pwm = getRadioPWM(6); + + #elif defined USE_SBUS_RX + if (sbus.read(&sbusChannels[0], &sbusFailSafe, &sbusLostFrame)) { + //sBus scaling below is for Taranis-Plus and X4R-SB + float scale = 0.615; + float bias = 895.0; + channel_1_pwm = sbusChannels[0] * scale + bias; + channel_2_pwm = sbusChannels[1] * scale + bias; + channel_3_pwm = sbusChannels[2] * scale + bias; + channel_4_pwm = sbusChannels[3] * scale + bias; + channel_5_pwm = sbusChannels[4] * scale + bias; + channel_6_pwm = sbusChannels[5] * scale + bias; + } + + #elif defined USE_DSM_RX + if (DSM.timedOut(micros())) { + //Serial.println("*** DSM RX TIMED OUT ***"); + } + else if (DSM.gotNewFrame()) { + uint16_t values[num_DSM_channels]; + DSM.getChannelValues(values, num_DSM_channels); + + channel_1_pwm = values[0]; + channel_2_pwm = values[1]; + channel_3_pwm = values[2]; + channel_4_pwm = values[3]; + channel_5_pwm = values[4]; + channel_6_pwm = values[5]; + } + #endif + + //Low-pass the critical commands and update previous values + float b = 0.7; //Lower=slower, higher=noiser + channel_1_pwm = (1.0 - b)*channel_1_pwm_prev + b*channel_1_pwm; + channel_2_pwm = (1.0 - b)*channel_2_pwm_prev + b*channel_2_pwm; + channel_3_pwm = (1.0 - b)*channel_3_pwm_prev + b*channel_3_pwm; + channel_4_pwm = (1.0 - b)*channel_4_pwm_prev + b*channel_4_pwm; + channel_1_pwm_prev = channel_1_pwm; + channel_2_pwm_prev = channel_2_pwm; + channel_3_pwm_prev = channel_3_pwm; + channel_4_pwm_prev = channel_4_pwm; +} + +void failSafe() { + //DESCRIPTION: If radio gives garbage values, set all commands to default values + /* + * Radio connection failsafe used to check if the getCommands() function is returning acceptable pwm values. If any of + * the commands are lower than 800 or higher than 2200, then we can be certain that there is an issue with the radio + * connection (most likely hardware related). If any of the channels show this failure, then all of the radio commands + * channel_x_pwm are set to default failsafe values specified in the setup. Comment out this function when troubleshooting + * your radio connection in case any extreme values are triggering this function to overwrite the printed variables. + * + * When using SBUS, the library handles the detection of lost frames and failsafe. Some receivers handle failsafe differently. + * Testing with FrSky SBUS receivers immediately triggered failsafe when Tx disconnected. A Radiomaster RP3-H ELRS receiver would + * not trigger failsafe when set to "No Pulses", but would trigger when set to "Last Position" after a few seconds. + * Test your receiver's behavior! You can do this with the example sketch in the "Bolder Flight Systems SBUS" library. + */ + + //If using SBUS, this will only check the boolean failsafe value from the SBUS library. If using PWM RX, it checks each channel. + #ifndef USE_SBUS_RX + unsigned minVal = 800; + unsigned maxVal = 2200; + int check1 = 0; + int check2 = 0; + int check3 = 0; + int check4 = 0; + int check5 = 0; + int check6 = 0; + + //Triggers for failure criteria + if (channel_1_pwm > maxVal || channel_1_pwm < minVal) check1 = 1; + if (channel_2_pwm > maxVal || channel_2_pwm < minVal) check2 = 1; + if (channel_3_pwm > maxVal || channel_3_pwm < minVal) check3 = 1; + if (channel_4_pwm > maxVal || channel_4_pwm < minVal) check4 = 1; + if (channel_5_pwm > maxVal || channel_5_pwm < minVal) check5 = 1; + if (channel_6_pwm > maxVal || channel_6_pwm < minVal) check6 = 1; + #endif + + //If any failures, set to default failsafe values + #ifdef USE_SBUS_RX + if (sbusFailSafe) { + #else + if ((check1 + check2 + check3 + check4 + check5 + check6) > 0) { + #endif + channel_1_pwm = channel_1_fs; + channel_2_pwm = channel_2_fs; + channel_3_pwm = channel_3_fs; + channel_4_pwm = channel_4_fs; + channel_5_pwm = channel_5_fs; + channel_6_pwm = channel_6_fs; + } +} + +void commandMotors() { + //DESCRIPTION: Send pulses to motor pins, oneshot125 protocol + /* + * My crude implimentation of OneShot125 protocol which sends 125 - 250us pulses to the ESCs (mXPin). The pulselengths being + * sent are mX_command_PWM, computed in scaleCommands(). This may be replaced by something more efficient in the future. + */ + int wentLow = 0; + int pulseStart, timer; + int flagM1 = 0; + int flagM2 = 0; + int flagM3 = 0; + int flagM4 = 0; + int flagM5 = 0; + int flagM6 = 0; + + //Write all motor pins high + digitalWrite(m1Pin, HIGH); + digitalWrite(m2Pin, HIGH); + digitalWrite(m3Pin, HIGH); + digitalWrite(m4Pin, HIGH); + digitalWrite(m5Pin, HIGH); + digitalWrite(m6Pin, HIGH); + pulseStart = micros(); + + //Write each motor pin low as correct pulse length is reached + while (wentLow < 6 ) { //Keep going until final (6th) pulse is finished, then done + timer = micros(); + if ((m1_command_PWM <= timer - pulseStart) && (flagM1==0)) { + digitalWrite(m1Pin, LOW); + wentLow = wentLow + 1; + flagM1 = 1; + } + if ((m2_command_PWM <= timer - pulseStart) && (flagM2==0)) { + digitalWrite(m2Pin, LOW); + wentLow = wentLow + 1; + flagM2 = 1; + } + if ((m3_command_PWM <= timer - pulseStart) && (flagM3==0)) { + digitalWrite(m3Pin, LOW); + wentLow = wentLow + 1; + flagM3 = 1; + } + if ((m4_command_PWM <= timer - pulseStart) && (flagM4==0)) { + digitalWrite(m4Pin, LOW); + wentLow = wentLow + 1; + flagM4 = 1; + } + if ((m5_command_PWM <= timer - pulseStart) && (flagM5==0)) { + digitalWrite(m5Pin, LOW); + wentLow = wentLow + 1; + flagM5 = 1; + } + if ((m6_command_PWM <= timer - pulseStart) && (flagM6==0)) { + digitalWrite(m6Pin, LOW); + wentLow = wentLow + 1; + flagM6 = 1; + } + } +} + +void armMotors() { + //DESCRIPTION: Sends many command pulses to the motors, to be used to arm motors in the void setup() + /* + * Loops over the commandMotors() function 50 times with a delay in between, simulating how the commandMotors() + * function is used in the main loop. Ensures motors arm within the void setup() where there are some delays + * for other processes that sometimes prevent motors from arming. + */ + for (int i = 0; i <= 50; i++) { + commandMotors(); + delay(2); + } +} + +void calibrateESCs() { + //DESCRIPTION: Used in void setup() to allow standard ESC calibration procedure with the radio to take place. + /* + * Simulates the void loop(), but only for the purpose of providing throttle pass through to the motors, so that you can + * power up with throttle at full, let ESCs begin arming sequence, and lower throttle to zero. This function should only be + * uncommented when performing an ESC calibration. + */ + while (true) { + prev_time = current_time; + current_time = micros(); + dt = (current_time - prev_time)/1000000.0; + + digitalWrite(13, HIGH); //LED on to indicate we are not in main loop + + getCommands(); //Pulls current available radio commands + failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup + getDesState(); //Convert raw commands to normalized values based on saturated control limits + myIMU.getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise + myIMU.Madgwick(dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU (degrees) + getDesState(); //Convert raw commands to normalized values based on saturated control limits + + m1_command_scaled = thro_des; + m2_command_scaled = thro_des; + m3_command_scaled = thro_des; + m4_command_scaled = thro_des; + m5_command_scaled = thro_des; + m6_command_scaled = thro_des; + s1_command_scaled = thro_des; + s2_command_scaled = thro_des; + s3_command_scaled = thro_des; + s4_command_scaled = thro_des; + s5_command_scaled = thro_des; + s6_command_scaled = thro_des; + s7_command_scaled = thro_des; + scaleCommands(); //Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) + + //throttleCut(); //Directly sets motor commands to low based on state of ch5 + + servo1.write(s1_command_PWM); + servo2.write(s2_command_PWM); + servo3.write(s3_command_PWM); + servo4.write(s4_command_PWM); + // servo5.write(s5_command_PWM); + // servo6.write(s6_command_PWM); + // servo7.write(s7_command_PWM); + commandMotors(); //Sends command pulses to each motor pin using OneShot125 protocol + + //printRadioData(); //Radio pwm values (expected: 1000 to 2000) + + loopRate(2000); //Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default + } +} + +float floatFaderLinear(float param, float param_min, float param_max, float fadeTime, int state, int loopFreq){ + //DESCRIPTION: Linearly fades a float type variable between min and max bounds based on desired high or low state and time + /* + * Takes in a float variable, desired minimum and maximum bounds, fade time, high or low desired state, and the loop frequency + * and linearly interpolates that param variable between the maximum and minimum bounds. This function can be called in controlMixer() + * and high/low states can be determined by monitoring the state of an auxillarly radio channel. For example, if channel_6_pwm is being + * monitored to switch between two dynamic configurations (hover and forward flight), this function can be called within the logical + * statements in order to fade controller gains, for example between the two dynamic configurations. The 'state' (1 or 0) can be used + * to designate the two final options for that control gain based on the dynamic configuration assignment to the auxillary radio channel. + * + */ + float diffParam = (param_max - param_min)/(fadeTime*loopFreq); //Difference to add or subtract from param for each loop iteration for desired fadeTime + + if (state == 1) { //Maximum param bound desired, increase param by diffParam for each loop iteration + param = param + diffParam; + } + else if (state == 0) { //Minimum param bound desired, decrease param by diffParam for each loop iteration + param = param - diffParam; + } + + param = constrain(param, param_min, param_max); //Constrain param within max bounds + + return param; +} + +float floatFaderLinear2(float param, float param_des, float param_lower, float param_upper, float fadeTime_up, float fadeTime_down, int loopFreq){ + //DESCRIPTION: Linearly fades a float type variable from its current value to the desired value, up or down + /* + * Takes in a float variable to be modified, desired new position, upper value, lower value, fade time, and the loop frequency + * and linearly fades that param variable up or down to the desired value. This function can be called in controlMixer() + * to fade up or down between flight modes monitored by an auxillary radio channel. For example, if channel_6_pwm is being + * monitored to switch between two dynamic configurations (hover and forward flight), this function can be called within the logical + * statements in order to fade controller gains, for example between the two dynamic configurations. + * + */ + if (param > param_des) { //Need to fade down to get to desired + float diffParam = (param_upper - param_des)/(fadeTime_down*loopFreq); + param = param - diffParam; + } + else if (param < param_des) { //Need to fade up to get to desired + float diffParam = (param_des - param_lower)/(fadeTime_up*loopFreq); + param = param + diffParam; + } + + param = constrain(param, param_lower, param_upper); //Constrain param within max bounds + + return param; +} + +void switchRollYaw(int reverseRoll, int reverseYaw) { + //DESCRIPTION: Switches roll_des and yaw_des variables for tailsitter-type configurations + /* + * Takes in two integers (either 1 or -1) corresponding to the desired reversing of the roll axis and yaw axis, respectively. + * Reversing of the roll or yaw axis may be needed when switching between the two for some dynamic configurations. Inputs of 1, 1 does not + * reverse either of them, while -1, 1 will reverse the output corresponding to the new roll axis. + * This function may be replaced in the future by a function that switches the IMU data instead (so that angle can also be estimated with the + * IMU tilted 90 degrees from default level). + */ + float switch_holder; + + switch_holder = yaw_des; + yaw_des = reverseYaw*roll_des; + roll_des = reverseRoll*switch_holder; +} + +void throttleCut() { + //DESCRIPTION: Directly set actuator outputs to minimum value if triggered + /* + Monitors the state of radio command channel_5_pwm and directly sets the mx_command_PWM values to minimum (120 is + minimum for oneshot125 protocol, 0 is minimum for standard PWM servo library used) if channel 5 is high. This is the last function + called before commandMotors() is called so that the last thing checked is if the user is giving permission to command + the motors to anything other than minimum value. Safety first. + + channel_5_pwm is LOW then throttle cut is OFF and throttle value can change. (ThrottleCut is DEACTIVATED) + channel_5_pwm is HIGH then throttle cut is ON and throttle value = 120 only. (ThrottleCut is ACTIVATED), (drone is DISARMED) + */ + if ((channel_5_pwm > 1500) || (armedFly == false)) { + armedFly = false; + m1_command_PWM = 120; + m2_command_PWM = 120; + m3_command_PWM = 120; + m4_command_PWM = 120; + m5_command_PWM = 120; + m6_command_PWM = 120; + + //Uncomment if using servo PWM variables to control motor ESCs + //s1_command_PWM = 0; + //s2_command_PWM = 0; + //s3_command_PWM = 0; + //s4_command_PWM = 0; + //s5_command_PWM = 0; + //s6_command_PWM = 0; + //s7_command_PWM = 0; + } +} + +void loopRate(int freq) { + //DESCRIPTION: Regulate main loop rate to specified frequency in Hz + /* + * It's good to operate at a constant loop rate for filters to remain stable and whatnot. Interrupt routines running in the + * background cause the loop rate to fluctuate. This function basically just waits at the end of every loop iteration until + * the correct time has passed since the start of the current loop for the desired loop rate in Hz. 2kHz is a good rate to + * be at because the loop nominally will run between 2.8kHz - 4.2kHz. This lets us have a little room to add extra computations + * and remain above 2kHz, without needing to retune all of our filtering parameters. + */ + float invFreq = 1.0/freq*1000000.0; + unsigned long checker = micros(); + + //Sit in loop until appropriate time has passed + while (invFreq > (checker - current_time)) { + checker = micros(); + } +} + +void loopBlink() { + //DESCRIPTION: Blink LED on board to indicate main loop is running + /* + * It looks cool. + */ + if (current_time - blink_counter > blink_delay) { + blink_counter = micros(); + digitalWrite(13, blinkAlternate); //Pin 13 is built in LED + + if (blinkAlternate == 1) { + blinkAlternate = 0; + blink_delay = 100000; + } + else if (blinkAlternate == 0) { + blinkAlternate = 1; + blink_delay = 2000000; + } + } +} + +void setupBlink(int numBlinks,int upTime, int downTime) { + //DESCRIPTION: Simple function to make LED on board blink as desired + for (int j = 1; j<= numBlinks; j++) { + digitalWrite(13, LOW); + delay(downTime); + digitalWrite(13, HIGH); + delay(upTime); + } +} + +void printRadioData() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:1000.0")); + Serial.print(F(" HIGH:2000.0")); + Serial.print(F(" CH1:")); + Serial.print(channel_1_pwm); + Serial.print(F(" CH2:")); + Serial.print(channel_2_pwm); + Serial.print(F(" CH3:")); + Serial.print(channel_3_pwm); + Serial.print(F(" CH4:")); + Serial.print(channel_4_pwm); + Serial.print(F(" CH5:")); + Serial.print(channel_5_pwm); + Serial.print(F(" CH6:")); + Serial.println(channel_6_pwm); + } +} + +void printDesiredState() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-180.0")); + Serial.print(F(" HIGH:180.0")); + Serial.print(F(" thro_des:")); + Serial.print(thro_des); + Serial.print(F(" roll_des:")); + Serial.print(roll_des); + Serial.print(F(" pitch_des:")); + Serial.print(pitch_des); + Serial.print(F(" yaw_des:")); + Serial.println(yaw_des); + } +} + +void printGyroData() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-300.0")); + Serial.print(F(" HIGH:300.0")); + Serial.print(F(" GyroX:")); + Serial.print(myIMU.GyroX); + Serial.print(F(" GyroY:")); + Serial.print(myIMU.GyroY); + Serial.print(F(" GyroZ:")); + Serial.println(myIMU.GyroZ); + } +} + +void printAccelData() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-2.0")); + Serial.print(F(" HIGH:2.0")); + Serial.print(F(" AccX:")); + Serial.print(myIMU.AccX); + Serial.print(F(" AccY:")); + Serial.print(myIMU.AccY); + Serial.print(F(" AccZ:")); + Serial.println(myIMU.AccZ); + } +} + +void printMagData() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-300.0")); + Serial.print(F(" HIGH:300.0")); + Serial.print(F(" MagX:")); + Serial.print(myIMU.MagX); + Serial.print(F(" MagY:")); + Serial.print(myIMU.MagY); + Serial.print(F(" MagZ:")); + Serial.println(myIMU.MagZ); + } +} + +void printRollPitchYaw() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-180.0")); + Serial.print(F(" HIGH:180.0")); + Serial.print(F(" roll:")); + Serial.print(myIMU.roll_IMU); + Serial.print(F(" pitch:")); + Serial.print(myIMU.pitch_IMU); + Serial.print(F(" yaw:")); + Serial.println(myIMU.yaw_IMU); + } +} + +void printPIDoutput() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-1.0")); + Serial.print(F(" HIGH:1.0")); + Serial.print(F(" roll_PID:")); + Serial.print(myIMU.roll_PID); + Serial.print(F(" pitch_PID:")); + Serial.print(myIMU.pitch_PID); + Serial.print(F(" yaw_PID:")); + Serial.println(myIMU.yaw_PID); + } +} + +void printMotorCommands() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:125.0")); + Serial.print(F(" HIGH:250.0")); + Serial.print(F(" m1_command:")); + Serial.print(m1_command_PWM); + Serial.print(F(" m2_command:")); + Serial.print(m2_command_PWM); + Serial.print(F(" m3_command:")); + Serial.print(m3_command_PWM); + Serial.print(F(" m4_command:")); + Serial.print(m4_command_PWM); + Serial.print(F(" m5_command:")); + Serial.print(m5_command_PWM); + Serial.print(F(" m6_command:")); + Serial.println(m6_command_PWM); + } +} + +void printServoCommands() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:0.0")); + Serial.print(F(" HIGH:180.0")); + Serial.print(F(" s1_command:")); + Serial.print(s1_command_PWM); + Serial.print(F(" s2_command:")); + Serial.print(s2_command_PWM); + Serial.print(F(" s3_command:")); + Serial.print(s3_command_PWM); + Serial.print(F(" s4_command:")); + Serial.print(s4_command_PWM); + Serial.print(F(" s5_command:")); + Serial.print(s5_command_PWM); + Serial.print(F(" s6_command:")); + Serial.print(s6_command_PWM); + Serial.print(F(" s7_command:")); + Serial.println(s7_command_PWM); + } +} + +void printLoopRate() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("dt:")); + Serial.println(dt*1000000.0); + } +} + diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/radioComm.ino b/Versions/dRehmFlight_Teensy_BETA_2.0/radioComm.ino new file mode 100644 index 00000000..eef7f1ec --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/radioComm.ino @@ -0,0 +1,198 @@ +//Arduino/Teensy Flight Controller - dRehmFlight +//Author: Nicholas Rehm +//Project Start: 1/6/2020 +//Last Updated: 7/29/2022 +//Version: Beta 1.3 + +//========================================================================================================================// + +//This file contains all necessary functions and code used for radio communication to avoid cluttering the main code + +unsigned long rising_edge_start_1, rising_edge_start_2, rising_edge_start_3, rising_edge_start_4, rising_edge_start_5, rising_edge_start_6; +unsigned long channel_1_raw, channel_2_raw, channel_3_raw, channel_4_raw, channel_5_raw, channel_6_raw; +int ppm_counter = 0; +unsigned long time_ms = 0; + +void radioSetup() { + //PPM Receiver + #if defined USE_PPM_RX + //Declare interrupt pin + pinMode(PPM_Pin, INPUT_PULLUP); + delay(20); + //Attach interrupt and point to corresponding ISR function + attachInterrupt(digitalPinToInterrupt(PPM_Pin), getPPM, CHANGE); + + //PWM Receiver + #elif defined USE_PWM_RX + //Declare interrupt pins + pinMode(ch1Pin, INPUT_PULLUP); + pinMode(ch2Pin, INPUT_PULLUP); + pinMode(ch3Pin, INPUT_PULLUP); + pinMode(ch4Pin, INPUT_PULLUP); + pinMode(ch5Pin, INPUT_PULLUP); + pinMode(ch6Pin, INPUT_PULLUP); + delay(20); + //Attach interrupt and point to corresponding ISR functions + attachInterrupt(digitalPinToInterrupt(ch1Pin), getCh1, CHANGE); + attachInterrupt(digitalPinToInterrupt(ch2Pin), getCh2, CHANGE); + attachInterrupt(digitalPinToInterrupt(ch3Pin), getCh3, CHANGE); + attachInterrupt(digitalPinToInterrupt(ch4Pin), getCh4, CHANGE); + attachInterrupt(digitalPinToInterrupt(ch5Pin), getCh5, CHANGE); + attachInterrupt(digitalPinToInterrupt(ch6Pin), getCh6, CHANGE); + delay(20); + + //SBUS Recevier + #elif defined USE_SBUS_RX + sbus.begin(); + + //DSM receiver + #elif defined USE_DSM_RX + Serial3.begin(115000); + #else + #error No RX type defined... + #endif +} + +unsigned long getRadioPWM(int ch_num) { + //DESCRIPTION: Get current radio commands from interrupt routines + unsigned long returnPWM = 0; + + if (ch_num == 1) { + returnPWM = channel_1_raw; + } + else if (ch_num == 2) { + returnPWM = channel_2_raw; + } + else if (ch_num == 3) { + returnPWM = channel_3_raw; + } + else if (ch_num == 4) { + returnPWM = channel_4_raw; + } + else if (ch_num == 5) { + returnPWM = channel_5_raw; + } + else if (ch_num == 6) { + returnPWM = channel_6_raw; + } + + return returnPWM; +} + +//For DSM type receivers +void serialEvent3(void) +{ + #if defined USE_DSM_RX + while (Serial3.available()) { + DSM.handleSerialEvent(Serial3.read(), micros()); + } + #endif +} + + + +//========================================================================================================================// + + + +//INTERRUPT SERVICE ROUTINES (for reading PWM and PPM) + +void getPPM() { + unsigned long dt_ppm; + int trig = digitalRead(PPM_Pin); + if (trig==1) { //Only care about rising edge + dt_ppm = micros() - time_ms; + time_ms = micros(); + + + if (dt_ppm > 5000) { //Waiting for long pulse to indicate a new pulse train has arrived + ppm_counter = 0; + } + + if (ppm_counter == 1) { //First pulse + channel_1_raw = dt_ppm; + } + + if (ppm_counter == 2) { //Second pulse + channel_2_raw = dt_ppm; + } + + if (ppm_counter == 3) { //Third pulse + channel_3_raw = dt_ppm; + } + + if (ppm_counter == 4) { //Fourth pulse + channel_4_raw = dt_ppm; + } + + if (ppm_counter == 5) { //Fifth pulse + channel_5_raw = dt_ppm; + } + + if (ppm_counter == 6) { //Sixth pulse + channel_6_raw = dt_ppm; + } + + ppm_counter = ppm_counter + 1; + } +} + +void getCh1() { + int trigger = digitalRead(ch1Pin); + if(trigger == 1) { + rising_edge_start_1 = micros(); + } + else if(trigger == 0) { + channel_1_raw = micros() - rising_edge_start_1; + } +} + +void getCh2() { + int trigger = digitalRead(ch2Pin); + if(trigger == 1) { + rising_edge_start_2 = micros(); + } + else if(trigger == 0) { + channel_2_raw = micros() - rising_edge_start_2; + } +} + +void getCh3() { + int trigger = digitalRead(ch3Pin); + if(trigger == 1) { + rising_edge_start_3 = micros(); + } + else if(trigger == 0) { + channel_3_raw = micros() - rising_edge_start_3; + } +} + +void getCh4() { + int trigger = digitalRead(ch4Pin); + if(trigger == 1) { + rising_edge_start_4 = micros(); + } + else if(trigger == 0) { + channel_4_raw = micros() - rising_edge_start_4; + } +} + +void getCh5() { + int trigger = digitalRead(ch5Pin); + if(trigger == 1) { + rising_edge_start_5 = micros(); + } + else if(trigger == 0) { + channel_5_raw = micros() - rising_edge_start_5; + } +} + +void getCh6() { + int trigger = digitalRead(ch6Pin); + if(trigger == 1) { + rising_edge_start_6 = micros(); + } + else if(trigger == 0) { + channel_6_raw = micros() - rising_edge_start_6; + } +} diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/DSMRX/DSMRX.cpp b/Versions/dRehmFlight_Teensy_BETA_2.0/src/DSMRX/DSMRX.cpp new file mode 100644 index 00000000..f26ee9cd --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/DSMRX/DSMRX.cpp @@ -0,0 +1,116 @@ +/* + Spektrum DSM receiver implementation + + Copyright (C) Simon D. Levy 2017 + + This file is part of DSMRX. + + DSMRX is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + DSMRX is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with DSMRX. If not, see . + */ + +#include "DSMRX.h" + +DSMRX::DSMRX(uint8_t rcChans, uint8_t chanShift, uint8_t chanMask, uint8_t valShift) +{ + _rcChans = rcChans; + _chanShift = chanShift; + _chanMask = chanMask; + _valShift = valShift; + + _gotNewFrame = false; + _lastInterruptMicros = 0; +} + +void DSMRX::handleSerialEvent(uint8_t value, uint32_t usec) +{ + // Reset time + _lastInterruptMicros = usec; + + // check for new frame, i.e. more than 2.5ms passed + static uint32_t spekTimeLast; + uint32_t spekTimeNow = usec; + uint32_t spekInterval = spekTimeNow - spekTimeLast; + spekTimeLast = spekTimeNow; + if (spekInterval > 2500) { + _rxBufPos = 0; + } + + // put the data in buffer + if (_rxBufPos < BUFFER_SIZE) { + _rxBuf[_rxBufPos++] = value; + } + + // parse frame if done + if (_rxBufPos == BUFFER_SIZE) { + + // grab fade count + _fadeCount = _rxBuf[0]; + + // convert to channel data in [0,1024] + for (int b = 2; b < BUFFER_SIZE; b += 2) { + uint8_t bh = _rxBuf[b]; + uint8_t bl = _rxBuf[b+1]; + uint8_t spekChannel = 0x0F & (bh >> _chanShift); + if (spekChannel < _rcChans) { + _rcValue[spekChannel] = ((((uint16_t)(bh & _chanMask) << 8) + bl) >> _valShift); + } + } + + // we have a new frame + _gotNewFrame = true; + } +} + + +bool DSMRX::gotNewFrame(void) +{ + bool retval = _gotNewFrame; + if (_gotNewFrame) { + _gotNewFrame = false; + } + return retval; +} + +void DSMRX::getChannelValues(uint16_t values[], uint8_t count) +{ + for (uint8_t k=0; k maxMicros; +} + +DSM1024::DSM1024(void) : DSMRX(7, 2, 0x03, 0) +{ +} + +DSM2048::DSM2048(void) : DSMRX(8, 3, 0x07, 1) +{ +} diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/DSMRX/DSMRX.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/DSMRX/DSMRX.h new file mode 100644 index 00000000..8e3da9db --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/DSMRX/DSMRX.h @@ -0,0 +1,85 @@ +/* + Spektrum DSM receiver class + + Copyright (C) Simon D. Levy 2017 + + This file is part of DSMRX. + + DSMRX is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + DSMRX is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with DSMRX. If not, see . + */ + +#pragma once + +#include + +class DSMRX { + + private: + + static const uint8_t BUFFER_SIZE = 16; + static const uint8_t MAX_CHANS = 8; + + // Modified by serial-event handler + volatile uint8_t _rxBuf[BUFFER_SIZE]; + volatile uint8_t _rxBufPos; + volatile uint16_t _rcValue[MAX_CHANS]; + volatile uint32_t _lastInterruptMicros; + + uint8_t _rcChans; + uint8_t _chanShift; + uint8_t _chanMask; + uint8_t _valShift; + uint8_t _fadeCount; + + bool _gotNewFrame; + + protected: + + DSMRX(uint8_t rc, uint8_t cs, uint8_t cm, uint8_t vs); + + public: + + void handleSerialEvent(uint8_t value, uint32_t usec); + + bool gotNewFrame(void); + + /** + * Returns channel values in [1000,2000] interval + */ + void getChannelValues(uint16_t values[], uint8_t count=8); + + /** + * Returns channel values in [-1,+1] interval + */ + void getChannelValuesNormalized(float values[], uint8_t count=8); + + uint8_t getFadeCount(void); + + bool timedOut(uint32_t usec, uint32_t maxMicros=40000); +}; + +class DSM1024 : public DSMRX { + + public: + + DSM1024(void); + +}; + +class DSM2048 : public DSMRX { + + public: + + DSM2048(void); +}; + diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/IMU_PID.cpp b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/IMU_PID.cpp new file mode 100644 index 00000000..e3f2ebec --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/IMU_PID.cpp @@ -0,0 +1,1015 @@ + + +// IMU_PID.cpp + + + +#include "IMU_PID.h" + + +IMU::IMU(IMUType type): IMU(type, GYRO_250DPS, ACCEL_2G) {}; + +IMU::IMU(IMUType type, Gyro_DPS gyroDPS, Accel_G accelG) { + _mpu6050 = nullptr; + _mpu9250 = nullptr; + _lsm6dsox = nullptr; + + _type = type; + _gyroDPS = gyroDPS; + _accelG = accelG; + + //Setup gyro and accel full scale value selection and scale factor + switch (_gyroDPS) { + case GYRO_250DPS: + _gyro_scale_factor = 131.0; + break; + + case GYRO_500DPS: + _gyro_scale_factor = 65.5; + break; + + case GYRO_1000DPS: + _gyro_scale_factor = 32.8; + break; + + case GYRO_2000DPS: + _gyro_scale_factor = 16.4; + break; + } + + switch (_accelG) { + case ACCEL_2G: + _accel_scale_factor = 16384.0; + break; + + case ACCEL_4G: + _accel_scale_factor = 8192.0; + break; + + case ACCEL_8G: + _accel_scale_factor = 4096.0; + break; + + case ACCEL_16G: + _accel_scale_factor = 2048.0; + break; + } +} + + + + + + + + + + + +bool IMU::begin() { + switch (_type) { + case MPU6050_I2C: + { + Wire.begin(); + Wire.setClock(1000000); //Note this is 2.5 times the spec sheet 400 kHz max... + + _mpu6050 = new MPU6050(); + _mpu6050->initialize(); + + if (_mpu6050->testConnection() == false) { + Serial.print("DeviceID: "); + Serial.println(_mpu6050->getDeviceID()); + Serial.println("MPU6050 initialization unsuccessful"); + Serial.println("Check MPU6050 wiring or try cycling power"); + return false; + } + else { + Serial.print("OK! DeviceID: "); + Serial.println(_mpu6050->getDeviceID()); + Serial.println("MPU6050 initialization successful"); + } + + //From the reset state all registers should be 0x00, so we should be at + //max sample rate with digital low pass filter(s) off. All we need to + //do is set the desired fullscale ranges + switch (_gyroDPS) { + case GYRO_250DPS: + _mpu6050->setFullScaleGyroRange(MPU6050_GYRO_FS_250); + break; + case GYRO_500DPS: + _mpu6050->setFullScaleGyroRange(MPU6050_GYRO_FS_500); + break; + case GYRO_1000DPS: + _mpu6050->setFullScaleGyroRange(MPU6050_GYRO_FS_1000); + break; + case GYRO_2000DPS: + _mpu6050->setFullScaleGyroRange(MPU6050_GYRO_FS_2000); + break; + } + switch (_accelG) { + case ACCEL_2G: + _mpu6050->setFullScaleAccelRange(MPU6050_ACCEL_FS_2); + break; + case ACCEL_4G: + _mpu6050->setFullScaleAccelRange(MPU6050_ACCEL_FS_4); + break; + case ACCEL_8G: + _mpu6050->setFullScaleAccelRange(MPU6050_ACCEL_FS_8); + break; + case ACCEL_16G: + _mpu6050->setFullScaleAccelRange(MPU6050_ACCEL_FS_16); + break; + } + return true; + } + + case MPU9250_SPI: + { + _mpu9250 = new MPU9250(SPI2,36); + int status = _mpu9250->begin(); + + if (status < 0) { + Serial.println("MPU9250 initialization unsuccessful"); + Serial.println("Check MPU9250 wiring or try cycling power"); + Serial.print("Status: "); + Serial.println(status); + return false; + } + + //From the reset state all registers should be 0x00, so we should be at + //max sample rate with digital low pass filter(s) off. All we need to + //do is set the desired fullscale ranges + switch (_gyroDPS) { + case GYRO_250DPS: + _mpu9250->setGyroRange(_mpu9250->GYRO_RANGE_250DPS); + break; + case GYRO_500DPS: + _mpu9250->setGyroRange(_mpu9250->GYRO_RANGE_500DPS); + break; + case GYRO_1000DPS: + _mpu9250->setGyroRange(_mpu9250->GYRO_RANGE_1000DPS); + break; + case GYRO_2000DPS: + _mpu9250->setGyroRange(_mpu9250->GYRO_RANGE_2000DPS); + break; + } + switch (_accelG) { + case ACCEL_2G: + _mpu9250->setAccelRange(_mpu9250->ACCEL_RANGE_2G); + break; + case ACCEL_4G: + _mpu9250->setAccelRange(_mpu9250->ACCEL_RANGE_4G); + break; + case ACCEL_8G: + _mpu9250->setAccelRange(_mpu9250->ACCEL_RANGE_8G); + break; + case ACCEL_16G: + _mpu9250->setAccelRange(_mpu9250->ACCEL_RANGE_16G); + break; + } + + _mpu9250->setMagCalX(MagErrorX, MagScaleX); + _mpu9250->setMagCalY(MagErrorY, MagScaleY); + _mpu9250->setMagCalZ(MagErrorZ, MagScaleZ); + _mpu9250->setSrd(0); //sets gyro and accel read to 1khz, magnetometer read to 100hz + return true; + } + + case LSM6DSOX_SPI: + { + _lsm6dsox = new Adafruit_LSM6DSOX(); + + #define LSM_MISO 12 // MISO2/DO--red + #define LSM_MOSI 11 // MOSI2/SDA--black + #define LSM_CS 10 // CS2--green-yellow + #define LSM_SCK 13 // SCK2/SCL--white + + // #define LSM_MISO 34 // MISO2/DO--red + // #define LSM_MOSI 35 // MOSI2/SDA--black + // #define LSM_CS 36 // CS2--green-yellow + // #define LSM_SCK 37 // SCK2(SCL)--white + + + if (!_lsm6dsox->begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) { + Serial.println("Failed to find LSM6DSOX chip"); + Serial.println("LSM6DSOX initialization unsuccessful"); + Serial.println("Check LSM6DSOX wiring or try cycling power"); + return false; + } + else { + Serial.println("LSM6DSOX Found!"); + } + + switch (_gyroDPS) { + case GYRO_250DPS: + _lsm6dsox->setGyroRange(lsm6ds_gyro_range_t::LSM6DS_GYRO_RANGE_250_DPS); + break; + case GYRO_500DPS: + _lsm6dsox->setGyroRange(lsm6ds_gyro_range_t::LSM6DS_GYRO_RANGE_500_DPS); + break; + case GYRO_1000DPS: + _lsm6dsox->setGyroRange(lsm6ds_gyro_range_t::LSM6DS_GYRO_RANGE_1000_DPS); + break; + case GYRO_2000DPS: + _lsm6dsox->setGyroRange(lsm6ds_gyro_range_t::LSM6DS_GYRO_RANGE_2000_DPS); + break; + } + switch (_accelG) { + case ACCEL_2G: + _lsm6dsox->setAccelRange(lsm6ds_accel_range_t::LSM6DS_ACCEL_RANGE_2_G); + break; + case ACCEL_4G: + _lsm6dsox->setAccelRange(lsm6ds_accel_range_t::LSM6DS_ACCEL_RANGE_4_G); + break; + case ACCEL_8G: + _lsm6dsox->setAccelRange(lsm6ds_accel_range_t::LSM6DS_ACCEL_RANGE_8_G); + break; + case ACCEL_16G: + _lsm6dsox->setAccelRange(lsm6ds_accel_range_t::LSM6DS_ACCEL_RANGE_16_G); + break; + } + + // Set the data rate of the board higher than stock 104Hz + // lsm6dsox.setAccelDataRate(LSM6DS_RATE_6_66K_HZ); + // lsm6dsox.setGyroDataRate(LSM6DS_RATE_6_66K_HZ); + return true; + } + + default: + return false; + } +} + + + + +void IMU::getIMUdata() { + //DESCRIPTION: Request full dataset from IMU and LP filter gyro, accelerometer, and magnetometer data + /* + * Reads accelerometer, gyro, and magnetometer data from IMU as AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, MagZ. + * These values are scaled according to the IMU datasheet to put them into correct units of g's, deg/sec, and uT. A simple first-order + * low-pass filter is used to get rid of high frequency noise in these raw signals. Generally you want to cut + * off everything past 80Hz, but if your loop rate is not fast enough, the low pass filter will cause a lag in + * the readings. The filter parameters B_gyro and B_accel are set to be good for a 2kHz loop rate. Finally, + * the constant errors found in calculate_IMU_error() on startup are subtracted from the accelerometer and gyro readings. + */ + + int16_t AcX = 0; + int16_t AcY = 0; + int16_t AcZ = 0; + int16_t GyX = 0; + int16_t GyY = 0; + int16_t GyZ = 0; + int16_t MgX = 0; + int16_t MgY = 0; + int16_t MgZ = 0; + + if (_type == MPU6050_I2C) { + _mpu6050->getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); + } + else if (_type == MPU9250_SPI) { + _mpu9250->getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); + } + else if (_type == LSM6DSOX_SPI) { + _lsm6dsox->getEvent(&accel, &gyro, &temp); + + // THe LSM6DSOX has the X and Y reversed from the MPU6050 as well has having the X/Y accelerometers reversed and Y gyro reversed + AccX = accel.acceleration.y * MS2G; // Convert m/s to G-factor + AccY = accel.acceleration.x * -MS2G; + AccZ = accel.acceleration.z * MS2G; + GyroX = gyro.gyro.y * RAD2DEG; // convert rad/s back to deg/sec + GyroY = gyro.gyro.x * -RAD2DEG; + GyroZ = gyro.gyro.z * RAD2DEG; + } + else { + // should throw an error probably + } + + // The MPU6050 and MPU9250 IMUs have a different scale factor from the LSM6DSOX + if (_type == MPU6050_I2C || _type == MPU9250_SPI) { + //Accelerometer + AccX = AcX / _accel_scale_factor; //G's + AccY = AcY / _accel_scale_factor; + AccZ = AcZ / _accel_scale_factor; + + //Gyro + GyroX = GyX / _gyro_scale_factor; //deg/sec + GyroY = GyY / _gyro_scale_factor; + GyroZ = GyZ / _gyro_scale_factor; + + //Magnetometer + MagX = MgX/6.0; //uT + MagY = MgY/6.0; + MagZ = MgZ/6.0; + } + + + //Correct the outputs with the calculated error values + AccX = AccX - AccErrorX; + AccY = AccY - AccErrorY; + AccZ = AccZ - AccErrorZ; + //LP filter accelerometer data + AccX = (1.0 - B_accel)*AccX_prev + B_accel*AccX; + AccY = (1.0 - B_accel)*AccY_prev + B_accel*AccY; + AccZ = (1.0 - B_accel)*AccZ_prev + B_accel*AccZ; + AccX_prev = AccX; + AccY_prev = AccY; + AccZ_prev = AccZ; + + //Correct the outputs with the calculated error values + GyroX = GyroX - GyroErrorX; + GyroY = GyroY - GyroErrorY; + GyroZ = GyroZ - GyroErrorZ; + //LP filter gyro data + GyroX = (1.0 - B_gyro)*GyroX_prev + B_gyro*GyroX; + GyroY = (1.0 - B_gyro)*GyroY_prev + B_gyro*GyroY; + GyroZ = (1.0 - B_gyro)*GyroZ_prev + B_gyro*GyroZ; + GyroX_prev = GyroX; + GyroY_prev = GyroY; + GyroZ_prev = GyroZ; + + //Correct the outputs with the calculated error values + MagX = (MagX - MagErrorX)*MagScaleX; + MagY = (MagY - MagErrorY)*MagScaleY; + MagZ = (MagZ - MagErrorZ)*MagScaleZ; + //LP filter magnetometer data + MagX = (1.0 - B_mag)*MagX_prev + B_mag*MagX; + MagY = (1.0 - B_mag)*MagY_prev + B_mag*MagY; + MagZ = (1.0 - B_mag)*MagZ_prev + B_mag*MagZ; + MagX_prev = MagX; + MagY_prev = MagY; + MagZ_prev = MagZ; +} + + + + +void IMU::calculate_IMU_error() { + //DESCRIPTION: Computes IMU accelerometer and gyro error on startup. Note: vehicle should be powered up on flat surface + /* + * Don't worry too much about what this is doing. The error values it computes are applied to the raw gyro and + * accelerometer values AccX, AccY, AccZ, GyroX, GyroY, GyroZ in getIMUdata(). This eliminates drift in the + * measurement. + */ + #ifndef USE_LSM6DSOX_SPI + int16_t AcX = 0; + int16_t AcY = 0; + int16_t AcZ = 0; + int16_t GyX = 0; + int16_t GyY = 0; + int16_t GyZ = 0; + #endif + AccErrorX = 0.0; + AccErrorY = 0.0; + AccErrorZ = 0.0; + GyroErrorX = 0.0; + GyroErrorY= 0.0; + GyroErrorZ = 0.0; + + //Read IMU values 12000 times + int c = 0; + while (c < 12000) { + #if defined USE_MPU6050_I2C + mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); + #elif defined USE_MPU9250_SPI + mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); + #elif defined USE_LSM6DSOX_SPI + lsm6dsox.getEvent(&accel, &gyro, &temp); + + // THe LSM6DSOX has the X and Y reversed from the MPU6050 as well has having the X/Y accelerometers reversed and Y gyro reversed + AccX = accel.acceleration.y * MS2G; // Convert m/s to G-factor + AccY = accel.acceleration.x * -MS2G; + AccZ = accel.acceleration.z * MS2G; + GyroX = gyro.gyro.y * RAD2DEG; // convert rad/s back to deg/sec + GyroY = gyro.gyro.x * -RAD2DEG; + GyroZ = gyro.gyro.z * RAD2DEG; + + #endif + + // The MPU6050 and MPU9250 IMUs have a different scale factor from the LSM6DSOX + #ifndef USE_LSM6DSOX_SPI + //Accelerometer + AccX = AcX / _accel_scale_factor; //G's + AccY = AcY / _accel_scale_factor; + AccZ = AcZ / _accel_scale_factor; + + //Gyro + GyroX = GyX / _gyro_scale_factor; //deg/sec + GyroY = GyY / _gyro_scale_factor; + GyroZ = GyZ / _gyro_scale_factor; + #endif + + //Sum all readings + AccErrorX = AccErrorX + AccX; + AccErrorY = AccErrorY + AccY; + AccErrorZ = AccErrorZ + AccZ; + GyroErrorX = GyroErrorX + GyroX; + GyroErrorY = GyroErrorY + GyroY; + GyroErrorZ = GyroErrorZ + GyroZ; + c++; + } + //Divide the sum by 12000 to get the error value + AccErrorX = AccErrorX / c; + AccErrorY = AccErrorY / c; + AccErrorZ = AccErrorZ / c - 1.0; + GyroErrorX = GyroErrorX / c; + GyroErrorY = GyroErrorY / c; + GyroErrorZ = GyroErrorZ / c; + + Serial.print("float AccErrorX = "); + Serial.print(AccErrorX); + Serial.println(";"); + Serial.print("float AccErrorY = "); + Serial.print(AccErrorY); + Serial.println(";"); + Serial.print("float AccErrorZ = "); + Serial.print(AccErrorZ); + Serial.println(";"); + + Serial.print("float GyroErrorX = "); + Serial.print(GyroErrorX); + Serial.println(";"); + Serial.print("float GyroErrorY = "); + Serial.print(GyroErrorY); + Serial.println(";"); + Serial.print("float GyroErrorZ = "); + Serial.print(GyroErrorZ); + Serial.println(";"); + + Serial.println("Paste these values in user specified variables section and comment out calculate_IMU_error() in void setup."); + while(1); +} + + + +void IMU::calibrateMagnetometer() { + #if defined USE_MPU9250_SPI + float success; + Serial.println("Beginning magnetometer calibration in"); + Serial.println("3..."); + delay(1000); + Serial.println("2..."); + delay(1000); + Serial.println("1..."); + delay(1000); + Serial.println("Rotate the IMU about all axes until complete."); + Serial.println(" "); + success = mpu9250.calibrateMag(); + if(success) { + Serial.println("Calibration Successful!"); + Serial.println("Please comment out the calibrateMagnetometer() function and copy these values into the code:"); + Serial.print("float MagErrorX = "); + Serial.print(mpu9250.getMagBiasX_uT()); + Serial.println(";"); + Serial.print("float MagErrorY = "); + Serial.print(mpu9250.getMagBiasY_uT()); + Serial.println(";"); + Serial.print("float MagErrorZ = "); + Serial.print(mpu9250.getMagBiasZ_uT()); + Serial.println(";"); + Serial.print("float MagScaleX = "); + Serial.print(mpu9250.getMagScaleFactorX()); + Serial.println(";"); + Serial.print("float MagScaleY = "); + Serial.print(mpu9250.getMagScaleFactorY()); + Serial.println(";"); + Serial.print("float MagScaleZ = "); + Serial.print(mpu9250.getMagScaleFactorZ()); + Serial.println(";"); + Serial.println(" "); + Serial.println("If you are having trouble with your attitude estimate at a new flying location, repeat this process as needed."); + } + else { + Serial.println("Calibration Unsuccessful. Please reset the board and try again."); + } + + while(1); //Halt code so it won't enter main loop until this function commented out + #endif + Serial.println("Error: MPU9250 not selected. Cannot calibrate non-existent magnetometer."); + while(1); //Halt code so it won't enter main loop until this function commented out +} + + + + +void IMU::Madgwick(float invSampleFreq) { + + float gx = GyroX; + float gy = -GyroY; + float gz = -GyroZ; + float ax = -AccX; + float ay = AccY; + float az = AccZ; + float mx = MagY; + float my = -MagX; + float mz = MagZ; + + + //DESCRIPTION: Attitude estimation through sensor fusion - 9DOF + /* + * This function fuses the accelerometer gyro, and magnetometer readings AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, and MagZ for attitude estimation. + * Don't worry about the math. There is a tunable parameter B_madgwick in the user specified variable section which basically + * adjusts the weight of gyro data in the state estimate. Higher beta leads to noisier estimate, lower + * beta leads to slower to respond estimate. It is currently tuned for 2kHz loop rate. This function updates the roll_IMU, + * pitch_IMU, and yaw_IMU variables which are in degrees. If magnetometer data is not available, this function calls Madgwick6DOF() instead. + */ + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float hx, hy; + float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; + + //use 6DOF algorithm if MPU6050 is being used + #if defined USE_MPU6050_I2C + Madgwick6DOF(gx, gy, gz, ax, ay, az, invSampleFreq); + return; + #endif + + //Use 6DOF algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) + if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + Madgwick6DOF(gx, gy, gz, ax, ay, az, invSampleFreq); + return; + } + + //Convert gyroscope degrees/sec to radians/sec + gx *= DEG2RAD; + gy *= DEG2RAD; + gz *= DEG2RAD; + + //Rate of change of quaternion from gyroscope + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + + //Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + //Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + //Normalise magnetometer measurement + recipNorm = invSqrt(mx * mx + my * my + mz * mz); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; + + //Auxiliary variables to avoid repeated arithmetic + _2q0mx = 2.0f * q0 * mx; + _2q0my = 2.0f * q0 * my; + _2q0mz = 2.0f * q0 * mz; + _2q1mx = 2.0f * q1 * mx; + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _2q0q2 = 2.0f * q0 * q2; + _2q2q3 = 2.0f * q2 * q3; + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q3q3 = q3 * q3; + + //Reference direction of Earth's magnetic field + hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; + hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; + _2bx = sqrtf(hx * hx + hy * hy); + _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + //Gradient decent algorithm corrective step + s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + //Apply feedback step + qDot1 -= B_madgwick * s0; + qDot2 -= B_madgwick * s1; + qDot3 -= B_madgwick * s2; + qDot4 -= B_madgwick * s3; + } + + //Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * invSampleFreq; + q1 += qDot2 * invSampleFreq; + q2 += qDot3 * invSampleFreq; + q3 += qDot4 * invSampleFreq; + + //Normalize quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + //compute angles - NWU + roll_IMU = atan2 (q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2) *57.29577951; //degrees + pitch_IMU = -asin (constrain(-2.0f * (q1*q3 - q0*q2),-0.999999,0.999999))*57.29577951; //degrees + yaw_IMU = -atan2(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3) *57.29577951; //degrees +} + +void IMU::Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, float invSampleFreq) { + //DESCRIPTION: Attitude estimation through sensor fusion - 6DOF + /* + * See description of Madgwick() for more information. This is a 6DOF implimentation for when magnetometer data is not + * available (for example when using the recommended MPU6050 IMU for the default setup). + */ + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + + //Convert gyroscope degrees/sec to radians/sec + gx *= DEG2RAD; + gy *= DEG2RAD; + gz *= DEG2RAD; + + //Rate of change of quaternion from gyroscope + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + + //Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + //Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + //Auxiliary variables to avoid repeated arithmetic + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _4q0 = 4.0f * q0; + _4q1 = 4.0f * q1; + _4q2 = 4.0f * q2; + _8q1 = 8.0f * q1; + _8q2 = 8.0f * q2; + q0q0 = q0 * q0; + q1q1 = q1 * q1; + q2q2 = q2 * q2; + q3q3 = q3 * q3; + + //Gradient decent algorithm corrective step + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); //normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + //Apply feedback step + qDot1 -= B_madgwick * s0; + qDot2 -= B_madgwick * s1; + qDot3 -= B_madgwick * s2; + qDot4 -= B_madgwick * s3; + } + + //Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * invSampleFreq; + q1 += qDot2 * invSampleFreq; + q2 += qDot3 * invSampleFreq; + q3 += qDot4 * invSampleFreq; + + //Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + //Compute angles + roll_IMU = atan2 (q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2) *57.29577951; //degrees + pitch_IMU = -asin (constrain(-2.0f * (q1*q3 - q0*q2),-0.999999,0.999999))*57.29577951; //degrees + yaw_IMU = -atan2(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3) *57.29577951; //degrees +} + + + + + +void IMU::controlANGLE(float roll_des, float pitch_des, float yaw_des, float dt, unsigned long throttle) { + //DESCRIPTION: Computes control commands based on state error (angle) + /* + * Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des computed in + * getDesState(). Error is simply the desired state minus the actual state (ex. roll_des - roll_IMU). Two safety features + * are implimented here regarding the I terms. The I terms are saturated within specified limits on startup to prevent + * excessive buildup. This can be seen by holding the vehicle at an angle and seeing the motors ramp up on one side until + * they've maxed out throttle...saturating I to a specified limit fixes this. The second feature defaults the I terms to 0 + * if the throttle is at the minimum setting. This means the motors will not start spooling up on the ground, and the I + * terms will always start from 0 on takeoff. This function updates the variables roll_PID, pitch_PID, and yaw_PID which + * can be thought of as 1-D stablized signals. They are mixed to the configuration of the vehicle in controlMixer(). + */ + + //Roll + error_roll = roll_des - roll_IMU; + integral_roll = integral_roll_prev + error_roll*dt; + if (throttle < 1060) { //Don't let integrator build if throttle is too low + integral_roll = 0; + } + integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_roll = GyroX; + roll_PID = 0.01*(Kp_roll_angle*error_roll + Ki_roll_angle*integral_roll - Kd_roll_angle*derivative_roll); //Scaled by .01 to bring within -1 to 1 range + + //Pitch + error_pitch = pitch_des - pitch_IMU; + integral_pitch = integral_pitch_prev + error_pitch*dt; + if (throttle < 1060) { //Don't let integrator build if throttle is too low + integral_pitch = 0; + } + integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_pitch = GyroY; + pitch_PID = .01*(Kp_pitch_angle*error_pitch + Ki_pitch_angle*integral_pitch - Kd_pitch_angle*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range + + //Yaw, stablize on rate from GyroZ + error_yaw = yaw_des - GyroZ; + integral_yaw = integral_yaw_prev + error_yaw*dt; + if (throttle < 1060) { //Don't let integrator build if throttle is too low + integral_yaw = 0; + } + integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_yaw = (error_yaw - error_yaw_prev)/dt; + yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range + + //Update roll variables + integral_roll_prev = integral_roll; + //Update pitch variables + integral_pitch_prev = integral_pitch; + //Update yaw variables + error_yaw_prev = error_yaw; + integral_yaw_prev = integral_yaw; +} + +void IMU::controlANGLE2(float roll_des, float pitch_des, float yaw_des, float dt, unsigned long throttle) { + //DESCRIPTION: Computes control commands based on state error (angle) in cascaded scheme + /* + * Gives better performance than controlANGLE() but requires much more tuning. Not reccommended for first-time setup. + * See the documentation for tuning this controller. + */ + //Outer loop - PID on angle + float roll_des_ol, pitch_des_ol; + //Roll + error_roll = roll_des - roll_IMU; + integral_roll_ol = integral_roll_prev_ol + error_roll*dt; + if (throttle < 1060) { //Don't let integrator build if throttle is too low + integral_roll_ol = 0; + } + integral_roll_ol = constrain(integral_roll_ol, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_roll = (roll_IMU - roll_IMU_prev)/dt; + roll_des_ol = Kp_roll_angle*error_roll + Ki_roll_angle*integral_roll_ol;// - Kd_roll_angle*derivative_roll; + + //Pitch + error_pitch = pitch_des - pitch_IMU; + integral_pitch_ol = integral_pitch_prev_ol + error_pitch*dt; + if (throttle < 1060) { //Don't let integrator build if throttle is too low + integral_pitch_ol = 0; + } + integral_pitch_ol = constrain(integral_pitch_ol, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup + derivative_pitch = (pitch_IMU - pitch_IMU_prev)/dt; + pitch_des_ol = Kp_pitch_angle*error_pitch + Ki_pitch_angle*integral_pitch_ol;// - Kd_pitch_angle*derivative_pitch; + + //Apply loop gain, constrain, and LP filter for artificial damping + float Kl = 30.0; + roll_des_ol = Kl*roll_des_ol; + pitch_des_ol = Kl*pitch_des_ol; + roll_des_ol = constrain(roll_des_ol, -240.0, 240.0); + pitch_des_ol = constrain(pitch_des_ol, -240.0, 240.0); + roll_des_ol = (1.0 - B_loop_roll)*roll_des_prev + B_loop_roll*roll_des_ol; + pitch_des_ol = (1.0 - B_loop_pitch)*pitch_des_prev + B_loop_pitch*pitch_des_ol; + + //Inner loop - PID on rate + //Roll + error_roll = roll_des_ol - GyroX; + integral_roll_il = integral_roll_prev_il + error_roll*dt; + if (throttle < 1060) { //Don't let integrator build if throttle is too low + integral_roll_il = 0; + } + integral_roll_il = constrain(integral_roll_il, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_roll = (error_roll - error_roll_prev)/dt; + roll_PID = 0.01*(Kp_roll_rate*error_roll + Ki_roll_rate*integral_roll_il + Kd_roll_rate*derivative_roll); //Scaled by .01 to bring within -1 to 1 range + + //Pitch + error_pitch = pitch_des_ol - GyroY; + integral_pitch_il = integral_pitch_prev_il + error_pitch*dt; + if (throttle < 1060) { //Don't let integrator build if throttle is too low + integral_pitch_il = 0; + } + integral_pitch_il = constrain(integral_pitch_il, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_pitch = (error_pitch - error_pitch_prev)/dt; + pitch_PID = 0.01*(Kp_pitch_rate*error_pitch + Ki_pitch_rate*integral_pitch_il + Kd_pitch_rate*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range + + //Yaw + error_yaw = yaw_des - GyroZ; + integral_yaw = integral_yaw_prev + error_yaw*dt; + if (throttle < 1060) { //Don't let integrator build if throttle is too low + integral_yaw = 0; + } + integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_yaw = (error_yaw - error_yaw_prev)/dt; + yaw_PID = 0.01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range + + //Update roll variables + integral_roll_prev_ol = integral_roll_ol; + integral_roll_prev_il = integral_roll_il; + error_roll_prev = error_roll; + roll_IMU_prev = roll_IMU; + roll_des_prev = roll_des_ol; + //Update pitch variables + integral_pitch_prev_ol = integral_pitch_ol; + integral_pitch_prev_il = integral_pitch_il; + error_pitch_prev = error_pitch; + pitch_IMU_prev = pitch_IMU; + pitch_des_prev = pitch_des_ol; + //Update yaw variables + error_yaw_prev = error_yaw; + integral_yaw_prev = integral_yaw; + +} + +void IMU::controlRATE(float roll_des, float pitch_des, float yaw_des, float dt, unsigned long throttle) { + //DESCRIPTION: Computes control commands based on state error (rate) + /* + * See explanation for controlANGLE(). Everything is the same here except the error is now the desired rate - raw gyro reading. + */ + //Roll + error_roll = roll_des - GyroX; + integral_roll = integral_roll_prev + error_roll*dt; + if (throttle < 1060) { //Don't let integrator build if throttle is too low + integral_roll = 0; + } + integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_roll = (error_roll - error_roll_prev)/dt; + roll_PID = .01*(Kp_roll_rate*error_roll + Ki_roll_rate*integral_roll + Kd_roll_rate*derivative_roll); //Scaled by .01 to bring within -1 to 1 range + + //Pitch + error_pitch = pitch_des - GyroY; + integral_pitch = integral_pitch_prev + error_pitch*dt; + if (throttle < 1060) { //Don't let integrator build if throttle is too low + integral_pitch = 0; + } + integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_pitch = (error_pitch - error_pitch_prev)/dt; + pitch_PID = .01*(Kp_pitch_rate*error_pitch + Ki_pitch_rate*integral_pitch + Kd_pitch_rate*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range + + //Yaw, stablize on rate from GyroZ + error_yaw = yaw_des - GyroZ; + integral_yaw = integral_yaw_prev + error_yaw*dt; + if (throttle < 1060) { //Don't let integrator build if throttle is too low + integral_yaw = 0; + } + integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + derivative_yaw = (error_yaw - error_yaw_prev)/dt; + yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range + + //Update roll variables + error_roll_prev = error_roll; + integral_roll_prev = integral_roll; + GyroX_prev = GyroX; + //Update pitch variables + error_pitch_prev = error_pitch; + integral_pitch_prev = integral_pitch; + GyroY_prev = GyroY; + //Update yaw variables + error_yaw_prev = error_yaw; + integral_yaw_prev = integral_yaw; +} + + + +float IMU::getGyro(char dir) { + if (dir == 'X') { + return GyroX; + } + if (dir == 'Y') { + return GyroY; + } + if (dir == 'Z') { + return GyroZ; + } + return 0.0; +} + +float IMU::getAccel(char dir) { + if (dir == 'X') { + return AccX; + } + if (dir == 'Y') { + return AccY; + } + if (dir == 'Z') { + return AccZ; + } + return 0.0; +} + + +float IMU::getMag(char dir) { + if (dir == 'X') { + return MagX; + } + if (dir == 'Y') { + return MagY; + } + if (dir == 'Z') { + return MagZ; + } + return 0.0; +} + + + void IMU::setMagError() { + + } + void IMU::setMagScale() { + + + } + void IMU::setAccError() { + + + } + void IMU::setGryoError() { + + + } + + + + + void IMU::setRollAnglePID(float Kp, float Ki, float Kd) { + Kp_roll_angle = Kp; + Ki_roll_angle = Ki; + Kd_roll_angle = Kd; + } + void IMU::setPitchAnglePID(float Kp, float Ki, float Kd) { + Kp_pitch_angle = Kp; + Ki_pitch_angle = Ki; + Kd_pitch_angle = Kd; + } + + void IMU::setRollRatePID(float Kp, float Ki, float Kd) { + Kp_roll_rate = Kp; + Ki_roll_rate = Ki; + Kd_roll_rate = Kd; + } + + void IMU::setPitchRatePID(float Kp, float Ki, float Kd) { + Kp_pitch_rate = Kp; + Ki_pitch_rate = Ki; + Kd_pitch_rate = Kd; + } + void IMU::setYawRatePID(float Kp, float Ki, float Kd) { + Kp_yaw = Kp; + Ki_yaw = Ki; + Kd_yaw = Kd; + } + + + + + + //HELPER FUNCTIONS + +float IMU::invSqrt(float x) { + //Fast inverse sqrt for madgwick filter + /* + float halfx = 0.5f * x; + float y = x; + long i = *(long*)&y; + i = 0x5f3759df - (i>>1); + y = *(float*)&i; + y = y * (1.5f - (halfx * y * y)); + y = y * (1.5f - (halfx * y * y)); + return y; + */ + /* + //alternate form: + unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1); + float tmp = *(float*)&i; + float y = tmp * (1.69000231f - 0.714158168f * x * tmp * tmp); + return y; + */ + return 1.0/sqrtf(x); //Teensy is fast enough to just take the compute penalty lol suck it arduino nano +} diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/IMU_PID.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/IMU_PID.h new file mode 100644 index 00000000..6c2d3c4a --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/IMU_PID.h @@ -0,0 +1,196 @@ + + + +// IMU_PID.h + +#ifndef IMU_PID_h +#define IMU_PID_h + +#include +#include //I2c communication +#include //SPI communication + +// Include all possible sensor libraries +#include "../MPU6050/MPU6050.h" +#include "../MPU9250/MPU9250.h" +#include + +enum IMUType { + MPU6050_I2C, + MPU9250_SPI, + LSM6DSOX_SPI +}; + +enum Gyro_DPS { + GYRO_250DPS, // DEFAULT + GYRO_500DPS, + GYRO_1000DPS, + GYRO_2000DPS +}; + +enum Accel_G { + ACCEL_2G, // DEFAULT + ACCEL_4G, + ACCEL_8G, + ACCEL_16G +}; + +// Conversion factors for the LSM6DSOX +// degrees to radians and back +#define DEG2RAD 0.0174533f +#define RAD2DEG 57.29575496f +// "G factor" to meters/second and back +#define G2MS 9.80665f +#define MS2G 0.10197162129779283f + + + + +class IMU { +public: + + IMU(IMUType type); + IMU(IMUType type, Gyro_DPS GyroDPS, Accel_G AccelG); + bool begin(); + + void getIMUdata(); + void calculate_IMU_error(); + void calibrateMagnetometer(); + + void controlANGLE(float roll_des, float pitch_des, float yaw_des, float dt, unsigned long throttle); + void controlANGLE2(float roll_des, float pitch_des, float yaw_des, float dt, unsigned long throttle); + void controlRATE(float roll_des, float pitch_des, float yaw_des, float dt, unsigned long throttle); + + void Madgwick(float invSampleFreq); + void Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, float invSampleFreq); + + void setMagError(); + void setMagScale(); + void setAccError(); + void setGryoError(); + + void setRollAnglePID(float Kp, float Ki, float Kd); + void setPitchAnglePID(float Kp, float Ki, float Kd); + + void setRollRatePID(float Kp, float Ki, float Kd); + void setPitchRatePID(float Kp, float Ki, float Kd); + void setYawRatePID(float Kp, float Ki, float Kd); + + float getGyro(char dir); + float getAccel(char dir); + float getMag(char dir); + + float AccX, AccY, AccZ; + float GyroX; + float GyroY; + float GyroZ; + float MagX, MagY, MagZ; + float roll_IMU, pitch_IMU, yaw_IMU; + + float roll_PID = 0; + float pitch_PID = 0; + float yaw_PID = 0; + + + +private: + //IMU: + IMUType _type; + Gyro_DPS _gyroDPS; + Accel_G _accelG; + + MPU6050* _mpu6050; + MPU9250* _mpu9250; + Adafruit_LSM6DSOX* _lsm6dsox; + + float _gyro_scale_factor = 131.0; + float _accel_scale_factor = 16384.0; + + // Used for LSM6DSOX + sensors_event_t accel; + sensors_event_t gyro; + sensors_event_t temp; //unused, but could be interesting + + + float invSqrt(float x); + + float AccX_prev, AccY_prev, AccZ_prev; + float GyroX_prev, GyroY_prev, GyroZ_prev; + float MagX_prev, MagY_prev, MagZ_prev; + float roll_IMU_prev, pitch_IMU_prev; + + //Controller: + float error_roll, error_roll_prev, roll_des_prev, integral_roll, integral_roll_il, integral_roll_ol, integral_roll_prev, integral_roll_prev_il, integral_roll_prev_ol, derivative_roll; + float error_pitch, error_pitch_prev, pitch_des_prev, integral_pitch, integral_pitch_il, integral_pitch_ol, integral_pitch_prev, integral_pitch_prev_il, integral_pitch_prev_ol, derivative_pitch; + float error_yaw, error_yaw_prev, integral_yaw, integral_yaw_prev, derivative_yaw; + + + float q0 = 1.0f; //Initialize quaternion for madgwick filter + float q1 = 0.0f; + float q2 = 0.0f; + float q3 = 0.0f; + + //Filter parameters - Defaults tuned for 2kHz loop rate; Do not touch unless you know what you are doing: + float B_madgwick = 0.04; //Madgwick filter parameter + float B_accel = 0.14; //Accelerometer LP filter paramter, (MPU6050 default: 0.14. MPU9250 default: 0.2) + float B_gyro = 0.1; //Gyro LP filter paramter, (MPU6050 default: 0.1. MPU9250 default: 0.17) + float B_mag = 1.0; //Magnetometer LP filter parameter + + //Magnetometer calibration parameters - if using MPU9250, uncomment calibrateMagnetometer() in void setup() to get these values, else just ignore these + float MagErrorX = 0.0; + float MagErrorY = 0.0; + float MagErrorZ = 0.0; + float MagScaleX = 1.0; + float MagScaleY = 1.0; + float MagScaleZ = 1.0; + + //IMU calibration parameters - calibrate IMU using calculate_IMU_error() in the void setup() to get these values, then comment out calculate_IMU_error() + float AccErrorX = -0.01; + float AccErrorY = 0.01; + float AccErrorZ = 0.04; + float GyroErrorX = 0.41; + float GyroErrorY = 0.17; + float GyroErrorZ = -0.67; + + + // PIDs + float i_limit = 25.0; //Integrator saturation level, mostly for safety (default 25.0) + // float maxRoll = 30.0; //Max roll angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode + // float maxPitch = 30.0; //Max pitch angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode + // float maxYaw = 160.0; //Max yaw rate in deg/sec + + float Kp_roll_angle = 0.2; //Roll P-gain - angle mode + float Ki_roll_angle = 0.3; //Roll I-gain - angle mode + float Kd_roll_angle = 0.05; //Roll D-gain - angle mode (has no effect on controlANGLE2) + float B_loop_roll = 0.9; //Roll damping term for controlANGLE2(), lower is more damping (must be between 0 to 1) + float Kp_pitch_angle = 0.2; //Pitch P-gain - angle mode + float Ki_pitch_angle = 0.3; //Pitch I-gain - angle mode + float Kd_pitch_angle = 0.05; //Pitch D-gain - angle mode (has no effect on controlANGLE2) + float B_loop_pitch = 0.9; //Pitch damping term for controlANGLE2(), lower is more damping (must be between 0 to 1) + + float Kp_roll_rate = 0.15; //Roll P-gain - rate mode + float Ki_roll_rate = 0.2; //Roll I-gain - rate mode + float Kd_roll_rate = 0.0002; //Roll D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) + float Kp_pitch_rate = 0.15; //Pitch P-gain - rate mode + float Ki_pitch_rate = 0.2; //Pitch I-gain - rate mode + float Kd_pitch_rate = 0.0002; //Pitch D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) + + float Kp_yaw = 0.3; //Yaw P-gain + float Ki_yaw = 0.05; //Yaw I-gain + float Kd_yaw = 0.00015; //Yaw D-gain (be careful when increasing too high, motors will begin to overheat!) + + + +}; + + + + + + + + + + + +#endif \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/keywords.txt b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/keywords.txt new file mode 100644 index 00000000..d77fe459 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/keywords.txt @@ -0,0 +1,34 @@ +####################################### +# Syntax Coloring Map ArduinoLog library +####################################### + +####################################### +# Instances (KEYWORD1) +####################################### +#Logging KEYWORD1 +#Log KEYWORD1 +####################################### +# Datatypes (KEYWORD1) +####################################### + +####################################### +# Methods and Functions (KEYWORD2) +####################################### +IMUinit KEYWORD2 +getIMUdata KEYWORD2 +calculate_IMU_error KEYWORD2 + + +####################################### +# Instances (KEYWORD2) +####################################### +#Logging KEYWORD2 +#Log KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### +GYRO_SCALE LITERAL1 Constants +GYRO_SCALE_FACTOR LITERAL1 Constants +ACCEL_SCALE LITERAL1 Constants +ACCEL_SCALE_FACTOR LITERAL1 Constants diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/I2Cdev.cpp b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/I2Cdev.cpp new file mode 100644 index 00000000..a22474dd --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/I2Cdev.cpp @@ -0,0 +1,1468 @@ +// I2Cdev library collection - Main I2C device class +// Abstracts bit and byte I2C R/W functions into a convenient class +// 2013-06-05 by Jeff Rowberg +// +// Changelog: +// 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications +// 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) +// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire +// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation +// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) +// 2011-10-03 - added automatic Arduino version detection for ease of use +// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications +// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) +// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default +// 2011-08-02 - added support for 16-bit registers +// - fixed incorrect Doxygen comments on some methods +// - added timeout value for read operations (thanks mem @ Arduino forums) +// 2011-07-30 - changed read/write function structures to return success or byte counts +// - made all methods static for multi-device memory savings +// 2011-07-28 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2013 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#include "I2Cdev.h" + +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE + + #ifdef I2CDEV_IMPLEMENTATION_WARNINGS + #if ARDUINO < 100 + #warning Using outdated Arduino IDE with Wire library is functionally limiting. + #warning Arduino IDE v1.6.5+ with I2Cdev Fastwire implementation is recommended. + #warning This I2Cdev implementation does not support: + #warning - Repeated starts conditions + #warning - Timeout detection (some Wire requests block forever) + #elif ARDUINO == 100 + #warning Using outdated Arduino IDE with Wire library is functionally limiting. + #warning Arduino IDE v1.6.5+ with I2Cdev Fastwire implementation is recommended. + #warning This I2Cdev implementation does not support: + #warning - Repeated starts conditions + #warning - Timeout detection (some Wire requests block forever) + #elif ARDUINO > 100 + /*#warning Using current Arduino IDE with Wire library is functionally limiting. + #warning Arduino IDE v1.6.5+ with I2CDEV_BUILTIN_FASTWIRE implementation is recommended. + #warning This I2Cdev implementation does not support: + #warning - Timeout detection (some Wire requests block forever)*/ + #endif + #endif + +#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + + //#error The I2CDEV_BUILTIN_FASTWIRE implementation is known to be broken right now. Patience, Iago! + +#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + + #ifdef I2CDEV_IMPLEMENTATION_WARNINGS + #warning Using I2CDEV_BUILTIN_NBWIRE implementation may adversely affect interrupt detection. + #warning This I2Cdev implementation does not support: + #warning - Repeated starts conditions + #endif + + // NBWire implementation based heavily on code by Gene Knight + // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html + // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html + TwoWire Wire; + +#endif + +#ifndef BUFFER_LENGTH +// band-aid fix for platforms without Wire-defined BUFFER_LENGTH (removed from some official implementations) +#define BUFFER_LENGTH 32 +#endif + +/** Default constructor. + */ +I2Cdev::I2Cdev() { +} + +/** Read a single bit from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitNum Bit position to read (0-7) + * @param data Container for single bit value + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) { + uint8_t b; + uint8_t count = readByte(devAddr, regAddr, &b, timeout); + *data = b & (1 << bitNum); + return count; +} + +/** Read a single bit from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitNum Bit position to read (0-15) + * @param data Container for single bit value + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) { + uint16_t b; + uint8_t count = readWord(devAddr, regAddr, &b, timeout); + *data = b & (1 << bitNum); + return count; +} + +/** Read multiple bits from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitStart First bit position to read (0-7) + * @param length Number of bits to read (not more than 8) + * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) { + // 01101001 read byte + // 76543210 bit numbers + // xxx args: bitStart=4, length=3 + // 010 masked + // -> 010 shifted + uint8_t count, b; + if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) { + uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); + b &= mask; + b >>= (bitStart - length + 1); + *data = b; + } + return count; +} + +/** Read multiple bits from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitStart First bit position to read (0-15) + * @param length Number of bits to read (not more than 16) + * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (1 = success, 0 = failure, -1 = timeout) + */ +int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) { + // 1101011001101001 read byte + // fedcba9876543210 bit numbers + // xxx args: bitStart=12, length=3 + // 010 masked + // -> 010 shifted + uint8_t count; + uint16_t w; + if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) { + uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); + w &= mask; + w >>= (bitStart - length + 1); + *data = w; + } + return count; +} + +/** Read single byte from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param data Container for byte value read from device + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) { + return readBytes(devAddr, regAddr, 1, data, timeout); +} + +/** Read single word from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param data Container for word value read from device + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) { + return readWords(devAddr, regAddr, 1, data, timeout); +} + +/** Read multiple bytes from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register regAddr to read from + * @param length Number of bytes to read + * @param data Buffer to store read data in + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Number of bytes read (-1 indicates failure) + */ +int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") reading "); + Serial.print(length, DEC); + Serial.print(" bytes from 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + + int8_t count = 0; + uint32_t t1 = millis(); + + #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE) + + #if (ARDUINO < 100) + // Arduino v00xx (before v1.0), Wire library + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.send(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); + + for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = Wire.receive(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + } + + Wire.endTransmission(); + } + #elif (ARDUINO == 100) + // Arduino v1.0.0, Wire library + // Adds standardized write() and read() stream methods instead of send() and receive() + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); + + for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + } + + Wire.endTransmission(); + } + #elif (ARDUINO > 100) + // Arduino v1.0.1+, Wire library + // Adds official support for repeated start condition, yay! + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); + + for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + } + } + #endif + + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + + // Fastwire library + // no loop required for fastwire + uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, data, length); + if (status == 0) { + count = length; // success + } else { + count = -1; // error + } + + #endif + + // check for timeout + if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout + + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(". Done ("); + Serial.print(count, DEC); + Serial.println(" read)."); + #endif + + return count; +} + +/** Read multiple words from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register regAddr to read from + * @param length Number of words to read + * @param data Buffer to store read data in + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Number of words read (-1 indicates failure) + */ +int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") reading "); + Serial.print(length, DEC); + Serial.print(" words from 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + + int8_t count = 0; + uint32_t t1 = millis(); + +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE + + #if (ARDUINO < 100) + // Arduino v00xx (before v1.0), Wire library + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.send(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + + bool msb = true; // starts with MSB, then LSB + for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + if (msb) { + // first byte is bits 15-8 (MSb=15) + data[count] = Wire.receive() << 8; + } else { + // second byte is bits 7-0 (LSb=0) + data[count] |= Wire.receive(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + count++; + } + msb = !msb; + } + + Wire.endTransmission(); + } + #elif (ARDUINO == 100) + // Arduino v1.0.0, Wire library + // Adds standardized write() and read() stream methods instead of send() and receive() + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + + bool msb = true; // starts with MSB, then LSB + for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + if (msb) { + // first byte is bits 15-8 (MSb=15) + data[count] = Wire.read() << 8; + } else { + // second byte is bits 7-0 (LSb=0) + data[count] |= Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + count++; + } + msb = !msb; + } + + Wire.endTransmission(); + } + #elif (ARDUINO > 100) + // Arduino v1.0.1+, Wire library + // Adds official support for repeated start condition, yay! + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + + bool msb = true; // starts with MSB, then LSB + for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + if (msb) { + // first byte is bits 15-8 (MSb=15) + data[count] = Wire.read() << 8; + } else { + // second byte is bits 7-0 (LSb=0) + data[count] |= Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + count++; + } + msb = !msb; + } + + Wire.endTransmission(); + } + #endif + + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + + // Fastwire library + // no loop required for fastwire + uint16_t intermediate[(uint8_t)length]; + uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, (uint8_t *)intermediate, (uint8_t)(length * 2)); + if (status == 0) { + count = length; // success + for (uint8_t i = 0; i < length; i++) { + data[i] = (intermediate[2*i] << 8) | intermediate[2*i + 1]; + } + } else { + count = -1; // error + } + + #endif + + if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout + + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(". Done ("); + Serial.print(count, DEC); + Serial.println(" read)."); + #endif + + return count; +} + +/** write a single bit in an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitNum Bit position to write (0-7) + * @param value New bit value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) { + uint8_t b; + readByte(devAddr, regAddr, &b); + b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); + return writeByte(devAddr, regAddr, b); +} + +/** write a single bit in a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitNum Bit position to write (0-15) + * @param value New bit value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) { + uint16_t w; + readWord(devAddr, regAddr, &w); + w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum)); + return writeWord(devAddr, regAddr, w); +} + +/** Write multiple bits in an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitStart First bit position to write (0-7) + * @param length Number of bits to write (not more than 8) + * @param data Right-aligned value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { + // 010 value to write + // 76543210 bit numbers + // xxx args: bitStart=4, length=3 + // 00011100 mask byte + // 10101111 original value (sample) + // 10100011 original & ~mask + // 10101011 masked | value + uint8_t b; + if (readByte(devAddr, regAddr, &b) != 0) { + uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); + data <<= (bitStart - length + 1); // shift data into correct position + data &= mask; // zero all non-important bits in data + b &= ~(mask); // zero all important bits in existing byte + b |= data; // combine data with existing byte + return writeByte(devAddr, regAddr, b); + } else { + return false; + } +} + +/** Write multiple bits in a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitStart First bit position to write (0-15) + * @param length Number of bits to write (not more than 16) + * @param data Right-aligned value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) { + // 010 value to write + // fedcba9876543210 bit numbers + // xxx args: bitStart=12, length=3 + // 0001110000000000 mask word + // 1010111110010110 original value (sample) + // 1010001110010110 original & ~mask + // 1010101110010110 masked | value + uint16_t w; + if (readWord(devAddr, regAddr, &w) != 0) { + uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); + data <<= (bitStart - length + 1); // shift data into correct position + data &= mask; // zero all non-important bits in data + w &= ~(mask); // zero all important bits in existing word + w |= data; // combine data with existing word + return writeWord(devAddr, regAddr, w); + } else { + return false; + } +} + +/** Write single byte to an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register address to write to + * @param data New byte value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) { + return writeBytes(devAddr, regAddr, 1, &data); +} + +/** Write single word to a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register address to write to + * @param data New word value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) { + return writeWords(devAddr, regAddr, 1, &data); +} + +/** Write multiple bytes to an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register address to write to + * @param length Number of bytes to write + * @param data Buffer to copy new data from + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") writing "); + Serial.print(length, DEC); + Serial.print(" bytes to 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + uint8_t status = 0; + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.beginTransmission(devAddr); + Wire.send((uint8_t) regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ + || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) + Wire.beginTransmission(devAddr); + Wire.write((uint8_t) regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::beginTransmission(devAddr); + Fastwire::write(regAddr); + #endif + for (uint8_t i = 0; i < length; i++) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[i], HEX); + if (i + 1 < length) Serial.print(" "); + #endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.send((uint8_t) data[i]); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ + || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) + Wire.write((uint8_t) data[i]); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::write((uint8_t) data[i]); + #endif + } + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ + || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) + status = Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::stop(); + //status = Fastwire::endTransmission(); + #endif + #ifdef I2CDEV_SERIAL_DEBUG + Serial.println(". Done."); + #endif + return status == 0; +} + +/** Write multiple words to a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register address to write to + * @param length Number of words to write + * @param data Buffer to copy new data from + * @return Status of operation (true = success) + */ +bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") writing "); + Serial.print(length, DEC); + Serial.print(" words to 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + uint8_t status = 0; + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.beginTransmission(devAddr); + Wire.send(regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ + || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) + Wire.beginTransmission(devAddr); + Wire.write(regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::beginTransmission(devAddr); + Fastwire::write(regAddr); + #endif + for (uint8_t i = 0; i < length; i++) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[i], HEX); + if (i + 1 < length) Serial.print(" "); + #endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.send((uint8_t)(data[i] >> 8)); // send MSB + Wire.send((uint8_t)data[i]); // send LSB + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ + || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) + Wire.write((uint8_t)(data[i] >> 8)); // send MSB + Wire.write((uint8_t)data[i]); // send LSB + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::write((uint8_t)(data[i] >> 8)); // send MSB + status = Fastwire::write((uint8_t)data[i]); // send LSB + if (status != 0) break; + #endif + } + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ + || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) + status = Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::stop(); + //status = Fastwire::endTransmission(); + #endif + #ifdef I2CDEV_SERIAL_DEBUG + Serial.println(". Done."); + #endif + return status == 0; +} + +/** Default timeout value for read operations. + * Set this to 0 to disable timeout detection. + */ +uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + // I2C library + ////////////////////// + // Copyright(C) 2012 + // Francesco Ferrara + // ferrara[at]libero[point]it + ////////////////////// + + /* + FastWire + - 0.24 added stop + - 0.23 added reset + + This is a library to help faster programs to read I2C devices. + Copyright(C) 2012 Francesco Ferrara + occhiobello at gmail dot com + [used by Jeff Rowberg for I2Cdevlib with permission] + */ + + boolean Fastwire::waitInt() { + int l = 250; + while (!(TWCR & (1 << TWINT)) && l-- > 0); + return l > 0; + } + + void Fastwire::setup(int khz, boolean pullup) { + TWCR = 0; + #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__) + // activate internal pull-ups for twi (PORTC bits 4 & 5) + // as per note from atmega8 manual pg167 + if (pullup) PORTC |= ((1 << 4) | (1 << 5)); + else PORTC &= ~((1 << 4) | (1 << 5)); + #elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) + // activate internal pull-ups for twi (PORTC bits 0 & 1) + if (pullup) PORTC |= ((1 << 0) | (1 << 1)); + else PORTC &= ~((1 << 0) | (1 << 1)); + #else + // activate internal pull-ups for twi (PORTD bits 0 & 1) + // as per note from atmega128 manual pg204 + if (pullup) PORTD |= ((1 << 0) | (1 << 1)); + else PORTD &= ~((1 << 0) | (1 << 1)); + #endif + + TWSR = 0; // no prescaler => prescaler = 1 + TWBR = ((16000L / khz) - 16) / 2; // change the I2C clock rate + TWCR = 1 << TWEN; // enable twi module, no interrupt + } + + // added by Jeff Rowberg 2013-05-07: + // Arduino Wire-style "beginTransmission" function + // (takes 7-bit device address like the Wire method, NOT 8-bit: 0x68, not 0xD0/0xD1) + byte Fastwire::beginTransmission(byte device) { + byte twst, retry; + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 2; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device << 1; // send device address without read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 3; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 4; + return 0; + } + + byte Fastwire::writeBuf(byte device, byte address, byte *data, byte num) { + byte twst, retry; + + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 2; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device & 0xFE; // send device address without read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 3; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 4; + + //Serial.print(address, HEX); + //Serial.print(" "); + TWDR = address; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 5; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 6; + + for (byte i = 0; i < num; i++) { + //Serial.print(data[i], HEX); + //Serial.print(" "); + TWDR = data[i]; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 7; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 8; + } + //Serial.print("\n"); + + return 0; + } + + byte Fastwire::write(byte value) { + byte twst; + //Serial.println(value, HEX); + TWDR = value; // send data + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 2; + return 0; + } + + byte Fastwire::readBuf(byte device, byte address, byte *data, byte num) { + byte twst, retry; + + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 16; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 17; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device & 0xfe; // send device address to write + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 18; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 19; + + //Serial.print(address, HEX); + //Serial.print(" "); + TWDR = address; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 20; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 21; + + /***/ + + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 22; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 23; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device | 0x01; // send device address with the read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 24; + twst = TWSR & 0xF8; + } while (twst == TW_MR_SLA_NACK && retry-- > 0); + if (twst != TW_MR_SLA_ACK) return 25; + + for (uint8_t i = 0; i < num; i++) { + if (i == num - 1) + TWCR = (1 << TWINT) | (1 << TWEN); + else + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA); + if (!waitInt()) return 26; + twst = TWSR & 0xF8; + if (twst != TW_MR_DATA_ACK && twst != TW_MR_DATA_NACK) return twst; + data[i] = TWDR; + //Serial.print(data[i], HEX); + //Serial.print(" "); + } + //Serial.print("\n"); + stop(); + + return 0; + } + + void Fastwire::reset() { + TWCR = 0; + } + + byte Fastwire::stop() { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO); + if (!waitInt()) return 1; + return 0; + } +#endif + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + // NBWire implementation based heavily on code by Gene Knight + // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html + // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html + + /* + call this version 1.0 + + Offhand, the only funky part that I can think of is in nbrequestFrom, where the buffer + length and index are set *before* the data is actually read. The problem is that these + are variables local to the TwoWire object, and by the time we actually have read the + data, and know what the length actually is, we have no simple access to the object's + variables. The actual bytes read *is* given to the callback function, though. + + The ISR code for a slave receiver is commented out. I don't have that setup, and can't + verify it at this time. Save it for 2.0! + + The handling of the read and write processes here is much like in the demo sketch code: + the process is broken down into sequential functions, where each registers the next as a + callback, essentially. + + For example, for the Read process, twi_read00 just returns if TWI is not yet in a + ready state. When there's another interrupt, and the interface *is* ready, then it + sets up the read, starts it, and registers twi_read01 as the function to call after + the *next* interrupt. twi_read01, then, just returns if the interface is still in a + "reading" state. When the reading is done, it copies the information to the buffer, + cleans up, and calls the user-requested callback function with the actual number of + bytes read. + + The writing is similar. + + Questions, comments and problems can go to Gene@Telobot.com. + + Thumbs Up! + Gene Knight + + */ + + uint8_t TwoWire::rxBuffer[NBWIRE_BUFFER_LENGTH]; + uint8_t TwoWire::rxBufferIndex = 0; + uint8_t TwoWire::rxBufferLength = 0; + + uint8_t TwoWire::txAddress = 0; + uint8_t TwoWire::txBuffer[NBWIRE_BUFFER_LENGTH]; + uint8_t TwoWire::txBufferIndex = 0; + uint8_t TwoWire::txBufferLength = 0; + + //uint8_t TwoWire::transmitting = 0; + void (*TwoWire::user_onRequest)(void); + void (*TwoWire::user_onReceive)(int); + + static volatile uint8_t twi_transmitting; + static volatile uint8_t twi_state; + static uint8_t twi_slarw; + static volatile uint8_t twi_error; + static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH]; + static volatile uint8_t twi_masterBufferIndex; + static uint8_t twi_masterBufferLength; + static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH]; + static volatile uint8_t twi_rxBufferIndex; + //static volatile uint8_t twi_Interrupt_Continue_Command; + static volatile uint8_t twi_Return_Value; + static volatile uint8_t twi_Done; + void (*twi_cbendTransmissionDone)(int); + void (*twi_cbreadFromDone)(int); + + void twi_init() { + // initialize state + twi_state = TWI_READY; + + // activate internal pull-ups for twi + // as per note from atmega8 manual pg167 + sbi(PORTC, 4); + sbi(PORTC, 5); + + // initialize twi prescaler and bit rate + cbi(TWSR, TWPS0); // TWI Status Register - Prescaler bits + cbi(TWSR, TWPS1); + + /* twi bit rate formula from atmega128 manual pg 204 + SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR)) + note: TWBR should be 10 or higher for master mode + It is 72 for a 16mhz Wiring board with 100kHz TWI */ + + TWBR = ((CPU_FREQ / TWI_FREQ) - 16) / 2; // bitrate register + // enable twi module, acks, and twi interrupt + + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); + + /* TWEN - TWI Enable Bit + TWIE - TWI Interrupt Enable + TWEA - TWI Enable Acknowledge Bit + TWINT - TWI Interrupt Flag + TWSTA - TWI Start Condition + */ + } + + typedef struct { + uint8_t address; + uint8_t* data; + uint8_t length; + uint8_t wait; + uint8_t i; + } twi_Write_Vars; + + twi_Write_Vars *ptwv = 0; + static void (*fNextInterruptFunction)(void) = 0; + + void twi_Finish(byte bRetVal) { + if (ptwv) { + free(ptwv); + ptwv = 0; + } + twi_Done = 0xFF; + twi_Return_Value = bRetVal; + fNextInterruptFunction = 0; + } + + uint8_t twii_WaitForDone(uint16_t timeout) { + uint32_t endMillis = millis() + timeout; + while (!twi_Done && (timeout == 0 || millis() < endMillis)) continue; + return twi_Return_Value; + } + + void twii_SetState(uint8_t ucState) { + twi_state = ucState; + } + + void twii_SetError(uint8_t ucError) { + twi_error = ucError ; + } + + void twii_InitBuffer(uint8_t ucPos, uint8_t ucLength) { + twi_masterBufferIndex = 0; + twi_masterBufferLength = ucLength; + } + + void twii_CopyToBuf(uint8_t* pData, uint8_t ucLength) { + uint8_t i; + for (i = 0; i < ucLength; ++i) { + twi_masterBuffer[i] = pData[i]; + } + } + + void twii_CopyFromBuf(uint8_t *pData, uint8_t ucLength) { + uint8_t i; + for (i = 0; i < ucLength; ++i) { + pData[i] = twi_masterBuffer[i]; + } + } + + void twii_SetSlaRW(uint8_t ucSlaRW) { + twi_slarw = ucSlaRW; + } + + void twii_SetStart() { + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA); + } + + void twi_write01() { + if (TWI_MTX == twi_state) return; // blocking test + twi_transmitting = 0 ; + if (twi_error == 0xFF) + twi_Finish (0); // success + else if (twi_error == TW_MT_SLA_NACK) + twi_Finish (2); // error: address send, nack received + else if (twi_error == TW_MT_DATA_NACK) + twi_Finish (3); // error: data send, nack received + else + twi_Finish (4); // other twi error + if (twi_cbendTransmissionDone) return twi_cbendTransmissionDone(twi_Return_Value); + return; + } + + + void twi_write00() { + if (TWI_READY != twi_state) return; // blocking test + if (TWI_BUFFER_LENGTH < ptwv -> length) { + twi_Finish(1); // end write with error 1 + return; + } + twi_Done = 0x00; // show as working + twii_SetState(TWI_MTX); // to transmitting + twii_SetError(0xFF); // to No Error + twii_InitBuffer(0, ptwv -> length); // pointer and length + twii_CopyToBuf(ptwv -> data, ptwv -> length); // get the data + twii_SetSlaRW((ptwv -> address << 1) | TW_WRITE); // write command + twii_SetStart(); // start the cycle + fNextInterruptFunction = twi_write01; // next routine + return twi_write01(); + } + + void twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait) { + uint8_t i; + ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars)); + ptwv -> address = address; + ptwv -> data = data; + ptwv -> length = length; + ptwv -> wait = wait; + fNextInterruptFunction = twi_write00; + return twi_write00(); + } + + void twi_read01() { + if (TWI_MRX == twi_state) return; // blocking test + if (twi_masterBufferIndex < ptwv -> length) ptwv -> length = twi_masterBufferIndex; + twii_CopyFromBuf(ptwv -> data, ptwv -> length); + twi_Finish(ptwv -> length); + if (twi_cbreadFromDone) return twi_cbreadFromDone(twi_Return_Value); + return; + } + + void twi_read00() { + if (TWI_READY != twi_state) return; // blocking test + if (TWI_BUFFER_LENGTH < ptwv -> length) twi_Finish(0); // error return + twi_Done = 0x00; // show as working + twii_SetState(TWI_MRX); // reading + twii_SetError(0xFF); // reset error + twii_InitBuffer(0, ptwv -> length - 1); // init to one less than length + twii_SetSlaRW((ptwv -> address << 1) | TW_READ); // read command + twii_SetStart(); // start cycle + fNextInterruptFunction = twi_read01; + return twi_read01(); + } + + void twi_readFrom(uint8_t address, uint8_t* data, uint8_t length) { + uint8_t i; + + ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars)); + ptwv -> address = address; + ptwv -> data = data; + ptwv -> length = length; + fNextInterruptFunction = twi_read00; + return twi_read00(); + } + + void twi_reply(uint8_t ack) { + // transmit master read ready signal, with or without ack + if (ack){ + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA); + } else { + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT); + } + } + + void twi_stop(void) { + // send stop condition + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO); + + // wait for stop condition to be exectued on bus + // TWINT is not set after a stop condition! + while (TWCR & _BV(TWSTO)) { + continue; + } + + // update twi state + twi_state = TWI_READY; + } + + void twi_releaseBus(void) { + // release bus + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT); + + // update twi state + twi_state = TWI_READY; + } + + SIGNAL(TWI_vect) { + switch (TW_STATUS) { + // All Master + case TW_START: // sent start condition + case TW_REP_START: // sent repeated start condition + // copy device address and r/w bit to output register and ack + TWDR = twi_slarw; + twi_reply(1); + break; + + // Master Transmitter + case TW_MT_SLA_ACK: // slave receiver acked address + case TW_MT_DATA_ACK: // slave receiver acked data + // if there is data to send, send it, otherwise stop + if (twi_masterBufferIndex < twi_masterBufferLength) { + // copy data to output register and ack + TWDR = twi_masterBuffer[twi_masterBufferIndex++]; + twi_reply(1); + } else { + twi_stop(); + } + break; + + case TW_MT_SLA_NACK: // address sent, nack received + twi_error = TW_MT_SLA_NACK; + twi_stop(); + break; + + case TW_MT_DATA_NACK: // data sent, nack received + twi_error = TW_MT_DATA_NACK; + twi_stop(); + break; + + case TW_MT_ARB_LOST: // lost bus arbitration + twi_error = TW_MT_ARB_LOST; + twi_releaseBus(); + break; + + // Master Receiver + case TW_MR_DATA_ACK: // data received, ack sent + // put byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + + case TW_MR_SLA_ACK: // address sent, ack received + // ack if more bytes are expected, otherwise nack + if (twi_masterBufferIndex < twi_masterBufferLength) { + twi_reply(1); + } else { + twi_reply(0); + } + break; + + case TW_MR_DATA_NACK: // data received, nack sent + // put final byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + + case TW_MR_SLA_NACK: // address sent, nack received + twi_stop(); + break; + + // TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case + + // Slave Receiver (NOT IMPLEMENTED YET) + /* + case TW_SR_SLA_ACK: // addressed, returned ack + case TW_SR_GCALL_ACK: // addressed generally, returned ack + case TW_SR_ARB_LOST_SLA_ACK: // lost arbitration, returned ack + case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack + // enter slave receiver mode + twi_state = TWI_SRX; + + // indicate that rx buffer can be overwritten and ack + twi_rxBufferIndex = 0; + twi_reply(1); + break; + + case TW_SR_DATA_ACK: // data received, returned ack + case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack + // if there is still room in the rx buffer + if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) { + // put byte in buffer and ack + twi_rxBuffer[twi_rxBufferIndex++] = TWDR; + twi_reply(1); + } else { + // otherwise nack + twi_reply(0); + } + break; + + case TW_SR_STOP: // stop or repeated start condition received + // put a null char after data if there's room + if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) { + twi_rxBuffer[twi_rxBufferIndex] = 0; + } + + // sends ack and stops interface for clock stretching + twi_stop(); + + // callback to user defined callback + twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex); + + // since we submit rx buffer to "wire" library, we can reset it + twi_rxBufferIndex = 0; + + // ack future responses and leave slave receiver state + twi_releaseBus(); + break; + + case TW_SR_DATA_NACK: // data received, returned nack + case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack + // nack back at master + twi_reply(0); + break; + + // Slave Transmitter + case TW_ST_SLA_ACK: // addressed, returned ack + case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack + // enter slave transmitter mode + twi_state = TWI_STX; + + // ready the tx buffer index for iteration + twi_txBufferIndex = 0; + + // set tx buffer length to be zero, to verify if user changes it + twi_txBufferLength = 0; + + // request for txBuffer to be filled and length to be set + // note: user must call twi_transmit(bytes, length) to do this + twi_onSlaveTransmit(); + + // if they didn't change buffer & length, initialize it + if (0 == twi_txBufferLength) { + twi_txBufferLength = 1; + twi_txBuffer[0] = 0x00; + } + + // transmit first byte from buffer, fall through + + case TW_ST_DATA_ACK: // byte sent, ack returned + // copy data to output register + TWDR = twi_txBuffer[twi_txBufferIndex++]; + + // if there is more to send, ack, otherwise nack + if (twi_txBufferIndex < twi_txBufferLength) { + twi_reply(1); + } else { + twi_reply(0); + } + break; + + case TW_ST_DATA_NACK: // received nack, we are done + case TW_ST_LAST_DATA: // received ack, but we are done already! + // ack future responses + twi_reply(1); + // leave slave receiver state + twi_state = TWI_READY; + break; + */ + + // all + case TW_NO_INFO: // no state information + break; + + case TW_BUS_ERROR: // bus error, illegal stop/start + twi_error = TW_BUS_ERROR; + twi_stop(); + break; + } + + if (fNextInterruptFunction) return fNextInterruptFunction(); + } + + TwoWire::TwoWire() { } + + void TwoWire::begin(void) { + rxBufferIndex = 0; + rxBufferLength = 0; + + txBufferIndex = 0; + txBufferLength = 0; + + twi_init(); + } + + void TwoWire::beginTransmission(uint8_t address) { + //beginTransmission((uint8_t)address); + + // indicate that we are transmitting + twi_transmitting = 1; + + // set address of targeted slave + txAddress = address; + + // reset tx buffer iterator vars + txBufferIndex = 0; + txBufferLength = 0; + } + + uint8_t TwoWire::endTransmission(uint16_t timeout) { + // transmit buffer (blocking) + //int8_t ret = + twi_cbendTransmissionDone = NULL; + twi_writeTo(txAddress, txBuffer, txBufferLength, 1); + int8_t ret = twii_WaitForDone(timeout); + + // reset tx buffer iterator vars + txBufferIndex = 0; + txBufferLength = 0; + + // indicate that we are done transmitting + // twi_transmitting = 0; + return ret; + } + + void TwoWire::nbendTransmission(void (*function)(int)) { + twi_cbendTransmissionDone = function; + twi_writeTo(txAddress, txBuffer, txBufferLength, 1); + return; + } + + void TwoWire::send(uint8_t data) { + if (twi_transmitting) { + // in master transmitter mode + // don't bother if buffer is full + if (txBufferLength >= NBWIRE_BUFFER_LENGTH) { + return; + } + + // put byte in tx buffer + txBuffer[txBufferIndex] = data; + ++txBufferIndex; + + // update amount in buffer + txBufferLength = txBufferIndex; + } else { + // in slave send mode + // reply to master + //twi_transmit(&data, 1); + } + } + + uint8_t TwoWire::receive(void) { + // default to returning null char + // for people using with char strings + uint8_t value = 0; + + // get each successive byte on each call + if (rxBufferIndex < rxBufferLength) { + value = rxBuffer[rxBufferIndex]; + ++rxBufferIndex; + } + + return value; + } + + uint8_t TwoWire::requestFrom(uint8_t address, int quantity, uint16_t timeout) { + // clamp to buffer length + if (quantity > NBWIRE_BUFFER_LENGTH) { + quantity = NBWIRE_BUFFER_LENGTH; + } + + // perform blocking read into buffer + twi_cbreadFromDone = NULL; + twi_readFrom(address, rxBuffer, quantity); + uint8_t read = twii_WaitForDone(timeout); + + // set rx buffer iterator vars + rxBufferIndex = 0; + rxBufferLength = read; + + return read; + } + + void TwoWire::nbrequestFrom(uint8_t address, int quantity, void (*function)(int)) { + // clamp to buffer length + if (quantity > NBWIRE_BUFFER_LENGTH) { + quantity = NBWIRE_BUFFER_LENGTH; + } + + // perform blocking read into buffer + twi_cbreadFromDone = function; + twi_readFrom(address, rxBuffer, quantity); + //uint8_t read = twii_WaitForDone(); + + // set rx buffer iterator vars + //rxBufferIndex = 0; + //rxBufferLength = read; + + rxBufferIndex = 0; + rxBufferLength = quantity; // this is a hack + + return; //read; + } + + uint8_t TwoWire::available(void) { + return rxBufferLength - rxBufferIndex; + } + +#endif diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/I2Cdev.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/I2Cdev.h new file mode 100644 index 00000000..0bff39ef --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/I2Cdev.h @@ -0,0 +1,283 @@ +// I2Cdev library collection - Main I2C device class header file +// Abstracts bit and byte I2C R/W functions into a convenient class +// 2013-06-05 by Jeff Rowberg +// +// Changelog: +// 2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1 +// 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications +// 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) +// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire +// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation +// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) +// 2011-10-03 - added automatic Arduino version detection for ease of use +// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications +// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) +// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default +// 2011-08-02 - added support for 16-bit registers +// - fixed incorrect Doxygen comments on some methods +// - added timeout value for read operations (thanks mem @ Arduino forums) +// 2011-07-30 - changed read/write function structures to return success or byte counts +// - made all methods static for multi-device memory savings +// 2011-07-28 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2013 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _I2CDEV_H_ +#define _I2CDEV_H_ + +// ----------------------------------------------------------------------------- +// I2C interface implementation setting +// ----------------------------------------------------------------------------- +#ifndef I2CDEV_IMPLEMENTATION +#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE +//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_SBWIRE +//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE +#endif // I2CDEV_IMPLEMENTATION + +// comment this out if you are using a non-optimal IDE/implementation setting +// but want the compiler to shut up about it +#define I2CDEV_IMPLEMENTATION_WARNINGS + +// ----------------------------------------------------------------------------- +// I2C interface implementation options +// ----------------------------------------------------------------------------- +#define I2CDEV_ARDUINO_WIRE 1 // Wire object from Arduino +#define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project + // ^^^ NBWire implementation is still buggy w/some interrupts! +#define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project +#define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library +#define I2CDEV_BUILTIN_SBWIRE 5 // I2C object from Shuning (Steve) Bian's SBWire Library at https://github.com/freespace/SBWire + +// ----------------------------------------------------------------------------- +// Arduino-style "Serial.print" debug constant (uncomment to enable) +// ----------------------------------------------------------------------------- +//#define I2CDEV_SERIAL_DEBUG + +#ifdef ARDUINO + #if ARDUINO < 100 + #include "WProgram.h" + #else + #include "Arduino.h" + #endif + #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + #include + #endif + #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY + #include + #endif + #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE + #include "SBWire.h" + #endif +#endif + +#ifdef SPARK + #include + #define ARDUINO 101 +#endif + + +// 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];") +#define I2CDEV_DEFAULT_READ_TIMEOUT 1000 + +class I2Cdev { + public: + I2Cdev(); + + static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); + static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); + static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); + static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); + static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); + static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); + static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); + static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); + + static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data); + static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data); + static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); + static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data); + static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data); + static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data); + static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data); + static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data); + + static uint16_t readTimeout; +}; + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + ////////////////////// + // FastWire 0.24 + // This is a library to help faster programs to read I2C devices. + // Copyright(C) 2012 + // Francesco Ferrara + ////////////////////// + + /* Master */ + #define TW_START 0x08 + #define TW_REP_START 0x10 + + /* Master Transmitter */ + #define TW_MT_SLA_ACK 0x18 + #define TW_MT_SLA_NACK 0x20 + #define TW_MT_DATA_ACK 0x28 + #define TW_MT_DATA_NACK 0x30 + #define TW_MT_ARB_LOST 0x38 + + /* Master Receiver */ + #define TW_MR_ARB_LOST 0x38 + #define TW_MR_SLA_ACK 0x40 + #define TW_MR_SLA_NACK 0x48 + #define TW_MR_DATA_ACK 0x50 + #define TW_MR_DATA_NACK 0x58 + + #define TW_OK 0 + #define TW_ERROR 1 + + class Fastwire { + private: + static boolean waitInt(); + + public: + static void setup(int khz, boolean pullup); + static byte beginTransmission(byte device); + static byte write(byte value); + static byte writeBuf(byte device, byte address, byte *data, byte num); + static byte readBuf(byte device, byte address, byte *data, byte num); + static void reset(); + static byte stop(); + }; +#endif + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + // NBWire implementation based heavily on code by Gene Knight + // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html + // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html + + #define NBWIRE_BUFFER_LENGTH 32 + + class TwoWire { + private: + static uint8_t rxBuffer[]; + static uint8_t rxBufferIndex; + static uint8_t rxBufferLength; + + static uint8_t txAddress; + static uint8_t txBuffer[]; + static uint8_t txBufferIndex; + static uint8_t txBufferLength; + + // static uint8_t transmitting; + static void (*user_onRequest)(void); + static void (*user_onReceive)(int); + static void onRequestService(void); + static void onReceiveService(uint8_t*, int); + + public: + TwoWire(); + void begin(); + void begin(uint8_t); + void begin(int); + void beginTransmission(uint8_t); + //void beginTransmission(int); + uint8_t endTransmission(uint16_t timeout=0); + void nbendTransmission(void (*function)(int)) ; + uint8_t requestFrom(uint8_t, int, uint16_t timeout=0); + //uint8_t requestFrom(int, int); + void nbrequestFrom(uint8_t, int, void (*function)(int)); + void send(uint8_t); + void send(uint8_t*, uint8_t); + //void send(int); + void send(char*); + uint8_t available(void); + uint8_t receive(void); + void onReceive(void (*)(int)); + void onRequest(void (*)(void)); + }; + + #define TWI_READY 0 + #define TWI_MRX 1 + #define TWI_MTX 2 + #define TWI_SRX 3 + #define TWI_STX 4 + + #define TW_WRITE 0 + #define TW_READ 1 + + #define TW_MT_SLA_NACK 0x20 + #define TW_MT_DATA_NACK 0x30 + + #define CPU_FREQ 16000000L + #define TWI_FREQ 100000L + #define TWI_BUFFER_LENGTH 32 + + /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */ + + #define TW_STATUS_MASK (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3)) + #define TW_STATUS (TWSR & TW_STATUS_MASK) + #define TW_START 0x08 + #define TW_REP_START 0x10 + #define TW_MT_SLA_ACK 0x18 + #define TW_MT_SLA_NACK 0x20 + #define TW_MT_DATA_ACK 0x28 + #define TW_MT_DATA_NACK 0x30 + #define TW_MT_ARB_LOST 0x38 + #define TW_MR_ARB_LOST 0x38 + #define TW_MR_SLA_ACK 0x40 + #define TW_MR_SLA_NACK 0x48 + #define TW_MR_DATA_ACK 0x50 + #define TW_MR_DATA_NACK 0x58 + #define TW_ST_SLA_ACK 0xA8 + #define TW_ST_ARB_LOST_SLA_ACK 0xB0 + #define TW_ST_DATA_ACK 0xB8 + #define TW_ST_DATA_NACK 0xC0 + #define TW_ST_LAST_DATA 0xC8 + #define TW_SR_SLA_ACK 0x60 + #define TW_SR_ARB_LOST_SLA_ACK 0x68 + #define TW_SR_GCALL_ACK 0x70 + #define TW_SR_ARB_LOST_GCALL_ACK 0x78 + #define TW_SR_DATA_ACK 0x80 + #define TW_SR_DATA_NACK 0x88 + #define TW_SR_GCALL_DATA_ACK 0x90 + #define TW_SR_GCALL_DATA_NACK 0x98 + #define TW_SR_STOP 0xA0 + #define TW_NO_INFO 0xF8 + #define TW_BUS_ERROR 0x00 + + //#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr)) + //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr)) + + #ifndef sbi // set bit + #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) + #endif // sbi + + #ifndef cbi // clear bit + #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) + #endif // cbi + + extern TwoWire Wire; + +#endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + +#endif /* _I2CDEV_H_ */ diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050.cpp b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050.cpp new file mode 100644 index 00000000..ae7a555e --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050.cpp @@ -0,0 +1,3330 @@ +// I2Cdev library collection - MPU6050 I2C device class +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 8/24/2011 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2019-07-08 - Added Auto Calibration routine +// ... - ongoing debug release + +// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE +// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF +// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING. + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#include "MPU6050.h" + +/** Specific address constructor. + * @param address I2C address, uses default I2C address if none is specified + * @see MPU6050_DEFAULT_ADDRESS + * @see MPU6050_ADDRESS_AD0_LOW + * @see MPU6050_ADDRESS_AD0_HIGH + */ +MPU6050::MPU6050(uint8_t address):devAddr(address) { +} + +/** Power on and prepare for general usage. + * This will activate the device and take it out of sleep mode (which must be done + * after start-up). This function also sets both the accelerometer and the gyroscope + * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets + * the clock source to use the X Gyro for reference, which is slightly better than + * the default internal clock source. + */ +void MPU6050::initialize() { + setClockSource(MPU6050_CLOCK_PLL_XGYRO); + setFullScaleGyroRange(MPU6050_GYRO_FS_250); + setFullScaleAccelRange(MPU6050_ACCEL_FS_2); + setSleepEnabled(false); // thanks to Jack Elston for pointing this one out! +} + +/** Verify the I2C connection. + * Make sure the device is connected and responds as expected. + * @return True if connection is valid, false otherwise + */ +bool MPU6050::testConnection() { + return getDeviceID() == 0x34; +} + +// AUX_VDDIO register (InvenSense demo code calls this RA_*G_OFFS_TC) + +/** Get the auxiliary I2C supply voltage level. + * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to + * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to + * the MPU-6000, which does not have a VLOGIC pin. + * @return I2C supply voltage level (0=VLOGIC, 1=VDD) + */ +uint8_t MPU6050::getAuxVDDIOLevel() { + I2Cdev::readBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, buffer); + return buffer[0]; +} +/** Set the auxiliary I2C supply voltage level. + * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to + * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to + * the MPU-6000, which does not have a VLOGIC pin. + * @param level I2C supply voltage level (0=VLOGIC, 1=VDD) + */ +void MPU6050::setAuxVDDIOLevel(uint8_t level) { + I2Cdev::writeBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level); +} + +// SMPLRT_DIV register + +/** Get gyroscope output rate divider. + * The sensor register output, FIFO output, DMP sampling, Motion detection, Zero + * Motion detection, and Free Fall detection are all based on the Sample Rate. + * The Sample Rate is generated by dividing the gyroscope output rate by + * SMPLRT_DIV: + * + * Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) + * + * where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or + * 7), and 1kHz when the DLPF is enabled (see Register 26). + * + * Note: The accelerometer output rate is 1kHz. This means that for a Sample + * Rate greater than 1kHz, the same accelerometer sample may be output to the + * FIFO, DMP, and sensor registers more than once. + * + * For a diagram of the gyroscope and accelerometer signal paths, see Section 8 + * of the MPU-6000/MPU-6050 Product Specification document. + * + * @return Current sample rate + * @see MPU6050_RA_SMPLRT_DIV + */ +uint8_t MPU6050::getRate() { + I2Cdev::readByte(devAddr, MPU6050_RA_SMPLRT_DIV, buffer); + return buffer[0]; +} +/** Set gyroscope sample rate divider. + * @param rate New sample rate divider + * @see getRate() + * @see MPU6050_RA_SMPLRT_DIV + */ +void MPU6050::setRate(uint8_t rate) { + I2Cdev::writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, rate); +} + +// CONFIG register + +/** Get external FSYNC configuration. + * Configures the external Frame Synchronization (FSYNC) pin sampling. An + * external signal connected to the FSYNC pin can be sampled by configuring + * EXT_SYNC_SET. Signal changes to the FSYNC pin are latched so that short + * strobes may be captured. The latched FSYNC signal will be sampled at the + * Sampling Rate, as defined in register 25. After sampling, the latch will + * reset to the current FSYNC signal state. + * + * The sampled value will be reported in place of the least significant bit in + * a sensor data register determined by the value of EXT_SYNC_SET according to + * the following table. + * + * 
+ * EXT_SYNC_SET | FSYNC Bit Location
+ * -------------+-------------------
+ * 0            | Input disabled
+ * 1            | TEMP_OUT_L[0]
+ * 2            | GYRO_XOUT_L[0]
+ * 3            | GYRO_YOUT_L[0]
+ * 4            | GYRO_ZOUT_L[0]
+ * 5            | ACCEL_XOUT_L[0]
+ * 6            | ACCEL_YOUT_L[0]
+ * 7            | ACCEL_ZOUT_L[0]
+ *
+ * + * @return FSYNC configuration value + */ +uint8_t MPU6050::getExternalFrameSync() { + I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer); + return buffer[0]; +} +/** Set external FSYNC configuration. + * @see getExternalFrameSync() + * @see MPU6050_RA_CONFIG + * @param sync New FSYNC configuration value + */ +void MPU6050::setExternalFrameSync(uint8_t sync) { + I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync); +} +/** Get digital low-pass filter configuration. + * The DLPF_CFG parameter sets the digital low pass filter configuration. It + * also determines the internal sampling rate used by the device as shown in + * the table below. + * + * Note: The accelerometer output rate is 1kHz. This means that for a Sample + * Rate greater than 1kHz, the same accelerometer sample may be output to the + * FIFO, DMP, and sensor registers more than once. + * + * 
+ *          |   ACCELEROMETER    |           GYROSCOPE
+ * DLPF_CFG | Bandwidth | Delay  | Bandwidth | Delay  | Sample Rate
+ * ---------+-----------+--------+-----------+--------+-------------
+ * 0        | 260Hz     | 0ms    | 256Hz     | 0.98ms | 8kHz
+ * 1        | 184Hz     | 2.0ms  | 188Hz     | 1.9ms  | 1kHz
+ * 2        | 94Hz      | 3.0ms  | 98Hz      | 2.8ms  | 1kHz
+ * 3        | 44Hz      | 4.9ms  | 42Hz      | 4.8ms  | 1kHz
+ * 4        | 21Hz      | 8.5ms  | 20Hz      | 8.3ms  | 1kHz
+ * 5        | 10Hz      | 13.8ms | 10Hz      | 13.4ms | 1kHz
+ * 6        | 5Hz       | 19.0ms | 5Hz       | 18.6ms | 1kHz
+ * 7        |   -- Reserved --   |   -- Reserved --   | Reserved
+ *
+ * + * @return DLFP configuration + * @see MPU6050_RA_CONFIG + * @see MPU6050_CFG_DLPF_CFG_BIT + * @see MPU6050_CFG_DLPF_CFG_LENGTH + */ +uint8_t MPU6050::getDLPFMode() { + I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, buffer); + return buffer[0]; +} +/** Set digital low-pass filter configuration. + * @param mode New DLFP configuration setting + * @see getDLPFBandwidth() + * @see MPU6050_DLPF_BW_256 + * @see MPU6050_RA_CONFIG + * @see MPU6050_CFG_DLPF_CFG_BIT + * @see MPU6050_CFG_DLPF_CFG_LENGTH + */ +void MPU6050::setDLPFMode(uint8_t mode) { + I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode); +} + +// GYRO_CONFIG register + +/** Get full-scale gyroscope range. + * The FS_SEL parameter allows setting the full-scale range of the gyro sensors, + * as described in the table below. + * + * 
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ *
+ * + * @return Current full-scale gyroscope range setting + * @see MPU6050_GYRO_FS_250 + * @see MPU6050_RA_GYRO_CONFIG + * @see MPU6050_GCONFIG_FS_SEL_BIT + * @see MPU6050_GCONFIG_FS_SEL_LENGTH + */ +uint8_t MPU6050::getFullScaleGyroRange() { + I2Cdev::readBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, buffer); + return buffer[0]; +} +/** Set full-scale gyroscope range. + * @param range New full-scale gyroscope range value + * @see getFullScaleRange() + * @see MPU6050_GYRO_FS_250 + * @see MPU6050_RA_GYRO_CONFIG + * @see MPU6050_GCONFIG_FS_SEL_BIT + * @see MPU6050_GCONFIG_FS_SEL_LENGTH + */ +void MPU6050::setFullScaleGyroRange(uint8_t range) { + I2Cdev::writeBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range); +} + +// SELF TEST FACTORY TRIM VALUES + +/** Get self-test factory trim value for accelerometer X axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_X + */ +uint8_t MPU6050::getAccelXSelfTestFactoryTrim() { + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, &buffer[0]); + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1]); + return (buffer[0]>>3) | ((buffer[1]>>4) & 0x03); +} + +/** Get self-test factory trim value for accelerometer Y axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_Y + */ +uint8_t MPU6050::getAccelYSelfTestFactoryTrim() { + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, &buffer[0]); + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1]); + return (buffer[0]>>3) | ((buffer[1]>>2) & 0x03); +} + +/** Get self-test factory trim value for accelerometer Z axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_Z + */ +uint8_t MPU6050::getAccelZSelfTestFactoryTrim() { + I2Cdev::readBytes(devAddr, MPU6050_RA_SELF_TEST_Z, 2, buffer); + return (buffer[0]>>3) | (buffer[1] & 0x03); +} + +/** Get self-test factory trim value for gyro X axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_X + */ +uint8_t MPU6050::getGyroXSelfTestFactoryTrim() { + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, buffer); + return (buffer[0] & 0x1F); +} + +/** Get self-test factory trim value for gyro Y axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_Y + */ +uint8_t MPU6050::getGyroYSelfTestFactoryTrim() { + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, buffer); + return (buffer[0] & 0x1F); +} + +/** Get self-test factory trim value for gyro Z axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_Z + */ +uint8_t MPU6050::getGyroZSelfTestFactoryTrim() { + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Z, buffer); + return (buffer[0] & 0x1F); +} + +// ACCEL_CONFIG register + +/** Get self-test enabled setting for accelerometer X axis. + * @return Self-test enabled value + * @see MPU6050_RA_ACCEL_CONFIG + */ +bool MPU6050::getAccelXSelfTest() { + I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, buffer); + return buffer[0]; +} +/** Get self-test enabled setting for accelerometer X axis. + * @param enabled Self-test enabled value + * @see MPU6050_RA_ACCEL_CONFIG + */ +void MPU6050::setAccelXSelfTest(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, enabled); +} +/** Get self-test enabled value for accelerometer Y axis. + * @return Self-test enabled value + * @see MPU6050_RA_ACCEL_CONFIG + */ +bool MPU6050::getAccelYSelfTest() { + I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, buffer); + return buffer[0]; +} +/** Get self-test enabled value for accelerometer Y axis. + * @param enabled Self-test enabled value + * @see MPU6050_RA_ACCEL_CONFIG + */ +void MPU6050::setAccelYSelfTest(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, enabled); +} +/** Get self-test enabled value for accelerometer Z axis. + * @return Self-test enabled value + * @see MPU6050_RA_ACCEL_CONFIG + */ +bool MPU6050::getAccelZSelfTest() { + I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, buffer); + return buffer[0]; +} +/** Set self-test enabled value for accelerometer Z axis. + * @param enabled Self-test enabled value + * @see MPU6050_RA_ACCEL_CONFIG + */ +void MPU6050::setAccelZSelfTest(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, enabled); +} +/** Get full-scale accelerometer range. + * The FS_SEL parameter allows setting the full-scale range of the accelerometer + * sensors, as described in the table below. + * + * 
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ *
+ * + * @return Current full-scale accelerometer range setting + * @see MPU6050_ACCEL_FS_2 + * @see MPU6050_RA_ACCEL_CONFIG + * @see MPU6050_ACONFIG_AFS_SEL_BIT + * @see MPU6050_ACONFIG_AFS_SEL_LENGTH + */ +uint8_t MPU6050::getFullScaleAccelRange() { + I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, buffer); + return buffer[0]; +} +/** Set full-scale accelerometer range. + * @param range New full-scale accelerometer range setting + * @see getFullScaleAccelRange() + */ +void MPU6050::setFullScaleAccelRange(uint8_t range) { + I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range); +} +/** Get the high-pass filter configuration. + * The DHPF is a filter module in the path leading to motion detectors (Free + * Fall, Motion threshold, and Zero Motion). The high pass filter output is not + * available to the data registers (see Figure in Section 8 of the MPU-6000/ + * MPU-6050 Product Specification document). + * + * The high pass filter has three modes: + * + * 
+ *    Reset: The filter output settles to zero within one sample. This
+ *           effectively disables the high pass filter. This mode may be toggled
+ *           to quickly settle the filter.
+ *
+ *    On:    The high pass filter will pass signals above the cut off frequency.
+ *
+ *    Hold:  When triggered, the filter holds the present sample. The filter
+ *           output will be the difference between the input sample and the held
+ *           sample.
+ *
+ * + * 
+ * ACCEL_HPF | Filter Mode | Cut-off Frequency
+ * ----------+-------------+------------------
+ * 0         | Reset       | None
+ * 1         | On          | 5Hz
+ * 2         | On          | 2.5Hz
+ * 3         | On          | 1.25Hz
+ * 4         | On          | 0.63Hz
+ * 7         | Hold        | None
+ *
+ * + * @return Current high-pass filter configuration + * @see MPU6050_DHPF_RESET + * @see MPU6050_RA_ACCEL_CONFIG + */ +uint8_t MPU6050::getDHPFMode() { + I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, buffer); + return buffer[0]; +} +/** Set the high-pass filter configuration. + * @param bandwidth New high-pass filter configuration + * @see setDHPFMode() + * @see MPU6050_DHPF_RESET + * @see MPU6050_RA_ACCEL_CONFIG + */ +void MPU6050::setDHPFMode(uint8_t bandwidth) { + I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, bandwidth); +} + +// FF_THR register + +/** Get free-fall event acceleration threshold. + * This register configures the detection threshold for Free Fall event + * detection. The unit of FF_THR is 1LSB = 2mg. Free Fall is detected when the + * absolute value of the accelerometer measurements for the three axes are each + * less than the detection threshold. This condition increments the Free Fall + * duration counter (Register 30). The Free Fall interrupt is triggered when the + * Free Fall duration counter reaches the time specified in FF_DUR. + * + * For more details on the Free Fall detection interrupt, see Section 8.2 of the + * MPU-6000/MPU-6050 Product Specification document as well as Registers 56 and + * 58 of this document. + * + * @return Current free-fall acceleration threshold value (LSB = 2mg) + * @see MPU6050_RA_FF_THR + */ +uint8_t MPU6050::getFreefallDetectionThreshold() { + I2Cdev::readByte(devAddr, MPU6050_RA_FF_THR, buffer); + return buffer[0]; +} +/** Get free-fall event acceleration threshold. + * @param threshold New free-fall acceleration threshold value (LSB = 2mg) + * @see getFreefallDetectionThreshold() + * @see MPU6050_RA_FF_THR + */ +void MPU6050::setFreefallDetectionThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, MPU6050_RA_FF_THR, threshold); +} + +// FF_DUR register + +/** Get free-fall event duration threshold. + * This register configures the duration counter threshold for Free Fall event + * detection. The duration counter ticks at 1kHz, therefore FF_DUR has a unit + * of 1 LSB = 1 ms. + * + * The Free Fall duration counter increments while the absolute value of the + * accelerometer measurements are each less than the detection threshold + * (Register 29). The Free Fall interrupt is triggered when the Free Fall + * duration counter reaches the time specified in this register. + * + * For more details on the Free Fall detection interrupt, see Section 8.2 of + * the MPU-6000/MPU-6050 Product Specification document as well as Registers 56 + * and 58 of this document. + * + * @return Current free-fall duration threshold value (LSB = 1ms) + * @see MPU6050_RA_FF_DUR + */ +uint8_t MPU6050::getFreefallDetectionDuration() { + I2Cdev::readByte(devAddr, MPU6050_RA_FF_DUR, buffer); + return buffer[0]; +} +/** Get free-fall event duration threshold. + * @param duration New free-fall duration threshold value (LSB = 1ms) + * @see getFreefallDetectionDuration() + * @see MPU6050_RA_FF_DUR + */ +void MPU6050::setFreefallDetectionDuration(uint8_t duration) { + I2Cdev::writeByte(devAddr, MPU6050_RA_FF_DUR, duration); +} + +// MOT_THR register + +/** Get motion detection event acceleration threshold. + * This register configures the detection threshold for Motion interrupt + * generation. The unit of MOT_THR is 1LSB = 2mg. Motion is detected when the + * absolute value of any of the accelerometer measurements exceeds this Motion + * detection threshold. This condition increments the Motion detection duration + * counter (Register 32). The Motion detection interrupt is triggered when the + * Motion Detection counter reaches the time count specified in MOT_DUR + * (Register 32). + * + * The Motion interrupt will indicate the axis and polarity of detected motion + * in MOT_DETECT_STATUS (Register 97). + * + * For more details on the Motion detection interrupt, see Section 8.3 of the + * MPU-6000/MPU-6050 Product Specification document as well as Registers 56 and + * 58 of this document. + * + * @return Current motion detection acceleration threshold value (LSB = 2mg) + * @see MPU6050_RA_MOT_THR + */ +uint8_t MPU6050::getMotionDetectionThreshold() { + I2Cdev::readByte(devAddr, MPU6050_RA_MOT_THR, buffer); + return buffer[0]; +} +/** Set motion detection event acceleration threshold. + * @param threshold New motion detection acceleration threshold value (LSB = 2mg) + * @see getMotionDetectionThreshold() + * @see MPU6050_RA_MOT_THR + */ +void MPU6050::setMotionDetectionThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_THR, threshold); +} + +// MOT_DUR register + +/** Get motion detection event duration threshold. + * This register configures the duration counter threshold for Motion interrupt + * generation. The duration counter ticks at 1 kHz, therefore MOT_DUR has a unit + * of 1LSB = 1ms. The Motion detection duration counter increments when the + * absolute value of any of the accelerometer measurements exceeds the Motion + * detection threshold (Register 31). The Motion detection interrupt is + * triggered when the Motion detection counter reaches the time count specified + * in this register. + * + * For more details on the Motion detection interrupt, see Section 8.3 of the + * MPU-6000/MPU-6050 Product Specification document. + * + * @return Current motion detection duration threshold value (LSB = 1ms) + * @see MPU6050_RA_MOT_DUR + */ +uint8_t MPU6050::getMotionDetectionDuration() { + I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DUR, buffer); + return buffer[0]; +} +/** Set motion detection event duration threshold. + * @param duration New motion detection duration threshold value (LSB = 1ms) + * @see getMotionDetectionDuration() + * @see MPU6050_RA_MOT_DUR + */ +void MPU6050::setMotionDetectionDuration(uint8_t duration) { + I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_DUR, duration); +} + +// ZRMOT_THR register + +/** Get zero motion detection event acceleration threshold. + * This register configures the detection threshold for Zero Motion interrupt + * generation. The unit of ZRMOT_THR is 1LSB = 2mg. Zero Motion is detected when + * the absolute value of the accelerometer measurements for the 3 axes are each + * less than the detection threshold. This condition increments the Zero Motion + * duration counter (Register 34). The Zero Motion interrupt is triggered when + * the Zero Motion duration counter reaches the time count specified in + * ZRMOT_DUR (Register 34). + * + * Unlike Free Fall or Motion detection, Zero Motion detection triggers an + * interrupt both when Zero Motion is first detected and when Zero Motion is no + * longer detected. + * + * When a zero motion event is detected, a Zero Motion Status will be indicated + * in the MOT_DETECT_STATUS register (Register 97). When a motion-to-zero-motion + * condition is detected, the status bit is set to 1. When a zero-motion-to- + * motion condition is detected, the status bit is set to 0. + * + * For more details on the Zero Motion detection interrupt, see Section 8.4 of + * the MPU-6000/MPU-6050 Product Specification document as well as Registers 56 + * and 58 of this document. + * + * @return Current zero motion detection acceleration threshold value (LSB = 2mg) + * @see MPU6050_RA_ZRMOT_THR + */ +uint8_t MPU6050::getZeroMotionDetectionThreshold() { + I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_THR, buffer); + return buffer[0]; +} +/** Set zero motion detection event acceleration threshold. + * @param threshold New zero motion detection acceleration threshold value (LSB = 2mg) + * @see getZeroMotionDetectionThreshold() + * @see MPU6050_RA_ZRMOT_THR + */ +void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_THR, threshold); +} + +// ZRMOT_DUR register + +/** Get zero motion detection event duration threshold. + * This register configures the duration counter threshold for Zero Motion + * interrupt generation. The duration counter ticks at 16 Hz, therefore + * ZRMOT_DUR has a unit of 1 LSB = 64 ms. The Zero Motion duration counter + * increments while the absolute value of the accelerometer measurements are + * each less than the detection threshold (Register 33). The Zero Motion + * interrupt is triggered when the Zero Motion duration counter reaches the time + * count specified in this register. + * + * For more details on the Zero Motion detection interrupt, see Section 8.4 of + * the MPU-6000/MPU-6050 Product Specification document, as well as Registers 56 + * and 58 of this document. + * + * @return Current zero motion detection duration threshold value (LSB = 64ms) + * @see MPU6050_RA_ZRMOT_DUR + */ +uint8_t MPU6050::getZeroMotionDetectionDuration() { + I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_DUR, buffer); + return buffer[0]; +} +/** Set zero motion detection event duration threshold. + * @param duration New zero motion detection duration threshold value (LSB = 1ms) + * @see getZeroMotionDetectionDuration() + * @see MPU6050_RA_ZRMOT_DUR + */ +void MPU6050::setZeroMotionDetectionDuration(uint8_t duration) { + I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_DUR, duration); +} + +// FIFO_EN register + +/** Get temperature FIFO enabled value. + * When set to 1, this bit enables TEMP_OUT_H and TEMP_OUT_L (Registers 65 and + * 66) to be written into the FIFO buffer. + * @return Current temperature FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getTempFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set temperature FIFO enabled value. + * @param enabled New temperature FIFO enabled value + * @see getTempFIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setTempFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, enabled); +} +/** Get gyroscope X-axis FIFO enabled value. + * When set to 1, this bit enables GYRO_XOUT_H and GYRO_XOUT_L (Registers 67 and + * 68) to be written into the FIFO buffer. + * @return Current gyroscope X-axis FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getXGyroFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set gyroscope X-axis FIFO enabled value. + * @param enabled New gyroscope X-axis FIFO enabled value + * @see getXGyroFIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setXGyroFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, enabled); +} +/** Get gyroscope Y-axis FIFO enabled value. + * When set to 1, this bit enables GYRO_YOUT_H and GYRO_YOUT_L (Registers 69 and + * 70) to be written into the FIFO buffer. + * @return Current gyroscope Y-axis FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getYGyroFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set gyroscope Y-axis FIFO enabled value. + * @param enabled New gyroscope Y-axis FIFO enabled value + * @see getYGyroFIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setYGyroFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, enabled); +} +/** Get gyroscope Z-axis FIFO enabled value. + * When set to 1, this bit enables GYRO_ZOUT_H and GYRO_ZOUT_L (Registers 71 and + * 72) to be written into the FIFO buffer. + * @return Current gyroscope Z-axis FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getZGyroFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set gyroscope Z-axis FIFO enabled value. + * @param enabled New gyroscope Z-axis FIFO enabled value + * @see getZGyroFIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setZGyroFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, enabled); +} +/** Get accelerometer FIFO enabled value. + * When set to 1, this bit enables ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H, + * ACCEL_YOUT_L, ACCEL_ZOUT_H, and ACCEL_ZOUT_L (Registers 59 to 64) to be + * written into the FIFO buffer. + * @return Current accelerometer FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getAccelFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set accelerometer FIFO enabled value. + * @param enabled New accelerometer FIFO enabled value + * @see getAccelFIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setAccelFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, enabled); +} +/** Get Slave 2 FIFO enabled value. + * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) + * associated with Slave 2 to be written into the FIFO buffer. + * @return Current Slave 2 FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getSlave2FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set Slave 2 FIFO enabled value. + * @param enabled New Slave 2 FIFO enabled value + * @see getSlave2FIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setSlave2FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, enabled); +} +/** Get Slave 1 FIFO enabled value. + * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) + * associated with Slave 1 to be written into the FIFO buffer. + * @return Current Slave 1 FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getSlave1FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set Slave 1 FIFO enabled value. + * @param enabled New Slave 1 FIFO enabled value + * @see getSlave1FIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setSlave1FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, enabled); +} +/** Get Slave 0 FIFO enabled value. + * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) + * associated with Slave 0 to be written into the FIFO buffer. + * @return Current Slave 0 FIFO enabled value + * @see MPU6050_RA_FIFO_EN + */ +bool MPU6050::getSlave0FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set Slave 0 FIFO enabled value. + * @param enabled New Slave 0 FIFO enabled value + * @see getSlave0FIFOEnabled() + * @see MPU6050_RA_FIFO_EN + */ +void MPU6050::setSlave0FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, enabled); +} + +// I2C_MST_CTRL register + +/** Get multi-master enabled value. + * Multi-master capability allows multiple I2C masters to operate on the same + * bus. In circuits where multi-master capability is required, set MULT_MST_EN + * to 1. This will increase current drawn by approximately 30uA. + * + * In circuits where multi-master capability is required, the state of the I2C + * bus must always be monitored by each separate I2C Master. Before an I2C + * Master can assume arbitration of the bus, it must first confirm that no other + * I2C Master has arbitration of the bus. When MULT_MST_EN is set to 1, the + * MPU-60X0's bus arbitration detection logic is turned on, enabling it to + * detect when the bus is available. + * + * @return Current multi-master enabled value + * @see MPU6050_RA_I2C_MST_CTRL + */ +bool MPU6050::getMultiMasterEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, buffer); + return buffer[0]; +} +/** Set multi-master enabled value. + * @param enabled New multi-master enabled value + * @see getMultiMasterEnabled() + * @see MPU6050_RA_I2C_MST_CTRL + */ +void MPU6050::setMultiMasterEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, enabled); +} +/** Get wait-for-external-sensor-data enabled value. + * When the WAIT_FOR_ES bit is set to 1, the Data Ready interrupt will be + * delayed until External Sensor data from the Slave Devices are loaded into the + * EXT_SENS_DATA registers. This is used to ensure that both the internal sensor + * data (i.e. from gyro and accel) and external sensor data have been loaded to + * their respective data registers (i.e. the data is synced) when the Data Ready + * interrupt is triggered. + * + * @return Current wait-for-external-sensor-data enabled value + * @see MPU6050_RA_I2C_MST_CTRL + */ +bool MPU6050::getWaitForExternalSensorEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, buffer); + return buffer[0]; +} +/** Set wait-for-external-sensor-data enabled value. + * @param enabled New wait-for-external-sensor-data enabled value + * @see getWaitForExternalSensorEnabled() + * @see MPU6050_RA_I2C_MST_CTRL + */ +void MPU6050::setWaitForExternalSensorEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, enabled); +} +/** Get Slave 3 FIFO enabled value. + * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) + * associated with Slave 3 to be written into the FIFO buffer. + * @return Current Slave 3 FIFO enabled value + * @see MPU6050_RA_MST_CTRL + */ +bool MPU6050::getSlave3FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set Slave 3 FIFO enabled value. + * @param enabled New Slave 3 FIFO enabled value + * @see getSlave3FIFOEnabled() + * @see MPU6050_RA_MST_CTRL + */ +void MPU6050::setSlave3FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, enabled); +} +/** Get slave read/write transition enabled value. + * The I2C_MST_P_NSR bit configures the I2C Master's transition from one slave + * read to the next slave read. If the bit equals 0, there will be a restart + * between reads. If the bit equals 1, there will be a stop followed by a start + * of the following read. When a write transaction follows a read transaction, + * the stop followed by a start of the successive write will be always used. + * + * @return Current slave read/write transition enabled value + * @see MPU6050_RA_I2C_MST_CTRL + */ +bool MPU6050::getSlaveReadWriteTransitionEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, buffer); + return buffer[0]; +} +/** Set slave read/write transition enabled value. + * @param enabled New slave read/write transition enabled value + * @see getSlaveReadWriteTransitionEnabled() + * @see MPU6050_RA_I2C_MST_CTRL + */ +void MPU6050::setSlaveReadWriteTransitionEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, enabled); +} +/** Get I2C master clock speed. + * I2C_MST_CLK is a 4 bit unsigned value which configures a divider on the + * MPU-60X0 internal 8MHz clock. It sets the I2C master clock speed according to + * the following table: + * + * 
+ * I2C_MST_CLK | I2C Master Clock Speed | 8MHz Clock Divider
+ * ------------+------------------------+-------------------
+ * 0           | 348kHz                 | 23
+ * 1           | 333kHz                 | 24
+ * 2           | 320kHz                 | 25
+ * 3           | 308kHz                 | 26
+ * 4           | 296kHz                 | 27
+ * 5           | 286kHz                 | 28
+ * 6           | 276kHz                 | 29
+ * 7           | 267kHz                 | 30
+ * 8           | 258kHz                 | 31
+ * 9           | 500kHz                 | 16
+ * 10          | 471kHz                 | 17
+ * 11          | 444kHz                 | 18
+ * 12          | 421kHz                 | 19
+ * 13          | 400kHz                 | 20
+ * 14          | 381kHz                 | 21
+ * 15          | 364kHz                 | 22
+ *
+ * + * @return Current I2C master clock speed + * @see MPU6050_RA_I2C_MST_CTRL + */ +uint8_t MPU6050::getMasterClockSpeed() { + I2Cdev::readBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, buffer); + return buffer[0]; +} +/** Set I2C master clock speed. + * @reparam speed Current I2C master clock speed + * @see MPU6050_RA_I2C_MST_CTRL + */ +void MPU6050::setMasterClockSpeed(uint8_t speed) { + I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, speed); +} + +// I2C_SLV* registers (Slave 0-3) + +/** Get the I2C address of the specified slave (0-3). + * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read + * operation, and if it is cleared, then it's a write operation. The remaining + * bits (6-0) are the 7-bit device address of the slave device. + * + * In read mode, the result of the read is placed in the lowest available + * EXT_SENS_DATA register. For further information regarding the allocation of + * read results, please refer to the EXT_SENS_DATA register description + * (Registers 73 - 96). + * + * The MPU-6050 supports a total of five slaves, but Slave 4 has unique + * characteristics, and so it has its own functions (getSlave4* and setSlave4*). + * + * I2C data transactions are performed at the Sample Rate, as defined in + * Register 25. The user is responsible for ensuring that I2C data transactions + * to and from each enabled Slave can be completed within a single period of the + * Sample Rate. + * + * The I2C slave access rate can be reduced relative to the Sample Rate. This + * reduced access rate is determined by I2C_MST_DLY (Register 52). Whether a + * slave's access rate is reduced relative to the Sample Rate is determined by + * I2C_MST_DELAY_CTRL (Register 103). + * + * The processing order for the slaves is fixed. The sequence followed for + * processing the slaves is Slave 0, Slave 1, Slave 2, Slave 3 and Slave 4. If a + * particular Slave is disabled it will be skipped. + * + * Each slave can either be accessed at the sample rate or at a reduced sample + * rate. In a case where some slaves are accessed at the Sample Rate and some + * slaves are accessed at the reduced rate, the sequence of accessing the slaves + * (Slave 0 to Slave 4) is still followed. However, the reduced rate slaves will + * be skipped if their access rate dictates that they should not be accessed + * during that particular cycle. For further information regarding the reduced + * access rate, please refer to Register 52. Whether a slave is accessed at the + * Sample Rate or at the reduced rate is determined by the Delay Enable bits in + * Register 103. + * + * @param num Slave number (0-3) + * @return Current address for specified slave + * @see MPU6050_RA_I2C_SLV0_ADDR + */ +uint8_t MPU6050::getSlaveAddress(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, buffer); + return buffer[0]; +} +/** Set the I2C address of the specified slave (0-3). + * @param num Slave number (0-3) + * @param address New address for specified slave + * @see getSlaveAddress() + * @see MPU6050_RA_I2C_SLV0_ADDR + */ +void MPU6050::setSlaveAddress(uint8_t num, uint8_t address) { + if (num > 3) return; + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, address); +} +/** Get the active internal register for the specified slave (0-3). + * Read/write operations for this slave will be done to whatever internal + * register address is stored in this MPU register. + * + * The MPU-6050 supports a total of five slaves, but Slave 4 has unique + * characteristics, and so it has its own functions. + * + * @param num Slave number (0-3) + * @return Current active register for specified slave + * @see MPU6050_RA_I2C_SLV0_REG + */ +uint8_t MPU6050::getSlaveRegister(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, buffer); + return buffer[0]; +} +/** Set the active internal register for the specified slave (0-3). + * @param num Slave number (0-3) + * @param reg New active register for specified slave + * @see getSlaveRegister() + * @see MPU6050_RA_I2C_SLV0_REG + */ +void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg) { + if (num > 3) return; + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, reg); +} +/** Get the enabled value for the specified slave (0-3). + * When set to 1, this bit enables Slave 0 for data transfer operations. When + * cleared to 0, this bit disables Slave 0 from data transfer operations. + * @param num Slave number (0-3) + * @return Current enabled value for specified slave + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +bool MPU6050::getSlaveEnabled(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, buffer); + return buffer[0]; +} +/** Set the enabled value for the specified slave (0-3). + * @param num Slave number (0-3) + * @param enabled New enabled value for specified slave + * @see getSlaveEnabled() + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +void MPU6050::setSlaveEnabled(uint8_t num, bool enabled) { + if (num > 3) return; + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, enabled); +} +/** Get word pair byte-swapping enabled for the specified slave (0-3). + * When set to 1, this bit enables byte swapping. When byte swapping is enabled, + * the high and low bytes of a word pair are swapped. Please refer to + * I2C_SLV0_GRP for the pairing convention of the word pairs. When cleared to 0, + * bytes transferred to and from Slave 0 will be written to EXT_SENS_DATA + * registers in the order they were transferred. + * + * @param num Slave number (0-3) + * @return Current word pair byte-swapping enabled value for specified slave + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +bool MPU6050::getSlaveWordByteSwap(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, buffer); + return buffer[0]; +} +/** Set word pair byte-swapping enabled for the specified slave (0-3). + * @param num Slave number (0-3) + * @param enabled New word pair byte-swapping enabled value for specified slave + * @see getSlaveWordByteSwap() + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled) { + if (num > 3) return; + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled); +} +/** Get write mode for the specified slave (0-3). + * When set to 1, the transaction will read or write data only. When cleared to + * 0, the transaction will write a register address prior to reading or writing + * data. This should equal 0 when specifying the register address within the + * Slave device to/from which the ensuing data transaction will take place. + * + * @param num Slave number (0-3) + * @return Current write mode for specified slave (0 = register address + data, 1 = data only) + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +bool MPU6050::getSlaveWriteMode(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, buffer); + return buffer[0]; +} +/** Set write mode for the specified slave (0-3). + * @param num Slave number (0-3) + * @param mode New write mode for specified slave (0 = register address + data, 1 = data only) + * @see getSlaveWriteMode() + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +void MPU6050::setSlaveWriteMode(uint8_t num, bool mode) { + if (num > 3) return; + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, mode); +} +/** Get word pair grouping order offset for the specified slave (0-3). + * This sets specifies the grouping order of word pairs received from registers. + * When cleared to 0, bytes from register addresses 0 and 1, 2 and 3, etc (even, + * then odd register addresses) are paired to form a word. When set to 1, bytes + * from register addresses are paired 1 and 2, 3 and 4, etc. (odd, then even + * register addresses) are paired to form a word. + * + * @param num Slave number (0-3) + * @return Current word pair grouping order offset for specified slave + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +bool MPU6050::getSlaveWordGroupOffset(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, buffer); + return buffer[0]; +} +/** Set word pair grouping order offset for the specified slave (0-3). + * @param num Slave number (0-3) + * @param enabled New word pair grouping order offset for specified slave + * @see getSlaveWordGroupOffset() + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled) { + if (num > 3) return; + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, enabled); +} +/** Get number of bytes to read for the specified slave (0-3). + * Specifies the number of bytes transferred to and from Slave 0. Clearing this + * bit to 0 is equivalent to disabling the register by writing 0 to I2C_SLV0_EN. + * @param num Slave number (0-3) + * @return Number of bytes to read for specified slave + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +uint8_t MPU6050::getSlaveDataLength(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, buffer); + return buffer[0]; +} +/** Set number of bytes to read for the specified slave (0-3). + * @param num Slave number (0-3) + * @param length Number of bytes to read for specified slave + * @see getSlaveDataLength() + * @see MPU6050_RA_I2C_SLV0_CTRL + */ +void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length) { + if (num > 3) return; + I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, length); +} + +// I2C_SLV* registers (Slave 4) + +/** Get the I2C address of Slave 4. + * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read + * operation, and if it is cleared, then it's a write operation. The remaining + * bits (6-0) are the 7-bit device address of the slave device. + * + * @return Current address for Slave 4 + * @see getSlaveAddress() + * @see MPU6050_RA_I2C_SLV4_ADDR + */ +uint8_t MPU6050::getSlave4Address() { + I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, buffer); + return buffer[0]; +} +/** Set the I2C address of Slave 4. + * @param address New address for Slave 4 + * @see getSlave4Address() + * @see MPU6050_RA_I2C_SLV4_ADDR + */ +void MPU6050::setSlave4Address(uint8_t address) { + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, address); +} +/** Get the active internal register for the Slave 4. + * Read/write operations for this slave will be done to whatever internal + * register address is stored in this MPU register. + * + * @return Current active register for Slave 4 + * @see MPU6050_RA_I2C_SLV4_REG + */ +uint8_t MPU6050::getSlave4Register() { + I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_REG, buffer); + return buffer[0]; +} +/** Set the active internal register for Slave 4. + * @param reg New active register for Slave 4 + * @see getSlave4Register() + * @see MPU6050_RA_I2C_SLV4_REG + */ +void MPU6050::setSlave4Register(uint8_t reg) { + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_REG, reg); +} +/** Set new byte to write to Slave 4. + * This register stores the data to be written into the Slave 4. If I2C_SLV4_RW + * is set 1 (set to read), this register has no effect. + * @param data New byte to write to Slave 4 + * @see MPU6050_RA_I2C_SLV4_DO + */ +void MPU6050::setSlave4OutputByte(uint8_t data) { + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_DO, data); +} +/** Get the enabled value for the Slave 4. + * When set to 1, this bit enables Slave 4 for data transfer operations. When + * cleared to 0, this bit disables Slave 4 from data transfer operations. + * @return Current enabled value for Slave 4 + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +bool MPU6050::getSlave4Enabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, buffer); + return buffer[0]; +} +/** Set the enabled value for Slave 4. + * @param enabled New enabled value for Slave 4 + * @see getSlave4Enabled() + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +void MPU6050::setSlave4Enabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, enabled); +} +/** Get the enabled value for Slave 4 transaction interrupts. + * When set to 1, this bit enables the generation of an interrupt signal upon + * completion of a Slave 4 transaction. When cleared to 0, this bit disables the + * generation of an interrupt signal upon completion of a Slave 4 transaction. + * The interrupt status can be observed in Register 54. + * + * @return Current enabled value for Slave 4 transaction interrupts. + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +bool MPU6050::getSlave4InterruptEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, buffer); + return buffer[0]; +} +/** Set the enabled value for Slave 4 transaction interrupts. + * @param enabled New enabled value for Slave 4 transaction interrupts. + * @see getSlave4InterruptEnabled() + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +void MPU6050::setSlave4InterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, enabled); +} +/** Get write mode for Slave 4. + * When set to 1, the transaction will read or write data only. When cleared to + * 0, the transaction will write a register address prior to reading or writing + * data. This should equal 0 when specifying the register address within the + * Slave device to/from which the ensuing data transaction will take place. + * + * @return Current write mode for Slave 4 (0 = register address + data, 1 = data only) + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +bool MPU6050::getSlave4WriteMode() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, buffer); + return buffer[0]; +} +/** Set write mode for the Slave 4. + * @param mode New write mode for Slave 4 (0 = register address + data, 1 = data only) + * @see getSlave4WriteMode() + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +void MPU6050::setSlave4WriteMode(bool mode) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mode); +} +/** Get Slave 4 master delay value. + * This configures the reduced access rate of I2C slaves relative to the Sample + * Rate. When a slave's access rate is decreased relative to the Sample Rate, + * the slave is accessed every: + * + * 1 / (1 + I2C_MST_DLY) samples + * + * This base Sample Rate in turn is determined by SMPLRT_DIV (register 25) and + * DLPF_CFG (register 26). Whether a slave's access rate is reduced relative to + * the Sample Rate is determined by I2C_MST_DELAY_CTRL (register 103). For + * further information regarding the Sample Rate, please refer to register 25. + * + * @return Current Slave 4 master delay value + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +uint8_t MPU6050::getSlave4MasterDelay() { + I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, buffer); + return buffer[0]; +} +/** Set Slave 4 master delay value. + * @param delay New Slave 4 master delay value + * @see getSlave4MasterDelay() + * @see MPU6050_RA_I2C_SLV4_CTRL + */ +void MPU6050::setSlave4MasterDelay(uint8_t delay) { + I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay); +} +/** Get last available byte read from Slave 4. + * This register stores the data read from Slave 4. This field is populated + * after a read transaction. + * @return Last available byte read from to Slave 4 + * @see MPU6050_RA_I2C_SLV4_DI + */ +uint8_t MPU6050::getSlate4InputByte() { + I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_DI, buffer); + return buffer[0]; +} + +// I2C_MST_STATUS register + +/** Get FSYNC interrupt status. + * This bit reflects the status of the FSYNC interrupt from an external device + * into the MPU-60X0. This is used as a way to pass an external interrupt + * through the MPU-60X0 to the host application processor. When set to 1, this + * bit will cause an interrupt if FSYNC_INT_EN is asserted in INT_PIN_CFG + * (Register 55). + * @return FSYNC interrupt status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getPassthroughStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, buffer); + return buffer[0]; +} +/** Get Slave 4 transaction done status. + * Automatically sets to 1 when a Slave 4 transaction has completed. This + * triggers an interrupt if the I2C_MST_INT_EN bit in the INT_ENABLE register + * (Register 56) is asserted and if the SLV_4_DONE_INT bit is asserted in the + * I2C_SLV4_CTRL register (Register 52). + * @return Slave 4 transaction done status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getSlave4IsDone() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, buffer); + return buffer[0]; +} +/** Get master arbitration lost status. + * This bit automatically sets to 1 when the I2C Master has lost arbitration of + * the auxiliary I2C bus (an error condition). This triggers an interrupt if the + * I2C_MST_INT_EN bit in the INT_ENABLE register (Register 56) is asserted. + * @return Master arbitration lost status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getLostArbitration() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, buffer); + return buffer[0]; +} +/** Get Slave 4 NACK status. + * This bit automatically sets to 1 when the I2C Master receives a NACK in a + * transaction with Slave 4. This triggers an interrupt if the I2C_MST_INT_EN + * bit in the INT_ENABLE register (Register 56) is asserted. + * @return Slave 4 NACK interrupt status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getSlave4Nack() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, buffer); + return buffer[0]; +} +/** Get Slave 3 NACK status. + * This bit automatically sets to 1 when the I2C Master receives a NACK in a + * transaction with Slave 3. This triggers an interrupt if the I2C_MST_INT_EN + * bit in the INT_ENABLE register (Register 56) is asserted. + * @return Slave 3 NACK interrupt status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getSlave3Nack() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, buffer); + return buffer[0]; +} +/** Get Slave 2 NACK status. + * This bit automatically sets to 1 when the I2C Master receives a NACK in a + * transaction with Slave 2. This triggers an interrupt if the I2C_MST_INT_EN + * bit in the INT_ENABLE register (Register 56) is asserted. + * @return Slave 2 NACK interrupt status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getSlave2Nack() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, buffer); + return buffer[0]; +} +/** Get Slave 1 NACK status. + * This bit automatically sets to 1 when the I2C Master receives a NACK in a + * transaction with Slave 1. This triggers an interrupt if the I2C_MST_INT_EN + * bit in the INT_ENABLE register (Register 56) is asserted. + * @return Slave 1 NACK interrupt status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getSlave1Nack() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, buffer); + return buffer[0]; +} +/** Get Slave 0 NACK status. + * This bit automatically sets to 1 when the I2C Master receives a NACK in a + * transaction with Slave 0. This triggers an interrupt if the I2C_MST_INT_EN + * bit in the INT_ENABLE register (Register 56) is asserted. + * @return Slave 0 NACK interrupt status + * @see MPU6050_RA_I2C_MST_STATUS + */ +bool MPU6050::getSlave0Nack() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, buffer); + return buffer[0]; +} + +// INT_PIN_CFG register + +/** Get interrupt logic level mode. + * Will be set 0 for active-high, 1 for active-low. + * @return Current interrupt mode (0=active-high, 1=active-low) + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_INT_LEVEL_BIT + */ +bool MPU6050::getInterruptMode() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, buffer); + return buffer[0]; +} +/** Set interrupt logic level mode. + * @param mode New interrupt mode (0=active-high, 1=active-low) + * @see getInterruptMode() + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_INT_LEVEL_BIT + */ +void MPU6050::setInterruptMode(bool mode) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mode); +} +/** Get interrupt drive mode. + * Will be set 0 for push-pull, 1 for open-drain. + * @return Current interrupt drive mode (0=push-pull, 1=open-drain) + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_INT_OPEN_BIT + */ +bool MPU6050::getInterruptDrive() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, buffer); + return buffer[0]; +} +/** Set interrupt drive mode. + * @param drive New interrupt drive mode (0=push-pull, 1=open-drain) + * @see getInterruptDrive() + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_INT_OPEN_BIT + */ +void MPU6050::setInterruptDrive(bool drive) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, drive); +} +/** Get interrupt latch mode. + * Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared. + * @return Current latch mode (0=50us-pulse, 1=latch-until-int-cleared) + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_LATCH_INT_EN_BIT + */ +bool MPU6050::getInterruptLatch() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, buffer); + return buffer[0]; +} +/** Set interrupt latch mode. + * @param latch New latch mode (0=50us-pulse, 1=latch-until-int-cleared) + * @see getInterruptLatch() + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_LATCH_INT_EN_BIT + */ +void MPU6050::setInterruptLatch(bool latch) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, latch); +} +/** Get interrupt latch clear mode. + * Will be set 0 for status-read-only, 1 for any-register-read. + * @return Current latch clear mode (0=status-read-only, 1=any-register-read) + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT + */ +bool MPU6050::getInterruptLatchClear() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, buffer); + return buffer[0]; +} +/** Set interrupt latch clear mode. + * @param clear New latch clear mode (0=status-read-only, 1=any-register-read) + * @see getInterruptLatchClear() + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT + */ +void MPU6050::setInterruptLatchClear(bool clear) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear); +} +/** Get FSYNC interrupt logic level mode. + * @return Current FSYNC interrupt mode (0=active-high, 1=active-low) + * @see getFSyncInterruptMode() + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT + */ +bool MPU6050::getFSyncInterruptLevel() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, buffer); + return buffer[0]; +} +/** Set FSYNC interrupt logic level mode. + * @param mode New FSYNC interrupt mode (0=active-high, 1=active-low) + * @see getFSyncInterruptMode() + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT + */ +void MPU6050::setFSyncInterruptLevel(bool level) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level); +} +/** Get FSYNC pin interrupt enabled setting. + * Will be set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled setting + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT + */ +bool MPU6050::getFSyncInterruptEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, buffer); + return buffer[0]; +} +/** Set FSYNC pin interrupt enabled setting. + * @param enabled New FSYNC pin interrupt enabled setting + * @see getFSyncInterruptEnabled() + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT + */ +void MPU6050::setFSyncInterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled); +} +/** Get I2C bypass enabled status. + * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to + * 0, the host application processor will be able to directly access the + * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host + * application processor will not be able to directly access the auxiliary I2C + * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106 + * bit[5]). + * @return Current I2C bypass enabled status + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT + */ +bool MPU6050::getI2CBypassEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, buffer); + return buffer[0]; +} +/** Set I2C bypass enabled status. + * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to + * 0, the host application processor will be able to directly access the + * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host + * application processor will not be able to directly access the auxiliary I2C + * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106 + * bit[5]). + * @param enabled New I2C bypass enabled status + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT + */ +void MPU6050::setI2CBypassEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled); +} +/** Get reference clock output enabled status. + * When this bit is equal to 1, a reference clock output is provided at the + * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For + * further information regarding CLKOUT, please refer to the MPU-60X0 Product + * Specification document. + * @return Current reference clock output enabled status + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_CLKOUT_EN_BIT + */ +bool MPU6050::getClockOutputEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, buffer); + return buffer[0]; +} +/** Set reference clock output enabled status. + * When this bit is equal to 1, a reference clock output is provided at the + * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For + * further information regarding CLKOUT, please refer to the MPU-60X0 Product + * Specification document. + * @param enabled New reference clock output enabled status + * @see MPU6050_RA_INT_PIN_CFG + * @see MPU6050_INTCFG_CLKOUT_EN_BIT + */ +void MPU6050::setClockOutputEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled); +} + +// INT_ENABLE register + +/** Get full interrupt enabled status. + * Full register byte for all interrupts, for quick reading. Each bit will be + * set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled status + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_FF_BIT + **/ +uint8_t MPU6050::getIntEnabled() { + I2Cdev::readByte(devAddr, MPU6050_RA_INT_ENABLE, buffer); + return buffer[0]; +} +/** Set full interrupt enabled status. + * Full register byte for all interrupts, for quick reading. Each bit should be + * set 0 for disabled, 1 for enabled. + * @param enabled New interrupt enabled status + * @see getIntFreefallEnabled() + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_FF_BIT + **/ +void MPU6050::setIntEnabled(uint8_t enabled) { + I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, enabled); +} +/** Get Free Fall interrupt enabled status. + * Will be set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled status + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_FF_BIT + **/ +bool MPU6050::getIntFreefallEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, buffer); + return buffer[0]; +} +/** Set Free Fall interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntFreefallEnabled() + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_FF_BIT + **/ +void MPU6050::setIntFreefallEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, enabled); +} +/** Get Motion Detection interrupt enabled status. + * Will be set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled status + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_MOT_BIT + **/ +bool MPU6050::getIntMotionEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, buffer); + return buffer[0]; +} +/** Set Motion Detection interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntMotionEnabled() + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_MOT_BIT + **/ +void MPU6050::setIntMotionEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, enabled); +} +/** Get Zero Motion Detection interrupt enabled status. + * Will be set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled status + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_ZMOT_BIT + **/ +bool MPU6050::getIntZeroMotionEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, buffer); + return buffer[0]; +} +/** Set Zero Motion Detection interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntZeroMotionEnabled() + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_ZMOT_BIT + **/ +void MPU6050::setIntZeroMotionEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, enabled); +} +/** Get FIFO Buffer Overflow interrupt enabled status. + * Will be set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled status + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT + **/ +bool MPU6050::getIntFIFOBufferOverflowEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer); + return buffer[0]; +} +/** Set FIFO Buffer Overflow interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntFIFOBufferOverflowEnabled() + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT + **/ +void MPU6050::setIntFIFOBufferOverflowEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled); +} +/** Get I2C Master interrupt enabled status. + * This enables any of the I2C Master interrupt sources to generate an + * interrupt. Will be set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled status + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT + **/ +bool MPU6050::getIntI2CMasterEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer); + return buffer[0]; +} +/** Set I2C Master interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntI2CMasterEnabled() + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT + **/ +void MPU6050::setIntI2CMasterEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, enabled); +} +/** Get Data Ready interrupt enabled setting. + * This event occurs each time a write operation to all of the sensor registers + * has been completed. Will be set 0 for disabled, 1 for enabled. + * @return Current interrupt enabled status + * @see MPU6050_RA_INT_ENABLE + * @see MPU6050_INTERRUPT_DATA_RDY_BIT + */ +bool MPU6050::getIntDataReadyEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer); + return buffer[0]; +} +/** Set Data Ready interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntDataReadyEnabled() + * @see MPU6050_RA_INT_CFG + * @see MPU6050_INTERRUPT_DATA_RDY_BIT + */ +void MPU6050::setIntDataReadyEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, enabled); +} + +// INT_STATUS register + +/** Get full set of interrupt status bits. + * These bits clear to 0 after the register has been read. Very useful + * for getting multiple INT statuses, since each single bit read clears + * all of them because it has to read the whole byte. + * @return Current interrupt status + * @see MPU6050_RA_INT_STATUS + */ +uint8_t MPU6050::getIntStatus() { + I2Cdev::readByte(devAddr, MPU6050_RA_INT_STATUS, buffer); + return buffer[0]; +} +/** Get Free Fall interrupt status. + * This bit automatically sets to 1 when a Free Fall interrupt has been + * generated. The bit clears to 0 after the register has been read. + * @return Current interrupt status + * @see MPU6050_RA_INT_STATUS + * @see MPU6050_INTERRUPT_FF_BIT + */ +bool MPU6050::getIntFreefallStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, buffer); + return buffer[0]; +} +/** Get Motion Detection interrupt status. + * This bit automatically sets to 1 when a Motion Detection interrupt has been + * generated. The bit clears to 0 after the register has been read. + * @return Current interrupt status + * @see MPU6050_RA_INT_STATUS + * @see MPU6050_INTERRUPT_MOT_BIT + */ +bool MPU6050::getIntMotionStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, buffer); + return buffer[0]; +} +/** Get Zero Motion Detection interrupt status. + * This bit automatically sets to 1 when a Zero Motion Detection interrupt has + * been generated. The bit clears to 0 after the register has been read. + * @return Current interrupt status + * @see MPU6050_RA_INT_STATUS + * @see MPU6050_INTERRUPT_ZMOT_BIT + */ +bool MPU6050::getIntZeroMotionStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, buffer); + return buffer[0]; +} +/** Get FIFO Buffer Overflow interrupt status. + * This bit automatically sets to 1 when a Free Fall interrupt has been + * generated. The bit clears to 0 after the register has been read. + * @return Current interrupt status + * @see MPU6050_RA_INT_STATUS + * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT + */ +bool MPU6050::getIntFIFOBufferOverflowStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer); + return buffer[0]; +} +/** Get I2C Master interrupt status. + * This bit automatically sets to 1 when an I2C Master interrupt has been + * generated. For a list of I2C Master interrupts, please refer to Register 54. + * The bit clears to 0 after the register has been read. + * @return Current interrupt status + * @see MPU6050_RA_INT_STATUS + * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT + */ +bool MPU6050::getIntI2CMasterStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer); + return buffer[0]; +} +/** Get Data Ready interrupt status. + * This bit automatically sets to 1 when a Data Ready interrupt has been + * generated. The bit clears to 0 after the register has been read. + * @return Current interrupt status + * @see MPU6050_RA_INT_STATUS + * @see MPU6050_INTERRUPT_DATA_RDY_BIT + */ +bool MPU6050::getIntDataReadyStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer); + return buffer[0]; +} + +// ACCEL_*OUT_* registers + +/** Get raw 9-axis motion sensor readings (accel/gyro/compass). + * FUNCTION NOT FULLY IMPLEMENTED YET. + * @param ax 16-bit signed integer container for accelerometer X-axis value + * @param ay 16-bit signed integer container for accelerometer Y-axis value + * @param az 16-bit signed integer container for accelerometer Z-axis value + * @param gx 16-bit signed integer container for gyroscope X-axis value + * @param gy 16-bit signed integer container for gyroscope Y-axis value + * @param gz 16-bit signed integer container for gyroscope Z-axis value + * @param mx 16-bit signed integer container for magnetometer X-axis value + * @param my 16-bit signed integer container for magnetometer Y-axis value + * @param mz 16-bit signed integer container for magnetometer Z-axis value + * @see getMotion6() + * @see getAcceleration() + * @see getRotation() + * @see MPU6050_RA_ACCEL_XOUT_H + */ +void MPU6050::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) { + getMotion6(ax, ay, az, gx, gy, gz); + // TODO: magnetometer integration +} +/** Get raw 6-axis motion sensor readings (accel/gyro). + * Retrieves all currently available motion sensor values. + * @param ax 16-bit signed integer container for accelerometer X-axis value + * @param ay 16-bit signed integer container for accelerometer Y-axis value + * @param az 16-bit signed integer container for accelerometer Z-axis value + * @param gx 16-bit signed integer container for gyroscope X-axis value + * @param gy 16-bit signed integer container for gyroscope Y-axis value + * @param gz 16-bit signed integer container for gyroscope Z-axis value + * @see getAcceleration() + * @see getRotation() + * @see MPU6050_RA_ACCEL_XOUT_H + */ +void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { + I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer); + *ax = (((int16_t)buffer[0]) << 8) | buffer[1]; + *ay = (((int16_t)buffer[2]) << 8) | buffer[3]; + *az = (((int16_t)buffer[4]) << 8) | buffer[5]; + *gx = (((int16_t)buffer[8]) << 8) | buffer[9]; + *gy = (((int16_t)buffer[10]) << 8) | buffer[11]; + *gz = (((int16_t)buffer[12]) << 8) | buffer[13]; +} +/** Get 3-axis accelerometer readings. + * These registers store the most recent accelerometer measurements. + * Accelerometer measurements are written to these registers at the Sample Rate + * as defined in Register 25. + * + * The accelerometer measurement registers, along with the temperature + * measurement registers, gyroscope measurement registers, and external sensor + * data registers, are composed of two sets of registers: an internal register + * set and a user-facing read register set. + * + * The data within the accelerometer sensors' internal register set is always + * updated at the Sample Rate. Meanwhile, the user-facing read register set + * duplicates the internal register set's data values whenever the serial + * interface is idle. This guarantees that a burst read of sensor registers will + * read measurements from the same sampling instant. Note that if burst reads + * are not used, the user is responsible for ensuring a set of single byte reads + * correspond to a single sampling instant by checking the Data Ready interrupt. + * + * Each 16-bit accelerometer measurement has a full scale defined in ACCEL_FS + * (Register 28). For each full scale setting, the accelerometers' sensitivity + * per LSB in ACCEL_xOUT is shown in the table below: + * + * 
+ * AFS_SEL | Full Scale Range | LSB Sensitivity
+ * --------+------------------+----------------
+ * 0       | +/- 2g           | 8192 LSB/mg
+ * 1       | +/- 4g           | 4096 LSB/mg
+ * 2       | +/- 8g           | 2048 LSB/mg
+ * 3       | +/- 16g          | 1024 LSB/mg
+ *
+ * + * @param x 16-bit signed integer container for X-axis acceleration + * @param y 16-bit signed integer container for Y-axis acceleration + * @param z 16-bit signed integer container for Z-axis acceleration + * @see MPU6050_RA_GYRO_XOUT_H + */ +void MPU6050::getAcceleration(int16_t* x, int16_t* y, int16_t* z) { + I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buffer); + *x = (((int16_t)buffer[0]) << 8) | buffer[1]; + *y = (((int16_t)buffer[2]) << 8) | buffer[3]; + *z = (((int16_t)buffer[4]) << 8) | buffer[5]; +} +/** Get X-axis accelerometer reading. + * @return X-axis acceleration measurement in 16-bit 2's complement format + * @see getMotion6() + * @see MPU6050_RA_ACCEL_XOUT_H + */ +int16_t MPU6050::getAccelerationX() { + I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +/** Get Y-axis accelerometer reading. + * @return Y-axis acceleration measurement in 16-bit 2's complement format + * @see getMotion6() + * @see MPU6050_RA_ACCEL_YOUT_H + */ +int16_t MPU6050::getAccelerationY() { + I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +/** Get Z-axis accelerometer reading. + * @return Z-axis acceleration measurement in 16-bit 2's complement format + * @see getMotion6() + * @see MPU6050_RA_ACCEL_ZOUT_H + */ +int16_t MPU6050::getAccelerationZ() { + I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} + +// TEMP_OUT_* registers + +/** Get current internal temperature. + * @return Temperature reading in 16-bit 2's complement format + * @see MPU6050_RA_TEMP_OUT_H + */ +int16_t MPU6050::getTemperature() { + I2Cdev::readBytes(devAddr, MPU6050_RA_TEMP_OUT_H, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} + +// GYRO_*OUT_* registers + +/** Get 3-axis gyroscope readings. + * These gyroscope measurement registers, along with the accelerometer + * measurement registers, temperature measurement registers, and external sensor + * data registers, are composed of two sets of registers: an internal register + * set and a user-facing read register set. + * The data within the gyroscope sensors' internal register set is always + * updated at the Sample Rate. Meanwhile, the user-facing read register set + * duplicates the internal register set's data values whenever the serial + * interface is idle. This guarantees that a burst read of sensor registers will + * read measurements from the same sampling instant. Note that if burst reads + * are not used, the user is responsible for ensuring a set of single byte reads + * correspond to a single sampling instant by checking the Data Ready interrupt. + * + * Each 16-bit gyroscope measurement has a full scale defined in FS_SEL + * (Register 27). For each full scale setting, the gyroscopes' sensitivity per + * LSB in GYRO_xOUT is shown in the table below: + * + * 
+ * FS_SEL | Full Scale Range   | LSB Sensitivity
+ * -------+--------------------+----------------
+ * 0      | +/- 250 degrees/s  | 131 LSB/deg/s
+ * 1      | +/- 500 degrees/s  | 65.5 LSB/deg/s
+ * 2      | +/- 1000 degrees/s | 32.8 LSB/deg/s
+ * 3      | +/- 2000 degrees/s | 16.4 LSB/deg/s
+ *
+ * + * @param x 16-bit signed integer container for X-axis rotation + * @param y 16-bit signed integer container for Y-axis rotation + * @param z 16-bit signed integer container for Z-axis rotation + * @see getMotion6() + * @see MPU6050_RA_GYRO_XOUT_H + */ +void MPU6050::getRotation(int16_t* x, int16_t* y, int16_t* z) { + I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer); + *x = (((int16_t)buffer[0]) << 8) | buffer[1]; + *y = (((int16_t)buffer[2]) << 8) | buffer[3]; + *z = (((int16_t)buffer[4]) << 8) | buffer[5]; +} +/** Get X-axis gyroscope reading. + * @return X-axis rotation measurement in 16-bit 2's complement format + * @see getMotion6() + * @see MPU6050_RA_GYRO_XOUT_H + */ +int16_t MPU6050::getRotationX() { + I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +/** Get Y-axis gyroscope reading. + * @return Y-axis rotation measurement in 16-bit 2's complement format + * @see getMotion6() + * @see MPU6050_RA_GYRO_YOUT_H + */ +int16_t MPU6050::getRotationY() { + I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_YOUT_H, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +/** Get Z-axis gyroscope reading. + * @return Z-axis rotation measurement in 16-bit 2's complement format + * @see getMotion6() + * @see MPU6050_RA_GYRO_ZOUT_H + */ +int16_t MPU6050::getRotationZ() { + I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} + +// EXT_SENS_DATA_* registers + +/** Read single byte from external sensor data register. + * These registers store data read from external sensors by the Slave 0, 1, 2, + * and 3 on the auxiliary I2C interface. Data read by Slave 4 is stored in + * I2C_SLV4_DI (Register 53). + * + * External sensor data is written to these registers at the Sample Rate as + * defined in Register 25. This access rate can be reduced by using the Slave + * Delay Enable registers (Register 103). + * + * External sensor data registers, along with the gyroscope measurement + * registers, accelerometer measurement registers, and temperature measurement + * registers, are composed of two sets of registers: an internal register set + * and a user-facing read register set. + * + * The data within the external sensors' internal register set is always updated + * at the Sample Rate (or the reduced access rate) whenever the serial interface + * is idle. This guarantees that a burst read of sensor registers will read + * measurements from the same sampling instant. Note that if burst reads are not + * used, the user is responsible for ensuring a set of single byte reads + * correspond to a single sampling instant by checking the Data Ready interrupt. + * + * Data is placed in these external sensor data registers according to + * I2C_SLV0_CTRL, I2C_SLV1_CTRL, I2C_SLV2_CTRL, and I2C_SLV3_CTRL (Registers 39, + * 42, 45, and 48). When more than zero bytes are read (I2C_SLVx_LEN > 0) from + * an enabled slave (I2C_SLVx_EN = 1), the slave is read at the Sample Rate (as + * defined in Register 25) or delayed rate (if specified in Register 52 and + * 103). During each Sample cycle, slave reads are performed in order of Slave + * number. If all slaves are enabled with more than zero bytes to be read, the + * order will be Slave 0, followed by Slave 1, Slave 2, and Slave 3. + * + * Each enabled slave will have EXT_SENS_DATA registers associated with it by + * number of bytes read (I2C_SLVx_LEN) in order of slave number, starting from + * EXT_SENS_DATA_00. Note that this means enabling or disabling a slave may + * change the higher numbered slaves' associated registers. Furthermore, if + * fewer total bytes are being read from the external sensors as a result of + * such a change, then the data remaining in the registers which no longer have + * an associated slave device (i.e. high numbered registers) will remain in + * these previously allocated registers unless reset. + * + * If the sum of the read lengths of all SLVx transactions exceed the number of + * available EXT_SENS_DATA registers, the excess bytes will be dropped. There + * are 24 EXT_SENS_DATA registers and hence the total read lengths between all + * the slaves cannot be greater than 24 or some bytes will be lost. + * + * Note: Slave 4's behavior is distinct from that of Slaves 0-3. For further + * information regarding the characteristics of Slave 4, please refer to + * Registers 49 to 53. + * + * EXAMPLE: + * Suppose that Slave 0 is enabled with 4 bytes to be read (I2C_SLV0_EN = 1 and + * I2C_SLV0_LEN = 4) while Slave 1 is enabled with 2 bytes to be read so that + * I2C_SLV1_EN = 1 and I2C_SLV1_LEN = 2. In such a situation, EXT_SENS_DATA _00 + * through _03 will be associated with Slave 0, while EXT_SENS_DATA _04 and 05 + * will be associated with Slave 1. If Slave 2 is enabled as well, registers + * starting from EXT_SENS_DATA_06 will be allocated to Slave 2. + * + * If Slave 2 is disabled while Slave 3 is enabled in this same situation, then + * registers starting from EXT_SENS_DATA_06 will be allocated to Slave 3 + * instead. + * + * REGISTER ALLOCATION FOR DYNAMIC DISABLE VS. NORMAL DISABLE: + * If a slave is disabled at any time, the space initially allocated to the + * slave in the EXT_SENS_DATA register, will remain associated with that slave. + * This is to avoid dynamic adjustment of the register allocation. + * + * The allocation of the EXT_SENS_DATA registers is recomputed only when (1) all + * slaves are disabled, or (2) the I2C_MST_RST bit is set (Register 106). + * + * This above is also true if one of the slaves gets NACKed and stops + * functioning. + * + * @param position Starting position (0-23) + * @return Byte read from register + */ +uint8_t MPU6050::getExternalSensorByte(int position) { + I2Cdev::readByte(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, buffer); + return buffer[0]; +} +/** Read word (2 bytes) from external sensor data registers. + * @param position Starting position (0-21) + * @return Word read from register + * @see getExternalSensorByte() + */ +uint16_t MPU6050::getExternalSensorWord(int position) { + I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, buffer); + return (((uint16_t)buffer[0]) << 8) | buffer[1]; +} +/** Read double word (4 bytes) from external sensor data registers. + * @param position Starting position (0-20) + * @return Double word read from registers + * @see getExternalSensorByte() + */ +uint32_t MPU6050::getExternalSensorDWord(int position) { + I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, buffer); + return (((uint32_t)buffer[0]) << 24) | (((uint32_t)buffer[1]) << 16) | (((uint16_t)buffer[2]) << 8) | buffer[3]; +} + +// MOT_DETECT_STATUS register + +/** Get full motion detection status register content (all bits). + * @return Motion detection status byte + * @see MPU6050_RA_MOT_DETECT_STATUS + */ +uint8_t MPU6050::getMotionStatus() { + I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DETECT_STATUS, buffer); + return buffer[0]; +} +/** Get X-axis negative motion detection interrupt status. + * @return Motion detection status + * @see MPU6050_RA_MOT_DETECT_STATUS + * @see MPU6050_MOTION_MOT_XNEG_BIT + */ +bool MPU6050::getXNegMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, buffer); + return buffer[0]; +} +/** Get X-axis positive motion detection interrupt status. + * @return Motion detection status + * @see MPU6050_RA_MOT_DETECT_STATUS + * @see MPU6050_MOTION_MOT_XPOS_BIT + */ +bool MPU6050::getXPosMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, buffer); + return buffer[0]; +} +/** Get Y-axis negative motion detection interrupt status. + * @return Motion detection status + * @see MPU6050_RA_MOT_DETECT_STATUS + * @see MPU6050_MOTION_MOT_YNEG_BIT + */ +bool MPU6050::getYNegMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, buffer); + return buffer[0]; +} +/** Get Y-axis positive motion detection interrupt status. + * @return Motion detection status + * @see MPU6050_RA_MOT_DETECT_STATUS + * @see MPU6050_MOTION_MOT_YPOS_BIT + */ +bool MPU6050::getYPosMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, buffer); + return buffer[0]; +} +/** Get Z-axis negative motion detection interrupt status. + * @return Motion detection status + * @see MPU6050_RA_MOT_DETECT_STATUS + * @see MPU6050_MOTION_MOT_ZNEG_BIT + */ +bool MPU6050::getZNegMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, buffer); + return buffer[0]; +} +/** Get Z-axis positive motion detection interrupt status. + * @return Motion detection status + * @see MPU6050_RA_MOT_DETECT_STATUS + * @see MPU6050_MOTION_MOT_ZPOS_BIT + */ +bool MPU6050::getZPosMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, buffer); + return buffer[0]; +} +/** Get zero motion detection interrupt status. + * @return Motion detection status + * @see MPU6050_RA_MOT_DETECT_STATUS + * @see MPU6050_MOTION_MOT_ZRMOT_BIT + */ +bool MPU6050::getZeroMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, buffer); + return buffer[0]; +} + +// I2C_SLV*_DO register + +/** Write byte to Data Output container for specified slave. + * This register holds the output data written into Slave when Slave is set to + * write mode. For further information regarding Slave control, please + * refer to Registers 37 to 39 and immediately following. + * @param num Slave number (0-3) + * @param data Byte to write + * @see MPU6050_RA_I2C_SLV0_DO + */ +void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data) { + if (num > 3) return; + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + num, data); +} + +// I2C_MST_DELAY_CTRL register + +/** Get external data shadow delay enabled status. + * This register is used to specify the timing of external sensor data + * shadowing. When DELAY_ES_SHADOW is set to 1, shadowing of external + * sensor data is delayed until all data has been received. + * @return Current external data shadow delay enabled status. + * @see MPU6050_RA_I2C_MST_DELAY_CTRL + * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT + */ +bool MPU6050::getExternalShadowDelayEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, buffer); + return buffer[0]; +} +/** Set external data shadow delay enabled status. + * @param enabled New external data shadow delay enabled status. + * @see getExternalShadowDelayEnabled() + * @see MPU6050_RA_I2C_MST_DELAY_CTRL + * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT + */ +void MPU6050::setExternalShadowDelayEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled); +} +/** Get slave delay enabled status. + * When a particular slave delay is enabled, the rate of access for the that + * slave device is reduced. When a slave's access rate is decreased relative to + * the Sample Rate, the slave is accessed every: + * + * 1 / (1 + I2C_MST_DLY) Samples + * + * This base Sample Rate in turn is determined by SMPLRT_DIV (register * 25) + * and DLPF_CFG (register 26). + * + * For further information regarding I2C_MST_DLY, please refer to register 52. + * For further information regarding the Sample Rate, please refer to register 25. + * + * @param num Slave number (0-4) + * @return Current slave delay enabled status. + * @see MPU6050_RA_I2C_MST_DELAY_CTRL + * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT + */ +bool MPU6050::getSlaveDelayEnabled(uint8_t num) { + // MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc. + if (num > 4) return 0; + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, buffer); + return buffer[0]; +} +/** Set slave delay enabled status. + * @param num Slave number (0-4) + * @param enabled New slave delay enabled status. + * @see MPU6050_RA_I2C_MST_DELAY_CTRL + * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT + */ +void MPU6050::setSlaveDelayEnabled(uint8_t num, bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled); +} + +// SIGNAL_PATH_RESET register + +/** Reset gyroscope signal path. + * The reset will revert the signal path analog to digital converters and + * filters to their power up configurations. + * @see MPU6050_RA_SIGNAL_PATH_RESET + * @see MPU6050_PATHRESET_GYRO_RESET_BIT + */ +void MPU6050::resetGyroscopePath() { + I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, true); +} +/** Reset accelerometer signal path. + * The reset will revert the signal path analog to digital converters and + * filters to their power up configurations. + * @see MPU6050_RA_SIGNAL_PATH_RESET + * @see MPU6050_PATHRESET_ACCEL_RESET_BIT + */ +void MPU6050::resetAccelerometerPath() { + I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, true); +} +/** Reset temperature sensor signal path. + * The reset will revert the signal path analog to digital converters and + * filters to their power up configurations. + * @see MPU6050_RA_SIGNAL_PATH_RESET + * @see MPU6050_PATHRESET_TEMP_RESET_BIT + */ +void MPU6050::resetTemperaturePath() { + I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, true); +} + +// MOT_DETECT_CTRL register + +/** Get accelerometer power-on delay. + * The accelerometer data path provides samples to the sensor registers, Motion + * detection, Zero Motion detection, and Free Fall detection modules. The + * signal path contains filters which must be flushed on wake-up with new + * samples before the detection modules begin operations. The default wake-up + * delay, of 4ms can be lengthened by up to 3ms. This additional delay is + * specified in ACCEL_ON_DELAY in units of 1 LSB = 1 ms. The user may select + * any value above zero unless instructed otherwise by InvenSense. Please refer + * to Section 8 of the MPU-6000/MPU-6050 Product Specification document for + * further information regarding the detection modules. + * @return Current accelerometer power-on delay + * @see MPU6050_RA_MOT_DETECT_CTRL + * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT + */ +uint8_t MPU6050::getAccelerometerPowerOnDelay() { + I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, buffer); + return buffer[0]; +} +/** Set accelerometer power-on delay. + * @param delay New accelerometer power-on delay (0-3) + * @see getAccelerometerPowerOnDelay() + * @see MPU6050_RA_MOT_DETECT_CTRL + * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT + */ +void MPU6050::setAccelerometerPowerOnDelay(uint8_t delay) { + I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay); +} +/** Get Free Fall detection counter decrement configuration. + * Detection is registered by the Free Fall detection module after accelerometer + * measurements meet their respective threshold conditions over a specified + * number of samples. When the threshold conditions are met, the corresponding + * detection counter increments by 1. The user may control the rate at which the + * detection counter decrements when the threshold condition is not met by + * configuring FF_COUNT. The decrement rate can be set according to the + * following table: + * + * 
+ * FF_COUNT | Counter Decrement
+ * ---------+------------------
+ * 0        | Reset
+ * 1        | 1
+ * 2        | 2
+ * 3        | 4
+ *
+ * + * When FF_COUNT is configured to 0 (reset), any non-qualifying sample will + * reset the counter to 0. For further information on Free Fall detection, + * please refer to Registers 29 to 32. + * + * @return Current decrement configuration + * @see MPU6050_RA_MOT_DETECT_CTRL + * @see MPU6050_DETECT_FF_COUNT_BIT + */ +uint8_t MPU6050::getFreefallDetectionCounterDecrement() { + I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, buffer); + return buffer[0]; +} +/** Set Free Fall detection counter decrement configuration. + * @param decrement New decrement configuration value + * @see getFreefallDetectionCounterDecrement() + * @see MPU6050_RA_MOT_DETECT_CTRL + * @see MPU6050_DETECT_FF_COUNT_BIT + */ +void MPU6050::setFreefallDetectionCounterDecrement(uint8_t decrement) { + I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, decrement); +} +/** Get Motion detection counter decrement configuration. + * Detection is registered by the Motion detection module after accelerometer + * measurements meet their respective threshold conditions over a specified + * number of samples. When the threshold conditions are met, the corresponding + * detection counter increments by 1. The user may control the rate at which the + * detection counter decrements when the threshold condition is not met by + * configuring MOT_COUNT. The decrement rate can be set according to the + * following table: + * + * 
+ * MOT_COUNT | Counter Decrement
+ * ----------+------------------
+ * 0         | Reset
+ * 1         | 1
+ * 2         | 2
+ * 3         | 4
+ *
+ * + * When MOT_COUNT is configured to 0 (reset), any non-qualifying sample will + * reset the counter to 0. For further information on Motion detection, + * please refer to Registers 29 to 32. + * + */ +uint8_t MPU6050::getMotionDetectionCounterDecrement() { + I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, buffer); + return buffer[0]; +} +/** Set Motion detection counter decrement configuration. + * @param decrement New decrement configuration value + * @see getMotionDetectionCounterDecrement() + * @see MPU6050_RA_MOT_DETECT_CTRL + * @see MPU6050_DETECT_MOT_COUNT_BIT + */ +void MPU6050::setMotionDetectionCounterDecrement(uint8_t decrement) { + I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, decrement); +} + +// USER_CTRL register + +/** Get FIFO enabled status. + * When this bit is set to 0, the FIFO buffer is disabled. The FIFO buffer + * cannot be written to or read from while disabled. The FIFO buffer's state + * does not change unless the MPU-60X0 is power cycled. + * @return Current FIFO enabled status + * @see MPU6050_RA_USER_CTRL + * @see MPU6050_USERCTRL_FIFO_EN_BIT + */ +bool MPU6050::getFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set FIFO enabled status. + * @param enabled New FIFO enabled status + * @see getFIFOEnabled() + * @see MPU6050_RA_USER_CTRL + * @see MPU6050_USERCTRL_FIFO_EN_BIT + */ +void MPU6050::setFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled); +} +/** Get I2C Master Mode enabled status. + * When this mode is enabled, the MPU-60X0 acts as the I2C Master to the + * external sensor slave devices on the auxiliary I2C bus. When this bit is + * cleared to 0, the auxiliary I2C bus lines (AUX_DA and AUX_CL) are logically + * driven by the primary I2C bus (SDA and SCL). This is a precondition to + * enabling Bypass Mode. For further information regarding Bypass Mode, please + * refer to Register 55. + * @return Current I2C Master Mode enabled status + * @see MPU6050_RA_USER_CTRL + * @see MPU6050_USERCTRL_I2C_MST_EN_BIT + */ +bool MPU6050::getI2CMasterModeEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, buffer); + return buffer[0]; +} +/** Set I2C Master Mode enabled status. + * @param enabled New I2C Master Mode enabled status + * @see getI2CMasterModeEnabled() + * @see MPU6050_RA_USER_CTRL + * @see MPU6050_USERCTRL_I2C_MST_EN_BIT + */ +void MPU6050::setI2CMasterModeEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled); +} +/** Switch from I2C to SPI mode (MPU-6000 only) + * If this is set, the primary SPI interface will be enabled in place of the + * disabled primary I2C interface. + */ +void MPU6050::switchSPIEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled); +} +/** Reset the FIFO. + * This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This + * bit automatically clears to 0 after the reset has been triggered. + * @see MPU6050_RA_USER_CTRL + * @see MPU6050_USERCTRL_FIFO_RESET_BIT + */ +void MPU6050::resetFIFO() { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, true); +} +/** Reset the I2C Master. + * This bit resets the I2C Master when set to 1 while I2C_MST_EN equals 0. + * This bit automatically clears to 0 after the reset has been triggered. + * @see MPU6050_RA_USER_CTRL + * @see MPU6050_USERCTRL_I2C_MST_RESET_BIT + */ +void MPU6050::resetI2CMaster() { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, true); +} +/** Reset all sensor registers and signal paths. + * When set to 1, this bit resets the signal paths for all sensors (gyroscopes, + * accelerometers, and temperature sensor). This operation will also clear the + * sensor registers. This bit automatically clears to 0 after the reset has been + * triggered. + * + * When resetting only the signal path (and not the sensor registers), please + * use Register 104, SIGNAL_PATH_RESET. + * + * @see MPU6050_RA_USER_CTRL + * @see MPU6050_USERCTRL_SIG_COND_RESET_BIT + */ +void MPU6050::resetSensors() { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, true); +} + +// PWR_MGMT_1 register + +/** Trigger a full device reset. + * A small delay of ~50ms may be desirable after triggering a reset. + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_DEVICE_RESET_BIT + */ +void MPU6050::reset() { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true); +} +/** Get sleep mode status. + * Setting the SLEEP bit in the register puts the device into very low power + * sleep mode. In this mode, only the serial interface and internal registers + * remain active, allowing for a very low standby current. Clearing this bit + * puts the device back into normal mode. To save power, the individual standby + * selections for each of the gyros should be used if any gyro axis is not used + * by the application. + * @return Current sleep mode enabled status + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_SLEEP_BIT + */ +bool MPU6050::getSleepEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, buffer); + return buffer[0]; +} +/** Set sleep mode status. + * @param enabled New sleep mode enabled status + * @see getSleepEnabled() + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_SLEEP_BIT + */ +void MPU6050::setSleepEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled); +} +/** Get wake cycle enabled status. + * When this bit is set to 1 and SLEEP is disabled, the MPU-60X0 will cycle + * between sleep mode and waking up to take a single sample of data from active + * sensors at a rate determined by LP_WAKE_CTRL (register 108). + * @return Current sleep mode enabled status + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_CYCLE_BIT + */ +bool MPU6050::getWakeCycleEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, buffer); + return buffer[0]; +} +/** Set wake cycle enabled status. + * @param enabled New sleep mode enabled status + * @see getWakeCycleEnabled() + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_CYCLE_BIT + */ +void MPU6050::setWakeCycleEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled); +} +/** Get temperature sensor enabled status. + * Control the usage of the internal temperature sensor. + * + * Note: this register stores the *disabled* value, but for consistency with the + * rest of the code, the function is named and used with standard true/false + * values to indicate whether the sensor is enabled or disabled, respectively. + * + * @return Current temperature sensor enabled status + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_TEMP_DIS_BIT + */ +bool MPU6050::getTempSensorEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, buffer); + return buffer[0] == 0; // 1 is actually disabled here +} +/** Set temperature sensor enabled status. + * Note: this register stores the *disabled* value, but for consistency with the + * rest of the code, the function is named and used with standard true/false + * values to indicate whether the sensor is enabled or disabled, respectively. + * + * @param enabled New temperature sensor enabled status + * @see getTempSensorEnabled() + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_TEMP_DIS_BIT + */ +void MPU6050::setTempSensorEnabled(bool enabled) { + // 1 is actually disabled here + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled); +} +/** Get clock source setting. + * @return Current clock source setting + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_CLKSEL_BIT + * @see MPU6050_PWR1_CLKSEL_LENGTH + */ +uint8_t MPU6050::getClockSource() { + I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, buffer); + return buffer[0]; +} +/** Set clock source setting. + * An internal 8MHz oscillator, gyroscope based clock, or external sources can + * be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillator + * or an external source is chosen as the clock source, the MPU-60X0 can operate + * in low power modes with the gyroscopes disabled. + * + * Upon power up, the MPU-60X0 clock source defaults to the internal oscillator. + * However, it is highly recommended that the device be configured to use one of + * the gyroscopes (or an external clock source) as the clock reference for + * improved stability. The clock source can be selected according to the following table: + * + * 
+ * CLK_SEL | Clock Source
+ * --------+--------------------------------------
+ * 0       | Internal oscillator
+ * 1       | PLL with X Gyro reference
+ * 2       | PLL with Y Gyro reference
+ * 3       | PLL with Z Gyro reference
+ * 4       | PLL with external 32.768kHz reference
+ * 5       | PLL with external 19.2MHz reference
+ * 6       | Reserved
+ * 7       | Stops the clock and keeps the timing generator in reset
+ *
+ * + * @param source New clock source setting + * @see getClockSource() + * @see MPU6050_RA_PWR_MGMT_1 + * @see MPU6050_PWR1_CLKSEL_BIT + * @see MPU6050_PWR1_CLKSEL_LENGTH + */ +void MPU6050::setClockSource(uint8_t source) { + I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source); +} + +// PWR_MGMT_2 register + +/** Get wake frequency in Accel-Only Low Power Mode. + * The MPU-60X0 can be put into Accerlerometer Only Low Power Mode by setting + * PWRSEL to 1 in the Power Management 1 register (Register 107). In this mode, + * the device will power off all devices except for the primary I2C interface, + * waking only the accelerometer at fixed intervals to take a single + * measurement. The frequency of wake-ups can be configured with LP_WAKE_CTRL + * as shown below: + * + * 
+ * LP_WAKE_CTRL | Wake-up Frequency
+ * -------------+------------------
+ * 0            | 1.25 Hz
+ * 1            | 2.5 Hz
+ * 2            | 5 Hz
+ * 3            | 10 Hz
+ *
+ * + * For further information regarding the MPU-60X0's power modes, please refer to + * Register 107. + * + * @return Current wake frequency + * @see MPU6050_RA_PWR_MGMT_2 + */ +uint8_t MPU6050::getWakeFrequency() { + I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, buffer); + return buffer[0]; +} +/** Set wake frequency in Accel-Only Low Power Mode. + * @param frequency New wake frequency + * @see MPU6050_RA_PWR_MGMT_2 + */ +void MPU6050::setWakeFrequency(uint8_t frequency) { + I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency); +} + +/** Get X-axis accelerometer standby enabled status. + * If enabled, the X-axis will not gather or report data (or use power). + * @return Current X-axis standby enabled status + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_XA_BIT + */ +bool MPU6050::getStandbyXAccelEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, buffer); + return buffer[0]; +} +/** Set X-axis accelerometer standby enabled status. + * @param New X-axis standby enabled status + * @see getStandbyXAccelEnabled() + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_XA_BIT + */ +void MPU6050::setStandbyXAccelEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, enabled); +} +/** Get Y-axis accelerometer standby enabled status. + * If enabled, the Y-axis will not gather or report data (or use power). + * @return Current Y-axis standby enabled status + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_YA_BIT + */ +bool MPU6050::getStandbyYAccelEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, buffer); + return buffer[0]; +} +/** Set Y-axis accelerometer standby enabled status. + * @param New Y-axis standby enabled status + * @see getStandbyYAccelEnabled() + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_YA_BIT + */ +void MPU6050::setStandbyYAccelEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, enabled); +} +/** Get Z-axis accelerometer standby enabled status. + * If enabled, the Z-axis will not gather or report data (or use power). + * @return Current Z-axis standby enabled status + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_ZA_BIT + */ +bool MPU6050::getStandbyZAccelEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, buffer); + return buffer[0]; +} +/** Set Z-axis accelerometer standby enabled status. + * @param New Z-axis standby enabled status + * @see getStandbyZAccelEnabled() + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_ZA_BIT + */ +void MPU6050::setStandbyZAccelEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, enabled); +} +/** Get X-axis gyroscope standby enabled status. + * If enabled, the X-axis will not gather or report data (or use power). + * @return Current X-axis standby enabled status + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_XG_BIT + */ +bool MPU6050::getStandbyXGyroEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, buffer); + return buffer[0]; +} +/** Set X-axis gyroscope standby enabled status. + * @param New X-axis standby enabled status + * @see getStandbyXGyroEnabled() + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_XG_BIT + */ +void MPU6050::setStandbyXGyroEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, enabled); +} +/** Get Y-axis gyroscope standby enabled status. + * If enabled, the Y-axis will not gather or report data (or use power). + * @return Current Y-axis standby enabled status + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_YG_BIT + */ +bool MPU6050::getStandbyYGyroEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, buffer); + return buffer[0]; +} +/** Set Y-axis gyroscope standby enabled status. + * @param New Y-axis standby enabled status + * @see getStandbyYGyroEnabled() + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_YG_BIT + */ +void MPU6050::setStandbyYGyroEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, enabled); +} +/** Get Z-axis gyroscope standby enabled status. + * If enabled, the Z-axis will not gather or report data (or use power). + * @return Current Z-axis standby enabled status + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_ZG_BIT + */ +bool MPU6050::getStandbyZGyroEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, buffer); + return buffer[0]; +} +/** Set Z-axis gyroscope standby enabled status. + * @param New Z-axis standby enabled status + * @see getStandbyZGyroEnabled() + * @see MPU6050_RA_PWR_MGMT_2 + * @see MPU6050_PWR2_STBY_ZG_BIT + */ +void MPU6050::setStandbyZGyroEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled); +} + +// FIFO_COUNT* registers + +/** Get current FIFO buffer size. + * This value indicates the number of bytes stored in the FIFO buffer. This + * number is in turn the number of bytes that can be read from the FIFO buffer + * and it is directly proportional to the number of samples available given the + * set of sensor data bound to be stored in the FIFO (register 35 and 36). + * @return Current FIFO buffer size + */ +uint16_t MPU6050::getFIFOCount() { + I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer); + return (((uint16_t)buffer[0]) << 8) | buffer[1]; +} + +// FIFO_R_W register + +/** Get byte from FIFO buffer. + * This register is used to read and write data from the FIFO buffer. Data is + * written to the FIFO in order of register number (from lowest to highest). If + * all the FIFO enable flags (see below) are enabled and all External Sensor + * Data registers (Registers 73 to 96) are associated with a Slave device, the + * contents of registers 59 through 96 will be written in order at the Sample + * Rate. + * + * The contents of the sensor data registers (Registers 59 to 96) are written + * into the FIFO buffer when their corresponding FIFO enable flags are set to 1 + * in FIFO_EN (Register 35). An additional flag for the sensor data registers + * associated with I2C Slave 3 can be found in I2C_MST_CTRL (Register 36). + * + * If the FIFO buffer has overflowed, the status bit FIFO_OFLOW_INT is + * automatically set to 1. This bit is located in INT_STATUS (Register 58). + * When the FIFO buffer has overflowed, the oldest data will be lost and new + * data will be written to the FIFO. + * + * If the FIFO buffer is empty, reading this register will return the last byte + * that was previously read from the FIFO until new data is available. The user + * should check FIFO_COUNT to ensure that the FIFO buffer is not read when + * empty. + * + * @return Byte from FIFO buffer + */ +uint8_t MPU6050::getFIFOByte() { + I2Cdev::readByte(devAddr, MPU6050_RA_FIFO_R_W, buffer); + return buffer[0]; +} +void MPU6050::getFIFOBytes(uint8_t *data, uint8_t length) { + if(length > 0){ + I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data); + } else { + *data = 0; + } +} +/** Write byte to FIFO buffer. + * @see getFIFOByte() + * @see MPU6050_RA_FIFO_R_W + */ +void MPU6050::setFIFOByte(uint8_t data) { + I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data); +} + +// WHO_AM_I register + +/** Get Device ID. + * This register is used to verify the identity of the device (0b110100, 0x34). + * @return Device ID (6 bits only! should be 0x34) + * @see MPU6050_RA_WHO_AM_I + * @see MPU6050_WHO_AM_I_BIT + * @see MPU6050_WHO_AM_I_LENGTH + */ +uint8_t MPU6050::getDeviceID() { + I2Cdev::readBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer); + return buffer[0]; +} +/** Set Device ID. + * Write a new ID into the WHO_AM_I register (no idea why this should ever be + * necessary though). + * @param id New device ID to set. + * @see getDeviceID() + * @see MPU6050_RA_WHO_AM_I + * @see MPU6050_WHO_AM_I_BIT + * @see MPU6050_WHO_AM_I_LENGTH + */ +void MPU6050::setDeviceID(uint8_t id) { + I2Cdev::writeBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, id); +} + +// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== + +// XG_OFFS_TC register + +uint8_t MPU6050::getOTPBankValid() { + I2Cdev::readBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer); + return buffer[0]; +} +void MPU6050::setOTPBankValid(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled); +} +int8_t MPU6050::getXGyroOffsetTC() { + I2Cdev::readBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer); + return buffer[0]; +} +void MPU6050::setXGyroOffsetTC(int8_t offset) { + I2Cdev::writeBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); +} + +// YG_OFFS_TC register + +int8_t MPU6050::getYGyroOffsetTC() { + I2Cdev::readBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer); + return buffer[0]; +} +void MPU6050::setYGyroOffsetTC(int8_t offset) { + I2Cdev::writeBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); +} + +// ZG_OFFS_TC register + +int8_t MPU6050::getZGyroOffsetTC() { + I2Cdev::readBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer); + return buffer[0]; +} +void MPU6050::setZGyroOffsetTC(int8_t offset) { + I2Cdev::writeBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); +} + +// X_FINE_GAIN register + +int8_t MPU6050::getXFineGain() { + I2Cdev::readByte(devAddr, MPU6050_RA_X_FINE_GAIN, buffer); + return buffer[0]; +} +void MPU6050::setXFineGain(int8_t gain) { + I2Cdev::writeByte(devAddr, MPU6050_RA_X_FINE_GAIN, gain); +} + +// Y_FINE_GAIN register + +int8_t MPU6050::getYFineGain() { + I2Cdev::readByte(devAddr, MPU6050_RA_Y_FINE_GAIN, buffer); + return buffer[0]; +} +void MPU6050::setYFineGain(int8_t gain) { + I2Cdev::writeByte(devAddr, MPU6050_RA_Y_FINE_GAIN, gain); +} + +// Z_FINE_GAIN register + +int8_t MPU6050::getZFineGain() { + I2Cdev::readByte(devAddr, MPU6050_RA_Z_FINE_GAIN, buffer); + return buffer[0]; +} +void MPU6050::setZFineGain(int8_t gain) { + I2Cdev::writeByte(devAddr, MPU6050_RA_Z_FINE_GAIN, gain); +} + +// XA_OFFS_* registers + +int16_t MPU6050::getXAccelOffset() { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +void MPU6050::setXAccelOffset(int16_t offset) { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::writeWord(devAddr, SaveAddress, offset); +} + +// YA_OFFS_* register + +int16_t MPU6050::getYAccelOffset() { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_YA_OFFS_H:0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +void MPU6050::setYAccelOffset(int16_t offset) { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_YA_OFFS_H:0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::writeWord(devAddr, SaveAddress, offset); +} + +// ZA_OFFS_* register + +int16_t MPU6050::getZAccelOffset() { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_ZA_OFFS_H:0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +void MPU6050::setZAccelOffset(int16_t offset) { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_ZA_OFFS_H:0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::writeWord(devAddr, SaveAddress, offset); +} + +// XG_OFFS_USR* registers + +int16_t MPU6050::getXGyroOffset() { + I2Cdev::readBytes(devAddr, MPU6050_RA_XG_OFFS_USRH, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +void MPU6050::setXGyroOffset(int16_t offset) { + I2Cdev::writeWord(devAddr, MPU6050_RA_XG_OFFS_USRH, offset); +} + +// YG_OFFS_USR* register + +int16_t MPU6050::getYGyroOffset() { + I2Cdev::readBytes(devAddr, MPU6050_RA_YG_OFFS_USRH, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +void MPU6050::setYGyroOffset(int16_t offset) { + I2Cdev::writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset); +} + +// ZG_OFFS_USR* register + +int16_t MPU6050::getZGyroOffset() { + I2Cdev::readBytes(devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} +void MPU6050::setZGyroOffset(int16_t offset) { + I2Cdev::writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset); +} + +// INT_ENABLE register (DMP functions) + +bool MPU6050::getIntPLLReadyEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer); + return buffer[0]; +} +void MPU6050::setIntPLLReadyEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled); +} +bool MPU6050::getIntDMPEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, buffer); + return buffer[0]; +} +void MPU6050::setIntDMPEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled); +} + +// DMP_INT_STATUS + +bool MPU6050::getDMPInt5Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer); + return buffer[0]; +} +bool MPU6050::getDMPInt4Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer); + return buffer[0]; +} +bool MPU6050::getDMPInt3Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, buffer); + return buffer[0]; +} +bool MPU6050::getDMPInt2Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer); + return buffer[0]; +} +bool MPU6050::getDMPInt1Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, buffer); + return buffer[0]; +} +bool MPU6050::getDMPInt0Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, buffer); + return buffer[0]; +} + +// INT_STATUS register (DMP functions) + +bool MPU6050::getIntPLLReadyStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer); + return buffer[0]; +} +bool MPU6050::getIntDMPStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, buffer); + return buffer[0]; +} + +// USER_CTRL register (DMP functions) + +bool MPU6050::getDMPEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer); + return buffer[0]; +} +void MPU6050::setDMPEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled); +} +void MPU6050::resetDMP() { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true); +} + +// BANK_SEL register + +void MPU6050::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) { + bank &= 0x1F; + if (userBank) bank |= 0x20; + if (prefetchEnabled) bank |= 0x40; + I2Cdev::writeByte(devAddr, MPU6050_RA_BANK_SEL, bank); +} + +// MEM_START_ADDR register + +void MPU6050::setMemoryStartAddress(uint8_t address) { + I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address); +} + +// MEM_R_W register + +uint8_t MPU6050::readMemoryByte() { + I2Cdev::readByte(devAddr, MPU6050_RA_MEM_R_W, buffer); + return buffer[0]; +} +void MPU6050::writeMemoryByte(uint8_t data) { + I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_R_W, data); +} +void MPU6050::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) { + setMemoryBank(bank); + setMemoryStartAddress(address); + uint8_t chunkSize; + for (uint16_t i = 0; i < dataSize;) { + // determine correct chunk size according to bank position and data size + chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE; + + // make sure we don't go past the data size + if (i + chunkSize > dataSize) chunkSize = dataSize - i; + + // make sure this chunk doesn't go past the bank boundary (256 bytes) + if (chunkSize > 256 - address) chunkSize = 256 - address; + + // read the chunk of data as specified + I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i); + + // increase byte index by [chunkSize] + i += chunkSize; + + // uint8_t automatically wraps to 0 at 256 + address += chunkSize; + + // if we aren't done, update bank (if necessary) and address + if (i < dataSize) { + if (address == 0) bank++; + setMemoryBank(bank); + setMemoryStartAddress(address); + } + } +} +bool MPU6050::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) { + setMemoryBank(bank); + setMemoryStartAddress(address); + uint8_t chunkSize; + uint8_t *verifyBuffer=0; + uint8_t *progBuffer=0; + uint16_t i; + uint8_t j; + if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); + if (useProgMem) progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); + for (i = 0; i < dataSize;) { + // determine correct chunk size according to bank position and data size + chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE; + + // make sure we don't go past the data size + if (i + chunkSize > dataSize) chunkSize = dataSize - i; + + // make sure this chunk doesn't go past the bank boundary (256 bytes) + if (chunkSize > 256 - address) chunkSize = 256 - address; + + if (useProgMem) { + // write the chunk of data as specified + for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j); + } else { + // write the chunk of data as specified + progBuffer = (uint8_t *)data + i; + } + + I2Cdev::writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer); + + // verify data if needed + if (verify && verifyBuffer) { + setMemoryBank(bank); + setMemoryStartAddress(address); + I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer); + if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) { + /*Serial.print("Block write verification error, bank "); + Serial.print(bank, DEC); + Serial.print(", address "); + Serial.print(address, DEC); + Serial.print("!\nExpected:"); + for (j = 0; j < chunkSize; j++) { + Serial.print(" 0x"); + if (progBuffer[j] < 16) Serial.print("0"); + Serial.print(progBuffer[j], HEX); + } + Serial.print("\nReceived:"); + for (uint8_t j = 0; j < chunkSize; j++) { + Serial.print(" 0x"); + if (verifyBuffer[i + j] < 16) Serial.print("0"); + Serial.print(verifyBuffer[i + j], HEX); + } + Serial.print("\n");*/ + free(verifyBuffer); + if (useProgMem) free(progBuffer); + return false; // uh oh. + } + } + + // increase byte index by [chunkSize] + i += chunkSize; + + // uint8_t automatically wraps to 0 at 256 + address += chunkSize; + + // if we aren't done, update bank (if necessary) and address + if (i < dataSize) { + if (address == 0) bank++; + setMemoryBank(bank); + setMemoryStartAddress(address); + } + } + if (verify) free(verifyBuffer); + if (useProgMem) free(progBuffer); + return true; +} +bool MPU6050::writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) { + return writeMemoryBlock(data, dataSize, bank, address, verify, true); +} +bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) { + uint8_t *progBuffer = 0; + uint8_t success, special; + uint16_t i, j; + if (useProgMem) { + progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary + } + + // config set data is a long string of blocks with the following structure: + // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]] + uint8_t bank, offset, length; + for (i = 0; i < dataSize;) { + if (useProgMem) { + bank = pgm_read_byte(data + i++); + offset = pgm_read_byte(data + i++); + length = pgm_read_byte(data + i++); + } else { + bank = data[i++]; + offset = data[i++]; + length = data[i++]; + } + + // write data or perform special action + if (length > 0) { + // regular block of data to write + /*Serial.print("Writing config block to bank "); + Serial.print(bank); + Serial.print(", offset "); + Serial.print(offset); + Serial.print(", length="); + Serial.println(length);*/ + if (useProgMem) { + if (sizeof(progBuffer) < length) progBuffer = (uint8_t *)realloc(progBuffer, length); + for (j = 0; j < length; j++) progBuffer[j] = pgm_read_byte(data + i + j); + } else { + progBuffer = (uint8_t *)data + i; + } + success = writeMemoryBlock(progBuffer, length, bank, offset, true); + i += length; + } else { + // special instruction + // NOTE: this kind of behavior (what and when to do certain things) + // is totally undocumented. This code is in here based on observed + // behavior only, and exactly why (or even whether) it has to be here + // is anybody's guess for now. + if (useProgMem) { + special = pgm_read_byte(data + i++); + } else { + special = data[i++]; + } + /*Serial.print("Special command code "); + Serial.print(special, HEX); + Serial.println(" found...");*/ + if (special == 0x01) { + // enable DMP-related interrupts + + //setIntZeroMotionEnabled(true); + //setIntFIFOBufferOverflowEnabled(true); + //setIntDMPEnabled(true); + I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, 0x32); // single operation + + success = true; + } else { + // unknown special command + success = false; + } + } + + if (!success) { + if (useProgMem) free(progBuffer); + return false; // uh oh + } + } + if (useProgMem) free(progBuffer); + return true; +} +bool MPU6050::writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) { + return writeDMPConfigurationSet(data, dataSize, true); +} + +// DMP_CFG_1 register + +uint8_t MPU6050::getDMPConfig1() { + I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_1, buffer); + return buffer[0]; +} +void MPU6050::setDMPConfig1(uint8_t config) { + I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config); +} + +// DMP_CFG_2 register + +uint8_t MPU6050::getDMPConfig2() { + I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_2, buffer); + return buffer[0]; +} +void MPU6050::setDMPConfig2(uint8_t config) { + I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config); +} + + +//*************************************************************************************** +//********************** Calibration Routines ********************** +//*************************************************************************************** +/** + @brief Fully calibrate Gyro from ZERO in about 6-7 Loops 600-700 readings +*/ +void MPU6050::CalibrateGyro(uint8_t Loops ) { + double kP = 0.3; + double kI = 90; + float x; + x = (100 - map(Loops, 1, 5, 20, 0)) * .01; + kP *= x; + kI *= x; + + PID( 0x43, kP, kI, Loops); +} + +/** + @brief Fully calibrate Accel from ZERO in about 6-7 Loops 600-700 readings +*/ +void MPU6050::CalibrateAccel(uint8_t Loops ) { + + float kP = 0.3; + float kI = 20; + float x; + x = (100 - map(Loops, 1, 5, 20, 0)) * .01; + kP *= x; + kI *= x; + PID( 0x3B, kP, kI, Loops); +} + +void MPU6050::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){ + uint8_t SaveAddress = (ReadAddress == 0x3B)?((getDeviceID() < 0x38 )? 0x06:0x77):0x13; + + int16_t Data; + float Reading; + int16_t BitZero[3]; + uint8_t shift =(SaveAddress == 0x77)?3:2; + float Error, PTerm, ITerm[3]; + int16_t eSample; + uint32_t eSum ; + Serial.write('>'); + for (int i = 0; i < 3; i++) { + I2Cdev::readWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word) + Reading = Data; + if(SaveAddress != 0x13){ + BitZero[i] = Data & 1; // Capture Bit Zero to properly handle Accelerometer calibration + ITerm[i] = ((float)Reading) * 8; + } else { + ITerm[i] = Reading * 4; + } + } + for (int L = 0; L < Loops; L++) { + eSample = 0; + for (int c = 0; c < 100; c++) {// 100 PI Calculations + eSum = 0; + for (int i = 0; i < 3; i++) { + I2Cdev::readWords(devAddr, ReadAddress + (i * 2), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word) + Reading = Data; + if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384; //remove Gravity + Error = -Reading; + eSum += abs(Reading); + PTerm = kP * Error; + ITerm[i] += (Error * 0.001) * kI; // Integral term 1000 Calculations a second = 0.001 + if(SaveAddress != 0x13){ + Data = round((PTerm + ITerm[i] ) / 8); //Compute PID Output + Data = ((Data)&0xFFFE) |BitZero[i]; // Insert Bit0 Saved at beginning + } else Data = round((PTerm + ITerm[i] ) / 4); //Compute PID Output + I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data); + } + if((c == 99) && eSum > 1000){ // Error is still to great to continue + c = 0; + Serial.write('*'); + } + if((eSum * ((ReadAddress == 0x3B)?.05: 1)) < 5) eSample++; // Successfully found offsets prepare to advance + if((eSum < 100) && (c > 10) && (eSample >= 10)) break; // Advance to next Loop + delay(1); + } + Serial.write('.'); + kP *= .75; + kI *= .75; + for (int i = 0; i < 3; i++){ + if(SaveAddress != 0x13) { + Data = round((ITerm[i] ) / 8); //Compute PID Output + Data = ((Data)&0xFFFE) |BitZero[i]; // Insert Bit0 Saved at beginning + } else Data = round((ITerm[i]) / 4); + I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data); + } + } + resetFIFO(); + resetDMP(); +} + +#define printfloatx(Name,Variable,Spaces,Precision,EndTxt) { Serial.print(F(Name)); {char S[(Spaces + Precision + 3)];Serial.print(F(" ")); Serial.print(dtostrf((float)Variable,Spaces,Precision ,S));}Serial.print(F(EndTxt)); }//Name,Variable,Spaces,Precision,EndTxt +void MPU6050::PrintActiveOffsets() { + uint8_t AOffsetRegister = (getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77; + int16_t Data[3]; + //Serial.print(F("Offset Register 0x")); + //Serial.print(AOffsetRegister>>4,HEX);Serial.print(AOffsetRegister&0x0F,HEX); + Serial.print(F("\n// X Accel Y Accel Z Accel X Gyro Y Gyro Z Gyro\n//OFFSETS ")); + if(AOffsetRegister == 0x06) I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)Data); + else { + I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)Data); + I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)Data+1); + I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)Data+2); + } + // A_OFFSET_H_READ_A_OFFS(Data); + printfloatx("", Data[0], 5, 0, ", "); + printfloatx("", Data[1], 5, 0, ", "); + printfloatx("", Data[2], 5, 0, ", "); + I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)Data); + // XG_OFFSET_H_READ_OFFS_USR(Data); + printfloatx("", Data[0], 5, 0, ", "); + printfloatx("", Data[1], 5, 0, ", "); + printfloatx("", Data[2], 5, 0, "\n"); +} diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050.h new file mode 100644 index 00000000..871f423a --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050.h @@ -0,0 +1,1041 @@ +// I2Cdev library collection - MPU6050 I2C device class +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 10/3/2011 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// ... - ongoing debug release + +// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE +// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF +// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING. + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _MPU6050_H_ +#define _MPU6050_H_ + +#include "I2Cdev.h" + +// supporting link: http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517 +// also: http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s + +#ifdef __AVR__ +#include +#elif defined(ARDUINO_ARCH_SAMD) +#include +#else +//#define PROGMEM /* empty */ +//#define pgm_read_byte(x) (*(x)) +//#define pgm_read_word(x) (*(x)) +//#define pgm_read_float(x) (*(x)) +//#define PSTR(STR) STR +#endif + + +#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board +#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) +#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW + +#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN +#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN +#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN +#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS +#define MPU6050_RA_XA_OFFS_L_TC 0x07 +#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS +#define MPU6050_RA_YA_OFFS_L_TC 0x09 +#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS +#define MPU6050_RA_ZA_OFFS_L_TC 0x0B +#define MPU6050_RA_SELF_TEST_X 0x0D //[7:5] XA_TEST[4-2], [4:0] XG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_Y 0x0E //[7:5] YA_TEST[4-2], [4:0] YG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_Z 0x0F //[7:5] ZA_TEST[4-2], [4:0] ZG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_A 0x10 //[5:4] XA_TEST[1-0], [3:2] YA_TEST[1-0], [1:0] ZA_TEST[1-0] +#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR +#define MPU6050_RA_XG_OFFS_USRL 0x14 +#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR +#define MPU6050_RA_YG_OFFS_USRL 0x16 +#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR +#define MPU6050_RA_ZG_OFFS_USRL 0x18 +#define MPU6050_RA_SMPLRT_DIV 0x19 +#define MPU6050_RA_CONFIG 0x1A +#define MPU6050_RA_GYRO_CONFIG 0x1B +#define MPU6050_RA_ACCEL_CONFIG 0x1C +#define MPU6050_RA_FF_THR 0x1D +#define MPU6050_RA_FF_DUR 0x1E +#define MPU6050_RA_MOT_THR 0x1F +#define MPU6050_RA_MOT_DUR 0x20 +#define MPU6050_RA_ZRMOT_THR 0x21 +#define MPU6050_RA_ZRMOT_DUR 0x22 +#define MPU6050_RA_FIFO_EN 0x23 +#define MPU6050_RA_I2C_MST_CTRL 0x24 +#define MPU6050_RA_I2C_SLV0_ADDR 0x25 +#define MPU6050_RA_I2C_SLV0_REG 0x26 +#define MPU6050_RA_I2C_SLV0_CTRL 0x27 +#define MPU6050_RA_I2C_SLV1_ADDR 0x28 +#define MPU6050_RA_I2C_SLV1_REG 0x29 +#define MPU6050_RA_I2C_SLV1_CTRL 0x2A +#define MPU6050_RA_I2C_SLV2_ADDR 0x2B +#define MPU6050_RA_I2C_SLV2_REG 0x2C +#define MPU6050_RA_I2C_SLV2_CTRL 0x2D +#define MPU6050_RA_I2C_SLV3_ADDR 0x2E +#define MPU6050_RA_I2C_SLV3_REG 0x2F +#define MPU6050_RA_I2C_SLV3_CTRL 0x30 +#define MPU6050_RA_I2C_SLV4_ADDR 0x31 +#define MPU6050_RA_I2C_SLV4_REG 0x32 +#define MPU6050_RA_I2C_SLV4_DO 0x33 +#define MPU6050_RA_I2C_SLV4_CTRL 0x34 +#define MPU6050_RA_I2C_SLV4_DI 0x35 +#define MPU6050_RA_I2C_MST_STATUS 0x36 +#define MPU6050_RA_INT_PIN_CFG 0x37 +#define MPU6050_RA_INT_ENABLE 0x38 +#define MPU6050_RA_DMP_INT_STATUS 0x39 +#define MPU6050_RA_INT_STATUS 0x3A +#define MPU6050_RA_ACCEL_XOUT_H 0x3B +#define MPU6050_RA_ACCEL_XOUT_L 0x3C +#define MPU6050_RA_ACCEL_YOUT_H 0x3D +#define MPU6050_RA_ACCEL_YOUT_L 0x3E +#define MPU6050_RA_ACCEL_ZOUT_H 0x3F +#define MPU6050_RA_ACCEL_ZOUT_L 0x40 +#define MPU6050_RA_TEMP_OUT_H 0x41 +#define MPU6050_RA_TEMP_OUT_L 0x42 +#define MPU6050_RA_GYRO_XOUT_H 0x43 +#define MPU6050_RA_GYRO_XOUT_L 0x44 +#define MPU6050_RA_GYRO_YOUT_H 0x45 +#define MPU6050_RA_GYRO_YOUT_L 0x46 +#define MPU6050_RA_GYRO_ZOUT_H 0x47 +#define MPU6050_RA_GYRO_ZOUT_L 0x48 +#define MPU6050_RA_EXT_SENS_DATA_00 0x49 +#define MPU6050_RA_EXT_SENS_DATA_01 0x4A +#define MPU6050_RA_EXT_SENS_DATA_02 0x4B +#define MPU6050_RA_EXT_SENS_DATA_03 0x4C +#define MPU6050_RA_EXT_SENS_DATA_04 0x4D +#define MPU6050_RA_EXT_SENS_DATA_05 0x4E +#define MPU6050_RA_EXT_SENS_DATA_06 0x4F +#define MPU6050_RA_EXT_SENS_DATA_07 0x50 +#define MPU6050_RA_EXT_SENS_DATA_08 0x51 +#define MPU6050_RA_EXT_SENS_DATA_09 0x52 +#define MPU6050_RA_EXT_SENS_DATA_10 0x53 +#define MPU6050_RA_EXT_SENS_DATA_11 0x54 +#define MPU6050_RA_EXT_SENS_DATA_12 0x55 +#define MPU6050_RA_EXT_SENS_DATA_13 0x56 +#define MPU6050_RA_EXT_SENS_DATA_14 0x57 +#define MPU6050_RA_EXT_SENS_DATA_15 0x58 +#define MPU6050_RA_EXT_SENS_DATA_16 0x59 +#define MPU6050_RA_EXT_SENS_DATA_17 0x5A +#define MPU6050_RA_EXT_SENS_DATA_18 0x5B +#define MPU6050_RA_EXT_SENS_DATA_19 0x5C +#define MPU6050_RA_EXT_SENS_DATA_20 0x5D +#define MPU6050_RA_EXT_SENS_DATA_21 0x5E +#define MPU6050_RA_EXT_SENS_DATA_22 0x5F +#define MPU6050_RA_EXT_SENS_DATA_23 0x60 +#define MPU6050_RA_MOT_DETECT_STATUS 0x61 +#define MPU6050_RA_I2C_SLV0_DO 0x63 +#define MPU6050_RA_I2C_SLV1_DO 0x64 +#define MPU6050_RA_I2C_SLV2_DO 0x65 +#define MPU6050_RA_I2C_SLV3_DO 0x66 +#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 +#define MPU6050_RA_SIGNAL_PATH_RESET 0x68 +#define MPU6050_RA_MOT_DETECT_CTRL 0x69 +#define MPU6050_RA_USER_CTRL 0x6A +#define MPU6050_RA_PWR_MGMT_1 0x6B +#define MPU6050_RA_PWR_MGMT_2 0x6C +#define MPU6050_RA_BANK_SEL 0x6D +#define MPU6050_RA_MEM_START_ADDR 0x6E +#define MPU6050_RA_MEM_R_W 0x6F +#define MPU6050_RA_DMP_CFG_1 0x70 +#define MPU6050_RA_DMP_CFG_2 0x71 +#define MPU6050_RA_FIFO_COUNTH 0x72 +#define MPU6050_RA_FIFO_COUNTL 0x73 +#define MPU6050_RA_FIFO_R_W 0x74 +#define MPU6050_RA_WHO_AM_I 0x75 + +#define MPU6050_SELF_TEST_XA_1_BIT 0x07 +#define MPU6050_SELF_TEST_XA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_XA_2_BIT 0x05 +#define MPU6050_SELF_TEST_XA_2_LENGTH 0x02 +#define MPU6050_SELF_TEST_YA_1_BIT 0x07 +#define MPU6050_SELF_TEST_YA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_YA_2_BIT 0x03 +#define MPU6050_SELF_TEST_YA_2_LENGTH 0x02 +#define MPU6050_SELF_TEST_ZA_1_BIT 0x07 +#define MPU6050_SELF_TEST_ZA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_ZA_2_BIT 0x01 +#define MPU6050_SELF_TEST_ZA_2_LENGTH 0x02 + +#define MPU6050_SELF_TEST_XG_1_BIT 0x04 +#define MPU6050_SELF_TEST_XG_1_LENGTH 0x05 +#define MPU6050_SELF_TEST_YG_1_BIT 0x04 +#define MPU6050_SELF_TEST_YG_1_LENGTH 0x05 +#define MPU6050_SELF_TEST_ZG_1_BIT 0x04 +#define MPU6050_SELF_TEST_ZG_1_LENGTH 0x05 + +#define MPU6050_TC_PWR_MODE_BIT 7 +#define MPU6050_TC_OFFSET_BIT 6 +#define MPU6050_TC_OFFSET_LENGTH 6 +#define MPU6050_TC_OTP_BNK_VLD_BIT 0 + +#define MPU6050_VDDIO_LEVEL_VLOGIC 0 +#define MPU6050_VDDIO_LEVEL_VDD 1 + +#define MPU6050_CFG_EXT_SYNC_SET_BIT 5 +#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 +#define MPU6050_CFG_DLPF_CFG_BIT 2 +#define MPU6050_CFG_DLPF_CFG_LENGTH 3 + +#define MPU6050_EXT_SYNC_DISABLED 0x0 +#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 +#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 +#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 +#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 +#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 +#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 +#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 + +#define MPU6050_DLPF_BW_256 0x00 +#define MPU6050_DLPF_BW_188 0x01 +#define MPU6050_DLPF_BW_98 0x02 +#define MPU6050_DLPF_BW_42 0x03 +#define MPU6050_DLPF_BW_20 0x04 +#define MPU6050_DLPF_BW_10 0x05 +#define MPU6050_DLPF_BW_5 0x06 + +#define MPU6050_GCONFIG_FS_SEL_BIT 4 +#define MPU6050_GCONFIG_FS_SEL_LENGTH 2 + +#define MPU6050_GYRO_FS_250 0x00 +#define MPU6050_GYRO_FS_500 0x01 +#define MPU6050_GYRO_FS_1000 0x02 +#define MPU6050_GYRO_FS_2000 0x03 + +#define MPU6050_ACONFIG_XA_ST_BIT 7 +#define MPU6050_ACONFIG_YA_ST_BIT 6 +#define MPU6050_ACONFIG_ZA_ST_BIT 5 +#define MPU6050_ACONFIG_AFS_SEL_BIT 4 +#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 +#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 +#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 + +#define MPU6050_ACCEL_FS_2 0x00 +#define MPU6050_ACCEL_FS_4 0x01 +#define MPU6050_ACCEL_FS_8 0x02 +#define MPU6050_ACCEL_FS_16 0x03 + +#define MPU6050_DHPF_RESET 0x00 +#define MPU6050_DHPF_5 0x01 +#define MPU6050_DHPF_2P5 0x02 +#define MPU6050_DHPF_1P25 0x03 +#define MPU6050_DHPF_0P63 0x04 +#define MPU6050_DHPF_HOLD 0x07 + +#define MPU6050_TEMP_FIFO_EN_BIT 7 +#define MPU6050_XG_FIFO_EN_BIT 6 +#define MPU6050_YG_FIFO_EN_BIT 5 +#define MPU6050_ZG_FIFO_EN_BIT 4 +#define MPU6050_ACCEL_FIFO_EN_BIT 3 +#define MPU6050_SLV2_FIFO_EN_BIT 2 +#define MPU6050_SLV1_FIFO_EN_BIT 1 +#define MPU6050_SLV0_FIFO_EN_BIT 0 + +#define MPU6050_MULT_MST_EN_BIT 7 +#define MPU6050_WAIT_FOR_ES_BIT 6 +#define MPU6050_SLV_3_FIFO_EN_BIT 5 +#define MPU6050_I2C_MST_P_NSR_BIT 4 +#define MPU6050_I2C_MST_CLK_BIT 3 +#define MPU6050_I2C_MST_CLK_LENGTH 4 + +#define MPU6050_CLOCK_DIV_348 0x0 +#define MPU6050_CLOCK_DIV_333 0x1 +#define MPU6050_CLOCK_DIV_320 0x2 +#define MPU6050_CLOCK_DIV_308 0x3 +#define MPU6050_CLOCK_DIV_296 0x4 +#define MPU6050_CLOCK_DIV_286 0x5 +#define MPU6050_CLOCK_DIV_276 0x6 +#define MPU6050_CLOCK_DIV_267 0x7 +#define MPU6050_CLOCK_DIV_258 0x8 +#define MPU6050_CLOCK_DIV_500 0x9 +#define MPU6050_CLOCK_DIV_471 0xA +#define MPU6050_CLOCK_DIV_444 0xB +#define MPU6050_CLOCK_DIV_421 0xC +#define MPU6050_CLOCK_DIV_400 0xD +#define MPU6050_CLOCK_DIV_381 0xE +#define MPU6050_CLOCK_DIV_364 0xF + +#define MPU6050_I2C_SLV_RW_BIT 7 +#define MPU6050_I2C_SLV_ADDR_BIT 6 +#define MPU6050_I2C_SLV_ADDR_LENGTH 7 +#define MPU6050_I2C_SLV_EN_BIT 7 +#define MPU6050_I2C_SLV_BYTE_SW_BIT 6 +#define MPU6050_I2C_SLV_REG_DIS_BIT 5 +#define MPU6050_I2C_SLV_GRP_BIT 4 +#define MPU6050_I2C_SLV_LEN_BIT 3 +#define MPU6050_I2C_SLV_LEN_LENGTH 4 + +#define MPU6050_I2C_SLV4_RW_BIT 7 +#define MPU6050_I2C_SLV4_ADDR_BIT 6 +#define MPU6050_I2C_SLV4_ADDR_LENGTH 7 +#define MPU6050_I2C_SLV4_EN_BIT 7 +#define MPU6050_I2C_SLV4_INT_EN_BIT 6 +#define MPU6050_I2C_SLV4_REG_DIS_BIT 5 +#define MPU6050_I2C_SLV4_MST_DLY_BIT 4 +#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 + +#define MPU6050_MST_PASS_THROUGH_BIT 7 +#define MPU6050_MST_I2C_SLV4_DONE_BIT 6 +#define MPU6050_MST_I2C_LOST_ARB_BIT 5 +#define MPU6050_MST_I2C_SLV4_NACK_BIT 4 +#define MPU6050_MST_I2C_SLV3_NACK_BIT 3 +#define MPU6050_MST_I2C_SLV2_NACK_BIT 2 +#define MPU6050_MST_I2C_SLV1_NACK_BIT 1 +#define MPU6050_MST_I2C_SLV0_NACK_BIT 0 + +#define MPU6050_INTCFG_INT_LEVEL_BIT 7 +#define MPU6050_INTCFG_INT_OPEN_BIT 6 +#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 +#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 +#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 +#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 +#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 +#define MPU6050_INTCFG_CLKOUT_EN_BIT 0 + +#define MPU6050_INTMODE_ACTIVEHIGH 0x00 +#define MPU6050_INTMODE_ACTIVELOW 0x01 + +#define MPU6050_INTDRV_PUSHPULL 0x00 +#define MPU6050_INTDRV_OPENDRAIN 0x01 + +#define MPU6050_INTLATCH_50USPULSE 0x00 +#define MPU6050_INTLATCH_WAITCLEAR 0x01 + +#define MPU6050_INTCLEAR_STATUSREAD 0x00 +#define MPU6050_INTCLEAR_ANYREAD 0x01 + +#define MPU6050_INTERRUPT_FF_BIT 7 +#define MPU6050_INTERRUPT_MOT_BIT 6 +#define MPU6050_INTERRUPT_ZMOT_BIT 5 +#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 +#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 +#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 +#define MPU6050_INTERRUPT_DMP_INT_BIT 1 +#define MPU6050_INTERRUPT_DATA_RDY_BIT 0 + +// TODO: figure out what these actually do +// UMPL source code is not very obivous +#define MPU6050_DMPINT_5_BIT 5 +#define MPU6050_DMPINT_4_BIT 4 +#define MPU6050_DMPINT_3_BIT 3 +#define MPU6050_DMPINT_2_BIT 2 +#define MPU6050_DMPINT_1_BIT 1 +#define MPU6050_DMPINT_0_BIT 0 + +#define MPU6050_MOTION_MOT_XNEG_BIT 7 +#define MPU6050_MOTION_MOT_XPOS_BIT 6 +#define MPU6050_MOTION_MOT_YNEG_BIT 5 +#define MPU6050_MOTION_MOT_YPOS_BIT 4 +#define MPU6050_MOTION_MOT_ZNEG_BIT 3 +#define MPU6050_MOTION_MOT_ZPOS_BIT 2 +#define MPU6050_MOTION_MOT_ZRMOT_BIT 0 + +#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 +#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 +#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 +#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 +#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 +#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 + +#define MPU6050_PATHRESET_GYRO_RESET_BIT 2 +#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 +#define MPU6050_PATHRESET_TEMP_RESET_BIT 0 + +#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 +#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 +#define MPU6050_DETECT_FF_COUNT_BIT 3 +#define MPU6050_DETECT_FF_COUNT_LENGTH 2 +#define MPU6050_DETECT_MOT_COUNT_BIT 1 +#define MPU6050_DETECT_MOT_COUNT_LENGTH 2 + +#define MPU6050_DETECT_DECREMENT_RESET 0x0 +#define MPU6050_DETECT_DECREMENT_1 0x1 +#define MPU6050_DETECT_DECREMENT_2 0x2 +#define MPU6050_DETECT_DECREMENT_4 0x3 + +#define MPU6050_USERCTRL_DMP_EN_BIT 7 +#define MPU6050_USERCTRL_FIFO_EN_BIT 6 +#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 +#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 +#define MPU6050_USERCTRL_DMP_RESET_BIT 3 +#define MPU6050_USERCTRL_FIFO_RESET_BIT 2 +#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 +#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 + +#define MPU6050_PWR1_DEVICE_RESET_BIT 7 +#define MPU6050_PWR1_SLEEP_BIT 6 +#define MPU6050_PWR1_CYCLE_BIT 5 +#define MPU6050_PWR1_TEMP_DIS_BIT 3 +#define MPU6050_PWR1_CLKSEL_BIT 2 +#define MPU6050_PWR1_CLKSEL_LENGTH 3 + +#define MPU6050_CLOCK_INTERNAL 0x00 +#define MPU6050_CLOCK_PLL_XGYRO 0x01 +#define MPU6050_CLOCK_PLL_YGYRO 0x02 +#define MPU6050_CLOCK_PLL_ZGYRO 0x03 +#define MPU6050_CLOCK_PLL_EXT32K 0x04 +#define MPU6050_CLOCK_PLL_EXT19M 0x05 +#define MPU6050_CLOCK_KEEP_RESET 0x07 + +#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 +#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 +#define MPU6050_PWR2_STBY_XA_BIT 5 +#define MPU6050_PWR2_STBY_YA_BIT 4 +#define MPU6050_PWR2_STBY_ZA_BIT 3 +#define MPU6050_PWR2_STBY_XG_BIT 2 +#define MPU6050_PWR2_STBY_YG_BIT 1 +#define MPU6050_PWR2_STBY_ZG_BIT 0 + +#define MPU6050_WAKE_FREQ_1P25 0x0 +#define MPU6050_WAKE_FREQ_2P5 0x1 +#define MPU6050_WAKE_FREQ_5 0x2 +#define MPU6050_WAKE_FREQ_10 0x3 + +#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 +#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 +#define MPU6050_BANKSEL_MEM_SEL_BIT 4 +#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 + +#define MPU6050_WHO_AM_I_BIT 6 +#define MPU6050_WHO_AM_I_LENGTH 6 + +#define MPU6050_DMP_MEMORY_BANKS 8 +#define MPU6050_DMP_MEMORY_BANK_SIZE 256 +#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 + +// note: DMP code memory blocks defined at end of header file + +class MPU6050 { + public: + MPU6050(uint8_t address=MPU6050_DEFAULT_ADDRESS); + + void initialize(); + bool testConnection(); + + // AUX_VDDIO register + uint8_t getAuxVDDIOLevel(); + void setAuxVDDIOLevel(uint8_t level); + + // SMPLRT_DIV register + uint8_t getRate(); + void setRate(uint8_t rate); + + // CONFIG register + uint8_t getExternalFrameSync(); + void setExternalFrameSync(uint8_t sync); + uint8_t getDLPFMode(); + void setDLPFMode(uint8_t bandwidth); + + // GYRO_CONFIG register + uint8_t getFullScaleGyroRange(); + void setFullScaleGyroRange(uint8_t range); + + // SELF_TEST registers + uint8_t getAccelXSelfTestFactoryTrim(); + uint8_t getAccelYSelfTestFactoryTrim(); + uint8_t getAccelZSelfTestFactoryTrim(); + + uint8_t getGyroXSelfTestFactoryTrim(); + uint8_t getGyroYSelfTestFactoryTrim(); + uint8_t getGyroZSelfTestFactoryTrim(); + + // ACCEL_CONFIG register + bool getAccelXSelfTest(); + void setAccelXSelfTest(bool enabled); + bool getAccelYSelfTest(); + void setAccelYSelfTest(bool enabled); + bool getAccelZSelfTest(); + void setAccelZSelfTest(bool enabled); + uint8_t getFullScaleAccelRange(); + void setFullScaleAccelRange(uint8_t range); + uint8_t getDHPFMode(); + void setDHPFMode(uint8_t mode); + + // FF_THR register + uint8_t getFreefallDetectionThreshold(); + void setFreefallDetectionThreshold(uint8_t threshold); + + // FF_DUR register + uint8_t getFreefallDetectionDuration(); + void setFreefallDetectionDuration(uint8_t duration); + + // MOT_THR register + uint8_t getMotionDetectionThreshold(); + void setMotionDetectionThreshold(uint8_t threshold); + + // MOT_DUR register + uint8_t getMotionDetectionDuration(); + void setMotionDetectionDuration(uint8_t duration); + + // ZRMOT_THR register + uint8_t getZeroMotionDetectionThreshold(); + void setZeroMotionDetectionThreshold(uint8_t threshold); + + // ZRMOT_DUR register + uint8_t getZeroMotionDetectionDuration(); + void setZeroMotionDetectionDuration(uint8_t duration); + + // FIFO_EN register + bool getTempFIFOEnabled(); + void setTempFIFOEnabled(bool enabled); + bool getXGyroFIFOEnabled(); + void setXGyroFIFOEnabled(bool enabled); + bool getYGyroFIFOEnabled(); + void setYGyroFIFOEnabled(bool enabled); + bool getZGyroFIFOEnabled(); + void setZGyroFIFOEnabled(bool enabled); + bool getAccelFIFOEnabled(); + void setAccelFIFOEnabled(bool enabled); + bool getSlave2FIFOEnabled(); + void setSlave2FIFOEnabled(bool enabled); + bool getSlave1FIFOEnabled(); + void setSlave1FIFOEnabled(bool enabled); + bool getSlave0FIFOEnabled(); + void setSlave0FIFOEnabled(bool enabled); + + // I2C_MST_CTRL register + bool getMultiMasterEnabled(); + void setMultiMasterEnabled(bool enabled); + bool getWaitForExternalSensorEnabled(); + void setWaitForExternalSensorEnabled(bool enabled); + bool getSlave3FIFOEnabled(); + void setSlave3FIFOEnabled(bool enabled); + bool getSlaveReadWriteTransitionEnabled(); + void setSlaveReadWriteTransitionEnabled(bool enabled); + uint8_t getMasterClockSpeed(); + void setMasterClockSpeed(uint8_t speed); + + // I2C_SLV* registers (Slave 0-3) + uint8_t getSlaveAddress(uint8_t num); + void setSlaveAddress(uint8_t num, uint8_t address); + uint8_t getSlaveRegister(uint8_t num); + void setSlaveRegister(uint8_t num, uint8_t reg); + bool getSlaveEnabled(uint8_t num); + void setSlaveEnabled(uint8_t num, bool enabled); + bool getSlaveWordByteSwap(uint8_t num); + void setSlaveWordByteSwap(uint8_t num, bool enabled); + bool getSlaveWriteMode(uint8_t num); + void setSlaveWriteMode(uint8_t num, bool mode); + bool getSlaveWordGroupOffset(uint8_t num); + void setSlaveWordGroupOffset(uint8_t num, bool enabled); + uint8_t getSlaveDataLength(uint8_t num); + void setSlaveDataLength(uint8_t num, uint8_t length); + + // I2C_SLV* registers (Slave 4) + uint8_t getSlave4Address(); + void setSlave4Address(uint8_t address); + uint8_t getSlave4Register(); + void setSlave4Register(uint8_t reg); + void setSlave4OutputByte(uint8_t data); + bool getSlave4Enabled(); + void setSlave4Enabled(bool enabled); + bool getSlave4InterruptEnabled(); + void setSlave4InterruptEnabled(bool enabled); + bool getSlave4WriteMode(); + void setSlave4WriteMode(bool mode); + uint8_t getSlave4MasterDelay(); + void setSlave4MasterDelay(uint8_t delay); + uint8_t getSlate4InputByte(); + + // I2C_MST_STATUS register + bool getPassthroughStatus(); + bool getSlave4IsDone(); + bool getLostArbitration(); + bool getSlave4Nack(); + bool getSlave3Nack(); + bool getSlave2Nack(); + bool getSlave1Nack(); + bool getSlave0Nack(); + + // INT_PIN_CFG register + bool getInterruptMode(); + void setInterruptMode(bool mode); + bool getInterruptDrive(); + void setInterruptDrive(bool drive); + bool getInterruptLatch(); + void setInterruptLatch(bool latch); + bool getInterruptLatchClear(); + void setInterruptLatchClear(bool clear); + bool getFSyncInterruptLevel(); + void setFSyncInterruptLevel(bool level); + bool getFSyncInterruptEnabled(); + void setFSyncInterruptEnabled(bool enabled); + bool getI2CBypassEnabled(); + void setI2CBypassEnabled(bool enabled); + bool getClockOutputEnabled(); + void setClockOutputEnabled(bool enabled); + + // INT_ENABLE register + uint8_t getIntEnabled(); + void setIntEnabled(uint8_t enabled); + bool getIntFreefallEnabled(); + void setIntFreefallEnabled(bool enabled); + bool getIntMotionEnabled(); + void setIntMotionEnabled(bool enabled); + bool getIntZeroMotionEnabled(); + void setIntZeroMotionEnabled(bool enabled); + bool getIntFIFOBufferOverflowEnabled(); + void setIntFIFOBufferOverflowEnabled(bool enabled); + bool getIntI2CMasterEnabled(); + void setIntI2CMasterEnabled(bool enabled); + bool getIntDataReadyEnabled(); + void setIntDataReadyEnabled(bool enabled); + + // INT_STATUS register + uint8_t getIntStatus(); + bool getIntFreefallStatus(); + bool getIntMotionStatus(); + bool getIntZeroMotionStatus(); + bool getIntFIFOBufferOverflowStatus(); + bool getIntI2CMasterStatus(); + bool getIntDataReadyStatus(); + + // ACCEL_*OUT_* registers + void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz); + void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); + void getAcceleration(int16_t* x, int16_t* y, int16_t* z); + int16_t getAccelerationX(); + int16_t getAccelerationY(); + int16_t getAccelerationZ(); + + // TEMP_OUT_* registers + int16_t getTemperature(); + + // GYRO_*OUT_* registers + void getRotation(int16_t* x, int16_t* y, int16_t* z); + int16_t getRotationX(); + int16_t getRotationY(); + int16_t getRotationZ(); + + // EXT_SENS_DATA_* registers + uint8_t getExternalSensorByte(int position); + uint16_t getExternalSensorWord(int position); + uint32_t getExternalSensorDWord(int position); + + // MOT_DETECT_STATUS register + uint8_t getMotionStatus(); + bool getXNegMotionDetected(); + bool getXPosMotionDetected(); + bool getYNegMotionDetected(); + bool getYPosMotionDetected(); + bool getZNegMotionDetected(); + bool getZPosMotionDetected(); + bool getZeroMotionDetected(); + + // I2C_SLV*_DO register + void setSlaveOutputByte(uint8_t num, uint8_t data); + + // I2C_MST_DELAY_CTRL register + bool getExternalShadowDelayEnabled(); + void setExternalShadowDelayEnabled(bool enabled); + bool getSlaveDelayEnabled(uint8_t num); + void setSlaveDelayEnabled(uint8_t num, bool enabled); + + // SIGNAL_PATH_RESET register + void resetGyroscopePath(); + void resetAccelerometerPath(); + void resetTemperaturePath(); + + // MOT_DETECT_CTRL register + uint8_t getAccelerometerPowerOnDelay(); + void setAccelerometerPowerOnDelay(uint8_t delay); + uint8_t getFreefallDetectionCounterDecrement(); + void setFreefallDetectionCounterDecrement(uint8_t decrement); + uint8_t getMotionDetectionCounterDecrement(); + void setMotionDetectionCounterDecrement(uint8_t decrement); + + // USER_CTRL register + bool getFIFOEnabled(); + void setFIFOEnabled(bool enabled); + bool getI2CMasterModeEnabled(); + void setI2CMasterModeEnabled(bool enabled); + void switchSPIEnabled(bool enabled); + void resetFIFO(); + void resetI2CMaster(); + void resetSensors(); + + // PWR_MGMT_1 register + void reset(); + bool getSleepEnabled(); + void setSleepEnabled(bool enabled); + bool getWakeCycleEnabled(); + void setWakeCycleEnabled(bool enabled); + bool getTempSensorEnabled(); + void setTempSensorEnabled(bool enabled); + uint8_t getClockSource(); + void setClockSource(uint8_t source); + + // PWR_MGMT_2 register + uint8_t getWakeFrequency(); + void setWakeFrequency(uint8_t frequency); + bool getStandbyXAccelEnabled(); + void setStandbyXAccelEnabled(bool enabled); + bool getStandbyYAccelEnabled(); + void setStandbyYAccelEnabled(bool enabled); + bool getStandbyZAccelEnabled(); + void setStandbyZAccelEnabled(bool enabled); + bool getStandbyXGyroEnabled(); + void setStandbyXGyroEnabled(bool enabled); + bool getStandbyYGyroEnabled(); + void setStandbyYGyroEnabled(bool enabled); + bool getStandbyZGyroEnabled(); + void setStandbyZGyroEnabled(bool enabled); + + // FIFO_COUNT_* registers + uint16_t getFIFOCount(); + + // FIFO_R_W register + uint8_t getFIFOByte(); + void setFIFOByte(uint8_t data); + void getFIFOBytes(uint8_t *data, uint8_t length); + + // WHO_AM_I register + uint8_t getDeviceID(); + void setDeviceID(uint8_t id); + + // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== + + // XG_OFFS_TC register + uint8_t getOTPBankValid(); + void setOTPBankValid(bool enabled); + int8_t getXGyroOffsetTC(); + void setXGyroOffsetTC(int8_t offset); + + // YG_OFFS_TC register + int8_t getYGyroOffsetTC(); + void setYGyroOffsetTC(int8_t offset); + + // ZG_OFFS_TC register + int8_t getZGyroOffsetTC(); + void setZGyroOffsetTC(int8_t offset); + + // X_FINE_GAIN register + int8_t getXFineGain(); + void setXFineGain(int8_t gain); + + // Y_FINE_GAIN register + int8_t getYFineGain(); + void setYFineGain(int8_t gain); + + // Z_FINE_GAIN register + int8_t getZFineGain(); + void setZFineGain(int8_t gain); + + // XA_OFFS_* registers + int16_t getXAccelOffset(); + void setXAccelOffset(int16_t offset); + + // YA_OFFS_* register + int16_t getYAccelOffset(); + void setYAccelOffset(int16_t offset); + + // ZA_OFFS_* register + int16_t getZAccelOffset(); + void setZAccelOffset(int16_t offset); + + // XG_OFFS_USR* registers + int16_t getXGyroOffset(); + void setXGyroOffset(int16_t offset); + + // YG_OFFS_USR* register + int16_t getYGyroOffset(); + void setYGyroOffset(int16_t offset); + + // ZG_OFFS_USR* register + int16_t getZGyroOffset(); + void setZGyroOffset(int16_t offset); + + // INT_ENABLE register (DMP functions) + bool getIntPLLReadyEnabled(); + void setIntPLLReadyEnabled(bool enabled); + bool getIntDMPEnabled(); + void setIntDMPEnabled(bool enabled); + + // DMP_INT_STATUS + bool getDMPInt5Status(); + bool getDMPInt4Status(); + bool getDMPInt3Status(); + bool getDMPInt2Status(); + bool getDMPInt1Status(); + bool getDMPInt0Status(); + + // INT_STATUS register (DMP functions) + bool getIntPLLReadyStatus(); + bool getIntDMPStatus(); + + // USER_CTRL register (DMP functions) + bool getDMPEnabled(); + void setDMPEnabled(bool enabled); + void resetDMP(); + + // BANK_SEL register + void setMemoryBank(uint8_t bank, bool prefetchEnabled=false, bool userBank=false); + + // MEM_START_ADDR register + void setMemoryStartAddress(uint8_t address); + + // MEM_R_W register + uint8_t readMemoryByte(); + void writeMemoryByte(uint8_t data); + void readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0); + bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true, bool useProgMem=false); + bool writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true); + + bool writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem=false); + bool writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize); + + // DMP_CFG_1 register + uint8_t getDMPConfig1(); + void setDMPConfig1(uint8_t config); + + // DMP_CFG_2 register + uint8_t getDMPConfig2(); + void setDMPConfig2(uint8_t config); + + // Calibration Routines + void CalibrateGyro(uint8_t Loops = 15); // Fine tune after setting offsets with less Loops. + void CalibrateAccel(uint8_t Loops = 15);// Fine tune after setting offsets with less Loops. + void PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops); // Does the math + void PrintActiveOffsets(); // See the results of the Calibration + + + + // special methods for MotionApps 2.0 implementation + #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20 + + uint8_t dmpInitialize(); + bool dmpPacketAvailable(); + + uint8_t dmpSetFIFORate(uint8_t fifoRate); + uint8_t dmpGetFIFORate(); + uint8_t dmpGetSampleStepSizeMS(); + uint8_t dmpGetSampleFrequency(); + int32_t dmpDecodeTemperature(int8_t tempReg); + + // Register callbacks after a packet of FIFO data is processed + //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); + //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); + uint8_t dmpRunFIFORateProcesses(); + + // Setup FIFO for various output + uint8_t dmpSendQuaternion(uint_fast16_t accuracy); + uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); + uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + + // Get Fixed Point data from FIFO + uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpSetLinearAccelFilterCoefficient(float coef); + uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); + uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); + uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); + uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); + uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); + + uint8_t dmpGetEuler(float *data, Quaternion *q); + uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); + + // Get Floating Point data from FIFO + uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); + + uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); + uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); + + uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); + + uint8_t dmpInitFIFOParam(); + uint8_t dmpCloseFIFO(); + uint8_t dmpSetGyroDataSource(uint8_t source); + uint8_t dmpDecodeQuantizedAccel(); + uint32_t dmpGetGyroSumOfSquare(); + uint32_t dmpGetAccelSumOfSquare(); + void dmpOverrideQuaternion(long *q); + uint16_t dmpGetFIFOPacketSize(); + #endif + + // special methods for MotionApps 4.1 implementation + #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41 + + uint8_t dmpInitialize(); + bool dmpPacketAvailable(); + + uint8_t dmpSetFIFORate(uint8_t fifoRate); + uint8_t dmpGetFIFORate(); + uint8_t dmpGetSampleStepSizeMS(); + uint8_t dmpGetSampleFrequency(); + int32_t dmpDecodeTemperature(int8_t tempReg); + + // Register callbacks after a packet of FIFO data is processed + //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); + //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); + uint8_t dmpRunFIFORateProcesses(); + + // Setup FIFO for various output + uint8_t dmpSendQuaternion(uint_fast16_t accuracy); + uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); + uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + + // Get Fixed Point data from FIFO + uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0); + uint8_t dmpSetLinearAccelFilterCoefficient(float coef); + uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); + uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); + uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); + uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); + uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); + + uint8_t dmpGetEuler(float *data, Quaternion *q); + uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); + + // Get Floating Point data from FIFO + uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); + + uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); + uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); + + uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); + + uint8_t dmpInitFIFOParam(); + uint8_t dmpCloseFIFO(); + uint8_t dmpSetGyroDataSource(uint8_t source); + uint8_t dmpDecodeQuantizedAccel(); + uint32_t dmpGetGyroSumOfSquare(); + uint32_t dmpGetAccelSumOfSquare(); + void dmpOverrideQuaternion(long *q); + uint16_t dmpGetFIFOPacketSize(); + #endif + + private: + uint8_t devAddr; + uint8_t buffer[14]; + #if defined(MPU6050_INCLUDE_DMP_MOTIONAPPS20) or defined(MPU6050_INCLUDE_DMP_MOTIONAPPS41) + uint8_t *dmpPacketBuffer; + uint16_t dmpPacketSize; + #endif +}; + +#endif /* _MPU6050_H_ */ diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050_6Axis_MotionApps20.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050_6Axis_MotionApps20.h new file mode 100644 index 00000000..d6a1c137 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050_6Axis_MotionApps20.h @@ -0,0 +1,617 @@ +// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 5/20/2013 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2019/07/08 - merged all DMP Firmware configuration items into the dmpMemory array +// - Simplified dmpInitialize() to accomidate the dmpmemory array alterations +// ... - ongoing debug release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ +#define _MPU6050_6AXIS_MOTIONAPPS20_H_ + +#include "I2Cdev.h" +#include "helper_3dmath.h" + +// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board +#define MPU6050_INCLUDE_DMP_MOTIONAPPS20 + +#include "MPU6050.h" + +// Tom Carpenter's conditional PROGMEM code +// http://forum.arduino.cc/index.php?topic=129407.0 +#ifdef __AVR__ + #include +#elif defined(ESP8266) || defined(ESP32) + #include +#else + // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen + #ifndef __PGMSPACE_H_ + #define __PGMSPACE_H_ 1 + #include + + #define PROGMEM + #define PGM_P const char * + #define PSTR(str) (str) + #define F(x) x + + typedef void prog_void; + typedef char prog_char; + typedef unsigned char prog_uchar; + typedef int8_t prog_int8_t; + typedef uint8_t prog_uint8_t; + typedef int16_t prog_int16_t; + typedef uint16_t prog_uint16_t; + typedef int32_t prog_int32_t; + typedef uint32_t prog_uint32_t; + + #define strcpy_P(dest, src) strcpy((dest), (src)) + #define strcat_P(dest, src) strcat((dest), (src)) + #define strcmp_P(a, b) strcmp((a), (b)) + + #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) + #define pgm_read_word(addr) (*(const unsigned short *)(addr)) + #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) + #define pgm_read_float(addr) (*(const float *)(addr)) + + #define pgm_read_byte_near(addr) pgm_read_byte(addr) + #define pgm_read_word_near(addr) pgm_read_word(addr) + #define pgm_read_dword_near(addr) pgm_read_dword(addr) + #define pgm_read_float_near(addr) pgm_read_float(addr) + #define pgm_read_byte_far(addr) pgm_read_byte(addr) + #define pgm_read_word_far(addr) pgm_read_word(addr) + #define pgm_read_dword_far(addr) pgm_read_dword(addr) + #define pgm_read_float_far(addr) pgm_read_float(addr) + #endif +#endif + +/* Source is from the InvenSense MotionApps v2 demo code. Original source is + * unavailable, unless you happen to be amazing as decompiling binary by + * hand (in which case, please contact me, and I'm totally serious). + * + * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the + * DMP reverse-engineering he did to help make this bit of wizardry + * possible. + */ + +// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. +// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) +// after moving string constants to flash memory storage using the F() +// compiler macro (Arduino IDE 1.0+ required). + +//#define DEBUG +#ifdef DEBUG + #define DEBUG_PRINT(x) Serial.print(x) + #define DEBUG_PRINTF(x, y) Serial.print(x, y) + #define DEBUG_PRINTLN(x) Serial.println(x) + #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) +#else + #define DEBUG_PRINT(x) + #define DEBUG_PRINTF(x, y) + #define DEBUG_PRINTLN(x) + #define DEBUG_PRINTLNF(x, y) +#endif + +#define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] +#define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] +#define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] + +/* ================================================================================================ * + | Default MotionApps v2.0 42-byte FIFO packet structure: | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | + | | + | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | + | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | + * ================================================================================================ */ + +// this block of memory gets written to the MPU on start-up, and it seems +// to be volatile memory, so it has to be done each time (it only takes ~1 +// second though) + +// I Only Changed this by applying all the configuration data and capturing it before startup: +// *** this is a capture of the DMP Firmware after all the messy changes were made so we can just load it +const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { + /* bank # 0 */ + 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x40, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCB, 0x47, 0xA2, 0x20, 0x00, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, + 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, + 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, + 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, + /* bank # 1 */ + 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, + 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, + 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x09, 0x23, 0xA1, 0x35, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, + 0x80, 0x00, 0xFF, 0xFF, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, + 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, + /* bank # 2 */ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0x8B, 0xC1, 0x00, 0x00, 0x01, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + /* bank # 3 */ + 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, + 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, + 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, + 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, + 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, + 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, + 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, + 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C, + 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, + 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, + 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, + 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, + 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, + 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, + 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, + 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, + /* bank # 4 */ + 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, + 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, + 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, + 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, + 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, + 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, + 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, + 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, + 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, + 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, + 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, + 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, + /* bank # 5 */ + 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, + 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, + 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, + 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, + 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, + 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, + 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, + 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, + 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, + 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, + 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, + 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, + 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, + 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, + 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, + 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, + /* bank # 6 */ + 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, + 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, + 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, + 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, + 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, + 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, + 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, + 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, + 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, + 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, + 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, + 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, + 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, + 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, + 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, + 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, + /* bank # 7 */ + 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, + 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, + 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, + 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, + 0xDD, 0xF1, 0x20, 0x28, 0x30, 0x38, 0x9A, 0xF1, 0x28, 0x30, 0x38, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, + 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, + 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0x28, 0x30, 0x38, + 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0x30, 0xDC, + 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xFE, 0xD8, 0xFF, + +}; + +#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR +#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default +#endif + +// I Simplified this: +uint8_t MPU6050::dmpInitialize() { + // reset device + DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); + reset(); + delay(30); // wait after reset + + // enable sleep mode and wake cycle + /*Serial.println(F("Enabling sleep mode...")); + setSleepEnabled(true); + Serial.println(F("Enabling wake cycle...")); + setWakeCycleEnabled(true);*/ + + // disable sleep mode + setSleepEnabled(false); + + // get MPU hardware revision + setMemoryBank(0x10, true, true); + setMemoryStartAddress(0x06); + Serial.println(F("Checking hardware revision...")); + Serial.print(F("Revision @ user[16][6] = ")); + Serial.println(readMemoryByte(), HEX); + Serial.println(F("Resetting memory bank selection to 0...")); + setMemoryBank(0, false, false); + + // check OTP bank valid + DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); + DEBUG_PRINT(F("OTP bank is ")); + DEBUG_PRINTLN(getOTPBankValid() ? F("valid!") : F("invalid!")); + + // setup weird slave stuff (?) + DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); + setSlaveAddress(0, 0x7F); + DEBUG_PRINTLN(F("Disabling I2C Master mode...")); + setI2CMasterModeEnabled(false); + DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); + setSlaveAddress(0, 0x68); + DEBUG_PRINTLN(F("Resetting I2C Master control...")); + resetI2CMaster(); + delay(20); + DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); + setClockSource(MPU6050_CLOCK_PLL_ZGYRO); + + DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); + setIntEnabled(1<= dmpGetFIFOPacketSize(); +} + +// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); +// uint8_t MPU6050::dmpGetFIFORate(); +// uint8_t MPU6050::dmpGetSampleStepSizeMS(); +// uint8_t MPU6050::dmpGetSampleFrequency(); +// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); + +//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); +//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); +//uint8_t MPU6050::dmpRunFIFORateProcesses(); + +// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + +uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]); + data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]); + data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]); + return 0; +} +uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[28] << 8) | packet[29]; + data[1] = (packet[32] << 8) | packet[33]; + data[2] = (packet[36] << 8) | packet[37]; + return 0; +} +uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[28] << 8) | packet[29]; + v -> y = (packet[32] << 8) | packet[33]; + v -> z = (packet[36] << 8) | packet[37]; + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); + data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); + data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); + data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[0] << 8) | packet[1]); + data[1] = ((packet[4] << 8) | packet[5]); + data[2] = ((packet[8] << 8) | packet[9]); + data[3] = ((packet[12] << 8) | packet[13]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + if (status == 0) { + q -> w = (float)qI[0] / 16384.0f; + q -> x = (float)qI[1] / 16384.0f; + q -> y = (float)qI[2] / 16384.0f; + q -> z = (float)qI[3] / 16384.0f; + return 0; + } + return status; // int16 return value, indicates error if this line is reached +} +// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); + data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); + data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); + return 0; +} +uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[16] << 8) | packet[17]; + data[1] = (packet[20] << 8) | packet[21]; + data[2] = (packet[24] << 8) | packet[25]; + return 0; +} +uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[16] << 8) | packet[17]; + v -> y = (packet[20] << 8) | packet[21]; + v -> z = (packet[24] << 8) | packet[25]; + return 0; +} +// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); +// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { + // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) + v -> x = vRaw -> x - gravity -> x*8192; + v -> y = vRaw -> y - gravity -> y*8192; + v -> z = vRaw -> z - gravity -> z*8192; + return 0; +} +// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { + // rotate measured 3D acceleration vector into original state + // frame of reference based on orientation quaternion + memcpy(v, vReal, sizeof(VectorInt16)); + v -> rotate(q); + return 0; +} +// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); + return status; +} + +uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { + v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); + v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); + v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; + return 0; +} +// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); +// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); + +uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi + data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta + data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi + return 0; +} + +#ifdef USE_OLD_DMPGETYAWPITCHROLL +uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); + return 0; +} +#else +uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan2(gravity -> y , gravity -> z); + if (gravity -> z < 0) { + if(data[1] > 0) { + data[1] = PI - data[1]; + } else { + data[1] = -PI - data[1]; + } + } + return 0; +} +#endif + +// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); + +uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { + /*for (uint8_t k = 0; k < dmpPacketSize; k++) { + if (dmpData[k] < 0x10) Serial.print("0"); + Serial.print(dmpData[k], HEX); + Serial.print(" "); + } + Serial.print("\n");*/ + //Serial.println((uint16_t)dmpPacketBuffer); + return 0; +} +uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { + uint8_t status; + uint8_t buf[dmpPacketSize]; + for (uint8_t i = 0; i < numPackets; i++) { + // read packet from FIFO + getFIFOBytes(buf, dmpPacketSize); + + // process packet + if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; + + // increment external process count variable, if supplied + if (processed != 0) (*processed)++; + } + return 0; +} + +// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); + +// uint8_t MPU6050::dmpInitFIFOParam(); +// uint8_t MPU6050::dmpCloseFIFO(); +// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); +// uint8_t MPU6050::dmpDecodeQuantizedAccel(); +// uint32_t MPU6050::dmpGetGyroSumOfSquare(); +// uint32_t MPU6050::dmpGetAccelSumOfSquare(); +// void MPU6050::dmpOverrideQuaternion(long *q); +uint16_t MPU6050::dmpGetFIFOPacketSize() { + return dmpPacketSize; +} + +#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h new file mode 100644 index 00000000..34a4ccc5 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h @@ -0,0 +1,617 @@ +// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 6.12 implementation +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 5/20/2013 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2019/7/10 - I incorporated DMP Firmware Version 6.12 Latest as of today with many features and bug fixes. +// - MPU6050 Registers have not changed just the DMP Image so that full backwards compatibility is present +// - Run-time calibration routine is enabled which calibrates after no motion state is detected +// - once no motion state is detected Calibration completes within 0.5 seconds +// - The Drawback is that the firmware image is larger. +// ... - ongoing debug release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ +#define _MPU6050_6AXIS_MOTIONAPPS20_H_ + +#include "I2Cdev.h" +#include "helper_3dmath.h" + +// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board +#define MPU6050_INCLUDE_DMP_MOTIONAPPS20 // same definitions Should work with V6.12 + +#include "MPU6050.h" + +// Tom Carpenter's conditional PROGMEM code +// http://forum.arduino.cc/index.php?topic=129407.0 +#ifdef __AVR__ + #include +#elif defined(ESP8266) || defined(ESP32) + #include +#else + // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen + #ifndef __PGMSPACE_H_ + #define __PGMSPACE_H_ 1 + #include + + #define PROGMEM + #define PGM_P const char * + #define PSTR(str) (str) + #define F(x) x + + typedef void prog_void; + typedef char prog_char; + typedef unsigned char prog_uchar; + typedef int8_t prog_int8_t; + typedef uint8_t prog_uint8_t; + typedef int16_t prog_int16_t; + typedef uint16_t prog_uint16_t; + typedef int32_t prog_int32_t; + typedef uint32_t prog_uint32_t; + + #define strcpy_P(dest, src) strcpy((dest), (src)) + #define strcat_P(dest, src) strcat((dest), (src)) + #define strcmp_P(a, b) strcmp((a), (b)) + + #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) + #define pgm_read_word(addr) (*(const unsigned short *)(addr)) + #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) + #define pgm_read_float(addr) (*(const float *)(addr)) + + #define pgm_read_byte_near(addr) pgm_read_byte(addr) + #define pgm_read_word_near(addr) pgm_read_word(addr) + #define pgm_read_dword_near(addr) pgm_read_dword(addr) + #define pgm_read_float_near(addr) pgm_read_float(addr) + #define pgm_read_byte_far(addr) pgm_read_byte(addr) + #define pgm_read_word_far(addr) pgm_read_word(addr) + #define pgm_read_dword_far(addr) pgm_read_dword(addr) + #define pgm_read_float_far(addr) pgm_read_float(addr) + #endif +#endif + +/* Source is from the InvenSense MotionApps v2 demo code. Original source is + * unavailable, unless you happen to be amazing as decompiling binary by + * hand (in which case, please contact me, and I'm totally serious). + * + * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the + * DMP reverse-engineering he did to help make this bit of wizardry + * possible. + */ + +// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. +// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) +// after moving string constants to flash memory storage using the F() +// compiler macro (Arduino IDE 1.0+ required). + +//#define DEBUG +#ifdef DEBUG + #define DEBUG_PRINT(x) Serial.print(x) + #define DEBUG_PRINTF(x, y) Serial.print(x, y) + #define DEBUG_PRINTLN(x) Serial.println(x) + #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) +#else + #define DEBUG_PRINT(x) + #define DEBUG_PRINTF(x, y) + #define DEBUG_PRINTLN(x) + #define DEBUG_PRINTLNF(x, y) +#endif + +#define MPU6050_DMP_CODE_SIZE 3062 // dmpMemory[] + +/* ================================================================ * + | Default MotionApps v6.12 28-byte FIFO packet structure: | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 | + | | + | [GYRO X][GYRO Y][GYRO Z][ACC X ][ACC Y ][ACC Z ] | + | 16 17 18 19 20 21 22 23 24 25 26 27 | + * ================================================================ */ + +// this block of memory gets written to the MPU on start-up, and it seems +// to be volatile memory, so it has to be done each time (it only takes ~1 +// second though) + +// *** this is a capture of the DMP Firmware V6.1.2 after all the messy changes were made so we can just load it +const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { +/* bank # 0 */ +0x00, 0xF8, 0xF6, 0x2A, 0x3F, 0x68, 0xF5, 0x7A, 0x00, 0x06, 0xFF, 0xFE, 0x00, 0x03, 0x00, 0x00, +0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, +0x03, 0x0C, 0x30, 0xC3, 0x0A, 0x74, 0x56, 0x2D, 0x0D, 0x62, 0xDB, 0xC7, 0x16, 0xF4, 0xBA, 0x02, +0x38, 0x83, 0xF8, 0x83, 0x30, 0x00, 0xF8, 0x83, 0x25, 0x8E, 0xF8, 0x83, 0x30, 0x00, 0xF8, 0x83, +0xFF, 0xFF, 0xFF, 0xFF, 0x0C, 0xBD, 0xD8, 0x11, 0x24, 0x00, 0x04, 0x00, 0x1A, 0x82, 0x79, 0xA1, +0x00, 0x36, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6F, 0xA2, +0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, +0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, +0x1F, 0xA4, 0xE8, 0xE4, 0xFF, 0xF5, 0xDC, 0xB9, 0x00, 0x5B, 0x79, 0xCF, 0x1F, 0x3F, 0x78, 0x76, +0x00, 0x86, 0x7C, 0x5A, 0x00, 0x86, 0x23, 0x47, 0xFA, 0xB9, 0x86, 0x31, 0x00, 0x74, 0x87, 0x8A, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x43, 0x05, 0xFF, 0xFF, 0xE9, 0xA8, 0x00, 0x00, 0x21, 0x82, +0xFA, 0xB8, 0x4D, 0x46, 0xFF, 0xFA, 0xDF, 0x3D, 0xFF, 0xFF, 0xB2, 0xB3, 0x00, 0x00, 0x00, 0x00, +0x3F, 0xFF, 0xBA, 0x98, 0x00, 0x5D, 0xAC, 0x08, 0x00, 0x0A, 0x63, 0x78, 0x00, 0x01, 0x46, 0x21, +0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x42, 0xB5, 0x00, 0x06, 0x00, 0x64, 0x00, 0x64, 0x00, 0x06, +0x14, 0x06, 0x02, 0x9F, 0x0F, 0x47, 0x91, 0x32, 0xD9, 0x0E, 0x9F, 0xC9, 0x1D, 0xCF, 0x4C, 0x34, +0x3B, 0xB6, 0x7A, 0xE8, 0x00, 0x64, 0x00, 0x06, 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFE, +/* bank # 1 */ +0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x07, 0x00, 0x00, 0xFF, 0xF1, 0x00, 0x00, 0xFA, 0x46, 0x00, 0x00, 0xA2, 0xB8, 0x00, 0x00, +0x10, 0x00, 0x00, 0x00, 0x04, 0xD6, 0x00, 0x00, 0x04, 0xCC, 0x00, 0x00, 0x04, 0xCC, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, +0x00, 0x00, 0x00, 0x32, 0xF8, 0x98, 0x00, 0x00, 0xFF, 0x65, 0x00, 0x00, 0x83, 0x0F, 0x00, 0x00, +0x00, 0x06, 0x00, 0x00, 0xFF, 0xF1, 0x00, 0x00, 0xFA, 0x46, 0x00, 0x00, 0xA2, 0xB8, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, +0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0D, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x02, 0x00, 0x00, +0x00, 0x01, 0xFB, 0x83, 0x00, 0x7C, 0x00, 0x00, 0xFB, 0x15, 0xFC, 0x00, 0x1F, 0xB4, 0xFF, 0x83, +0x00, 0x00, 0x00, 0x01, 0x00, 0x65, 0x00, 0x07, 0x00, 0x64, 0x03, 0xE8, 0x00, 0x64, 0x00, 0x28, +0x00, 0x00, 0xFF, 0xFD, 0x00, 0x00, 0x00, 0x00, 0x16, 0xA0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x10, 0x00, 0x00, 0x2F, 0x00, 0x00, 0x00, 0x00, 0x01, 0xF4, 0x00, 0x00, 0x10, 0x00, +/* bank # 2 */ +0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0xBA, 0xC6, 0x00, 0x47, 0x78, 0xA2, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, +0x00, 0x00, 0x23, 0xBB, 0x00, 0x2E, 0xA2, 0x5B, 0x00, 0x00, 0x05, 0x68, 0x00, 0x0B, 0xCF, 0x49, +0x00, 0x04, 0xFF, 0xFD, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, +0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x64, 0x00, 0x07, 0x00, 0x08, 0x00, 0x06, 0x00, 0x06, 0xFF, 0xFE, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x2E, 0xA2, 0x5B, 0x00, 0x00, 0x05, 0x68, 0x00, 0x0B, 0xCF, 0x49, 0x00, 0x00, 0x00, 0x00, +0x00, 0xF8, 0xF6, 0x2A, 0x3F, 0x68, 0xF5, 0x7A, 0x00, 0x04, 0xFF, 0xFD, 0x00, 0x02, 0x00, 0x00, +0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x0E, +0xFF, 0xFF, 0xFF, 0xCF, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xFF, 0xFF, 0xFF, 0x9C, +0x00, 0x00, 0x43, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, +0xFF, 0xE5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +/* bank # 3 */ +0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xD3, +0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3C, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1F, 0x9E, 0x65, 0x5D, +0x0C, 0x0A, 0x4E, 0x68, 0xCD, 0xCF, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xC6, 0x19, 0xCE, 0x82, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x71, 0x1C, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xD7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, +0x00, 0x11, 0xDC, 0x47, 0x03, 0x00, 0x00, 0x00, 0xC7, 0x93, 0x8F, 0x9D, 0x1E, 0x1B, 0x1C, 0x19, +0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0E, 0xDF, 0xA4, 0x38, 0x1F, 0x9E, 0x65, 0x5D, +0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x71, 0x1C, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x3F, 0xFF, 0xFF, 0xFD, 0xFF, 0xFF, 0xF4, 0xC9, 0xFF, 0xFF, 0xBC, 0xF0, 0x00, 0x01, 0x0C, 0x0F, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0xF5, 0xB7, 0xBA, 0xB3, 0x67, 0x7D, 0xDF, 0x7E, 0x72, 0x90, 0x2E, 0x55, 0x4C, 0xF6, 0xE6, 0x88, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +/* bank # 4 */ +0xD8, 0xDC, 0xB4, 0xB8, 0xB0, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xB3, 0xB7, 0xBB, 0x8E, 0x9E, +0xAE, 0xF1, 0x32, 0xF5, 0x1B, 0xF1, 0xB4, 0xB8, 0xB0, 0x80, 0x97, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, +0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0xF1, 0xA9, +0x89, 0x26, 0x46, 0x66, 0xB2, 0x89, 0x99, 0xA9, 0x2D, 0x55, 0x7D, 0xB0, 0xB0, 0x8A, 0xA8, 0x96, +0x36, 0x56, 0x76, 0xF1, 0xBA, 0xA3, 0xB4, 0xB2, 0x80, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB2, 0x83, +0x98, 0xBA, 0xA3, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xB2, 0xB9, 0xB4, 0x98, 0x83, 0xF1, +0xA3, 0x29, 0x55, 0x7D, 0xBA, 0xB5, 0xB1, 0xA3, 0x83, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xB2, +0xB6, 0xAA, 0x83, 0x93, 0x28, 0x54, 0x7C, 0xF1, 0xB9, 0xA3, 0x82, 0x93, 0x61, 0xBA, 0xA2, 0xDA, +0xDE, 0xDF, 0xDB, 0x81, 0x9A, 0xB9, 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xF1, 0xDA, 0xBA, 0xA2, 0xDF, +0xD9, 0xBA, 0xA2, 0xFA, 0xB9, 0xA3, 0x82, 0x92, 0xDB, 0x31, 0xBA, 0xA2, 0xD9, 0xBA, 0xA2, 0xF8, +0xDF, 0x85, 0xA4, 0xD0, 0xC1, 0xBB, 0xAD, 0x83, 0xC2, 0xC5, 0xC7, 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, +0xBA, 0xA0, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xAA, 0xB3, 0x8D, 0xB4, 0x98, 0x0D, 0x35, +0x5D, 0xB2, 0xB6, 0xBA, 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, +0xB8, 0xAA, 0x87, 0x2C, 0x54, 0x7C, 0xBA, 0xA4, 0xB0, 0x8A, 0xB6, 0x91, 0x32, 0x56, 0x76, 0xB2, +0x84, 0x94, 0xA4, 0xC8, 0x08, 0xCD, 0xD8, 0xB8, 0xB4, 0xB0, 0xF1, 0x99, 0x82, 0xA8, 0x2D, 0x55, +0x7D, 0x98, 0xA8, 0x0E, 0x16, 0x1E, 0xA2, 0x2C, 0x54, 0x7C, 0x92, 0xA4, 0xF0, 0x2C, 0x50, 0x78, +/* bank # 5 */ +0xF1, 0x84, 0xA8, 0x98, 0xC4, 0xCD, 0xFC, 0xD8, 0x0D, 0xDB, 0xA8, 0xFC, 0x2D, 0xF3, 0xD9, 0xBA, +0xA6, 0xF8, 0xDA, 0xBA, 0xA6, 0xDE, 0xD8, 0xBA, 0xB2, 0xB6, 0x86, 0x96, 0xA6, 0xD0, 0xF3, 0xC8, +0x41, 0xDA, 0xA6, 0xC8, 0xF8, 0xD8, 0xB0, 0xB4, 0xB8, 0x82, 0xA8, 0x92, 0xF5, 0x2C, 0x54, 0x88, +0x98, 0xF1, 0x35, 0xD9, 0xF4, 0x18, 0xD8, 0xF1, 0xA2, 0xD0, 0xF8, 0xF9, 0xA8, 0x84, 0xD9, 0xC7, +0xDF, 0xF8, 0xF8, 0x83, 0xC5, 0xDA, 0xDF, 0x69, 0xDF, 0x83, 0xC1, 0xD8, 0xF4, 0x01, 0x14, 0xF1, +0xA8, 0x82, 0x4E, 0xA8, 0x84, 0xF3, 0x11, 0xD1, 0x82, 0xF5, 0xD9, 0x92, 0x28, 0x97, 0x88, 0xF1, +0x09, 0xF4, 0x1C, 0x1C, 0xD8, 0x84, 0xA8, 0xF3, 0xC0, 0xF9, 0xD1, 0xD9, 0x97, 0x82, 0xF1, 0x29, +0xF4, 0x0D, 0xD8, 0xF3, 0xF9, 0xF9, 0xD1, 0xD9, 0x82, 0xF4, 0xC2, 0x03, 0xD8, 0xDE, 0xDF, 0x1A, +0xD8, 0xF1, 0xA2, 0xFA, 0xF9, 0xA8, 0x84, 0x98, 0xD9, 0xC7, 0xDF, 0xF8, 0xF8, 0xF8, 0x83, 0xC7, +0xDA, 0xDF, 0x69, 0xDF, 0xF8, 0x83, 0xC3, 0xD8, 0xF4, 0x01, 0x14, 0xF1, 0x98, 0xA8, 0x82, 0x2E, +0xA8, 0x84, 0xF3, 0x11, 0xD1, 0x82, 0xF5, 0xD9, 0x92, 0x50, 0x97, 0x88, 0xF1, 0x09, 0xF4, 0x1C, +0xD8, 0x84, 0xA8, 0xF3, 0xC0, 0xF8, 0xF9, 0xD1, 0xD9, 0x97, 0x82, 0xF1, 0x49, 0xF4, 0x0D, 0xD8, +0xF3, 0xF9, 0xF9, 0xD1, 0xD9, 0x82, 0xF4, 0xC4, 0x03, 0xD8, 0xDE, 0xDF, 0xD8, 0xF1, 0xAD, 0x88, +0x98, 0xCC, 0xA8, 0x09, 0xF9, 0xD9, 0x82, 0x92, 0xA8, 0xF5, 0x7C, 0xF1, 0x88, 0x3A, 0xCF, 0x94, +0x4A, 0x6E, 0x98, 0xDB, 0x69, 0x31, 0xDA, 0xAD, 0xF2, 0xDE, 0xF9, 0xD8, 0x87, 0x95, 0xA8, 0xF2, +0x21, 0xD1, 0xDA, 0xA5, 0xF9, 0xF4, 0x17, 0xD9, 0xF1, 0xAE, 0x8E, 0xD0, 0xC0, 0xC3, 0xAE, 0x82, +/* bank # 6 */ +0xC6, 0x84, 0xC3, 0xA8, 0x85, 0x95, 0xC8, 0xA5, 0x88, 0xF2, 0xC0, 0xF1, 0xF4, 0x01, 0x0E, 0xF1, +0x8E, 0x9E, 0xA8, 0xC6, 0x3E, 0x56, 0xF5, 0x54, 0xF1, 0x88, 0x72, 0xF4, 0x01, 0x15, 0xF1, 0x98, +0x45, 0x85, 0x6E, 0xF5, 0x8E, 0x9E, 0x04, 0x88, 0xF1, 0x42, 0x98, 0x5A, 0x8E, 0x9E, 0x06, 0x88, +0x69, 0xF4, 0x01, 0x1C, 0xF1, 0x98, 0x1E, 0x11, 0x08, 0xD0, 0xF5, 0x04, 0xF1, 0x1E, 0x97, 0x02, +0x02, 0x98, 0x36, 0x25, 0xDB, 0xF9, 0xD9, 0x85, 0xA5, 0xF3, 0xC1, 0xDA, 0x85, 0xA5, 0xF3, 0xDF, +0xD8, 0x85, 0x95, 0xA8, 0xF3, 0x09, 0xDA, 0xA5, 0xFA, 0xD8, 0x82, 0x92, 0xA8, 0xF5, 0x78, 0xF1, +0x88, 0x1A, 0x84, 0x9F, 0x26, 0x88, 0x98, 0x21, 0xDA, 0xF4, 0x1D, 0xF3, 0xD8, 0x87, 0x9F, 0x39, +0xD1, 0xAF, 0xD9, 0xDF, 0xDF, 0xFB, 0xF9, 0xF4, 0x0C, 0xF3, 0xD8, 0xFA, 0xD0, 0xF8, 0xDA, 0xF9, +0xF9, 0xD0, 0xDF, 0xD9, 0xF9, 0xD8, 0xF4, 0x0B, 0xD8, 0xF3, 0x87, 0x9F, 0x39, 0xD1, 0xAF, 0xD9, +0xDF, 0xDF, 0xF4, 0x1D, 0xF3, 0xD8, 0xFA, 0xFC, 0xA8, 0x69, 0xF9, 0xF9, 0xAF, 0xD0, 0xDA, 0xDE, +0xFA, 0xD9, 0xF8, 0x8F, 0x9F, 0xA8, 0xF1, 0xCC, 0xF3, 0x98, 0xDB, 0x45, 0xD9, 0xAF, 0xDF, 0xD0, +0xF8, 0xD8, 0xF1, 0x8F, 0x9F, 0xA8, 0xCA, 0xF3, 0x88, 0x09, 0xDA, 0xAF, 0x8F, 0xCB, 0xF8, 0xD8, +0xF2, 0xAD, 0x97, 0x8D, 0x0C, 0xD9, 0xA5, 0xDF, 0xF9, 0xBA, 0xA6, 0xF3, 0xFA, 0xF4, 0x12, 0xF2, +0xD8, 0x95, 0x0D, 0xD1, 0xD9, 0xBA, 0xA6, 0xF3, 0xFA, 0xDA, 0xA5, 0xF2, 0xC1, 0xBA, 0xA6, 0xF3, +0xDF, 0xD8, 0xF1, 0xBA, 0xB2, 0xB6, 0x86, 0x96, 0xA6, 0xD0, 0xCA, 0xF3, 0x49, 0xDA, 0xA6, 0xCB, +0xF8, 0xD8, 0xB0, 0xB4, 0xB8, 0xD8, 0xAD, 0x84, 0xF2, 0xC0, 0xDF, 0xF1, 0x8F, 0xCB, 0xC3, 0xA8, +/* bank # 7 */ +0xB2, 0xB6, 0x86, 0x96, 0xC8, 0xC1, 0xCB, 0xC3, 0xF3, 0xB0, 0xB4, 0x88, 0x98, 0xA8, 0x21, 0xDB, +0x71, 0x8D, 0x9D, 0x71, 0x85, 0x95, 0x21, 0xD9, 0xAD, 0xF2, 0xFA, 0xD8, 0x85, 0x97, 0xA8, 0x28, +0xD9, 0xF4, 0x08, 0xD8, 0xF2, 0x8D, 0x29, 0xDA, 0xF4, 0x05, 0xD9, 0xF2, 0x85, 0xA4, 0xC2, 0xF2, +0xD8, 0xA8, 0x8D, 0x94, 0x01, 0xD1, 0xD9, 0xF4, 0x11, 0xF2, 0xD8, 0x87, 0x21, 0xD8, 0xF4, 0x0A, +0xD8, 0xF2, 0x84, 0x98, 0xA8, 0xC8, 0x01, 0xD1, 0xD9, 0xF4, 0x11, 0xD8, 0xF3, 0xA4, 0xC8, 0xBB, +0xAF, 0xD0, 0xF2, 0xDE, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xD8, 0xF1, 0xB8, 0xF6, +0xB5, 0xB9, 0xB0, 0x8A, 0x95, 0xA3, 0xDE, 0x3C, 0xA3, 0xD9, 0xF8, 0xD8, 0x5C, 0xA3, 0xD9, 0xF8, +0xD8, 0x7C, 0xA3, 0xD9, 0xF8, 0xD8, 0xF8, 0xF9, 0xD1, 0xA5, 0xD9, 0xDF, 0xDA, 0xFA, 0xD8, 0xB1, +0x85, 0x30, 0xF7, 0xD9, 0xDE, 0xD8, 0xF8, 0x30, 0xAD, 0xDA, 0xDE, 0xD8, 0xF2, 0xB4, 0x8C, 0x99, +0xA3, 0x2D, 0x55, 0x7D, 0xA0, 0x83, 0xDF, 0xDF, 0xDF, 0xB5, 0x91, 0xA0, 0xF6, 0x29, 0xD9, 0xFB, +0xD8, 0xA0, 0xFC, 0x29, 0xD9, 0xFA, 0xD8, 0xA0, 0xD0, 0x51, 0xD9, 0xF8, 0xD8, 0xFC, 0x51, 0xD9, +0xF9, 0xD8, 0x79, 0xD9, 0xFB, 0xD8, 0xA0, 0xD0, 0xFC, 0x79, 0xD9, 0xFA, 0xD8, 0xA1, 0xF9, 0xF9, +0xF9, 0xF9, 0xF9, 0xA0, 0xDA, 0xDF, 0xDF, 0xDF, 0xD8, 0xA1, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xAC, +0xDE, 0xF8, 0xAD, 0xDE, 0x83, 0x93, 0xAC, 0x2C, 0x54, 0x7C, 0xF1, 0xA8, 0xDF, 0xDF, 0xDF, 0xF6, +0x9D, 0x2C, 0xDA, 0xA0, 0xDF, 0xD9, 0xFA, 0xDB, 0x2D, 0xF8, 0xD8, 0xA8, 0x50, 0xDA, 0xA0, 0xD0, +0xDE, 0xD9, 0xD0, 0xF8, 0xF8, 0xF8, 0xDB, 0x55, 0xF8, 0xD8, 0xA8, 0x78, 0xDA, 0xA0, 0xD0, 0xDF, +/* bank # 8 */ +0xD9, 0xD0, 0xFA, 0xF8, 0xF8, 0xF8, 0xF8, 0xDB, 0x7D, 0xF8, 0xD8, 0x9C, 0xA8, 0x8C, 0xF5, 0x30, +0xDB, 0x38, 0xD9, 0xD0, 0xDE, 0xDF, 0xA0, 0xD0, 0xDE, 0xDF, 0xD8, 0xA8, 0x48, 0xDB, 0x58, 0xD9, +0xDF, 0xD0, 0xDE, 0xA0, 0xDF, 0xD0, 0xDE, 0xD8, 0xA8, 0x68, 0xDB, 0x70, 0xD9, 0xDF, 0xDF, 0xA0, +0xDF, 0xDF, 0xD8, 0xF1, 0xA8, 0x88, 0x90, 0x2C, 0x54, 0x7C, 0x98, 0xA8, 0xD0, 0x5C, 0x38, 0xD1, +0xDA, 0xF2, 0xAE, 0x8C, 0xDF, 0xF9, 0xD8, 0xB0, 0x87, 0xA8, 0xC1, 0xC1, 0xB1, 0x88, 0xA8, 0xC6, +0xF9, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, +0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xF7, 0x8D, 0x9D, 0xAD, 0xF8, 0x18, 0xDA, +0xF2, 0xAE, 0xDF, 0xD8, 0xF7, 0xAD, 0xFA, 0x30, 0xD9, 0xA4, 0xDE, 0xF9, 0xD8, 0xF2, 0xAE, 0xDE, +0xFA, 0xF9, 0x83, 0xA7, 0xD9, 0xC3, 0xC5, 0xC7, 0xF1, 0x88, 0x9B, 0xA7, 0x7A, 0xAD, 0xF7, 0xDE, +0xDF, 0xA4, 0xF8, 0x84, 0x94, 0x08, 0xA7, 0x97, 0xF3, 0x00, 0xAE, 0xF2, 0x98, 0x19, 0xA4, 0x88, +0xC6, 0xA3, 0x94, 0x88, 0xF6, 0x32, 0xDF, 0xF2, 0x83, 0x93, 0xDB, 0x09, 0xD9, 0xF2, 0xAA, 0xDF, +0xD8, 0xD8, 0xAE, 0xF8, 0xF9, 0xD1, 0xDA, 0xF3, 0xA4, 0xDE, 0xA7, 0xF1, 0x88, 0x9B, 0x7A, 0xD8, +0xF3, 0x84, 0x94, 0xAE, 0x19, 0xF9, 0xDA, 0xAA, 0xF1, 0xDF, 0xD8, 0xA8, 0x81, 0xC0, 0xC3, 0xC5, +0xC7, 0xA3, 0x92, 0x83, 0xF6, 0x28, 0xAD, 0xDE, 0xD9, 0xF8, 0xD8, 0xA3, 0x50, 0xAD, 0xD9, 0xF8, +0xD8, 0xA3, 0x78, 0xAD, 0xD9, 0xF8, 0xD8, 0xF8, 0xF9, 0xD1, 0xA1, 0xDA, 0xDE, 0xC3, 0xC5, 0xC7, +0xD8, 0xA1, 0x81, 0x94, 0xF8, 0x18, 0xF2, 0xB0, 0x89, 0xAC, 0xC3, 0xC5, 0xC7, 0xF1, 0xD8, 0xB8, +/* bank # 9 */ +0xB4, 0xB0, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, +0x0C, 0x20, 0x14, 0x40, 0xB0, 0xB4, 0xB8, 0xF0, 0xA8, 0x8A, 0x9A, 0x28, 0x50, 0x78, 0xB7, 0x9B, +0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xF1, 0xBB, 0xAB, +0x88, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0xB3, 0x8B, 0xB8, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0xB0, +0x88, 0xB4, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xBB, 0xAB, 0xB3, 0x8B, 0x02, 0x26, 0x46, 0x66, 0xB0, +0xB8, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 0x8A, 0x24, 0x70, 0x59, +0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 0x8A, 0x64, 0x48, 0x31, +0x8B, 0x30, 0x49, 0x60, 0x88, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 0x28, +0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 0xF0, +0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xA9, +0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 0x8C, +0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 0x7E, +0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB5, 0xB9, 0xA3, 0xDF, 0xDF, 0xDF, 0xAE, 0xD0, +0xDF, 0xAA, 0xD0, 0xDE, 0xF2, 0xAB, 0xF8, 0xF9, 0xD9, 0xB0, 0x87, 0xC4, 0xAA, 0xF1, 0xDF, 0xDF, +0xBB, 0xAF, 0xDF, 0xDF, 0xB9, 0xD8, 0xB1, 0xF1, 0xA3, 0x97, 0x8E, 0x60, 0xDF, 0xB0, 0x84, 0xF2, +0xC8, 0xF8, 0xF9, 0xD9, 0xDE, 0xD8, 0x93, 0x85, 0xF1, 0x4A, 0xB1, 0x83, 0xA3, 0x08, 0xB5, 0x83, +/* bank # 10 */ +0x9A, 0x08, 0x10, 0xB7, 0x9F, 0x10, 0xD8, 0xF1, 0xB0, 0xBA, 0xAE, 0xB0, 0x8A, 0xC2, 0xB2, 0xB6, +0x8E, 0x9E, 0xF1, 0xFB, 0xD9, 0xF4, 0x1D, 0xD8, 0xF9, 0xD9, 0x0C, 0xF1, 0xD8, 0xF8, 0xF8, 0xAD, +0x61, 0xD9, 0xAE, 0xFB, 0xD8, 0xF4, 0x0C, 0xF1, 0xD8, 0xF8, 0xF8, 0xAD, 0x19, 0xD9, 0xAE, 0xFB, +0xDF, 0xD8, 0xF4, 0x16, 0xF1, 0xD8, 0xF8, 0xAD, 0x8D, 0x61, 0xD9, 0xF4, 0xF4, 0xAC, 0xF5, 0x9C, +0x9C, 0x8D, 0xDF, 0x2B, 0xBA, 0xB6, 0xAE, 0xFA, 0xF8, 0xF4, 0x0B, 0xD8, 0xF1, 0xAE, 0xD0, 0xF8, +0xAD, 0x51, 0xDA, 0xAE, 0xFA, 0xF8, 0xF1, 0xD8, 0xB9, 0xB1, 0xB6, 0xA3, 0x83, 0x9C, 0x08, 0xB9, +0xB1, 0x83, 0x9A, 0xB5, 0xAA, 0xC0, 0xFD, 0x30, 0x83, 0xB7, 0x9F, 0x10, 0xB5, 0x8B, 0x93, 0xF2, +0x02, 0x02, 0xD1, 0xAB, 0xDA, 0xDE, 0xD8, 0xF1, 0xB0, 0x80, 0xBA, 0xAB, 0xC0, 0xC3, 0xB2, 0x84, +0xC1, 0xC3, 0xD8, 0xB1, 0xB9, 0xF3, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, +0x87, 0x9C, 0xB9, 0xA3, 0xDD, 0xF1, 0xB3, 0x8B, 0x8B, 0x8B, 0x8B, 0x8B, 0xB0, 0x87, 0x20, 0x28, +0x30, 0x38, 0xB2, 0x8B, 0xB6, 0x9B, 0xF2, 0xA3, 0xC0, 0xC8, 0xC2, 0xC4, 0xCC, 0xC6, 0xA3, 0xA3, +0xA3, 0xF1, 0xB0, 0x87, 0xB5, 0x9A, 0xD8, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 0xBA, 0xAC, 0xDF, 0xB9, +0xA3, 0xFE, 0xF2, 0xAB, 0xC4, 0xAA, 0xF1, 0xDF, 0xDF, 0xBB, 0xAF, 0xDF, 0xDF, 0xA3, 0xA3, 0xA3, +0xD8, 0xD8, 0xD8, 0xBB, 0xB3, 0xB7, 0xF1, 0xAA, 0xF9, 0xDA, 0xFF, 0xD9, 0x80, 0x9A, 0xAA, 0x28, +0xB4, 0x80, 0x98, 0xA7, 0x20, 0xB7, 0x97, 0x87, 0xA8, 0x66, 0x88, 0xF0, 0x79, 0x51, 0xF1, 0x90, +0x2C, 0x87, 0x0C, 0xA7, 0x81, 0x97, 0x62, 0x93, 0xF0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, +/* bank # 11 */ +0x51, 0x79, 0x90, 0xA5, 0xF1, 0x28, 0x4C, 0x6C, 0x87, 0x0C, 0x95, 0x18, 0x85, 0x78, 0xA3, 0x83, +0x90, 0x28, 0x4C, 0x6C, 0x88, 0x6C, 0xD8, 0xF3, 0xA2, 0x82, 0x00, 0xF2, 0x10, 0xA8, 0x92, 0x19, +0x80, 0xA2, 0xF2, 0xD9, 0x26, 0xD8, 0xF1, 0x88, 0xA8, 0x4D, 0xD9, 0x48, 0xD8, 0x96, 0xA8, 0x39, +0x80, 0xD9, 0x3C, 0xD8, 0x95, 0x80, 0xA8, 0x39, 0xA6, 0x86, 0x98, 0xD9, 0x2C, 0xDA, 0x87, 0xA7, +0x2C, 0xD8, 0xA8, 0x89, 0x95, 0x19, 0xA9, 0x80, 0xD9, 0x38, 0xD8, 0xA8, 0x89, 0x39, 0xA9, 0x80, +0xDA, 0x3C, 0xD8, 0xA8, 0x2E, 0xA8, 0x39, 0x90, 0xD9, 0x0C, 0xD8, 0xA8, 0x95, 0x31, 0x98, 0xD9, +0x0C, 0xD8, 0xA8, 0x09, 0xD9, 0xFF, 0xD8, 0x01, 0xDA, 0xFF, 0xD8, 0x95, 0x39, 0xA9, 0xDA, 0x26, +0xFF, 0xD8, 0x90, 0xA8, 0x0D, 0x89, 0x99, 0xA8, 0x10, 0x80, 0x98, 0x21, 0xDA, 0x2E, 0xD8, 0x89, +0x99, 0xA8, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, 0x86, 0x96, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, +0x87, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, 0x82, 0x92, 0xF3, 0x41, 0x80, 0xF1, 0xD9, 0x2E, 0xD8, +0xA8, 0x82, 0xF3, 0x19, 0x80, 0xF1, 0xD9, 0x2E, 0xD8, 0x82, 0xAC, 0xF3, 0xC0, 0xA2, 0x80, 0x22, +0xF1, 0xA6, 0x2E, 0xA7, 0x2E, 0xA9, 0x22, 0x98, 0xA8, 0x29, 0xDA, 0xAC, 0xDE, 0xFF, 0xD8, 0xA2, +0xF2, 0x2A, 0xF1, 0xA9, 0x2E, 0x82, 0x92, 0xA8, 0xF2, 0x31, 0x80, 0xA6, 0x96, 0xF1, 0xD9, 0x00, +0xAC, 0x8C, 0x9C, 0x0C, 0x30, 0xAC, 0xDE, 0xD0, 0xDE, 0xFF, 0xD8, 0x8C, 0x9C, 0xAC, 0xD0, 0x10, +0xAC, 0xDE, 0x80, 0x92, 0xA2, 0xF2, 0x4C, 0x82, 0xA8, 0xF1, 0xCA, 0xF2, 0x35, 0xF1, 0x96, 0x88, +0xA6, 0xD9, 0x00, 0xD8, 0xF1, 0xFF, +}; + +// this divisor is pre configured into the above image and can't be modified at this time. +#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR +#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default +#endif + +// this is the most basic initialization I can create. with the intent that we access the register bytes as few times as needed to get the job done. +// for detailed descriptins of all registers and there purpose google "MPU-6000/MPU-6050 Register Map and Descriptions" +uint8_t MPU6050::dmpInitialize() { // Lets get it over with fast Write everything once and set it up necely + uint8_t val; + uint16_t ival; + // Reset procedure per instructions in the "MPU-6000/MPU-6050 Register Map and Descriptions" page 41 + I2Cdev::writeBit(devAddr,0x6B, 7, (val = 1)); //PWR_MGMT_1: reset with 100ms delay + delay(100); + I2Cdev::writeBits(devAddr,0x6A, 2, 3, (val = 0b111)); // full SIGNAL_PATH_RESET: with another 100ms delay + delay(100); + I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 1000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro + I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x00)); // 0000 0000 INT_ENABLE: no Interrupt + I2Cdev::writeBytes(devAddr,0x23, 1, &(val = 0x00)); // 0000 0000 MPU FIFO_EN: (all off) Using DMP's FIFO instead + I2Cdev::writeBytes(devAddr,0x1C, 1, &(val = 0x00)); // 0000 0000 ACCEL_CONFIG: 0 = Accel Full Scale Select: 2g + I2Cdev::writeBytes(devAddr,0x37, 1, &(val = 0x80)); // 1001 0000 INT_PIN_CFG: ACTL The logic level for int pin is active low. and interrupt status bits are cleared on any read + I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 0000 0001 PWR_MGMT_1: Clock Source Select PLL_X_gyro + I2Cdev::writeBytes(devAddr,0x19, 1, &(val = 0x04)); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)) + I2Cdev::writeBytes(devAddr,0x1A, 1, &(val = 0x01)); // 0000 0001 CONFIG: Digital Low Pass Filter (DLPF) Configuration 188HZ //Im betting this will be the beat + if (!writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) return 1; // Loads the DMP image into the MPU6050 Memory // Should Never Fail + I2Cdev::writeWords(devAddr, 0x70, 1, &(ival = 0x0400)); // DMP Program Start Address + I2Cdev::writeBytes(devAddr,0x1B, 1, &(val = 0x18)); // 0001 1000 GYRO_CONFIG: 3 = +2000 Deg/sec + I2Cdev::writeBytes(devAddr,0x6A, 1, &(val = 0xC0)); // 1100 1100 USER_CTRL: Enable Fifo and Reset Fifo + I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x02)); // 0000 0010 INT_ENABLE: RAW_DMP_INT_EN on + I2Cdev::writeBit(devAddr,0x6A, 2, 1); // Reset FIFO one last time just for kicks. (MPUi2cWrite reads 0x6A first and only alters 1 bit and then saves the byte) + + setDMPEnabled(false); // disable DMP for compatibility with the MPU6050 library +/* + dmpPacketSize += 16;//DMP_FEATURE_6X_LP_QUAT + dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_ACCEL + dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_GYRO +*/ + dmpPacketSize = 28; + return 0; +} + +bool MPU6050::dmpPacketAvailable() { + return getFIFOCount() >= dmpGetFIFOPacketSize(); +} + +// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); +// uint8_t MPU6050::dmpGetFIFORate(); +// uint8_t MPU6050::dmpGetSampleStepSizeMS(); +// uint8_t MPU6050::dmpGetSampleFrequency(); +// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); + +//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); +//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); +//uint8_t MPU6050::dmpRunFIFORateProcesses(); + +// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + +uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[16] << 8) | packet[17]); + data[1] = (((uint32_t)packet[18] << 8) | packet[19]); + data[2] = (((uint32_t)packet[20] << 8) | packet[21]); + return 0; +} +uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[16] << 8) | packet[17]; + data[1] = (packet[18] << 8) | packet[19]; + data[2] = (packet[20] << 8) | packet[21]; + return 0; +} +uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[16] << 8) | packet[17]; + v -> y = (packet[18] << 8) | packet[19]; + v -> z = (packet[20] << 8) | packet[21]; + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); + data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); + data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); + data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[0] << 8) | packet[1]); + data[1] = ((packet[4] << 8) | packet[5]); + data[2] = ((packet[8] << 8) | packet[9]); + data[3] = ((packet[12] << 8) | packet[13]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + if (status == 0) { + q -> w = (float)qI[0] / 16384.0f; + q -> x = (float)qI[1] / 16384.0f; + q -> y = (float)qI[2] / 16384.0f; + q -> z = (float)qI[3] / 16384.0f; + return 0; + } + return status; // int16 return value, indicates error if this line is reached +} +// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[22] << 8) | packet[23]); + data[1] = (((uint32_t)packet[24] << 8) | packet[25]); + data[2] = (((uint32_t)packet[26] << 8) | packet[27]); + return 0; +} +uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[22] << 8) | packet[23]; + data[1] = (packet[24] << 8) | packet[25]; + data[2] = (packet[26] << 8) | packet[27]; + return 0; +} +uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[22] << 8) | packet[23]; + v -> y = (packet[24] << 8) | packet[25]; + v -> z = (packet[26] << 8) | packet[27]; + return 0; +} +// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); +// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { + // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) + v -> x = vRaw -> x - gravity -> x*8192; + v -> y = vRaw -> y - gravity -> y*8192; + v -> z = vRaw -> z - gravity -> z*8192; + return 0; +} +// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { + // rotate measured 3D acceleration vector into original state + // frame of reference based on orientation quaternion + memcpy(v, vReal, sizeof(VectorInt16)); + v -> rotate(q); + return 0; +} +// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); + return status; +} + +uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { + v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); + v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); + v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; + return 0; +} +// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); +// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); + +uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi + data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta + data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi + return 0; +} + +#ifdef USE_OLD_DMPGETYAWPITCHROLL +uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); + return 0; +} +#else +uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan2(gravity -> y , gravity -> z); + if (gravity -> z < 0) { + if(data[1] > 0) { + data[1] = PI - data[1]; + } else { + data[1] = -PI - data[1]; + } + } + return 0; +} +#endif + +// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); + +uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { + /*for (uint8_t k = 0; k < dmpPacketSize; k++) { + if (dmpData[k] < 0x10) Serial.print("0"); + Serial.print(dmpData[k], HEX); + Serial.print(" "); + } + Serial.print("\n");*/ + //Serial.println((uint16_t)dmpPacketBuffer); + return 0; +} +uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { + uint8_t status; + uint8_t buf[dmpPacketSize]; + for (uint8_t i = 0; i < numPackets; i++) { + // read packet from FIFO + getFIFOBytes(buf, dmpPacketSize); + + // process packet + if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; + + // increment external process count variable, if supplied + if (processed != 0) (*processed)++; + } + return 0; +} + +// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); + +// uint8_t MPU6050::dmpInitFIFOParam(); +// uint8_t MPU6050::dmpCloseFIFO(); +// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); +// uint8_t MPU6050::dmpDecodeQuantizedAccel(); +// uint32_t MPU6050::dmpGetGyroSumOfSquare(); +// uint32_t MPU6050::dmpGetAccelSumOfSquare(); +// void MPU6050::dmpOverrideQuaternion(long *q); +uint16_t MPU6050::dmpGetFIFOPacketSize() { + return dmpPacketSize; +} + +#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050_9Axis_MotionApps41.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050_9Axis_MotionApps41.h new file mode 100644 index 00000000..763a49b9 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/MPU6050_9Axis_MotionApps41.h @@ -0,0 +1,887 @@ +// I2Cdev library collection - MPU6050 I2C device class, 9-axis MotionApps 4.1 implementation +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 6/18/2012 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// ... - ongoing debug release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _MPU6050_9AXIS_MOTIONAPPS41_H_ +#define _MPU6050_9AXIS_MOTIONAPPS41_H_ + +#include "I2Cdev.h" +#include "helper_3dmath.h" + +// MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board +#define MPU6050_INCLUDE_DMP_MOTIONAPPS41 + +#include "MPU6050.h" + +// Tom Carpenter's conditional PROGMEM code +// http://forum.arduino.cc/index.php?topic=129407.0 +#ifdef __AVR__ + #include +#else + // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen + #ifndef __PGMSPACE_H_ + #define __PGMSPACE_H_ 1 + #include + + #define PROGMEM + #define PGM_P const char * + #define PSTR(str) (str) + #define F(x) x + + typedef void prog_void; + typedef char prog_char; + //typedef unsigned char prog_uchar; + typedef int8_t prog_int8_t; + typedef uint8_t prog_uint8_t; + typedef int16_t prog_int16_t; + typedef uint16_t prog_uint16_t; + typedef int32_t prog_int32_t; + typedef uint32_t prog_uint32_t; + + #define strcpy_P(dest, src) strcpy((dest), (src)) + #define strcat_P(dest, src) strcat((dest), (src)) + #define strcmp_P(a, b) strcmp((a), (b)) + + #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) + #define pgm_read_word(addr) (*(const unsigned short *)(addr)) + #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) + #define pgm_read_float(addr) (*(const float *)(addr)) + + #define pgm_read_byte_near(addr) pgm_read_byte(addr) + #define pgm_read_word_near(addr) pgm_read_word(addr) + #define pgm_read_dword_near(addr) pgm_read_dword(addr) + #define pgm_read_float_near(addr) pgm_read_float(addr) + #define pgm_read_byte_far(addr) pgm_read_byte(addr) + #define pgm_read_word_far(addr) pgm_read_word(addr) + #define pgm_read_dword_far(addr) pgm_read_dword(addr) + #define pgm_read_float_far(addr) pgm_read_float(addr) + #endif +#endif + +// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. +// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) +// after moving string constants to flash memory storage using the F() +// compiler macro (Arduino IDE 1.0+ required). + +//#define DEBUG +#ifdef DEBUG + #define DEBUG_PRINT(x) Serial.print(x) + #define DEBUG_PRINTF(x, y) Serial.print(x, y) + #define DEBUG_PRINTLN(x) Serial.println(x) + #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) +#else + #define DEBUG_PRINT(x) + #define DEBUG_PRINTF(x, y) + #define DEBUG_PRINTLN(x) + #define DEBUG_PRINTLNF(x, y) +#endif + +#define MPU6050_DMP_CODE_SIZE 1962 // dmpMemory[] +#define MPU6050_DMP_CONFIG_SIZE 232 // dmpConfig[] +#define MPU6050_DMP_UPDATES_SIZE 140 // dmpUpdates[] + +/* ================================================================================================ * + | Default MotionApps v4.1 48-byte FIFO packet structure: | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | + | | + | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | + | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | + * ================================================================================================ */ + +// this block of memory gets written to the MPU on start-up, and it seems +// to be volatile memory, so it has to be done each time (it only takes ~1 +// second though) +const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { + // bank 0, 256 bytes + 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, + 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, + 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, + 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, + + // bank 1, 256 bytes + 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, + 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, + 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, + 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, + + // bank 2, 256 bytes + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + + // bank 3, 256 bytes + 0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4, + 0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, + 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, + 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, + 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, + 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, + 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, + 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, + 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, + 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, + 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, + 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, + 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, + 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54, + 0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D, + 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86, + + // bank 4, 256 bytes + 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, + 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, + 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, + 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, + 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, + 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, + 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, + 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, + 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, + 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, + 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, + 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, + + // bank 5, 256 bytes + 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, + 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, + 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, + 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, + 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, + 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, + 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, + 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, + 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, + 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, + 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86, + 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, + 0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9, + 0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30, + 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0, + 0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24, + + // bank 6, 256 bytes + 0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40, + 0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, + 0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, + 0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, + 0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, + 0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, + 0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, + 0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, + 0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, + 0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, + 0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, + 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, + 0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, + 0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, + 0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, + 0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, + + // bank 7, 170 bytes (remainder) + 0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, + 0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, + 0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, + 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, + 0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, + 0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9, + 0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, + 0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, + 0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, + 0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, + 0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF +}; + +#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR +#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x03 +#endif + +const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { +// BANK OFFSET LENGTH [DATA] + 0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ? + 0x03, 0x82, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration + 0x03, 0xB2, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration + 0x00, 0x68, 0x04, 0x02, 0xCA, 0xE3, 0x09, // D_0_104 inv_set_gyro_calibration + 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration + 0x03, 0x86, 0x03, 0x0C, 0xC9, 0x2C, // FCFG_2 inv_set_accel_calibration + 0x03, 0x90, 0x03, 0x26, 0x46, 0x66, // (continued)...FCFG_2 inv_set_accel_calibration + 0x00, 0x6C, 0x02, 0x40, 0x00, // D_0_108 inv_set_accel_calibration + + 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration + 0x02, 0x44, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_01 + 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 + 0x02, 0x4C, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_10 + 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 + 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 + 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 + 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 + 0x02, 0xBC, 0x04, 0xC0, 0x00, 0x00, 0x00, // CPASS_MTX_22 + + 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel + 0x03, 0x86, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors + 0x04, 0x22, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion + 0x00, 0xA3, 0x01, 0x00, // ? + 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update + 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion + 0x07, 0x9F, 0x01, 0x30, // CFG_16 inv_set_footer + 0x07, 0x67, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro + 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo + 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? + 0x02, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // ? + 0x07, 0x83, 0x06, 0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ? + // SPECIAL 0x01 = enable interrupts + 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION + 0x07, 0xA7, 0x01, 0xFE, // ? + 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? + 0x07, 0x67, 0x01, 0x9A, // ? + 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo + 0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo + 0x02, 0x16, 0x02, 0x00, MPU6050_DMP_FIFO_RATE_DIVISOR // D_0_22 inv_set_fifo_rate + + // This very last 0x03 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, + // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. + // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) + + // It is important to make sure the host processor can keep up with reading and processing + // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. +}; + +const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { + 0x01, 0xB2, 0x02, 0xFF, 0xF5, + 0x01, 0x90, 0x04, 0x0A, 0x0D, 0x97, 0xC0, + 0x00, 0xA3, 0x01, 0x00, + 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, + 0x01, 0x6A, 0x02, 0x06, 0x00, + 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, + 0x02, 0x60, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x01, 0x08, 0x02, 0x01, 0x20, + 0x01, 0x0A, 0x02, 0x00, 0x4E, + 0x01, 0x02, 0x02, 0xFE, 0xB3, + 0x02, 0x6C, 0x04, 0x00, 0x00, 0x00, 0x00, // READ + 0x02, 0x6C, 0x04, 0xFA, 0xFE, 0x00, 0x00, + 0x02, 0x60, 0x0C, 0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C, + 0x02, 0xF4, 0x04, 0x00, 0x00, 0x00, 0x00, + 0x02, 0xF8, 0x04, 0x00, 0x00, 0x00, 0x00, + 0x02, 0xFC, 0x04, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 +}; + +uint8_t MPU6050::dmpInitialize() { + // reset device + DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); + reset(); + delay(30); // wait after reset + + // disable sleep mode + DEBUG_PRINTLN(F("Disabling sleep mode...")); + setSleepEnabled(false); + + // get MPU product ID + DEBUG_PRINTLN(F("Getting product ID...")); + //uint8_t productID = 0; //getProductID(); + DEBUG_PRINT(F("Product ID = ")); + DEBUG_PRINT(productID); + + // get MPU hardware revision + DEBUG_PRINTLN(F("Selecting user bank 16...")); + setMemoryBank(0x10, true, true); + DEBUG_PRINTLN(F("Selecting memory byte 6...")); + setMemoryStartAddress(0x06); + DEBUG_PRINTLN(F("Checking hardware revision...")); + uint8_t hwRevision = readMemoryByte(); + DEBUG_PRINT(F("Revision @ user[16][6] = ")); + DEBUG_PRINTLNF(hwRevision, HEX); + DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); + setMemoryBank(0, false, false); + + // check OTP bank valid + DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); + uint8_t otpValid = getOTPBankValid(); + DEBUG_PRINT(F("OTP bank is ")); + DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); + + // get X/Y/Z gyro offsets + DEBUG_PRINTLN(F("Reading gyro offset values...")); + int8_t xgOffset = getXGyroOffset(); + int8_t ygOffset = getYGyroOffset(); + int8_t zgOffset = getZGyroOffset(); + DEBUG_PRINT(F("X gyro offset = ")); + DEBUG_PRINTLN(xgOffset); + DEBUG_PRINT(F("Y gyro offset = ")); + DEBUG_PRINTLN(ygOffset); + DEBUG_PRINT(F("Z gyro offset = ")); + DEBUG_PRINTLN(zgOffset); + + I2Cdev::readByte(devAddr, MPU6050_RA_USER_CTRL, buffer); // ? + + DEBUG_PRINTLN(F("Enabling interrupt latch, clear on any read, AUX bypass enabled")); + I2Cdev::writeByte(devAddr, MPU6050_RA_INT_PIN_CFG, 0x32); + + // enable MPU AUX I2C bypass mode + //DEBUG_PRINTLN(F("Enabling AUX I2C bypass mode...")); + //setI2CBypassEnabled(true); + + DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); + //mag -> setMode(0); + I2Cdev::writeByte(0x0E, 0x0A, 0x00); + + DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access...")); + //mag -> setMode(0x0F); + I2Cdev::writeByte(0x0E, 0x0A, 0x0F); + + DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration...")); + int8_t asax, asay, asaz; + //mag -> getAdjustment(&asax, &asay, &asaz); + I2Cdev::readBytes(0x0E, 0x10, 3, buffer); + asax = (int8_t)buffer[0]; + asay = (int8_t)buffer[1]; + asaz = (int8_t)buffer[2]; + DEBUG_PRINT(F("Adjustment X/Y/Z = ")); + DEBUG_PRINT(asax); + DEBUG_PRINT(F(" / ")); + DEBUG_PRINT(asay); + DEBUG_PRINT(F(" / ")); + DEBUG_PRINTLN(asaz); + + DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); + //mag -> setMode(0); + I2Cdev::writeByte(0x0E, 0x0A, 0x00); + + // load DMP code into memory banks + DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); + DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); + DEBUG_PRINTLN(F(" bytes)")); + if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { + DEBUG_PRINTLN(F("Success! DMP code written and verified.")); + + DEBUG_PRINTLN(F("Configuring DMP and related settings...")); + + // write DMP configuration + DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); + DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); + DEBUG_PRINTLN(F(" bytes in config def)")); + if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { + DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); + + DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); + setIntEnabled(1< setMode(1); + I2Cdev::writeByte(0x0E, 0x0A, 0x01); + + // setup AK8975 (0x0E) as Slave 0 in read mode + DEBUG_PRINTLN(F("Setting up AK8975 read slave 0...")); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_ADDR, 0x8E); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_REG, 0x01); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_CTRL, 0xDA); + + // setup AK8975 (0x0E) as Slave 2 in write mode + DEBUG_PRINTLN(F("Setting up AK8975 write slave 2...")); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_ADDR, 0x0E); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_REG, 0x0A); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_CTRL, 0x81); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_DO, 0x01); + + // setup I2C timing/delay control + DEBUG_PRINTLN(F("Setting up slave access delay...")); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV4_CTRL, 0x18); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05); + + // enable interrupts + DEBUG_PRINTLN(F("Enabling default interrupt behavior/no bypass...")); + I2Cdev::writeByte(0x68, MPU6050_RA_INT_PIN_CFG, 0x00); + + // enable I2C master mode and reset DMP/FIFO + DEBUG_PRINTLN(F("Enabling I2C master mode...")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20); + DEBUG_PRINTLN(F("Resetting FIFO...")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x24); + DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20); + DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO...")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0xE8); + + DEBUG_PRINTLN(F("Writing final memory update 5/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 6/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 7/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 8/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 9/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 10/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 11/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Reading final memory update 12/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + #ifdef DEBUG + DEBUG_PRINT(F("Read bytes: ")); + for (j = 0; j < 4; j++) { + DEBUG_PRINTF(dmpUpdate[3 + j], HEX); + DEBUG_PRINT(" "); + } + DEBUG_PRINTLN(""); + #endif + + DEBUG_PRINTLN(F("Writing final memory update 13/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 14/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 15/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 16/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 17/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Waiting for FIRO count >= 46...")); + while ((fifoCount = getFIFOCount()) < 46); + DEBUG_PRINTLN(F("Reading FIFO...")); + getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes + DEBUG_PRINTLN(F("Reading interrupt status...")); + getIntStatus(); + + DEBUG_PRINTLN(F("Writing final memory update 18/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); + while ((fifoCount = getFIFOCount()) < 48); + DEBUG_PRINTLN(F("Reading FIFO...")); + getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes + DEBUG_PRINTLN(F("Reading interrupt status...")); + getIntStatus(); + DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); + while ((fifoCount = getFIFOCount()) < 48); + DEBUG_PRINTLN(F("Reading FIFO...")); + getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes + DEBUG_PRINTLN(F("Reading interrupt status...")); + getIntStatus(); + + DEBUG_PRINTLN(F("Writing final memory update 19/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); + setDMPEnabled(false); + + DEBUG_PRINTLN(F("Setting up internal 48-byte (default) DMP packet buffer...")); + dmpPacketSize = 48; + /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { + return 3; // TODO: proper error code for no memory + }*/ + + DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); + resetFIFO(); + getIntStatus(); + } else { + DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); + return 2; // configuration block loading failed + } + } else { + DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); + return 1; // main binary block loading failed + } + return 0; // success +} + +bool MPU6050::dmpPacketAvailable() { + return getFIFOCount() >= dmpGetFIFOPacketSize(); +} + +// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); +// uint8_t MPU6050::dmpGetFIFORate(); +// uint8_t MPU6050::dmpGetSampleStepSizeMS(); +// uint8_t MPU6050::dmpGetSampleFrequency(); +// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); + +//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); +//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); +//uint8_t MPU6050::dmpRunFIFORateProcesses(); + +// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + +uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[34] << 24) | ((uint32_t)packet[35] << 16) | ((uint32_t)packet[36] << 8) | packet[37]); + data[1] = (((uint32_t)packet[38] << 24) | ((uint32_t)packet[39] << 16) | ((uint32_t)packet[40] << 8) | packet[41]); + data[2] = (((uint32_t)packet[42] << 24) | ((uint32_t)packet[43] << 16) | ((uint32_t)packet[44] << 8) | packet[45]); + return 0; +} +uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[34] << 8) | packet[35]; + data[1] = (packet[38] << 8) | packet[39]; + data[2] = (packet[42] << 8) | packet[43]; + return 0; +} +uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[34] << 8) | packet[35]; + v -> y = (packet[38] << 8) | packet[39]; + v -> z = (packet[42] << 8) | packet[43]; + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); + data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); + data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); + data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[0] << 8) | packet[1]); + data[1] = ((packet[4] << 8) | packet[5]); + data[2] = ((packet[8] << 8) | packet[9]); + data[3] = ((packet[12] << 8) | packet[13]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + if (status == 0) { + q -> w = (float)qI[0] / 16384.0f; + q -> x = (float)qI[1] / 16384.0f; + q -> y = (float)qI[2] / 16384.0f; + q -> z = (float)qI[3] / 16384.0f; + return 0; + } + return status; // int16 return value, indicates error if this line is reached +} +// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); + data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); + data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); + return 0; +} +uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[16] << 8) | packet[17]; + data[1] = (packet[20] << 8) | packet[21]; + data[2] = (packet[24] << 8) | packet[25]; + return 0; +} +uint8_t MPU6050::dmpGetMag(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[28] << 8) | packet[29]; + data[1] = (packet[30] << 8) | packet[31]; + data[2] = (packet[32] << 8) | packet[33]; + return 0; +} +// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); +// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { + // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) + v -> x = vRaw -> x - gravity -> x*4096; + v -> y = vRaw -> y - gravity -> y*4096; + v -> z = vRaw -> z - gravity -> z*4096; + return 0; +} +// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { + // rotate measured 3D acceleration vector into original state + // frame of reference based on orientation quaternion + memcpy(v, vReal, sizeof(VectorInt16)); + v -> rotate(q); + return 0; +} +// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); + return status; +} + +uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { + v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); + v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); + v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; + return 0; +} +// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); +// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); + +uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi + data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta + data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi + return 0; +} + +#ifdef USE_OLD_DMPGETYAWPITCHROLL +uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); + return 0; +} +#else +uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan2(gravity -> y , gravity -> z); + if(gravity->z<0) { + if(data[1]>0) { + data[1] = PI - data[1]; + } else { + data[1] = -PI - data[1]; + } + } + return 0; +} +#endif + +// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); + +uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { + /*for (uint8_t k = 0; k < dmpPacketSize; k++) { + if (dmpData[k] < 0x10) Serial.print("0"); + Serial.print(dmpData[k], HEX); + Serial.print(" "); + } + Serial.print("\n");*/ + //Serial.println((uint16_t)dmpPacketBuffer); + return 0; +} +uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { + uint8_t status; + uint8_t buf[dmpPacketSize]; + for (uint8_t i = 0; i < numPackets; i++) { + // read packet from FIFO + getFIFOBytes(buf, dmpPacketSize); + + // process packet + if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; + + // increment external process count variable, if supplied + if (processed != 0) *processed++; + } + return 0; +} + +// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); + +// uint8_t MPU6050::dmpInitFIFOParam(); +// uint8_t MPU6050::dmpCloseFIFO(); +// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); +// uint8_t MPU6050::dmpDecodeQuantizedAccel(); +// uint32_t MPU6050::dmpGetGyroSumOfSquare(); +// uint32_t MPU6050::dmpGetAccelSumOfSquare(); +// void MPU6050::dmpOverrideQuaternion(long *q); +uint16_t MPU6050::dmpGetFIFOPacketSize() { + return dmpPacketSize; +} + +#endif /* _MPU6050_9AXIS_MOTIONAPPS41_H_ */ diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/helper_3dmath.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/helper_3dmath.h new file mode 100644 index 00000000..9ed260ec --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU6050/helper_3dmath.h @@ -0,0 +1,216 @@ +// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper +// 6/5/2012 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2012-06-05 - add 3D math helper file to DMP6 example sketch + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _HELPER_3DMATH_H_ +#define _HELPER_3DMATH_H_ + +class Quaternion { + public: + float w; + float x; + float y; + float z; + + Quaternion() { + w = 1.0f; + x = 0.0f; + y = 0.0f; + z = 0.0f; + } + + Quaternion(float nw, float nx, float ny, float nz) { + w = nw; + x = nx; + y = ny; + z = nz; + } + + Quaternion getProduct(Quaternion q) { + // Quaternion multiplication is defined by: + // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2) + // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2) + // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2) + // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2 + return Quaternion( + w*q.w - x*q.x - y*q.y - z*q.z, // new w + w*q.x + x*q.w + y*q.z - z*q.y, // new x + w*q.y - x*q.z + y*q.w + z*q.x, // new y + w*q.z + x*q.y - y*q.x + z*q.w); // new z + } + + Quaternion getConjugate() { + return Quaternion(w, -x, -y, -z); + } + + float getMagnitude() { + return sqrt(w*w + x*x + y*y + z*z); + } + + void normalize() { + float m = getMagnitude(); + w /= m; + x /= m; + y /= m; + z /= m; + } + + Quaternion getNormalized() { + Quaternion r(w, x, y, z); + r.normalize(); + return r; + } +}; + +class VectorInt16 { + public: + int16_t x; + int16_t y; + int16_t z; + + VectorInt16() { + x = 0; + y = 0; + z = 0; + } + + VectorInt16(int16_t nx, int16_t ny, int16_t nz) { + x = nx; + y = ny; + z = nz; + } + + float getMagnitude() { + return sqrt(x*x + y*y + z*z); + } + + void normalize() { + float m = getMagnitude(); + x /= m; + y /= m; + z /= m; + } + + VectorInt16 getNormalized() { + VectorInt16 r(x, y, z); + r.normalize(); + return r; + } + + void rotate(Quaternion *q) { + // http://www.cprogramming.com/tutorial/3d/quaternions.html + // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm + // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation + // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1 + + // P_out = q * P_in * conj(q) + // - P_out is the output vector + // - q is the orientation quaternion + // - P_in is the input vector (a*aReal) + // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z]) + Quaternion p(0, x, y, z); + + // quaternion multiplication: q * p, stored back in p + p = q -> getProduct(p); + + // quaternion multiplication: p * conj(q), stored back in p + p = p.getProduct(q -> getConjugate()); + + // p quaternion is now [0, x', y', z'] + x = p.x; + y = p.y; + z = p.z; + } + + VectorInt16 getRotated(Quaternion *q) { + VectorInt16 r(x, y, z); + r.rotate(q); + return r; + } +}; + +class VectorFloat { + public: + float x; + float y; + float z; + + VectorFloat() { + x = 0; + y = 0; + z = 0; + } + + VectorFloat(float nx, float ny, float nz) { + x = nx; + y = ny; + z = nz; + } + + float getMagnitude() { + return sqrt(x*x + y*y + z*z); + } + + void normalize() { + float m = getMagnitude(); + x /= m; + y /= m; + z /= m; + } + + VectorFloat getNormalized() { + VectorFloat r(x, y, z); + r.normalize(); + return r; + } + + void rotate(Quaternion *q) { + Quaternion p(0, x, y, z); + + // quaternion multiplication: q * p, stored back in p + p = q -> getProduct(p); + + // quaternion multiplication: p * conj(q), stored back in p + p = p.getProduct(q -> getConjugate()); + + // p quaternion is now [0, x', y', z'] + x = p.x; + y = p.y; + z = p.z; + } + + VectorFloat getRotated(Quaternion *q) { + VectorFloat r(x, y, z); + r.rotate(q); + return r; + } +}; + +#endif /* _HELPER_3DMATH_H_ */ \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU9250/MPU9250.cpp b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU9250/MPU9250.cpp new file mode 100644 index 00000000..7d2722a8 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU9250/MPU9250.cpp @@ -0,0 +1,1202 @@ +/* +MPU9250.cpp +Brian R Taylor +brian.taylor@bolderflight.com +Copyright (c) 2017 Bolder Flight Systems +Permission is hereby granted, free of charge, to any person obtaining a copy of this software +and associated documentation files (the "Software"), to deal in the Software without restriction, +including without limitation the rights to use, copy, modify, merge, publish, distribute, +sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in all copies or +substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING +BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +#include "Arduino.h" +#include "MPU9250.h" + +/* MPU9250 object, input the I2C bus and address */ +MPU9250::MPU9250(TwoWire &bus,uint8_t address){ + _i2c = &bus; // I2C bus + _address = address; // I2C address + _useSPI = false; // set to use I2C +} + +/* MPU9250 object, input the SPI bus and chip select pin */ +MPU9250::MPU9250(SPIClass &bus,uint8_t csPin){ + _spi = &bus; // SPI bus + _csPin = csPin; // chip select pin + _useSPI = true; // set to use SPI +} + +/* starts communication with the MPU-9250 */ +int MPU9250::begin(){ + if( _useSPI ) { // using SPI for communication + // use low speed SPI for register setting + _useSPIHS = false; + // setting CS pin to output + pinMode(_csPin,OUTPUT); + // setting CS pin high + digitalWrite(_csPin,HIGH); + // begin SPI communication + _spi->begin(); + } else { // using I2C for communication + // starting the I2C bus + _i2c->begin(); + // setting the I2C clock + _i2c->setClock(_i2cRate); + } + // select clock source to gyro + if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){ + return -1; + } + // enable I2C master mode + if(writeRegister(USER_CTRL,I2C_MST_EN) < 0){ + return -2; + } + // set the I2C bus speed to 400 kHz + if(writeRegister(I2C_MST_CTRL,I2C_MST_CLK) < 0){ + return -3; + } + // set AK8963 to Power Down + writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN); + // reset the MPU9250 + writeRegister(PWR_MGMNT_1,PWR_RESET); + // wait for MPU-9250 to come back up + delay(1); + // reset the AK8963 + writeAK8963Register(AK8963_CNTL2,AK8963_RESET); + // select clock source to gyro + if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){ + return -4; + } + // check the WHO AM I byte, expected value is 0x71 (decimal 113) or 0x73 (decimal 115) + if((whoAmI() != 113)&&(whoAmI() != 115)){ + return -5; + } + // enable accelerometer and gyro + if(writeRegister(PWR_MGMNT_2,SEN_ENABLE) < 0){ + return -6; + } + // setting accel range to 16G as default + if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_16G) < 0){ + return -7; + } + _accelScale = G * 16.0f/32767.5f; // setting the accel scale to 16G + _accelRange = ACCEL_RANGE_16G; + // setting the gyro range to 2000DPS as default + if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_2000DPS) < 0){ + return -8; + } + _gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS + _gyroRange = GYRO_RANGE_2000DPS; + // setting bandwidth to 184Hz as default + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0){ + return -9; + } + if(writeRegister(CONFIG,GYRO_DLPF_184) < 0){ // setting gyro bandwidth to 184Hz + return -10; + } + _bandwidth = DLPF_BANDWIDTH_184HZ; + // setting the sample rate divider to 0 as default + if(writeRegister(SMPDIV,0x00) < 0){ + return -11; + } + _srd = 0; + // enable I2C master mode + if(writeRegister(USER_CTRL,I2C_MST_EN) < 0){ + return -12; + } + // set the I2C bus speed to 400 kHz + if( writeRegister(I2C_MST_CTRL,I2C_MST_CLK) < 0){ + return -13; + } + // check AK8963 WHO AM I register, expected value is 0x48 (decimal 72) + if( whoAmIAK8963() != 72 ){ + return -14; + } + /* get the magnetometer calibration */ + // set AK8963 to Power Down + if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ + return -15; + } + delay(100); // long wait between AK8963 mode changes + // set AK8963 to FUSE ROM access + if(writeAK8963Register(AK8963_CNTL1,AK8963_FUSE_ROM) < 0){ + return -16; + } + delay(100); // long wait between AK8963 mode changes + // read the AK8963 ASA registers and compute magnetometer scale factors + readAK8963Registers(AK8963_ASA,3,_buffer); + _magScaleX = ((((float)_buffer[0]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla + _magScaleY = ((((float)_buffer[1]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla + _magScaleZ = ((((float)_buffer[2]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla + // set AK8963 to Power Down + if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ + return -17; + } + delay(100); // long wait between AK8963 mode changes + // set AK8963 to 16 bit resolution, 100 Hz update rate + if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS2) < 0){ + return -18; + } + delay(100); // long wait between AK8963 mode changes + // select clock source to gyro + if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){ + return -19; + } + // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate + readAK8963Registers(AK8963_HXL,7,_buffer); + // estimate gyro bias + if (calibrateGyro() < 0) { + return -20; + } + // successful init, return 1 + return 1; +} + +/* sets the accelerometer full scale range to values other than default */ +int MPU9250::setAccelRange(AccelRange range) { + // use low speed SPI for register setting + _useSPIHS = false; + switch(range) { + case ACCEL_RANGE_2G: { + // setting the accel range to 2G + if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_2G) < 0){ + return -1; + } + _accelScale = G * 2.0f/32767.5f; // setting the accel scale to 2G + break; + } + case ACCEL_RANGE_4G: { + // setting the accel range to 4G + if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_4G) < 0){ + return -1; + } + _accelScale = G * 4.0f/32767.5f; // setting the accel scale to 4G + break; + } + case ACCEL_RANGE_8G: { + // setting the accel range to 8G + if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_8G) < 0){ + return -1; + } + _accelScale = G * 8.0f/32767.5f; // setting the accel scale to 8G + break; + } + case ACCEL_RANGE_16G: { + // setting the accel range to 16G + if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_16G) < 0){ + return -1; + } + _accelScale = G * 16.0f/32767.5f; // setting the accel scale to 16G + break; + } + } + _accelRange = range; + return 1; +} + +/* sets the gyro full scale range to values other than default */ +int MPU9250::setGyroRange(GyroRange range) { + // use low speed SPI for register setting + _useSPIHS = false; + switch(range) { + case GYRO_RANGE_250DPS: { + // setting the gyro range to 250DPS + if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_250DPS) < 0){ + return -1; + } + _gyroScale = 250.0f/32767.5f * _d2r; // setting the gyro scale to 250DPS + break; + } + case GYRO_RANGE_500DPS: { + // setting the gyro range to 500DPS + if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_500DPS) < 0){ + return -1; + } + _gyroScale = 500.0f/32767.5f * _d2r; // setting the gyro scale to 500DPS + break; + } + case GYRO_RANGE_1000DPS: { + // setting the gyro range to 1000DPS + if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_1000DPS) < 0){ + return -1; + } + _gyroScale = 1000.0f/32767.5f * _d2r; // setting the gyro scale to 1000DPS + break; + } + case GYRO_RANGE_2000DPS: { + // setting the gyro range to 2000DPS + if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_2000DPS) < 0){ + return -1; + } + _gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS + break; + } + } + _gyroRange = range; + return 1; +} + +/* sets the DLPF bandwidth to values other than default */ +int MPU9250::setDlpfBandwidth(DlpfBandwidth bandwidth) { + // use low speed SPI for register setting + _useSPIHS = false; + switch(bandwidth) { + case DLPF_BANDWIDTH_184HZ: { + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0){ // setting accel bandwidth to 184Hz + return -1; + } + if(writeRegister(CONFIG,GYRO_DLPF_184) < 0){ // setting gyro bandwidth to 184Hz + return -2; + } + break; + } + case DLPF_BANDWIDTH_92HZ: { + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_92) < 0){ // setting accel bandwidth to 92Hz + return -1; + } + if(writeRegister(CONFIG,GYRO_DLPF_92) < 0){ // setting gyro bandwidth to 92Hz + return -2; + } + break; + } + case DLPF_BANDWIDTH_41HZ: { + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_41) < 0){ // setting accel bandwidth to 41Hz + return -1; + } + if(writeRegister(CONFIG,GYRO_DLPF_41) < 0){ // setting gyro bandwidth to 41Hz + return -2; + } + break; + } + case DLPF_BANDWIDTH_20HZ: { + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_20) < 0){ // setting accel bandwidth to 20Hz + return -1; + } + if(writeRegister(CONFIG,GYRO_DLPF_20) < 0){ // setting gyro bandwidth to 20Hz + return -2; + } + break; + } + case DLPF_BANDWIDTH_10HZ: { + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_10) < 0){ // setting accel bandwidth to 10Hz + return -1; + } + if(writeRegister(CONFIG,GYRO_DLPF_10) < 0){ // setting gyro bandwidth to 10Hz + return -2; + } + break; + } + case DLPF_BANDWIDTH_5HZ: { + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_5) < 0){ // setting accel bandwidth to 5Hz + return -1; + } + if(writeRegister(CONFIG,GYRO_DLPF_5) < 0){ // setting gyro bandwidth to 5Hz + return -2; + } + break; + } + } + _bandwidth = bandwidth; + return 1; +} + +/* sets the sample rate divider to values other than default */ +int MPU9250::setSrd(uint8_t srd) { + // use low speed SPI for register setting + _useSPIHS = false; + /* setting the sample rate divider to 19 to facilitate setting up magnetometer */ + if(writeRegister(SMPDIV,19) < 0){ // setting the sample rate divider + return -1; + } + if(srd > 9){ + // set AK8963 to Power Down + if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ + return -2; + } + delay(100); // long wait between AK8963 mode changes + // set AK8963 to 16 bit resolution, 8 Hz update rate + if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS1) < 0){ + return -3; + } + delay(100); // long wait between AK8963 mode changes + // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate + readAK8963Registers(AK8963_HXL,7,_buffer); + } else { + // set AK8963 to Power Down + if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ + return -2; + } + delay(100); // long wait between AK8963 mode changes + // set AK8963 to 16 bit resolution, 100 Hz update rate + if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS2) < 0){ + return -3; + } + delay(100); // long wait between AK8963 mode changes + // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate + readAK8963Registers(AK8963_HXL,7,_buffer); + } + /* setting the sample rate divider */ + if(writeRegister(SMPDIV,srd) < 0){ // setting the sample rate divider + return -4; + } + _srd = srd; + return 1; +} + +/* enables the data ready interrupt */ +int MPU9250::enableDataReadyInterrupt() { + // use low speed SPI for register setting + _useSPIHS = false; + /* setting the interrupt */ + if (writeRegister(INT_PIN_CFG,INT_PULSE_50US) < 0){ // setup interrupt, 50 us pulse + return -1; + } + if (writeRegister(INT_ENABLE,INT_RAW_RDY_EN) < 0){ // set to data ready + return -2; + } + return 1; +} + +/* disables the data ready interrupt */ +int MPU9250::disableDataReadyInterrupt() { + // use low speed SPI for register setting + _useSPIHS = false; + if(writeRegister(INT_ENABLE,INT_DISABLE) < 0){ // disable interrupt + return -1; + } + return 1; +} + +/* configures and enables wake on motion, low power mode */ +int MPU9250::enableWakeOnMotion(float womThresh_mg,LpAccelOdr odr) { + // use low speed SPI for register setting + _useSPIHS = false; + // set AK8963 to Power Down + writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN); + // reset the MPU9250 + writeRegister(PWR_MGMNT_1,PWR_RESET); + // wait for MPU-9250 to come back up + delay(1); + if(writeRegister(PWR_MGMNT_1,0x00) < 0){ // cycle 0, sleep 0, standby 0 + return -1; + } + if(writeRegister(PWR_MGMNT_2,DIS_GYRO) < 0){ // disable gyro measurements + return -2; + } + if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0){ // setting accel bandwidth to 184Hz + return -3; + } + if(writeRegister(INT_ENABLE,INT_WOM_EN) < 0){ // enabling interrupt to wake on motion + return -4; + } + if(writeRegister(MOT_DETECT_CTRL,(ACCEL_INTEL_EN | ACCEL_INTEL_MODE)) < 0){ // enabling accel hardware intelligence + return -5; + } + _womThreshold = map(womThresh_mg, 0, 1020, 0, 255); + if(writeRegister(WOM_THR,_womThreshold) < 0){ // setting wake on motion threshold + return -6; + } + if(writeRegister(LP_ACCEL_ODR,(uint8_t)odr) < 0){ // set frequency of wakeup + return -7; + } + if(writeRegister(PWR_MGMNT_1,PWR_CYCLE) < 0){ // switch to accel low power mode + return -8; + } + return 1; +} + +/* configures and enables the FIFO buffer */ +int MPU9250FIFO::enableFifo(bool accel,bool gyro,bool mag,bool temp) { + // use low speed SPI for register setting + _useSPIHS = false; + if(writeRegister(USER_CTRL, (0x40 | I2C_MST_EN)) < 0){ + return -1; + } + if(writeRegister(FIFO_EN,(accel*FIFO_ACCEL)|(gyro*FIFO_GYRO)|(mag*FIFO_MAG)|(temp*FIFO_TEMP)) < 0){ + return -2; + } + _enFifoAccel = accel; + _enFifoGyro = gyro; + _enFifoMag = mag; + _enFifoTemp = temp; + _fifoFrameSize = accel*6 + gyro*6 + mag*7 + temp*2; + return 1; +} + +/* reads the most current data from MPU9250 and stores in buffer */ +int MPU9250::readSensor() { + _useSPIHS = true; // use the high speed SPI for data readout + // grab the data from the MPU9250 + if (readRegisters(ACCEL_OUT, 21, _buffer) < 0) { + return -1; + } + // combine into 16 bit values + _axcounts = (((int16_t)_buffer[0]) << 8) | _buffer[1]; + _aycounts = (((int16_t)_buffer[2]) << 8) | _buffer[3]; + _azcounts = (((int16_t)_buffer[4]) << 8) | _buffer[5]; + _tcounts = (((int16_t)_buffer[6]) << 8) | _buffer[7]; + _gxcounts = (((int16_t)_buffer[8]) << 8) | _buffer[9]; + _gycounts = (((int16_t)_buffer[10]) << 8) | _buffer[11]; + _gzcounts = (((int16_t)_buffer[12]) << 8) | _buffer[13]; + _hxcounts = (((int16_t)_buffer[15]) << 8) | _buffer[14]; + _hycounts = (((int16_t)_buffer[17]) << 8) | _buffer[16]; + _hzcounts = (((int16_t)_buffer[19]) << 8) | _buffer[18]; + // transform and convert to float values + _ax = (((float)(tX[0]*_axcounts + tX[1]*_aycounts + tX[2]*_azcounts) * _accelScale) - _axb)*_axs; + _ay = (((float)(tY[0]*_axcounts + tY[1]*_aycounts + tY[2]*_azcounts) * _accelScale) - _ayb)*_ays; + _az = (((float)(tZ[0]*_axcounts + tZ[1]*_aycounts + tZ[2]*_azcounts) * _accelScale) - _azb)*_azs; + _gx = ((float)(tX[0]*_gxcounts + tX[1]*_gycounts + tX[2]*_gzcounts) * _gyroScale) - _gxb; + _gy = ((float)(tY[0]*_gxcounts + tY[1]*_gycounts + tY[2]*_gzcounts) * _gyroScale) - _gyb; + _gz = ((float)(tZ[0]*_gxcounts + tZ[1]*_gycounts + tZ[2]*_gzcounts) * _gyroScale) - _gzb; + _hx = (((float)(_hxcounts) * _magScaleX) - _hxb)*_hxs; + _hy = (((float)(_hycounts) * _magScaleY) - _hyb)*_hys; + _hz = (((float)(_hzcounts) * _magScaleZ) - _hzb)*_hzs; + _t = ((((float) _tcounts) - _tempOffset)/_tempScale) + _tempOffset; + return 1; +} + +/* returns the accelerometer measurement in the x direction, m/s/s */ +float MPU9250::getAccelX_mss() { + return _ax; +} + +/* returns the accelerometer measurement in the y direction, m/s/s */ +float MPU9250::getAccelY_mss() { + return _ay; +} + +/* returns the accelerometer measurement in the z direction, m/s/s */ +float MPU9250::getAccelZ_mss() { + return _az; +} + +/* returns the gyroscope measurement in the x direction, rad/s */ +float MPU9250::getGyroX_rads() { + return _gx; +} + +/* returns the gyroscope measurement in the y direction, rad/s */ +float MPU9250::getGyroY_rads() { + return _gy; +} + +/* returns the gyroscope measurement in the z direction, rad/s */ +float MPU9250::getGyroZ_rads() { + return _gz; +} + +/* returns the magnetometer measurement in the x direction, uT */ +float MPU9250::getMagX_uT() { + return _hx; +} + +/* returns the magnetometer measurement in the y direction, uT */ +float MPU9250::getMagY_uT() { + return _hy; +} + +/* returns the magnetometer measurement in the z direction, uT */ +float MPU9250::getMagZ_uT() { + return _hz; +} + +/* returns the die temperature, C */ +float MPU9250::getTemperature_C() { + return _t; +} + +/* reads data from the MPU9250 FIFO and stores in buffer */ +int MPU9250FIFO::readFifo() { + _useSPIHS = true; // use the high speed SPI for data readout + // get the fifo size + readRegisters(FIFO_COUNT, 2, _buffer); + _fifoSize = (((uint16_t) (_buffer[0]&0x0F)) <<8) + (((uint16_t) _buffer[1])); + // read and parse the buffer + for (size_t i=0; i < _fifoSize/_fifoFrameSize; i++) { + // grab the data from the MPU9250 + if (readRegisters(FIFO_READ,_fifoFrameSize,_buffer) < 0) { + return -1; + } + if (_enFifoAccel) { + // combine into 16 bit values + _axcounts = (((int16_t)_buffer[0]) << 8) | _buffer[1]; + _aycounts = (((int16_t)_buffer[2]) << 8) | _buffer[3]; + _azcounts = (((int16_t)_buffer[4]) << 8) | _buffer[5]; + // transform and convert to float values + _axFifo[i] = (((float)(tX[0]*_axcounts + tX[1]*_aycounts + tX[2]*_azcounts) * _accelScale)-_axb)*_axs; + _ayFifo[i] = (((float)(tY[0]*_axcounts + tY[1]*_aycounts + tY[2]*_azcounts) * _accelScale)-_ayb)*_ays; + _azFifo[i] = (((float)(tZ[0]*_axcounts + tZ[1]*_aycounts + tZ[2]*_azcounts) * _accelScale)-_azb)*_azs; + _aSize = _fifoSize/_fifoFrameSize; + } + if (_enFifoTemp) { + // combine into 16 bit values + _tcounts = (((int16_t)_buffer[0 + _enFifoAccel*6]) << 8) | _buffer[1 + _enFifoAccel*6]; + // transform and convert to float values + _tFifo[i] = ((((float) _tcounts) - _tempOffset)/_tempScale) + _tempOffset; + _tSize = _fifoSize/_fifoFrameSize; + } + if (_enFifoGyro) { + // combine into 16 bit values + _gxcounts = (((int16_t)_buffer[0 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[1 + _enFifoAccel*6 + _enFifoTemp*2]; + _gycounts = (((int16_t)_buffer[2 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[3 + _enFifoAccel*6 + _enFifoTemp*2]; + _gzcounts = (((int16_t)_buffer[4 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[5 + _enFifoAccel*6 + _enFifoTemp*2]; + // transform and convert to float values + _gxFifo[i] = ((float)(tX[0]*_gxcounts + tX[1]*_gycounts + tX[2]*_gzcounts) * _gyroScale) - _gxb; + _gyFifo[i] = ((float)(tY[0]*_gxcounts + tY[1]*_gycounts + tY[2]*_gzcounts) * _gyroScale) - _gyb; + _gzFifo[i] = ((float)(tZ[0]*_gxcounts + tZ[1]*_gycounts + tZ[2]*_gzcounts) * _gyroScale) - _gzb; + _gSize = _fifoSize/_fifoFrameSize; + } + if (_enFifoMag) { + // combine into 16 bit values + _hxcounts = (((int16_t)_buffer[1 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]) << 8) | _buffer[0 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]; + _hycounts = (((int16_t)_buffer[3 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]) << 8) | _buffer[2 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]; + _hzcounts = (((int16_t)_buffer[5 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]) << 8) | _buffer[4 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]; + // transform and convert to float values + _hxFifo[i] = (((float)(_hxcounts) * _magScaleX) - _hxb)*_hxs; + _hyFifo[i] = (((float)(_hycounts) * _magScaleY) - _hyb)*_hys; + _hzFifo[i] = (((float)(_hzcounts) * _magScaleZ) - _hzb)*_hzs; + _hSize = _fifoSize/_fifoFrameSize; + } + } + return 1; +} + +/* returns the accelerometer FIFO size and data in the x direction, m/s/s */ +void MPU9250FIFO::getFifoAccelX_mss(size_t *size,float* data) { + *size = _aSize; + memcpy(data,_axFifo,_aSize*sizeof(float)); +} + +/* returns the accelerometer FIFO size and data in the y direction, m/s/s */ +void MPU9250FIFO::getFifoAccelY_mss(size_t *size,float* data) { + *size = _aSize; + memcpy(data,_ayFifo,_aSize*sizeof(float)); +} + +/* returns the accelerometer FIFO size and data in the z direction, m/s/s */ +void MPU9250FIFO::getFifoAccelZ_mss(size_t *size,float* data) { + *size = _aSize; + memcpy(data,_azFifo,_aSize*sizeof(float)); +} + +/* returns the gyroscope FIFO size and data in the x direction, rad/s */ +void MPU9250FIFO::getFifoGyroX_rads(size_t *size,float* data) { + *size = _gSize; + memcpy(data,_gxFifo,_gSize*sizeof(float)); +} + +/* returns the gyroscope FIFO size and data in the y direction, rad/s */ +void MPU9250FIFO::getFifoGyroY_rads(size_t *size,float* data) { + *size = _gSize; + memcpy(data,_gyFifo,_gSize*sizeof(float)); +} + +/* returns the gyroscope FIFO size and data in the z direction, rad/s */ +void MPU9250FIFO::getFifoGyroZ_rads(size_t *size,float* data) { + *size = _gSize; + memcpy(data,_gzFifo,_gSize*sizeof(float)); +} + +/* returns the magnetometer FIFO size and data in the x direction, uT */ +void MPU9250FIFO::getFifoMagX_uT(size_t *size,float* data) { + *size = _hSize; + memcpy(data,_hxFifo,_hSize*sizeof(float)); +} + +/* returns the magnetometer FIFO size and data in the y direction, uT */ +void MPU9250FIFO::getFifoMagY_uT(size_t *size,float* data) { + *size = _hSize; + memcpy(data,_hyFifo,_hSize*sizeof(float)); +} + +/* returns the magnetometer FIFO size and data in the z direction, uT */ +void MPU9250FIFO::getFifoMagZ_uT(size_t *size,float* data) { + *size = _hSize; + memcpy(data,_hzFifo,_hSize*sizeof(float)); +} + +/* returns the die temperature FIFO size and data, C */ +void MPU9250FIFO::getFifoTemperature_C(size_t *size,float* data) { + *size = _tSize; + memcpy(data,_tFifo,_tSize*sizeof(float)); +} + +/* estimates the gyro biases */ +int MPU9250::calibrateGyro() { + // set the range, bandwidth, and srd + if (setGyroRange(GYRO_RANGE_250DPS) < 0) { + return -1; + } + if (setDlpfBandwidth(DLPF_BANDWIDTH_20HZ) < 0) { + return -2; + } + if (setSrd(19) < 0) { + return -3; + } + + // take samples and find bias + _gxbD = 0; + _gybD = 0; + _gzbD = 0; + for (size_t i=0; i < _numSamples; i++) { + readSensor(); + _gxbD += (getGyroX_rads() + _gxb)/((double)_numSamples); + _gybD += (getGyroY_rads() + _gyb)/((double)_numSamples); + _gzbD += (getGyroZ_rads() + _gzb)/((double)_numSamples); + delay(20); + } + _gxb = (float)_gxbD; + _gyb = (float)_gybD; + _gzb = (float)_gzbD; + + // set the range, bandwidth, and srd back to what they were + if (setGyroRange(_gyroRange) < 0) { + return -4; + } + if (setDlpfBandwidth(_bandwidth) < 0) { + return -5; + } + if (setSrd(_srd) < 0) { + return -6; + } + return 1; +} + +/* returns the gyro bias in the X direction, rad/s */ +float MPU9250::getGyroBiasX_rads() { + return _gxb; +} + +/* returns the gyro bias in the Y direction, rad/s */ +float MPU9250::getGyroBiasY_rads() { + return _gyb; +} + +/* returns the gyro bias in the Z direction, rad/s */ +float MPU9250::getGyroBiasZ_rads() { + return _gzb; +} + +/* sets the gyro bias in the X direction to bias, rad/s */ +void MPU9250::setGyroBiasX_rads(float bias) { + _gxb = bias; +} + +/* sets the gyro bias in the Y direction to bias, rad/s */ +void MPU9250::setGyroBiasY_rads(float bias) { + _gyb = bias; +} + +/* sets the gyro bias in the Z direction to bias, rad/s */ +void MPU9250::setGyroBiasZ_rads(float bias) { + _gzb = bias; +} + +/* finds bias and scale factor calibration for the accelerometer, +this should be run for each axis in each direction (6 total) to find +the min and max values along each */ +int MPU9250::calibrateAccel() { + // set the range, bandwidth, and srd + if (setAccelRange(ACCEL_RANGE_2G) < 0) { + return -1; + } + if (setDlpfBandwidth(DLPF_BANDWIDTH_20HZ) < 0) { + return -2; + } + if (setSrd(19) < 0) { + return -3; + } + + // take samples and find min / max + _axbD = 0; + _aybD = 0; + _azbD = 0; + for (size_t i=0; i < _numSamples; i++) { + readSensor(); + _axbD += (getAccelX_mss()/_axs + _axb)/((double)_numSamples); + _aybD += (getAccelY_mss()/_ays + _ayb)/((double)_numSamples); + _azbD += (getAccelZ_mss()/_azs + _azb)/((double)_numSamples); + delay(20); + } + if (_axbD > 9.0f) { + _axmax = (float)_axbD; + } + if (_aybD > 9.0f) { + _aymax = (float)_aybD; + } + if (_azbD > 9.0f) { + _azmax = (float)_azbD; + } + if (_axbD < -9.0f) { + _axmin = (float)_axbD; + } + if (_aybD < -9.0f) { + _aymin = (float)_aybD; + } + if (_azbD < -9.0f) { + _azmin = (float)_azbD; + } + + // find bias and scale factor + if ((abs(_axmin) > 9.0f) && (abs(_axmax) > 9.0f)) { + _axb = (_axmin + _axmax) / 2.0f; + _axs = G/((abs(_axmin) + abs(_axmax)) / 2.0f); + } + if ((abs(_aymin) > 9.0f) && (abs(_aymax) > 9.0f)) { + _ayb = (_aymin + _aymax) / 2.0f; + _ays = G/((abs(_aymin) + abs(_aymax)) / 2.0f); + } + if ((abs(_azmin) > 9.0f) && (abs(_azmax) > 9.0f)) { + _azb = (_azmin + _azmax) / 2.0f; + _azs = G/((abs(_azmin) + abs(_azmax)) / 2.0f); + } + + // set the range, bandwidth, and srd back to what they were + if (setAccelRange(_accelRange) < 0) { + return -4; + } + if (setDlpfBandwidth(_bandwidth) < 0) { + return -5; + } + if (setSrd(_srd) < 0) { + return -6; + } + return 1; +} + +/* returns the accelerometer bias in the X direction, m/s/s */ +float MPU9250::getAccelBiasX_mss() { + return _axb; +} + +/* returns the accelerometer scale factor in the X direction */ +float MPU9250::getAccelScaleFactorX() { + return _axs; +} + +/* returns the accelerometer bias in the Y direction, m/s/s */ +float MPU9250::getAccelBiasY_mss() { + return _ayb; +} + +/* returns the accelerometer scale factor in the Y direction */ +float MPU9250::getAccelScaleFactorY() { + return _ays; +} + +/* returns the accelerometer bias in the Z direction, m/s/s */ +float MPU9250::getAccelBiasZ_mss() { + return _azb; +} + +/* returns the accelerometer scale factor in the Z direction */ +float MPU9250::getAccelScaleFactorZ() { + return _azs; +} + +/* sets the accelerometer bias (m/s/s) and scale factor in the X direction */ +void MPU9250::setAccelCalX(float bias,float scaleFactor) { + _axb = bias; + _axs = scaleFactor; +} + +/* sets the accelerometer bias (m/s/s) and scale factor in the Y direction */ +void MPU9250::setAccelCalY(float bias,float scaleFactor) { + _ayb = bias; + _ays = scaleFactor; +} + +/* sets the accelerometer bias (m/s/s) and scale factor in the Z direction */ +void MPU9250::setAccelCalZ(float bias,float scaleFactor) { + _azb = bias; + _azs = scaleFactor; +} + +/* finds bias and scale factor calibration for the magnetometer, +the sensor should be rotated in a figure 8 motion until complete */ +int MPU9250::calibrateMag() { + // set the srd + if (setSrd(19) < 0) { + return -1; + } + + // get a starting set of data + readSensor(); + _hxmax = getMagX_uT(); + _hxmin = getMagX_uT(); + _hymax = getMagY_uT(); + _hymin = getMagY_uT(); + _hzmax = getMagZ_uT(); + _hzmin = getMagZ_uT(); + + // collect data to find max / min in each channel + _counter = 0; + while (_counter < _maxCounts) { + _delta = 0.0f; + _framedelta = 0.0f; + readSensor(); + _hxfilt = (_hxfilt*((float)_coeff-1)+(getMagX_uT()/_hxs+_hxb))/((float)_coeff); + _hyfilt = (_hyfilt*((float)_coeff-1)+(getMagY_uT()/_hys+_hyb))/((float)_coeff); + _hzfilt = (_hzfilt*((float)_coeff-1)+(getMagZ_uT()/_hzs+_hzb))/((float)_coeff); + if (_hxfilt > _hxmax) { + _delta = _hxfilt - _hxmax; + _hxmax = _hxfilt; + } + if (_delta > _framedelta) { + _framedelta = _delta; + } + if (_hyfilt > _hymax) { + _delta = _hyfilt - _hymax; + _hymax = _hyfilt; + } + if (_delta > _framedelta) { + _framedelta = _delta; + } + if (_hzfilt > _hzmax) { + _delta = _hzfilt - _hzmax; + _hzmax = _hzfilt; + } + if (_delta > _framedelta) { + _framedelta = _delta; + } + if (_hxfilt < _hxmin) { + _delta = abs(_hxfilt - _hxmin); + _hxmin = _hxfilt; + } + if (_delta > _framedelta) { + _framedelta = _delta; + } + if (_hyfilt < _hymin) { + _delta = abs(_hyfilt - _hymin); + _hymin = _hyfilt; + } + if (_delta > _framedelta) { + _framedelta = _delta; + } + if (_hzfilt < _hzmin) { + _delta = abs(_hzfilt - _hzmin); + _hzmin = _hzfilt; + } + if (_delta > _framedelta) { + _framedelta = _delta; + } + if (_framedelta > _deltaThresh) { + _counter = 0; + } else { + _counter++; + } + delay(20); + } + + // find the magnetometer bias + _hxb = (_hxmax + _hxmin) / 2.0f; + _hyb = (_hymax + _hymin) / 2.0f; + _hzb = (_hzmax + _hzmin) / 2.0f; + + // find the magnetometer scale factor + _hxs = (_hxmax - _hxmin) / 2.0f; + _hys = (_hymax - _hymin) / 2.0f; + _hzs = (_hzmax - _hzmin) / 2.0f; + _avgs = (_hxs + _hys + _hzs) / 3.0f; + _hxs = _avgs/_hxs; + _hys = _avgs/_hys; + _hzs = _avgs/_hzs; + + // set the srd back to what it was + if (setSrd(_srd) < 0) { + return -2; + } + return 1; +} + +/* returns the magnetometer bias in the X direction, uT */ +float MPU9250::getMagBiasX_uT() { + return _hxb; +} + +/* returns the magnetometer scale factor in the X direction */ +float MPU9250::getMagScaleFactorX() { + return _hxs; +} + +/* returns the magnetometer bias in the Y direction, uT */ +float MPU9250::getMagBiasY_uT() { + return _hyb; +} + +/* returns the magnetometer scale factor in the Y direction */ +float MPU9250::getMagScaleFactorY() { + return _hys; +} + +/* returns the magnetometer bias in the Z direction, uT */ +float MPU9250::getMagBiasZ_uT() { + return _hzb; +} + +/* returns the magnetometer scale factor in the Z direction */ +float MPU9250::getMagScaleFactorZ() { + return _hzs; +} + +/* sets the magnetometer bias (uT) and scale factor in the X direction */ +void MPU9250::setMagCalX(float bias,float scaleFactor) { + _hxb = bias; + _hxs = scaleFactor; +} + +/* sets the magnetometer bias (uT) and scale factor in the Y direction */ +void MPU9250::setMagCalY(float bias,float scaleFactor) { + _hyb = bias; + _hys = scaleFactor; +} + +/* sets the magnetometer bias (uT) and scale factor in the Z direction */ +void MPU9250::setMagCalZ(float bias,float scaleFactor) { + _hzb = bias; + _hzs = scaleFactor; +} + +#if defined(__IMXRT1062__) +/* writes a byte to MPU9250 register given a register address and data */ +int MPU9250::writeRegister(uint8_t subAddress, uint8_t data){ + /* write data to device */ + if( _useSPI ){ + _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction + digitalWriteFast(_csPin,LOW); // select the MPU9250 chip + delayNanoseconds(200); + _spi->transfer(subAddress); // write the register address + _spi->transfer(data); // write the data + digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip + delayNanoseconds(200); + _spi->endTransaction(); // end the transaction + } + else{ + _i2c->beginTransmission(_address); // open the device + _i2c->write(subAddress); // write the register address + _i2c->write(data); // write the data + _i2c->endTransmission(); + } + + delay(10); + + /* read back the register */ + readRegisters(subAddress,1,_buffer); + /* check the read back register against the written register */ + if(_buffer[0] == data) { + return 1; + } + else{ + return -1; + } +} + +/* reads registers from MPU9250 given a starting register address, number of bytes, and a pointer to store data */ +int MPU9250::readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest){ + if( _useSPI ){ + // begin the transaction + if(_useSPIHS){ + _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3)); + } + else{ + _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); + } + digitalWriteFast(_csPin,LOW); // select the MPU9250 chip + delayNanoseconds(200); + _spi->transfer(subAddress | SPI_READ); // specify the starting register address + for(uint8_t i = 0; i < count; i++){ + dest[i] = _spi->transfer(0x00); // read the data + } + digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip + delayNanoseconds(200); + _spi->endTransaction(); // end the transaction + return 1; + } + else{ + _i2c->beginTransmission(_address); // open the device + _i2c->write(subAddress); // specify the starting register address + _i2c->endTransmission(false); + _numBytes = _i2c->requestFrom(_address, count); // specify the number of bytes to receive + if (_numBytes == count) { + for(uint8_t i = 0; i < count; i++){ + dest[i] = _i2c->read(); + } + return 1; + } else { + return -1; + } + } +} +#else +/* writes a byte to MPU9250 register given a register address and data */ +int MPU9250::writeRegister(uint8_t subAddress, uint8_t data){ + /* write data to device */ + if( _useSPI ){ + _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction + digitalWrite(_csPin,LOW); // select the MPU9250 chip + _spi->transfer(subAddress); // write the register address + _spi->transfer(data); // write the data + digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip + _spi->endTransaction(); // end the transaction + } + else{ + _i2c->beginTransmission(_address); // open the device + _i2c->write(subAddress); // write the register address + _i2c->write(data); // write the data + _i2c->endTransmission(); + } + + delay(10); + + /* read back the register */ + readRegisters(subAddress,1,_buffer); + /* check the read back register against the written register */ + if(_buffer[0] == data) { + return 1; + } + else{ + return -1; + } +} + +/* reads registers from MPU9250 given a starting register address, number of bytes, and a pointer to store data */ +int MPU9250::readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest){ + if( _useSPI ){ + // begin the transaction + if(_useSPIHS){ + _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3)); + } + else{ + _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); + } + digitalWrite(_csPin,LOW); // select the MPU9250 chip + _spi->transfer(subAddress | SPI_READ); // specify the starting register address + for(uint8_t i = 0; i < count; i++){ + dest[i] = _spi->transfer(0x00); // read the data + } + digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip + _spi->endTransaction(); // end the transaction + return 1; + } + else{ + _i2c->beginTransmission(_address); // open the device + _i2c->write(subAddress); // specify the starting register address + _i2c->endTransmission(false); + _numBytes = _i2c->requestFrom(_address, count); // specify the number of bytes to receive + if (_numBytes == count) { + for(uint8_t i = 0; i < count; i++){ + dest[i] = _i2c->read(); + } + return 1; + } else { + return -1; + } + } +} +#endif + +/* writes a register to the AK8963 given a register address and data */ +int MPU9250::writeAK8963Register(uint8_t subAddress, uint8_t data){ + // set slave 0 to the AK8963 and set for write + if (writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR) < 0) { + return -1; + } + // set the register to the desired AK8963 sub address + if (writeRegister(I2C_SLV0_REG,subAddress) < 0) { + return -2; + } + // store the data for write + if (writeRegister(I2C_SLV0_DO,data) < 0) { + return -3; + } + // enable I2C and send 1 byte + if (writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1) < 0) { + return -4; + } + // read the register and confirm + if (readAK8963Registers(subAddress,1,_buffer) < 0) { + return -5; + } + if(_buffer[0] == data) { + return 1; + } else{ + return -6; + } +} + +/* reads registers from the AK8963 */ +int MPU9250::readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest){ + // set slave 0 to the AK8963 and set for read + if (writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) < 0) { + return -1; + } + // set the register to the desired AK8963 sub address + if (writeRegister(I2C_SLV0_REG,subAddress) < 0) { + return -2; + } + // enable I2C and request the bytes + if (writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | count) < 0) { + return -3; + } + delay(1); // takes some time for these registers to fill + // read the bytes off the MPU9250 EXT_SENS_DATA registers + _status = readRegisters(EXT_SENS_DATA_00,count,dest); + return _status; +} + +/* gets the MPU9250 WHO_AM_I register value, expected to be 0x71 */ +int MPU9250::whoAmI(){ + // read the WHO AM I register + if (readRegisters(WHO_AM_I,1,_buffer) < 0) { + return -1; + } + // return the register value + return _buffer[0]; +} + +/* gets the AK8963 WHO_AM_I register value, expected to be 0x48 */ +int MPU9250::whoAmIAK8963(){ + // read the WHO AM I register + if (readAK8963Registers(AK8963_WHO_AM_I,1,_buffer) < 0) { + return -1; + } + // return the register value + return _buffer[0]; +} + +// jihlein additions start + +void MPU9250::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { + uint8_t buffer[14]; + readRegisters(ACCEL_OUT, 14, buffer); + *ax = (((int16_t)buffer[0]) << 8) | buffer[1]; + *ay = (((int16_t)buffer[2]) << 8) | buffer[3]; + *az = (((int16_t)buffer[4]) << 8) | buffer[5]; + *gx = (((int16_t)buffer[8]) << 8) | buffer[9]; + *gy = (((int16_t)buffer[10]) << 8) | buffer[11]; + *gz = (((int16_t)buffer[12]) << 8) | buffer[13]; +} + +void MPU9250::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) { + uint8_t buffer[20]; + readRegisters(ACCEL_OUT, 20, buffer); + *ax = (((int16_t)buffer[0]) << 8) | buffer[1]; + *ay = (((int16_t)buffer[2]) << 8) | buffer[3]; + *az = (((int16_t)buffer[4]) << 8) | buffer[5]; + *gx = (((int16_t)buffer[8]) << 8) | buffer[9]; + *gy = (((int16_t)buffer[10]) << 8) | buffer[11]; + *gz = (((int16_t)buffer[12]) << 8) | buffer[13]; + *mx = (((int16_t)buffer[15]) << 8) | buffer[14]; + *my = (((int16_t)buffer[17]) << 8) | buffer[16]; + *mz = (((int16_t)buffer[19]) << 8) | buffer[18]; +} + +// jihlein additions end \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU9250/MPU9250.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU9250/MPU9250.h new file mode 100644 index 00000000..bbbe2c5a --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/MPU9250/MPU9250.h @@ -0,0 +1,313 @@ +/* + MPU9250.h + Brian R Taylor + brian.taylor@bolderflight.com + + Copyright (c) 2017 Bolder Flight Systems + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#ifndef MPU9250_h +#define MPU9250_h + +#include "Arduino.h" +#include "Wire.h" // I2C library +#include "SPI.h" // SPI library + +class MPU9250{ + public: + enum GyroRange + { + GYRO_RANGE_250DPS, + GYRO_RANGE_500DPS, + GYRO_RANGE_1000DPS, + GYRO_RANGE_2000DPS + }; + enum AccelRange + { + ACCEL_RANGE_2G, + ACCEL_RANGE_4G, + ACCEL_RANGE_8G, + ACCEL_RANGE_16G + }; + enum DlpfBandwidth + { + DLPF_BANDWIDTH_184HZ, + DLPF_BANDWIDTH_92HZ, + DLPF_BANDWIDTH_41HZ, + DLPF_BANDWIDTH_20HZ, + DLPF_BANDWIDTH_10HZ, + DLPF_BANDWIDTH_5HZ + }; + enum LpAccelOdr + { + LP_ACCEL_ODR_0_24HZ = 0, + LP_ACCEL_ODR_0_49HZ = 1, + LP_ACCEL_ODR_0_98HZ = 2, + LP_ACCEL_ODR_1_95HZ = 3, + LP_ACCEL_ODR_3_91HZ = 4, + LP_ACCEL_ODR_7_81HZ = 5, + LP_ACCEL_ODR_15_63HZ = 6, + LP_ACCEL_ODR_31_25HZ = 7, + LP_ACCEL_ODR_62_50HZ = 8, + LP_ACCEL_ODR_125HZ = 9, + LP_ACCEL_ODR_250HZ = 10, + LP_ACCEL_ODR_500HZ = 11 + }; + MPU9250(TwoWire &bus,uint8_t address); + MPU9250(SPIClass &bus,uint8_t csPin); + int begin(); + int setAccelRange(AccelRange range); + int setGyroRange(GyroRange range); + int setDlpfBandwidth(DlpfBandwidth bandwidth); + int setSrd(uint8_t srd); + int enableDataReadyInterrupt(); + int disableDataReadyInterrupt(); + int enableWakeOnMotion(float womThresh_mg,LpAccelOdr odr); + int readSensor(); + float getAccelX_mss(); + float getAccelY_mss(); + float getAccelZ_mss(); + float getGyroX_rads(); + float getGyroY_rads(); + float getGyroZ_rads(); + float getMagX_uT(); + float getMagY_uT(); + float getMagZ_uT(); + float getTemperature_C(); + + int calibrateGyro(); + float getGyroBiasX_rads(); + float getGyroBiasY_rads(); + float getGyroBiasZ_rads(); + void setGyroBiasX_rads(float bias); + void setGyroBiasY_rads(float bias); + void setGyroBiasZ_rads(float bias); + int calibrateAccel(); + float getAccelBiasX_mss(); + float getAccelScaleFactorX(); + float getAccelBiasY_mss(); + float getAccelScaleFactorY(); + float getAccelBiasZ_mss(); + float getAccelScaleFactorZ(); + void setAccelCalX(float bias,float scaleFactor); + void setAccelCalY(float bias,float scaleFactor); + void setAccelCalZ(float bias,float scaleFactor); + int calibrateMag(); + float getMagBiasX_uT(); + float getMagScaleFactorX(); + float getMagBiasY_uT(); + float getMagScaleFactorY(); + float getMagBiasZ_uT(); + float getMagScaleFactorZ(); + void setMagCalX(float bias,float scaleFactor); + void setMagCalY(float bias,float scaleFactor); + void setMagCalZ(float bias,float scaleFactor); + // jihlein additions start + void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz); + void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); + // jihlein additions end + protected: + // i2c + uint8_t _address; + TwoWire *_i2c; + const uint32_t _i2cRate = 400000; // 400 kHz + size_t _numBytes; // number of bytes received from I2C + // spi + SPIClass *_spi; + uint8_t _csPin; + bool _useSPI; + bool _useSPIHS; + const uint8_t SPI_READ = 0x80; + const uint32_t SPI_LS_CLOCK = 1000000; // 1 MHz + const uint32_t SPI_HS_CLOCK = 15000000; // 15 MHz + // track success of interacting with sensor + int _status; + // buffer for reading from sensor + uint8_t _buffer[21]; + // data counts + int16_t _axcounts,_aycounts,_azcounts; + int16_t _gxcounts,_gycounts,_gzcounts; + int16_t _hxcounts,_hycounts,_hzcounts; + int16_t _tcounts; + // data buffer + float _ax, _ay, _az; + float _gx, _gy, _gz; + float _hx, _hy, _hz; + float _t; + // wake on motion + uint8_t _womThreshold; + // scale factors + float _accelScale; + float _gyroScale; + float _magScaleX, _magScaleY, _magScaleZ; + const float _tempScale = 333.87f; + const float _tempOffset = 21.0f; + // configuration + AccelRange _accelRange; + GyroRange _gyroRange; + DlpfBandwidth _bandwidth; + uint8_t _srd; + // gyro bias estimation + size_t _numSamples = 100; + double _gxbD, _gybD, _gzbD; + float _gxb, _gyb, _gzb; + // accel bias and scale factor estimation + double _axbD, _aybD, _azbD; + float _axmax, _aymax, _azmax; + float _axmin, _aymin, _azmin; + float _axb, _ayb, _azb; + float _axs = 1.0f; + float _ays = 1.0f; + float _azs = 1.0f; + // magnetometer bias and scale factor estimation + uint16_t _maxCounts = 1000; + float _deltaThresh = 0.3f; + uint8_t _coeff = 8; + uint16_t _counter; + float _framedelta, _delta; + float _hxfilt, _hyfilt, _hzfilt; + float _hxmax, _hymax, _hzmax; + float _hxmin, _hymin, _hzmin; + float _hxb, _hyb, _hzb; + float _hxs = 1.0f; + float _hys = 1.0f; + float _hzs = 1.0f; + float _avgs; + // transformation matrix + /* transform the accel and gyro axes to match the magnetometer axes */ + const int16_t tX[3] = {0, 1, 0}; + const int16_t tY[3] = {1, 0, 0}; + const int16_t tZ[3] = {0, 0, -1}; + // constants + const float G = 9.807f; + const float _d2r = 3.14159265359f/180.0f; + // MPU9250 registers + const uint8_t ACCEL_OUT = 0x3B; + const uint8_t GYRO_OUT = 0x43; + const uint8_t TEMP_OUT = 0x41; + const uint8_t EXT_SENS_DATA_00 = 0x49; + const uint8_t ACCEL_CONFIG = 0x1C; + const uint8_t ACCEL_FS_SEL_2G = 0x00; + const uint8_t ACCEL_FS_SEL_4G = 0x08; + const uint8_t ACCEL_FS_SEL_8G = 0x10; + const uint8_t ACCEL_FS_SEL_16G = 0x18; + const uint8_t GYRO_CONFIG = 0x1B; + const uint8_t GYRO_FS_SEL_250DPS = 0x00; + const uint8_t GYRO_FS_SEL_500DPS = 0x08; + const uint8_t GYRO_FS_SEL_1000DPS = 0x10; + const uint8_t GYRO_FS_SEL_2000DPS = 0x18; + const uint8_t ACCEL_CONFIG2 = 0x1D; + const uint8_t ACCEL_DLPF_184 = 0x01; + const uint8_t ACCEL_DLPF_92 = 0x02; + const uint8_t ACCEL_DLPF_41 = 0x03; + const uint8_t ACCEL_DLPF_20 = 0x04; + const uint8_t ACCEL_DLPF_10 = 0x05; + const uint8_t ACCEL_DLPF_5 = 0x06; + const uint8_t CONFIG = 0x1A; + const uint8_t GYRO_DLPF_184 = 0x01; + const uint8_t GYRO_DLPF_92 = 0x02; + const uint8_t GYRO_DLPF_41 = 0x03; + const uint8_t GYRO_DLPF_20 = 0x04; + const uint8_t GYRO_DLPF_10 = 0x05; + const uint8_t GYRO_DLPF_5 = 0x06; + const uint8_t SMPDIV = 0x19; + const uint8_t INT_PIN_CFG = 0x37; + const uint8_t INT_ENABLE = 0x38; + const uint8_t INT_DISABLE = 0x00; + const uint8_t INT_PULSE_50US = 0x00; + const uint8_t INT_WOM_EN = 0x40; + const uint8_t INT_RAW_RDY_EN = 0x01; + const uint8_t PWR_MGMNT_1 = 0x6B; + const uint8_t PWR_CYCLE = 0x20; + const uint8_t PWR_RESET = 0x80; + const uint8_t CLOCK_SEL_PLL = 0x01; + const uint8_t PWR_MGMNT_2 = 0x6C; + const uint8_t SEN_ENABLE = 0x00; + const uint8_t DIS_GYRO = 0x07; + const uint8_t USER_CTRL = 0x6A; + const uint8_t I2C_MST_EN = 0x20; + const uint8_t I2C_MST_CLK = 0x0D; + const uint8_t I2C_MST_CTRL = 0x24; + const uint8_t I2C_SLV0_ADDR = 0x25; + const uint8_t I2C_SLV0_REG = 0x26; + const uint8_t I2C_SLV0_DO = 0x63; + const uint8_t I2C_SLV0_CTRL = 0x27; + const uint8_t I2C_SLV0_EN = 0x80; + const uint8_t I2C_READ_FLAG = 0x80; + const uint8_t MOT_DETECT_CTRL = 0x69; + const uint8_t ACCEL_INTEL_EN = 0x80; + const uint8_t ACCEL_INTEL_MODE = 0x40; + const uint8_t LP_ACCEL_ODR = 0x1E; + const uint8_t WOM_THR = 0x1F; + const uint8_t WHO_AM_I = 0x75; + const uint8_t FIFO_EN = 0x23; + const uint8_t FIFO_TEMP = 0x80; + const uint8_t FIFO_GYRO = 0x70; + const uint8_t FIFO_ACCEL = 0x08; + const uint8_t FIFO_MAG = 0x01; + const uint8_t FIFO_COUNT = 0x72; + const uint8_t FIFO_READ = 0x74; + // AK8963 registers + const uint8_t AK8963_I2C_ADDR = 0x0C; + const uint8_t AK8963_HXL = 0x03; + const uint8_t AK8963_CNTL1 = 0x0A; + const uint8_t AK8963_PWR_DOWN = 0x00; + const uint8_t AK8963_CNT_MEAS1 = 0x12; + const uint8_t AK8963_CNT_MEAS2 = 0x16; + const uint8_t AK8963_FUSE_ROM = 0x0F; + const uint8_t AK8963_CNTL2 = 0x0B; + const uint8_t AK8963_RESET = 0x01; + const uint8_t AK8963_ASA = 0x10; + const uint8_t AK8963_WHO_AM_I = 0x00; + // private functions + int writeRegister(uint8_t subAddress, uint8_t data); + int readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest); + int writeAK8963Register(uint8_t subAddress, uint8_t data); + int readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest); + int whoAmI(); + int whoAmIAK8963(); +}; + +class MPU9250FIFO: public MPU9250 { + public: + using MPU9250::MPU9250; + int enableFifo(bool accel,bool gyro,bool mag,bool temp); + int readFifo(); + void getFifoAccelX_mss(size_t *size,float* data); + void getFifoAccelY_mss(size_t *size,float* data); + void getFifoAccelZ_mss(size_t *size,float* data); + void getFifoGyroX_rads(size_t *size,float* data); + void getFifoGyroY_rads(size_t *size,float* data); + void getFifoGyroZ_rads(size_t *size,float* data); + void getFifoMagX_uT(size_t *size,float* data); + void getFifoMagY_uT(size_t *size,float* data); + void getFifoMagZ_uT(size_t *size,float* data); + void getFifoTemperature_C(size_t *size,float* data); + protected: + // fifo + bool _enFifoAccel,_enFifoGyro,_enFifoMag,_enFifoTemp; + size_t _fifoSize,_fifoFrameSize; + float _axFifo[85], _ayFifo[85], _azFifo[85]; + size_t _aSize; + float _gxFifo[85], _gyFifo[85], _gzFifo[85]; + size_t _gSize; + float _hxFifo[73], _hyFifo[73], _hzFifo[73]; + size_t _hSize; + float _tFifo[256]; + size_t _tSize; +}; + +#endif diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/SBUS/SBUS.cpp b/Versions/dRehmFlight_Teensy_BETA_2.0/src/SBUS/SBUS.cpp new file mode 100644 index 00000000..07085420 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/SBUS/SBUS.cpp @@ -0,0 +1,376 @@ +/* + SBUS.cpp + Brian R Taylor + brian.taylor@bolderflight.com + + Copyright (c) 2016 Bolder Flight Systems + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#include "SBUS.h" + +// SEE: https://learn.adafruit.com/adafruit-feather-m0-basic-proto/adapting-sketches-to-m0 +#if defined(ARDUINO_SAMD_ZERO) && defined(SERIAL_PORT_USBVIRTUAL) + // Required for Serial on Zero based boards + #define Serial SERIAL_PORT_USBVIRTUAL +#endif + +#if defined(__MK20DX128__) || defined(__MK20DX256__) + // globals needed for emulating two stop bytes on Teensy 3.0 and 3.1/3.2 + IntervalTimer serialTimer; + HardwareSerial* SERIALPORT; + uint8_t PACKET[25]; + volatile int SENDINDEX; + void sendByte(); +#endif +/* SBUS object, input the serial bus */ +SBUS::SBUS(HardwareSerial& bus) +{ + _bus = &bus; +} + +/* starts the serial communication */ +void SBUS::begin() +{ + // initialize parsing state + _parserState = 0; + // initialize default scale factors and biases + for (uint8_t i = 0; i < _numChannels; i++) { + setEndPoints(i,_defaultMin,_defaultMax); + } + // begin the serial port for SBUS + #if defined(__MK20DX128__) || defined(__MK20DX256__) // Teensy 3.0 || Teensy 3.1/3.2 + _bus->begin(_sbusBaud,SERIAL_8E1_RXINV_TXINV); + SERIALPORT = _bus; + #elif defined(__IMXRT1062__) || defined(__IMXRT1052__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) || defined(__MKL26Z64__) // Teensy 4.0 || Teensy 4.0 Beta || Teensy 3.5 || Teensy 3.6 || Teensy LC + _bus->begin(_sbusBaud,SERIAL_8E2_RXINV_TXINV); + #elif defined(STM32L496xx) || defined(STM32L476xx) || defined(STM32L433xx) || defined(STM32L432xx) // STM32L4 + _bus->begin(_sbusBaud,SERIAL_SBUS); + #elif defined(_BOARD_MAPLE_MINI_H_) // Maple Mini + _bus->begin(_sbusBaud,SERIAL_8E2); + #elif defined(ESP32) // ESP32 + _bus->begin(_sbusBaud,SERIAL_8E2); + #elif defined(__AVR_ATmega2560__) || defined(__AVR_ATmega328P__) || defined(__AVR_ATmega32U4__) // Arduino Mega 2560, 328P or 32u4 + _bus->begin(_sbusBaud, SERIAL_8E2); + #elif defined(ARDUINO_SAMD_ZERO) // Adafruit Feather M0 + _bus->begin(_sbusBaud, SERIAL_8E2); + #else + #error unsupported device + #endif +} + +/* read the SBUS data */ +bool SBUS::read(uint16_t* channels, bool* failsafe, bool* lostFrame) +{ + // parse the SBUS packet + if (parse()) { + if (channels) { + // 16 channels of 11 bit data + channels[0] = (uint16_t) ((_payload[0] |_payload[1] <<8) & 0x07FF); + channels[1] = (uint16_t) ((_payload[1]>>3 |_payload[2] <<5) & 0x07FF); + channels[2] = (uint16_t) ((_payload[2]>>6 |_payload[3] <<2 |_payload[4]<<10) & 0x07FF); + channels[3] = (uint16_t) ((_payload[4]>>1 |_payload[5] <<7) & 0x07FF); + channels[4] = (uint16_t) ((_payload[5]>>4 |_payload[6] <<4) & 0x07FF); + channels[5] = (uint16_t) ((_payload[6]>>7 |_payload[7] <<1 |_payload[8]<<9) & 0x07FF); + channels[6] = (uint16_t) ((_payload[8]>>2 |_payload[9] <<6) & 0x07FF); + channels[7] = (uint16_t) ((_payload[9]>>5 |_payload[10]<<3) & 0x07FF); + channels[8] = (uint16_t) ((_payload[11] |_payload[12]<<8) & 0x07FF); + channels[9] = (uint16_t) ((_payload[12]>>3|_payload[13]<<5) & 0x07FF); + channels[10] = (uint16_t) ((_payload[13]>>6|_payload[14]<<2 |_payload[15]<<10) & 0x07FF); + channels[11] = (uint16_t) ((_payload[15]>>1|_payload[16]<<7) & 0x07FF); + channels[12] = (uint16_t) ((_payload[16]>>4|_payload[17]<<4) & 0x07FF); + channels[13] = (uint16_t) ((_payload[17]>>7|_payload[18]<<1 |_payload[19]<<9) & 0x07FF); + channels[14] = (uint16_t) ((_payload[19]>>2|_payload[20]<<6) & 0x07FF); + channels[15] = (uint16_t) ((_payload[20]>>5|_payload[21]<<3) & 0x07FF); + } + if (lostFrame) { + // count lost frames + if (_payload[22] & _sbusLostFrame) { + *lostFrame = true; + } else { + *lostFrame = false; + } + } + if (failsafe) { + // failsafe state + if (_payload[22] & _sbusFailSafe) { + *failsafe = true; + } + else{ + *failsafe = false; + } + } + // return true on receiving a full packet + return true; + } else { + // return false if a full packet is not received + return false; + } +} + +/* read the SBUS data and calibrate it to +/- 1 */ +bool SBUS::readCal(float* calChannels, bool* failsafe, bool* lostFrame) +{ + uint16_t channels[_numChannels]; + // read the SBUS data + if(read(&channels[0],failsafe,lostFrame)) { + // linear calibration + for (uint8_t i = 0; i < _numChannels; i++) { + calChannels[i] = channels[i] * _sbusScale[i] + _sbusBias[i]; + if (_useReadCoeff[i]) { + calChannels[i] = PolyVal(_readLen[i],_readCoeff[i],calChannels[i]); + } + } + // return true on receiving a full packet + return true; + } else { + // return false if a full packet is not received + return false; + } +} + +/* write SBUS packets */ +void SBUS::write(uint16_t* channels) +{ + static uint8_t packet[25]; + /* assemble the SBUS packet */ + // SBUS header + packet[0] = _sbusHeader; + // 16 channels of 11 bit data + if (channels) { + packet[1] = (uint8_t) ((channels[0] & 0x07FF)); + packet[2] = (uint8_t) ((channels[0] & 0x07FF)>>8 | (channels[1] & 0x07FF)<<3); + packet[3] = (uint8_t) ((channels[1] & 0x07FF)>>5 | (channels[2] & 0x07FF)<<6); + packet[4] = (uint8_t) ((channels[2] & 0x07FF)>>2); + packet[5] = (uint8_t) ((channels[2] & 0x07FF)>>10 | (channels[3] & 0x07FF)<<1); + packet[6] = (uint8_t) ((channels[3] & 0x07FF)>>7 | (channels[4] & 0x07FF)<<4); + packet[7] = (uint8_t) ((channels[4] & 0x07FF)>>4 | (channels[5] & 0x07FF)<<7); + packet[8] = (uint8_t) ((channels[5] & 0x07FF)>>1); + packet[9] = (uint8_t) ((channels[5] & 0x07FF)>>9 | (channels[6] & 0x07FF)<<2); + packet[10] = (uint8_t) ((channels[6] & 0x07FF)>>6 | (channels[7] & 0x07FF)<<5); + packet[11] = (uint8_t) ((channels[7] & 0x07FF)>>3); + packet[12] = (uint8_t) ((channels[8] & 0x07FF)); + packet[13] = (uint8_t) ((channels[8] & 0x07FF)>>8 | (channels[9] & 0x07FF)<<3); + packet[14] = (uint8_t) ((channels[9] & 0x07FF)>>5 | (channels[10] & 0x07FF)<<6); + packet[15] = (uint8_t) ((channels[10] & 0x07FF)>>2); + packet[16] = (uint8_t) ((channels[10] & 0x07FF)>>10 | (channels[11] & 0x07FF)<<1); + packet[17] = (uint8_t) ((channels[11] & 0x07FF)>>7 | (channels[12] & 0x07FF)<<4); + packet[18] = (uint8_t) ((channels[12] & 0x07FF)>>4 | (channels[13] & 0x07FF)<<7); + packet[19] = (uint8_t) ((channels[13] & 0x07FF)>>1); + packet[20] = (uint8_t) ((channels[13] & 0x07FF)>>9 | (channels[14] & 0x07FF)<<2); + packet[21] = (uint8_t) ((channels[14] & 0x07FF)>>6 | (channels[15] & 0x07FF)<<5); + packet[22] = (uint8_t) ((channels[15] & 0x07FF)>>3); + } + // flags + packet[23] = 0x00; + // footer + packet[24] = _sbusFooter; + #if defined(__MK20DX128__) || defined(__MK20DX256__) // Teensy 3.0 || Teensy 3.1/3.2 + // use ISR to send byte at a time, + // 130 us between bytes to emulate 2 stop bits + noInterrupts(); + memcpy(&PACKET,&packet,sizeof(packet)); + interrupts(); + serialTimer.priority(255); + serialTimer.begin(sendByte,130); + #else + // write packet + _bus->write(packet,25); + #endif +} + +/* write SBUS packets from calibrated inputs */ +void SBUS::writeCal(float* calChannels) +{ + uint16_t channels[_numChannels] = {0}; + // linear calibration + if (calChannels) { + for (uint8_t i = 0; i < _numChannels; i++) { + if (_useWriteCoeff[i]) { + calChannels[i] = PolyVal(_writeLen[i],_writeCoeff[i],calChannels[i]); + } + channels[i] = (calChannels[i] - _sbusBias[i]) / _sbusScale[i]; + } + } + write(channels); +} + +void SBUS::setEndPoints(uint8_t channel,uint16_t min,uint16_t max) +{ + _sbusMin[channel] = min; + _sbusMax[channel] = max; + scaleBias(channel); +} + +void SBUS::getEndPoints(uint8_t channel,uint16_t *min,uint16_t *max) +{ + if (min&&max) { + *min = _sbusMin[channel]; + *max = _sbusMax[channel]; + } +} + +void SBUS::setReadCal(uint8_t channel,float *coeff,uint8_t len) +{ + if (coeff) { + if (!_readCoeff) { + _readCoeff = (float**) malloc(sizeof(float*)*_numChannels); + } + if (!_readCoeff[channel]) { + _readCoeff[channel] = (float*) malloc(sizeof(float)*len); + } else { + free(_readCoeff[channel]); + _readCoeff[channel] = (float*) malloc(sizeof(float)*len); + } + for (uint8_t i = 0; i < len; i++) { + _readCoeff[channel][i] = coeff[i]; + } + _readLen[channel] = len; + _useReadCoeff[channel] = true; + } +} + +void SBUS::getReadCal(uint8_t channel,float *coeff,uint8_t len) +{ + if (coeff) { + for (uint8_t i = 0; (i < _readLen[channel]) && (i < len); i++) { + coeff[i] = _readCoeff[channel][i]; + } + } +} + +void SBUS::setWriteCal(uint8_t channel,float *coeff,uint8_t len) +{ + if (coeff) { + if (!_writeCoeff) { + _writeCoeff = (float**) malloc(sizeof(float*)*_numChannels); + } + if (!_writeCoeff[channel]) { + _writeCoeff[channel] = (float*) malloc(sizeof(float)*len); + } else { + free(_writeCoeff[channel]); + _writeCoeff[channel] = (float*) malloc(sizeof(float)*len); + } + for (uint8_t i = 0; i < len; i++) { + _writeCoeff[channel][i] = coeff[i]; + } + _writeLen[channel] = len; + _useWriteCoeff[channel] = true; + } +} + +void SBUS::getWriteCal(uint8_t channel,float *coeff,uint8_t len) +{ + if (coeff) { + for (uint8_t i = 0; (i < _writeLen[channel]) && (i < len); i++) { + coeff[i] = _writeCoeff[channel][i]; + } + } +} + +/* destructor, free dynamically allocated memory */ +SBUS::~SBUS() +{ + if (_readCoeff) { + for (uint8_t i = 0; i < _numChannels; i++) { + if (_readCoeff[i]) { + free(_readCoeff[i]); + } + } + free(_readCoeff); + } + if (_writeCoeff) { + for (uint8_t i = 0; i < _numChannels; i++) { + if (_writeCoeff[i]) { + free(_writeCoeff[i]); + } + } + free(_writeCoeff); + } +} + +/* parse the SBUS data */ +bool SBUS::parse() +{ + // reset the parser state if too much time has passed + static elapsedMicros _sbusTime = 0; + if (_sbusTime > SBUS_TIMEOUT_US) {_parserState = 0;} + // see if serial data is available + while (_bus->available() > 0) { + _sbusTime = 0; + _curByte = _bus->read(); + // find the header + if (_parserState == 0) { + if ((_curByte == _sbusHeader) && ((_prevByte == _sbusFooter) || ((_prevByte & _sbus2Mask) == _sbus2Footer))) { + _parserState++; + } else { + _parserState = 0; + } + } else { + // strip off the data + if ((_parserState-1) < _payloadSize) { + _payload[_parserState-1] = _curByte; + _parserState++; + } + // check the end byte + if ((_parserState-1) == _payloadSize) { + if ((_curByte == _sbusFooter) || ((_curByte & _sbus2Mask) == _sbus2Footer)) { + _parserState = 0; + return true; + } else { + _parserState = 0; + return false; + } + } + } + _prevByte = _curByte; + } + // return false if a partial packet + return false; +} + +/* compute scale factor and bias from end points */ +void SBUS::scaleBias(uint8_t channel) +{ + _sbusScale[channel] = 2.0f / ((float)_sbusMax[channel] - (float)_sbusMin[channel]); + _sbusBias[channel] = -1.0f*((float)_sbusMin[channel] + ((float)_sbusMax[channel] - (float)_sbusMin[channel]) / 2.0f) * _sbusScale[channel]; +} + +float SBUS::PolyVal(size_t PolySize, float *Coefficients, float X) { + if (Coefficients) { + float Y = Coefficients[0]; + for (uint8_t i = 1; i < PolySize; i++) { + Y = Y*X + Coefficients[i]; + } + return(Y); + } else { + return 0; + } +} + +// function to send byte at a time with +// ISR to emulate 2 stop bits on Teensy 3.0 and 3.1/3.2 +#if defined(__MK20DX128__) || defined(__MK20DX256__) // Teensy 3.0 || Teensy 3.1/3.2 + void sendByte() + { + if (SENDINDEX < 25) { + SERIALPORT->write(PACKET[SENDINDEX]); + SENDINDEX++; + } else { + serialTimer.end(); + SENDINDEX = 0; + } + } +#endif diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/SBUS/SBUS.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/SBUS/SBUS.h new file mode 100644 index 00000000..215cb9b7 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/SBUS/SBUS.h @@ -0,0 +1,82 @@ +/* + SBUS.h + Brian R Taylor + brian.taylor@bolderflight.com + + Copyright (c) 2016 Bolder Flight Systems + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#ifndef SBUS_h +#define SBUS_h + +#include "Arduino.h" +#include "elapsedMillis.h" + +/* +* Hardware Serial Supported: +* Teensy 3.0 || Teensy 3.1/3.2 || Teensy 3.5 || Teensy 3.6 || Teensy LC || STM32L4 || Maple Mini || Arduino Mega 2560 || ESP32 +*/ +#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) \ + || defined(__MK66FX1M0__) || defined(__MKL26Z64__) || defined(__IMXRT1052__) \ + || defined(STM32L496xx) || defined(STM32L476xx) || defined(STM32L433xx) \ + || defined(STM32L432xx) || defined(_BOARD_MAPLE_MINI_H_) \ + || defined(__AVR_ATmega2560__) || defined(ESP32) +#endif + +class SBUS{ + public: + SBUS(HardwareSerial& bus); + void begin(); + bool read(uint16_t* channels, bool* failsafe, bool* lostFrame); + bool readCal(float* calChannels, bool* failsafe, bool* lostFrame); + void write(uint16_t* channels); + void writeCal(float *channels); + void setEndPoints(uint8_t channel,uint16_t min,uint16_t max); + void getEndPoints(uint8_t channel,uint16_t *min,uint16_t *max); + void setReadCal(uint8_t channel,float *coeff,uint8_t len); + void getReadCal(uint8_t channel,float *coeff,uint8_t len); + void setWriteCal(uint8_t channel,float *coeff,uint8_t len); + void getWriteCal(uint8_t channel,float *coeff,uint8_t len); + ~SBUS(); + private: + const uint32_t _sbusBaud = 100000; + static const uint8_t _numChannels = 16; + const uint8_t _sbusHeader = 0x0F; + const uint8_t _sbusFooter = 0x00; + const uint8_t _sbus2Footer = 0x04; + const uint8_t _sbus2Mask = 0x0F; + const uint32_t SBUS_TIMEOUT_US = 7000; + uint8_t _parserState, _prevByte = _sbusFooter, _curByte; + static const uint8_t _payloadSize = 24; + uint8_t _payload[_payloadSize]; + const uint8_t _sbusLostFrame = 0x04; + const uint8_t _sbusFailSafe = 0x08; + const uint16_t _defaultMin = 172; + const uint16_t _defaultMax = 1811; + uint16_t _sbusMin[_numChannels]; + uint16_t _sbusMax[_numChannels]; + float _sbusScale[_numChannels]; + float _sbusBias[_numChannels]; + float **_readCoeff, **_writeCoeff; + uint8_t _readLen[_numChannels],_writeLen[_numChannels]; + bool _useReadCoeff[_numChannels], _useWriteCoeff[_numChannels]; + HardwareSerial* _bus; + bool parse(); + void scaleBias(uint8_t channel); + float PolyVal(size_t PolySize, float *Coefficients, float X); +}; + +#endif diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/SBUS/elapsedMillis.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/SBUS/elapsedMillis.h new file mode 100644 index 00000000..48cff11d --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/SBUS/elapsedMillis.h @@ -0,0 +1,81 @@ +/* Elapsed time types - for easy-to-use measurements of elapsed time + * http://www.pjrc.com/teensy/ + * Copyright (c) 2017 PJRC.COM, LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifndef elapsedMillis_h +#define elapsedMillis_h +#ifdef __cplusplus + +#if ARDUINO >= 100 +#include "Arduino.h" +#else +#include "WProgram.h" +#endif + +class elapsedMillis +{ +private: + unsigned long ms; +public: + elapsedMillis(void) { ms = millis(); } + elapsedMillis(unsigned long val) { ms = millis() - val; } + elapsedMillis(const elapsedMillis &orig) { ms = orig.ms; } + operator unsigned long () const { return millis() - ms; } + elapsedMillis & operator = (const elapsedMillis &rhs) { ms = rhs.ms; return *this; } + elapsedMillis & operator = (unsigned long val) { ms = millis() - val; return *this; } + elapsedMillis & operator -= (unsigned long val) { ms += val ; return *this; } + elapsedMillis & operator += (unsigned long val) { ms -= val ; return *this; } + elapsedMillis operator - (int val) const { elapsedMillis r(*this); r.ms += val; return r; } + elapsedMillis operator - (unsigned int val) const { elapsedMillis r(*this); r.ms += val; return r; } + elapsedMillis operator - (long val) const { elapsedMillis r(*this); r.ms += val; return r; } + elapsedMillis operator - (unsigned long val) const { elapsedMillis r(*this); r.ms += val; return r; } + elapsedMillis operator + (int val) const { elapsedMillis r(*this); r.ms -= val; return r; } + elapsedMillis operator + (unsigned int val) const { elapsedMillis r(*this); r.ms -= val; return r; } + elapsedMillis operator + (long val) const { elapsedMillis r(*this); r.ms -= val; return r; } + elapsedMillis operator + (unsigned long val) const { elapsedMillis r(*this); r.ms -= val; return r; } +}; + +class elapsedMicros +{ +private: + unsigned long us; +public: + elapsedMicros(void) { us = micros(); } + elapsedMicros(unsigned long val) { us = micros() - val; } + elapsedMicros(const elapsedMicros &orig) { us = orig.us; } + operator unsigned long () const { return micros() - us; } + elapsedMicros & operator = (const elapsedMicros &rhs) { us = rhs.us; return *this; } + elapsedMicros & operator = (unsigned long val) { us = micros() - val; return *this; } + elapsedMicros & operator -= (unsigned long val) { us += val ; return *this; } + elapsedMicros & operator += (unsigned long val) { us -= val ; return *this; } + elapsedMicros operator - (int val) const { elapsedMicros r(*this); r.us += val; return r; } + elapsedMicros operator - (unsigned int val) const { elapsedMicros r(*this); r.us += val; return r; } + elapsedMicros operator - (long val) const { elapsedMicros r(*this); r.us += val; return r; } + elapsedMicros operator - (unsigned long val) const { elapsedMicros r(*this); r.us += val; return r; } + elapsedMicros operator + (int val) const { elapsedMicros r(*this); r.us -= val; return r; } + elapsedMicros operator + (unsigned int val) const { elapsedMicros r(*this); r.us -= val; return r; } + elapsedMicros operator + (long val) const { elapsedMicros r(*this); r.us -= val; return r; } + elapsedMicros operator + (unsigned long val) const { elapsedMicros r(*this); r.us -= val; return r; } +}; + +#endif // __cplusplus +#endif // elapsedMillis_h diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/TFMPlus/TFMPlus.cpp b/Versions/dRehmFlight_Teensy_BETA_2.0/src/TFMPlus/TFMPlus.cpp new file mode 100644 index 00000000..8a4a63a3 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/TFMPlus/TFMPlus.cpp @@ -0,0 +1,385 @@ +/* File Name: TFMPlus.cpp + * Version: 1.5.0 + * Described: Arduino Library for the Benewake TFMini-Plus Lidar sensor + * The TFMini-Plus is a unique product, and the various + * TFMini Libraries are not compatible with the Plus. + * Developer: Bud Ryerson + * Inception: v0.2.0 - 31 JAN 2019 + * v1.0.0 - 25FEB19 - Initial release + * v1.0.1 - 09MAR19 - 'build()' function always returned TRUE. + Corrected to return FALSE if serial data is not available. + And other minor corrections to textual descriptions. + * v1.1.0 - 13MAR19 - To simplify, all interface functions now + return boolean. Status code is still set and can be read. + 'testSum()' is deleted and 'makeSum()' is used instead. + Example code is updated. + * v1.1.1 - 14MAR19 - Two commands: RESTORE_FACTORY_SETTINGS + and SAVE_SETTINGS were not defined correctly. + * v1.2.1 - 02APR19 - Rewrote 'getData()' function to make it faster + and more robust when serial read skips a byte or fails entirely. + * v1.3.1 - 08APR19 - Redefined commands to include response length + ********************** IMPORTANT ************************ + **** Changed name of 'buildCommand()' to 'sendCommand()'. **** + **************************************************************** + * v.1.3.2 - Added a line to getData() to flush the serial buffer + of all but last frame of data before reading. This does not + effect usage, but will ensure that the latest data is read. + * v.1.3.3 - 20MAY19 - Changed 'sendCommand()' to add a second byte, + to the HEADER recognition routine, the reply length byte. + This makes recognition of the command reply more robust. + Zeroed out 'data frame' snd 'command reply' buffer arrays + completely before reading from device. Added but did not + implement some I2C command codes. + * v.1.3.4 - 07JUN19 - Added 'TFMP_' to all error status defines. + The ubiquitous 'Arduino.h' also contains a 'SERIAL' define. + * v.1.3.5 - 25OCT19 - Added missing underscore to parameter + in header file TFMPlus.h: + Line 138 #define BAUD_14400 0x003840 + * v.1.3.6 - 27APR20 - a little cleanup in 'getData()' + * v.1.4.0 - 15JUN20 - Changed all data variables from unsigned + to signed integers. Defined abnormal data codes + as per TFMini-S Producut Manual + - - - - - - - - - - - - - - - + Dist | Strength | Comment + -1 Other value Strength ≤ 100 + -2 -1 Signal strength saturation + -4 Other value Ambient light saturation + - - - - - - - - - - - - - - - + * v.1.4.1 - 22JUL20 - Fixed bug in sendCommand() checksum calculation + - Changed two printf()s to Serial.print()s + - Fixed printReply() to show data from 'reply' rather than 'frame' + * v.1.4.2 - 19MAY21 - Changed command paramter 'FRAME_5' to correct value. + It was set to 0x0003. Now it's set to 0x0005 + * v.1.5.0 - 06SEP21 - Corrected (reversed) Enable/Disable commands in 'TFMPlus.h' + Changed three command names + OBTAIN_FIRMWARE_VERSION is now GET_FIRMWARE_VERSION + RESTORE_FACTORY_SETTINGS is now HARD_RESET + SYSTEM_RESET is now SOFT_RESET + * + * Default settings for the TFMini-Plus are a 115200 serial baud rate + * and a 100Hz measurement frame rate. The device will begin returning + * measurement data immediately on power up. + * + * 'begin()' function passes a serial stream to library and + * returns TRUE/FALSE whether serial data is available. + * Function also sets a public one byte status code. + * Status codes are defined within the library. + * + * 'getData( dist, flux, temp)' passes back measurement data + * • dist = distance in centimeters, + * • flux = signal strength in arbitrary units, and + * • temp = an encoded number in degrees centigrade + * Function returns TRUE/FALSE whether completed without error. + * Error, if any, is saved as a one byte 'status' code. + * + * 'sendCommand( cmnd, param)' sends a 32bit command code (cmnd) + * and a 32bit parameter value (param). Returns TRUE/FALSE and + * sets a one byte status code. + * Commands are selected from the library's list of defined commands. + * Parameter values can be entered directly (115200, 250, etc) but + * for safety, they should be chosen from the Library's defined lists. + * An incorrect value can render the device uncommunicative. + * + */ + +#include "TFMPlus.h" +//#include // Future I2C Implementation + +// Constructor +TFMPlus::TFMPlus(){} +TFMPlus::~TFMPlus(){} + +// Return TRUE/FALSE whether receiving serial data from +// device, and set system status to provide more information. +bool TFMPlus::begin(Stream *streamPtr) +{ + pStream = streamPtr; // Save reference to stream/serial object. + delay( 10); // Delay for device data in serial buffer. + if( (*pStream).available()) // If data present... + { + status = TFMP_READY; // set status to READY + return true; // and return TRUE. + } + else // Otherwise... + { + status = TFMP_SERIAL; // set status to SERIAL error + return false; // and return false. + } +} + +bool TFMPlus::getData( int16_t &dist, int16_t &flux, int16_t &temp) +{ + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 1 - Get data from the device. + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Set timer to one second timeout if HEADER never appears + // or serial data never becomes available. + uint32_t serialTimeout = millis() + 1000; + + if( !(*pStream).available()) + { + return false; + } + + // Flush all but last frame of data from the serial buffer. + while( (*pStream).available() > TFMP_FRAME_SIZE) (*pStream).read(); + + // Zero out the entire frame data buffer. + memset( frame, 0, sizeof( frame)); + + // Read one byte from the serial buffer into the last byte of + // the frame buffer, then left shift the whole array one byte. + // Repeat until the two HEADER bytes show up as the first + // two bytes in the array. + while( ( frame[ 0] != 0x59) || ( frame[ 1] != 0x59)) + { + if( (*pStream).available()) + { + // Read one byte into the framebuffer's + // last plus one position. + frame[ TFMP_FRAME_SIZE] = (*pStream).read(); + // Shift the last nine bytes one byte left. + memcpy( frame, frame + 1, TFMP_FRAME_SIZE); + } + // If HEADER or serial data are not available + // after more than one second... + if( millis() > serialTimeout) + { + status = TFMP_HEADER; // then set error... + return false; // and return "false". + } + } + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 2 - Perform a checksum test. + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Clear the 'chkSum' variable declared in 'TFMPlus.h' + chkSum = 0; + // Add together all bytes but the last. + for( uint8_t i = 0; i < ( TFMP_FRAME_SIZE - 1); i++) chkSum += frame[ i]; + // If the low order byte does not equal the last byte... + if( ( uint8_t)chkSum != frame[ TFMP_FRAME_SIZE - 1]) + { + status = TFMP_CHECKSUM; // then set error... + return false; // and return "false." + } + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 3 - Interpret the frame data + // and if okay, then go home + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + dist = frame[ 2] + ( frame[ 3] << 8); + flux = frame[ 4] + ( frame[ 5] << 8); + temp = frame[ 6] + ( frame[ 7] << 8); + // Convert temp code to degrees Celsius. + temp = ( temp >> 3) - 256; + // Convert Celsius to degrees Farenheit + // temp = uint8_t( temp * 9 / 5) + 32; + + // - - Evaluate Abnormal Data Values - - + // Values are from the TFMini-S Product Manual + // Signal strength <= 100 + if( dist == -1) status = TFMP_WEAK; + // Signal Strength saturation + else if( flux == -1) status = TFMP_STRONG; + // Ambient Light saturation + else if( dist == -4) status = TFMP_FLOOD; + // Data is apparently okay + else status = TFMP_READY; + + if( status != TFMP_READY) return false; + else return true; +} + +// Pass back only the distance data +bool TFMPlus::getData( int16_t &dist) +{ + static int16_t flux, temp; + return getData( dist, flux, temp); +} + +// = = = = = SEND A COMMAND TO THE DEVICE = = = = = = = = = =0 +// +// Create a proper command byte array, send the command, +// get a repsonse, and return the status +bool TFMPlus::sendCommand( uint32_t cmnd, uint32_t param) +{ + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 1 - Build the command data to send to the device + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + static uint8_t cmndLen; // Length of command + static uint8_t replyLen; // Length of command reply data + static uint8_t cmndData[ 8]; // 8 byte send command array + + memset( cmndData, 0, 8); // Clear the send command array. + memcpy( &cmndData[ 0], &cmnd, 4); // Copy 4 bytes of data: reply length, + // command length, command number and + // a one byte parameter, all encoded as + // a 32 bit unsigned integer. + + replyLen = cmndData[ 0]; // Save the first byte as reply length. + cmndLen = cmndData[ 1]; // Save the second byte as command length. + cmndData[ 0] = 0x5A; // Set the first byte to HEADER code. + + if( cmnd == SET_FRAME_RATE) // If the command is Set FrameRate... + { + memcpy( &cmndData[ 3], ¶m, 2); // add the 2 byte FrameRate parameter. + } + else if( cmnd == SET_BAUD_RATE) // If the command is Set BaudRate... + { + memcpy( &cmndData[ 3], ¶m, 4); // add the 3 byte BaudRate parameter. + } + + // Create a checksum byte for the command data array. + chkSum = 0; + // Add together all bytes but the last... + for( uint8_t i = 0; i < ( cmndLen - 1); i++) chkSum += cmndData[ i]; + // and save it as the last byte of command data. + cmndData[ cmndLen - 1] = (uint8_t)chkSum; + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 2 - Send the command data array to the device + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + while( (*pStream).available()) (*pStream).read(); // flush input buffer + (*pStream).flush(); // flush output buffer + for( uint8_t i = 0; i < cmndLen; i++) (*pStream).write( cmndData[ i]); + + + // + + + + + + + + + + + + + + + + + + + + + + + + + + // If the command does not expect a reply, then we're + // finished here. Call the getData() function instead. + if( replyLen == 0) return true; + // + + + + + + + + + + + + + + + + + + + + + + + + + + + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 3 - Get command reply data back from the device. + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Set a one second timer to timeout if HEADER never appears + // or serial data never becomes available + uint32_t serialTimeout = millis() + 1000; + // Clear out the entire command reply data buffer + memset( reply, 0, sizeof( reply)); + // Read one byte from the serial buffer into the end of + // the reply buffer and then left shift the whole array. + // Repeat until the HEADER byte and reply length byte + // show up as the first two bytes in the array. + while( ( reply[ 0] != 0x5A) || ( reply[ 1] != replyLen)) + { + if( (*pStream).available()) + { + // Read one byte into the reply buffer's + // last-plus-one position. + reply[ replyLen] = (*pStream).read(); + // Shift the last nine bytes one byte left. + memcpy( reply, reply+1, TFMP_REPLY_SIZE); + } + // If HEADER pattern or Serial data are not available + // after more than one second... + if( millis() > serialTimeout) + { + status = TFMP_TIMEOUT; // then set error... + return false; // and return "false". + } + } + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 4 - Perform a checksum test. + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Clear the 'chkSum' variable declared in 'TFMPlus.h' + chkSum = 0; + // Add together all bytes but the last... + for( uint8_t i = 0; i < ( replyLen - 1); i++) chkSum += reply[ i]; + // If the low order byte of the Sum does not equal the last byte... + if( reply[ replyLen - 1] != (uint8_t)chkSum) + { + status = TFMP_CHECKSUM; // then set error... + return false; // and return "false." + } + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 5 - Interpret different command responses. + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + if( cmnd == GET_FIRMWARE_VERSION) + { + version[ 0] = reply[5]; // set firmware version. + version[ 1] = reply[4]; + version[ 2] = reply[3]; + } + else + { + if( cmnd == SOFT_RESET || + cmnd == HARD_RESET || + cmnd == SAVE_SETTINGS ) + { + if( reply[ 3] == 1) // If PASS/FAIL byte not zero ... + { + status = TFMP_FAIL; // set status 'FAIL'... + return false; // and return 'false'. + } + } + } + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Step 6 - Set READY status and go home + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - + status = TFMP_READY; + return true; +} + +// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +// - - - - - The following is for testing purposes - - - - +// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + +// Called by either 'printFrame()' or 'printReply()' +// Print status condition either 'READY' or error type +void TFMPlus::printStatus() +{ + Serial.print("Status: "); + if( status == TFMP_READY) Serial.print( "READY"); + else if( status == TFMP_SERIAL) Serial.print( "SERIAL"); + else if( status == TFMP_HEADER) Serial.print( "HEADER"); + else if( status == TFMP_CHECKSUM) Serial.print( "CHECKSUM"); + else if( status == TFMP_TIMEOUT) Serial.print( "TIMEOUT"); + else if( status == TFMP_PASS) Serial.print( "PASS"); + else if( status == TFMP_FAIL) Serial.print( "FAIL"); + else if( status == TFMP_I2CREAD) Serial.print( "I2C-READ"); + else if( status == TFMP_I2CWRITE) Serial.print( "I2C-WRITE"); + else if( status == TFMP_I2CLENGTH) Serial.print( "I2C-LENGTH"); + else if( status == TFMP_WEAK) Serial.print( "Signal weak"); + else if( status == TFMP_STRONG) Serial.print( "Signal saturation"); + else if( status == TFMP_FLOOD) Serial.print( "Ambient light saturation"); + else Serial.print( "OTHER"); + Serial.println(); +} + +// Print error type and HEX values +// of each byte in the data frame +void TFMPlus::printFrame() +{ + printStatus(); + // Print the Hex value of each byte of data + Serial.print("Data:"); + for( uint8_t i = 0; i < TFMP_FRAME_SIZE; i++) + { + Serial.print(" "); + Serial.print( frame[ i] < 16 ? "0" : ""); + Serial.print( frame[ i], HEX); + } + Serial.println(); +} + +// Print error type and HEX values of +// each byte in the command response frame. +void TFMPlus::printReply() +{ + printStatus(); + // Print the Hex value of each byte + for( uint8_t i = 0; i < TFMP_REPLY_SIZE; i++) + { + Serial.print(" "); + Serial.print( reply[ i] < 16 ? "0" : ""); + Serial.print( reply[ i], HEX); + } + Serial.println(); +} diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/TFMPlus/TFMPlus.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/TFMPlus/TFMPlus.h new file mode 100644 index 00000000..64721987 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/TFMPlus/TFMPlus.h @@ -0,0 +1,227 @@ +/* File Name: TFMPlus.h + * Version: 1.5.0 + * Described: Arduino Library for the Benewake TFMini-Plus Lidar sensor + * The TFMini-Plus is a unique product, and the various + * TFMini Libraries are not compatible with the Plus. + * Developer: Bud Ryerson + * Inception: v0.2.0 - 31 JAN 2019 + * v1.0.0 - 25FEB19 - Initial release + * v1.0.1 - 09MAR19 - 'build()' function always returned TRUE. + Corrected to return FALSE if serial data is not available. + And other minor corrections to textual descriptions. + * v1.1.0 - 13MAR19 - To simplify, all interface functions now + return boolean. Status code is still set and can be read. + 'testSum()' is deleted and 'makeSum()' is used instead. + Example code is updated. + * v1.1.1 - 14MAR19 - Two commands: RESTORE_FACTORY_SETTINGS + and SAVE_SETTINGS were not defined correctly. + * v1.2.1 - 02APR19 - Rewrote 'getData()' function to make it faster + and more robust when serial read skips a byte or fails entirely. + * v1.3.1 - 08APR19 - Redefined commands to include response length + ********************** IMPORTANT ************************ + **** Changed name of 'buildCommand()' to 'sendCommand()'. **** + **************************************************************** + * v.1.3.2 - Added a line to getData() to flush the serial buffer + of all but last frame of data before reading. This does not + effect usage, but will ensure that the latest data is read. + * v.1.3.3 - 20MAY19 - Changed 'sendCommand()' to add a second byte, + to the HEADER recognition routine, the reply length byte. + Zeroed out 'data frame' snd 'command reply' buffer arrays + completely before reading from device. Added but did not + implement some I2C command codes. + * v.1.3.4 - 07JUN19 - Added 'TFMP_' to all error status defines. + The ubiquitous 'Arduino.h' also contains a 'SERIAL' define. + * v.1.3.5 - 25OCT19 - Added missing underscore to paramter: + #define BAUD_14400 0x003840 + * v.1.3.6 - 27APR20 - a little cleanup in 'getData()' + * v.1.4.0 - 15JUN20 + 1. Changed all data variables from unsigned to signed integers. + 2. Defined abnormal data codes as per TFMini-S Producut Manual + - - - - - - - - - - - - - - - - + Distance | Strength | Comment + -1 Other value Strength ≤ 100 + -2 -1 Signal strength saturation + -4 Other value Ambient light saturation + - - - - - - - - - - - - - - - - + * v.1.4.1 - 22JUL20 - Fixed bugs in TFMPlus.cpp + * v.1.5.0 - 06SEP21 - Corrected (reversed) Enable/Disable commands + Changed three command names + OBTAIN_FIRMWARE_VERSION is now GET_FIRMWARE_VERSION + RESTORE_FACTORY_SETTINGS is now HARD_RESET + SYSTEM_RESET is now SOFT_RESET + * + * Default settings for the TFMini-Plus are a 115200 serial baud rate + * and a 100Hz measurement frame rate. The device will begin returning + * measurement data immediately on power up. + * + * 'begin()' function passes a serial stream to library and + * returns TRUE/FALSE whether serial data is available. + * Function also sets a public one byte status code. + * Status codes are defined within the library. + * + * 'getData( dist, flux, temp)' passes back measurement data + * • dist = distance in centimeters, + * • flux = signal strength in arbitrary units, and + * • temp = an encoded number in degrees centigrade + * Function returns TRUE/FALSE whether completed without error. + * Error, if any, is saved as a one byte 'status' code. + * + * 'sendCommand( cmnd, param)' sends a 32bit command code (cmnd) + * and a 32bit parameter value (param). Returns TRUE/FALSE and + * sets a one byte status code. + * Commands are selected from the library's list of defined commands. + * Parameter values can be entered directly (115200, 250, etc) but + * for safety, they should be chosen from the Library's defined lists. + * An incorrect value can render the device uncommunicative. + * + */ + +#ifndef TFMPLUS_H // Guard to compile only once +#define TFMPLUS_H + +#include // Always include this. It's important. + +// Buffer sizes +#define TFMP_FRAME_SIZE 9 // Size of data frame = 9 bytes +#define TFMP_REPLY_SIZE 8 // Longest command reply = 8 bytes +#define TFMP_COMMAND_MAX 8 // Longest command = 8 bytes + +// Timeout Limits for various functions +#define TFMP_MAX_READS 20 // readData() sets SERIAL error +#define MAX_BYTES_BEFORE_HEADER 20 // getData() sets HEADER error +#define MAX_ATTEMPTS_TO_MEASURE 20 + +#define TFMP_DEFAULT_ADDRESS 0x10 // default I2C slave address + // as hexidecimal integer +// System Error Status Condition +#define TFMP_READY 0 // no error +#define TFMP_SERIAL 1 // serial timeout +#define TFMP_HEADER 2 // no header found +#define TFMP_CHECKSUM 3 // checksum doesn't match +#define TFMP_TIMEOUT 4 // I2C timeout +#define TFMP_PASS 5 // reply from some system commands +#define TFMP_FAIL 6 // " +#define TFMP_I2CREAD 7 +#define TFMP_I2CWRITE 8 +#define TFMP_I2CLENGTH 9 +#define TFMP_WEAK 10 // Signal Strength ≤ 100 +#define TFMP_STRONG 11 // Signal Strength saturation +#define TFMP_FLOOD 12 // Ambient Light saturation +#define TFMP_MEASURE 13 + + +/* - - - - - - - - - TFMini Plus - - - - - - - - - + Data Frame format: + Byte0 Byte1 Byte2 Byte3 Byte4 Byte5 Byte6 Byte7 Byte8 + 0x59 0x59 Dist_L Dist_H Flux_L Flux_H Temp_L Temp_H CheckSum_ + Data Frame Header character: Hex 0x59, Decimal 89, or "Y" + + Command format: + Byte0 Byte1 Byte2 Byte3 to Len-2 Byte Len-1 + 0x5A Length Cmd ID Payload if any Checksum + - - - - - - - - - - - - - - - - - - - - - - - - - */ + +// The library 'sendCommand( cmnd, param)' function +// defines a command (cmnd) in the the following format: +// 0x 00 00 00 00 +// one byte command command reply +// payload number length length +#define GET_FIRMWARE_VERSION 0x00010407 // returns 3 byte firmware version +#define TRIGGER_DETECTION 0x00040400 // must have set frame rate to zero + // returns a 9 byte data frame +#define SOFT_RESET 0x00020405 // returns a 1 byte pass/fail (0/1) +#define HARD_RESET 0x00100405 // " +#define SAVE_SETTINGS 0x00110405 // This must follow every command + // that modifies volatile parameters. + // Returns a 1 byte pass/fail (0/1) + +#define SET_FRAME_RATE 0x00030606 // These commands each return +#define SET_BAUD_RATE 0x00060808 // an echo of the command +#define STANDARD_FORMAT_CM 0x01050505 // " +#define PIXHAWK_FORMAT 0x02050505 // " +#define STANDARD_FORMAT_MM 0x06050505 // " +#define ENABLE_OUTPUT 0x01070505 // " +#define DISABLE_OUTPUT 0x00070505 // " +#define SET_I2C_ADDRESS 0x100B0505 // " + +#define SET_SERIAL_MODE 0x000A0500 // default is Serial (UART) +#define SET_I2C_MODE 0x010A0500 // set device as I2C slave + +#define I2C_FORMAT_CM 0x01000500 // returns a 9 byte data frame +#define I2C_FORMAT_MM 0x06000500 // " + +// * * * * * * * Description of I/O Mode * * * * * * * +// Normally, device Pin 3 is either Serial transmit (TX) or I2C clock (SCL). +// When 'I/O Mode' is set other than 'Standard,' Pin 3 becomes a simple HI/LO +// (near/far) binary output. Thereafter, only Pin 2, the Serial RX line, is +// functional, and only Serial data sent to the device is possible. +//#define SET_IO_MODE_STANDARD 0x003B0900 // 'Standard' is default mode +//#define SET_IO_MODE_HILO 0x013B0900 // I/O, near high and far low +//#define SET_IO_MODE_LOHI 0x023B0900 // I/O, near low and far high +// * * * This library does not support the I/O Mode interface * * * + +// Command Parameters +#define BAUD_9600 0x002580 // UART serial baud rate +#define BAUD_14400 0x003840 // expressed in hexidecimal +#define BAUD_19200 0x004B00 +#define BAUD_56000 0x00DAC0 +#define BAUD_115200 0x01C200 +#define BAUD_460800 0x070800 +#define BAUD_921600 0x0E1000 + +#define FRAME_0 0x0000 // internal measurement rate +#define FRAME_1 0x0001 // expressed in hexidecimal +#define FRAME_2 0x0002 +#define FRAME_5 0x0005 // incorrectly set to 3 in prior versions +#define FRAME_10 0x000A +#define FRAME_20 0x0014 +#define FRAME_25 0x0019 +#define FRAME_50 0x0032 +#define FRAME_100 0x0064 +#define FRAME_125 0x007D +#define FRAME_200 0x00C8 +#define FRAME_250 0x00FA +#define FRAME_500 0x01F4 +#define FRAME_1000 0x03E8 + +// Object Class Definitions +class TFMPlus +{ + public: + TFMPlus(); + ~TFMPlus(); + + uint8_t version[ 3]; // to save firmware version + uint8_t status; // to save library error status + + // Return T/F whether serial data available, set error status if not. + bool begin( Stream *streamPtr); + // Read device data and pass back three values + bool getData( int16_t &dist, int16_t &flux, int16_t &temp); + // Short version, passes back distance data only + bool getData( int16_t &dist); + // Build and send a command, and check response + bool sendCommand( uint32_t cmnd, uint32_t param); + + // For testing purposes: print frame or reply data and status + // as a string of HEX characters + void printFrame(); + void printReply(); + bool getResponse(); + + private: + Stream* pStream; // pointer to the device serial stream + // The data buffers are one byte longer than necessary + // because we read one byte into the last position, then + // shift the whole array left by one byte after each read. + uint8_t frame[ TFMP_FRAME_SIZE + 1]; + uint8_t reply[ TFMP_REPLY_SIZE + 1]; + + uint16_t chkSum; // to calculate the check sum byte. + + // for testing - called by 'printFrame()' or 'printReply()' + void printStatus(); + +}; + +#endif diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/dRhemFlight_hardware.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/dRhemFlight_hardware.h new file mode 100644 index 00000000..2e0d2940 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/dRhemFlight_hardware.h @@ -0,0 +1,78 @@ +#ifndef dRhemFlight_hardware_h +#define dRhemFlight_hardware_h + +#include + +//========================================================================================================================// +// USER-SPECIFIED DEFINES // +//========================================================================================================================// + +//Uncomment only one receiver type +//#define USE_PWM_RX +//#define USE_PPM_RX +#define USE_SBUS_RX +//#define USE_DSM_RX +static const uint8_t num_DSM_channels = 6; //If using DSM RX, change this to match the number of transmitter channels you have + + +// capture number of input channels + +// Define input pins on the board of your choice + + + +//Uncomment only one IMU +//#define USE_MPU6050_I2C //Default +//#define USE_MPU9250_SPI +//#define USE_LSM6DSOX_SPI + +// Uncomment only one full scale gyro range (deg/sec) +// #define GYRO_250DPS //Default +// #define GYRO_500DPS +// #define GYRO_1000DPS +// #define GYRO_2000DPS + +// Uncomment only one full scale accelerometer range (G's) +// #define ACCEL_2G //Default +// #define ACCEL_4G +// #define ACCEL_8G +// #define ACCEL_16G + + + + +//Capture number of output channels: Motors (DSHOT125, etc.), Servos (50Hz), and possibly other Digital Servos + + + +//Define output pins based on above and board type / craft type + + + + + + + + + + + + + +// Other defines that depend on the above + + +#if defined USE_SBUS_RX + #include "SBUS/SBUS.h" //sBus interface +#endif + +#if defined USE_DSM_RX + #include "DSMRX/DSMRX.h" +#endif + + + + + + +#endif \ No newline at end of file From 5c37f053f19be6fe1772fd807313ea2ce4b643c4 Mon Sep 17 00:00:00 2001 From: Brian Jones Date: Sun, 10 Aug 2025 09:45:42 -0500 Subject: [PATCH 3/6] More work to separate functions into wrapper libraries to clean the main code base. --- .../dRehmFlight_Teensy_BETA_2.0.ino | 332 ++++++++++-------- .../dRehmFlight_Teensy_BETA_2.0/radioComm.ino | 332 +++++++++--------- .../src/IMU_PID/keywords.txt | 34 -- .../IMU_Wrapper.cpp} | 330 +++++++++-------- .../IMU_PID.h => IMU_Wrapper/IMU_Wrapper.h} | 74 ++-- .../src/IMU_Wrapper/keywords.txt | 59 ++++ .../src/RadioComm/RadioComm.cpp | 181 ++++++++++ .../src/RadioComm/RadioComm.h | 82 +++++ 8 files changed, 885 insertions(+), 539 deletions(-) delete mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/keywords.txt rename Versions/dRehmFlight_Teensy_BETA_2.0/src/{IMU_PID/IMU_PID.cpp => IMU_Wrapper/IMU_Wrapper.cpp} (82%) rename Versions/dRehmFlight_Teensy_BETA_2.0/src/{IMU_PID/IMU_PID.h => IMU_Wrapper/IMU_Wrapper.h} (81%) create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/keywords.txt create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.cpp create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.h diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino b/Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino index 294d411b..09d58c85 100644 --- a/Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino @@ -31,10 +31,9 @@ Everyone that sends me pictures and videos of your flying creations! -Nick //REQUIRED LIBRARIES (included with download in main sketch folder) // #include "src/dRhemFlight_hardware.h" -// #include //I2c communication -// #include //SPI communication #include //Commanding any extra actuators, installed with teensyduino installer -#include "src/IMU_PID/IMU_PID.h" +#include "src/IMU_Wrapper/IMU_Wrapper.h" +#include "src/RadioComm/RadioComm.h" @@ -83,6 +82,10 @@ const int ch4Pin = 20; //rudd const int ch5Pin = 21; //gear (throttle cut) const int ch6Pin = 22; //aux1 (free aux channel) const int PPM_Pin = 23; + + + + //OneShot125 ESC pin outputs: const int m1Pin = 0; const int m2Pin = 1; @@ -112,8 +115,16 @@ PWMServo servo4; // declare IMU and pins -IMU myIMU(MPU6050_I2C); +IMU_Wrapper imu(LSM6DSOX_SPI); // values are MPU6050_I2C (Default), MPU9250_SPI, LSM6DSOX_SPI +// Defaults to GYRO_250DPS and ACCEL_2G. You can change the sensitivity by altering these in the call above. +// example: IMU imu(LSM6DSOX_SPI, GYRO_1000DPS, ACCEL_8G); + +// Needed for MPU9250_SPI and LSM6DSOX_SPI. Comment these out for MPU6050_I2C +#define IMU_CS 10 // CS2--green-yellow +#define IMU_SCK 13 // SCK2/SCL--white +#define IMU_MISO 12 // MISO2/DO--red +#define IMU_MOSI 11 // MOSI2/SDA--black /* IMU Pin locations for IMU: @@ -124,16 +135,16 @@ defaults for Teensy 4.0: SCL -> pin 19 default pins for MPU9250 using SPI: - MISO -> pin 34 - MOSI -> pin 35 CS -> pin 36 SCK -> pin 37 - - default pins for Adafruit LSM6DSOX: MISO -> pin 34 MOSI -> pin 35 + + default pins for Adafruit LSM6DSOX: CS -> pin 36 SCK -> pin 37 + MISO -> pin 34 + MOSI -> pin 35 */ @@ -152,34 +163,31 @@ bool blinkAlternate; unsigned long channel_1_pwm, channel_2_pwm, channel_3_pwm, channel_4_pwm, channel_5_pwm, channel_6_pwm; unsigned long channel_1_pwm_prev, channel_2_pwm_prev, channel_3_pwm_prev, channel_4_pwm_prev; +// How many input channels do you have? +#define NUM_CHANNELS 12 +// Holder array for radio PWM data +unsigned long channel_pwm[NUM_CHANNELS]; +// Define the radio object +RadioComm radio(sBUS, NUM_CHANNELS); +// RadioComm radio(DSM, NUM_CHANNELS); +// RadioComm radio(PPM, NUM_CHANNELS); +// RadioComm radio(PWM, NUM_CHANNELS); +// Set the type of RC radio you are using and the number of input channels you are using +// Radio options are: PWM, PPM, DSM, or sBUS. +// Max number of channels is 8 for PPM/PWM and 16 for SBUS. The default is 6. +// example: RadioComm radio(PPM, 6); -//Uncomment only one receiver type -//#define USE_PWM_RX -//#define USE_PPM_RX -#define USE_SBUS_RX -//#define USE_DSM_RX -static const uint8_t num_DSM_channels = 6; //If using DSM RX, change this to match the number of transmitter channels you have - -// Other defines that depend on the above -#if defined USE_SBUS_RX - #include "src/SBUS/SBUS.h" //sBus interface -#endif - -#if defined USE_DSM_RX - #include "src/DSMRX/DSMRX.h" -#endif +//Note: If using SBUS, connect to pin 21 (RX5), if using DSM, connect to pin 15 (RX3) +// const int ch1Pin = 15; //throttle +// const int ch2Pin = 16; //ail +// const int ch3Pin = 17; //ele +// const int ch4Pin = 20; //rudd +// const int ch5Pin = 21; //gear (throttle cut) +// const int ch6Pin = 22; //aux1 (free aux channel) +// const int PPM_Pin = 23; -#if defined USE_SBUS_RX - SBUS sbus(Serial5); - uint16_t sbusChannels[16]; - bool sbusFailSafe; - bool sbusLostFrame; -#endif -#if defined USE_DSM_RX - DSM1024 DSM; -#endif @@ -224,10 +232,12 @@ void setup() { //Set built in LED to turn on to signal startup digitalWrite(13, HIGH); - delay(5); + delay(500); //Initialize radio communication - radioSetup(); + // radioSetup(); + radio.begin(); + //Set radio channels to default (safe) values before entering main loop channel_1_pwm = channel_1_fs; @@ -238,14 +248,36 @@ void setup() { channel_6_pwm = channel_6_fs; //Initialize IMU communication - if (!myIMU.begin()) { + // Needed for MPU9250_SPI and LSM6DSOX_SPI. Comment this out for MPU6050_I2C + imu.setSPIpins(IMU_CS, IMU_SCK, IMU_MISO, IMU_MOSI); + + if (!imu.begin()) { Serial.println("IMU init failed!"); while (1); } - delay(5); + delay(500); + + // Get IMU error to zero accelerometer and gyro readings, assuming vehicle is level when powered up + // imu.calculate_IMU_error(); //Calibration parameters printed to serial monitor. Paste these in the user specified variables section, then comment this out forever. + + // Replace the following with the values from the calculate_IMU_error() output above: + imu.setAccError(0.0, 0.0, 0.0); + imu.setGyroError(0.0, 0.0, 0.0); + + // LSM6DSOX + // imu.setAccError(-0.02, 0.01, 0.03); + // imu.setGyroError(-0.24, -0.37, -0.42); + + + // If using MPU9250 IMU, uncomment for one-time magnetometer calibration (may need to repeat for new locations) + // imu.calibrateMagnetometer(); //Generates magentometer error and scale factors to be pasted in user-specified variables section + + // If using MPU9250 IMU, replace the below with the output from calibrateMagnetometer() above + // imu.setMagError(0.0, 0.0, 0.0); + // imu.setMagScale(1.0, 1.0, 1.0); + + - //Get IMU error to zero accelerometer and gyro readings, assuming vehicle is level when powered up - //myIMU.calculate_IMU_error(); //Calibration parameters printed to serial monitor. Paste these in the user specified variables section, then comment this out forever. //Arm servo channels servo1.write(0); //Command servo angle from 0-180 degrees (1000 to 2000 PWM) @@ -256,7 +288,7 @@ void setup() { // servo6.write(0); // servo7.write(0); - delay(5); + delay(500); //calibrateESCs(); //PROPS OFF. Uncomment this to calibrate your ESCs by setting throttle stick to max, powering on, and lowering throttle to zero after the beeps //Code will not proceed past here if this function is uncommented! @@ -273,8 +305,6 @@ void setup() { //Indicate entering main loop with 3 quick blinks setupBlink(3,160,70); //numBlinks, upTime (ms), downTime (ms) - //If using MPU9250 IMU, uncomment for one-time magnetometer calibration (may need to repeat for new locations) - //myIMU.calibrateMagnetometer(); //Generates magentometer error and scale factors to be pasted in user-specified variables section } @@ -293,9 +323,9 @@ void loop() { loopBlink(); //Indicate we are in main loop with short blink every 1.5 seconds //Print data at 100hz (uncomment one at a time for troubleshooting) - SELECT ONE: - //printRadioData(); //Prints radio pwm values (expected: 1000 to 2000) + printRadioData(); //Prints radio pwm values (expected: 1000 to 2000) //printDesiredState(); //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle) - printGyroData(); //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest) + //printGyroData(); //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest) //printAccelData(); //Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level) //printMagData(); //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300) //printRollPitchYaw(); //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level) @@ -305,41 +335,42 @@ void loop() { //printLoopRate(); //Prints the time between loops in microseconds (expected: microseconds between loop iterations) // Get arming status - armedStatus(); //Check if the throttle cut is off and throttle is low. + // armedStatus(); //Check if the throttle cut is off and throttle is low. //Get vehicle state - myIMU.getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise - // myIMU.Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU angle estimates (degrees - myIMU.Madgwick(dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU angle estimates (degrees) + imu.getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise + imu.Madgwick(dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU angle estimates (degrees) //Compute desired state - getDesState(); //Convert raw commands to normalized values based on saturated control limits + // getDesState(); //Convert raw commands to normalized values based on saturated control limits //PID Controller - SELECT ONE: - myIMU.controlANGLE(roll_des, pitch_des, yaw_des, dt, channel_1_pwm); //Stabilize on angle setpoint - //myIMU.controlANGLE2(roll_des, pitch_des, yaw_des, dt, channel_1_pwm); //Stabilize on angle setpoint using cascaded method. Rate controller must be tuned well first! - //myIMU.controlRATE(roll_des, pitch_des, yaw_des, dt, channel_1_pwm); //Stabilize on rate setpoint + imu.controlANGLE(roll_des, pitch_des, yaw_des, dt, channel_1_pwm); //Stabilize on angle setpoint + //imu.controlANGLE2(roll_des, pitch_des, yaw_des, dt, channel_1_pwm); //Stabilize on angle setpoint using cascaded method. Rate controller must be tuned well first! + //imu.controlRATE(roll_des, pitch_des, yaw_des, dt, channel_1_pwm); //Stabilize on rate setpoint //Actuator mixing and scaling to PWM values - controlMixer(); //Mixes PID outputs to scaled actuator commands -- custom mixing assignments done here - scaleCommands(); //Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) + // controlMixer(); //Mixes PID outputs to scaled actuator commands -- custom mixing assignments done here + // scaleCommands(); //Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) //Throttle cut check - throttleCut(); //Directly sets motor commands to low based on state of ch5 + // throttleCut(); //Directly sets motor commands to low based on state of ch5 //Command actuators - commandMotors(); //Sends command pulses to each motor pin using OneShot125 protocol - servo1.write(s1_command_PWM); //Writes PWM value to servo object - servo2.write(s2_command_PWM); - servo3.write(s3_command_PWM); - servo4.write(s4_command_PWM); + // commandMotors(); //Sends command pulses to each motor pin using OneShot125 protocol + // servo1.write(s1_command_PWM); //Writes PWM value to servo object + // servo2.write(s2_command_PWM); + // servo3.write(s3_command_PWM); + // servo4.write(s4_command_PWM); // servo5.write(s5_command_PWM); // servo6.write(s6_command_PWM); // servo7.write(s7_command_PWM); //Get vehicle commands for next loop iteration - getCommands(); //Pulls current available radio commands - failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup + // getCommands(); //Pulls current available radio commands + radio.getCommands(channel_pwm); //Pulls current available radio commands + + // failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup //Regulate loop rate loopRate(2000); //Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default @@ -371,10 +402,10 @@ void controlMixer() { */ //Quad mixing - EXAMPLE - m1_command_scaled = thro_des - myIMU.pitch_PID + myIMU.roll_PID + myIMU.yaw_PID; //Front Left - m2_command_scaled = thro_des - myIMU.pitch_PID - myIMU.roll_PID - myIMU.yaw_PID; //Front Right - m3_command_scaled = thro_des + myIMU.pitch_PID - myIMU.roll_PID + myIMU.yaw_PID; //Back Right - m4_command_scaled = thro_des + myIMU.pitch_PID + myIMU.roll_PID - myIMU.yaw_PID; //Back Left + m1_command_scaled = thro_des - imu.pitch_PID + imu.roll_PID + imu.yaw_PID; //Front Left + m2_command_scaled = thro_des - imu.pitch_PID - imu.roll_PID - imu.yaw_PID; //Front Right + m3_command_scaled = thro_des + imu.pitch_PID - imu.roll_PID + imu.yaw_PID; //Back Right + m4_command_scaled = thro_des + imu.pitch_PID + imu.roll_PID - imu.yaw_PID; //Back Left m5_command_scaled = 0; m6_command_scaled = 0; @@ -408,8 +439,8 @@ void calibrateAttitude() { prev_time = current_time; current_time = micros(); dt = (current_time - prev_time)/1000000.0; - myIMU.getIMUdata(); - myIMU.Madgwick(dt); + imu.getIMUdata(); + imu.Madgwick(dt); loopRate(2000); //do not exceed 2000Hz } } @@ -484,64 +515,64 @@ void scaleCommands() { } -void getCommands() { - //DESCRIPTION: Get raw PWM values for every channel from the radio - /* - * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of - * the loop. If using a PWM or PPM receiver, the radio commands are retrieved from a function in the readPWM file separate from this one which - * is running a bunch of interrupts to continuously update the radio readings. If using an SBUS receiver, the alues are pulled from the SBUS library directly. - * The raw radio commands are filtered with a first order low-pass filter to eliminate any really high frequency noise. - */ - - #if defined USE_PPM_RX || defined USE_PWM_RX - channel_1_pwm = getRadioPWM(1); - channel_2_pwm = getRadioPWM(2); - channel_3_pwm = getRadioPWM(3); - channel_4_pwm = getRadioPWM(4); - channel_5_pwm = getRadioPWM(5); - channel_6_pwm = getRadioPWM(6); +// void getCommands() { +// //DESCRIPTION: Get raw PWM values for every channel from the radio +// /* +// * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of +// * the loop. If using a PWM or PPM receiver, the radio commands are retrieved from a function in the readPWM file separate from this one which +// * is running a bunch of interrupts to continuously update the radio readings. If using an SBUS receiver, the alues are pulled from the SBUS library directly. +// * The raw radio commands are filtered with a first order low-pass filter to eliminate any really high frequency noise. +// */ + +// #if defined USE_PPM_RX || defined USE_PWM_RX +// channel_1_pwm = getRadioPWM(1); +// channel_2_pwm = getRadioPWM(2); +// channel_3_pwm = getRadioPWM(3); +// channel_4_pwm = getRadioPWM(4); +// channel_5_pwm = getRadioPWM(5); +// channel_6_pwm = getRadioPWM(6); - #elif defined USE_SBUS_RX - if (sbus.read(&sbusChannels[0], &sbusFailSafe, &sbusLostFrame)) { - //sBus scaling below is for Taranis-Plus and X4R-SB - float scale = 0.615; - float bias = 895.0; - channel_1_pwm = sbusChannels[0] * scale + bias; - channel_2_pwm = sbusChannels[1] * scale + bias; - channel_3_pwm = sbusChannels[2] * scale + bias; - channel_4_pwm = sbusChannels[3] * scale + bias; - channel_5_pwm = sbusChannels[4] * scale + bias; - channel_6_pwm = sbusChannels[5] * scale + bias; - } - - #elif defined USE_DSM_RX - if (DSM.timedOut(micros())) { - //Serial.println("*** DSM RX TIMED OUT ***"); - } - else if (DSM.gotNewFrame()) { - uint16_t values[num_DSM_channels]; - DSM.getChannelValues(values, num_DSM_channels); - - channel_1_pwm = values[0]; - channel_2_pwm = values[1]; - channel_3_pwm = values[2]; - channel_4_pwm = values[3]; - channel_5_pwm = values[4]; - channel_6_pwm = values[5]; - } - #endif +// #elif defined USE_SBUS_RX +// if (sbus.read(&sbusChannels[0], &sbusFailSafe, &sbusLostFrame)) { +// //sBus scaling below is for Taranis-Plus and X4R-SB +// float scale = 0.615; +// float bias = 895.0; +// channel_1_pwm = sbusChannels[0] * scale + bias; +// channel_2_pwm = sbusChannels[1] * scale + bias; +// channel_3_pwm = sbusChannels[2] * scale + bias; +// channel_4_pwm = sbusChannels[3] * scale + bias; +// channel_5_pwm = sbusChannels[4] * scale + bias; +// channel_6_pwm = sbusChannels[5] * scale + bias; +// } + +// #elif defined USE_DSM_RX +// if (DSM.timedOut(micros())) { +// //Serial.println("*** DSM RX TIMED OUT ***"); +// } +// else if (DSM.gotNewFrame()) { +// uint16_t values[num_DSM_channels]; +// DSM.getChannelValues(values, num_DSM_channels); + +// channel_1_pwm = values[0]; +// channel_2_pwm = values[1]; +// channel_3_pwm = values[2]; +// channel_4_pwm = values[3]; +// channel_5_pwm = values[4]; +// channel_6_pwm = values[5]; +// } +// #endif - //Low-pass the critical commands and update previous values - float b = 0.7; //Lower=slower, higher=noiser - channel_1_pwm = (1.0 - b)*channel_1_pwm_prev + b*channel_1_pwm; - channel_2_pwm = (1.0 - b)*channel_2_pwm_prev + b*channel_2_pwm; - channel_3_pwm = (1.0 - b)*channel_3_pwm_prev + b*channel_3_pwm; - channel_4_pwm = (1.0 - b)*channel_4_pwm_prev + b*channel_4_pwm; - channel_1_pwm_prev = channel_1_pwm; - channel_2_pwm_prev = channel_2_pwm; - channel_3_pwm_prev = channel_3_pwm; - channel_4_pwm_prev = channel_4_pwm; -} +// //Low-pass the critical commands and update previous values +// float b = 0.7; //Lower=slower, higher=noiser +// channel_1_pwm = (1.0 - b)*channel_1_pwm_prev + b*channel_1_pwm; +// channel_2_pwm = (1.0 - b)*channel_2_pwm_prev + b*channel_2_pwm; +// channel_3_pwm = (1.0 - b)*channel_3_pwm_prev + b*channel_3_pwm; +// channel_4_pwm = (1.0 - b)*channel_4_pwm_prev + b*channel_4_pwm; +// channel_1_pwm_prev = channel_1_pwm; +// channel_2_pwm_prev = channel_2_pwm; +// channel_3_pwm_prev = channel_3_pwm; +// channel_4_pwm_prev = channel_4_pwm; +// } void failSafe() { //DESCRIPTION: If radio gives garbage values, set all commands to default values @@ -680,11 +711,11 @@ void calibrateESCs() { digitalWrite(13, HIGH); //LED on to indicate we are not in main loop - getCommands(); //Pulls current available radio commands + // getCommands(); //Pulls current available radio commands failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup getDesState(); //Convert raw commands to normalized values based on saturated control limits - myIMU.getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise - myIMU.Madgwick(dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU (degrees) + imu.getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise + imu.Madgwick(dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU (degrees) getDesState(); //Convert raw commands to normalized values based on saturated control limits m1_command_scaled = thro_des; @@ -868,18 +899,13 @@ void printRadioData() { print_counter = micros(); Serial.print(F("LOW:1000.0")); Serial.print(F(" HIGH:2000.0")); - Serial.print(F(" CH1:")); - Serial.print(channel_1_pwm); - Serial.print(F(" CH2:")); - Serial.print(channel_2_pwm); - Serial.print(F(" CH3:")); - Serial.print(channel_3_pwm); - Serial.print(F(" CH4:")); - Serial.print(channel_4_pwm); - Serial.print(F(" CH5:")); - Serial.print(channel_5_pwm); - Serial.print(F(" CH6:")); - Serial.println(channel_6_pwm); + for (int8_t i = 0; i < NUM_CHANNELS; i++) { + Serial.print(F(" CH")); + Serial.print(i+1); + Serial.print(F(":")); + Serial.print(channel_pwm[i]); + } + Serial.println(F("")); } } @@ -905,11 +931,11 @@ void printGyroData() { Serial.print(F("LOW:-300.0")); Serial.print(F(" HIGH:300.0")); Serial.print(F(" GyroX:")); - Serial.print(myIMU.GyroX); + Serial.print(imu.GyroX); Serial.print(F(" GyroY:")); - Serial.print(myIMU.GyroY); + Serial.print(imu.GyroY); Serial.print(F(" GyroZ:")); - Serial.println(myIMU.GyroZ); + Serial.println(imu.GyroZ); } } @@ -919,11 +945,11 @@ void printAccelData() { Serial.print(F("LOW:-2.0")); Serial.print(F(" HIGH:2.0")); Serial.print(F(" AccX:")); - Serial.print(myIMU.AccX); + Serial.print(imu.AccX); Serial.print(F(" AccY:")); - Serial.print(myIMU.AccY); + Serial.print(imu.AccY); Serial.print(F(" AccZ:")); - Serial.println(myIMU.AccZ); + Serial.println(imu.AccZ); } } @@ -933,11 +959,11 @@ void printMagData() { Serial.print(F("LOW:-300.0")); Serial.print(F(" HIGH:300.0")); Serial.print(F(" MagX:")); - Serial.print(myIMU.MagX); + Serial.print(imu.MagX); Serial.print(F(" MagY:")); - Serial.print(myIMU.MagY); + Serial.print(imu.MagY); Serial.print(F(" MagZ:")); - Serial.println(myIMU.MagZ); + Serial.println(imu.MagZ); } } @@ -947,11 +973,11 @@ void printRollPitchYaw() { Serial.print(F("LOW:-180.0")); Serial.print(F(" HIGH:180.0")); Serial.print(F(" roll:")); - Serial.print(myIMU.roll_IMU); + Serial.print(imu.roll_IMU); Serial.print(F(" pitch:")); - Serial.print(myIMU.pitch_IMU); + Serial.print(imu.pitch_IMU); Serial.print(F(" yaw:")); - Serial.println(myIMU.yaw_IMU); + Serial.println(imu.yaw_IMU); } } @@ -961,11 +987,11 @@ void printPIDoutput() { Serial.print(F("LOW:-1.0")); Serial.print(F(" HIGH:1.0")); Serial.print(F(" roll_PID:")); - Serial.print(myIMU.roll_PID); + Serial.print(imu.roll_PID); Serial.print(F(" pitch_PID:")); - Serial.print(myIMU.pitch_PID); + Serial.print(imu.pitch_PID); Serial.print(F(" yaw_PID:")); - Serial.println(myIMU.yaw_PID); + Serial.println(imu.yaw_PID); } } diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/radioComm.ino b/Versions/dRehmFlight_Teensy_BETA_2.0/radioComm.ino index eef7f1ec..c28709cb 100644 --- a/Versions/dRehmFlight_Teensy_BETA_2.0/radioComm.ino +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/radioComm.ino @@ -8,86 +8,86 @@ //This file contains all necessary functions and code used for radio communication to avoid cluttering the main code -unsigned long rising_edge_start_1, rising_edge_start_2, rising_edge_start_3, rising_edge_start_4, rising_edge_start_5, rising_edge_start_6; -unsigned long channel_1_raw, channel_2_raw, channel_3_raw, channel_4_raw, channel_5_raw, channel_6_raw; -int ppm_counter = 0; -unsigned long time_ms = 0; - -void radioSetup() { - //PPM Receiver - #if defined USE_PPM_RX - //Declare interrupt pin - pinMode(PPM_Pin, INPUT_PULLUP); - delay(20); - //Attach interrupt and point to corresponding ISR function - attachInterrupt(digitalPinToInterrupt(PPM_Pin), getPPM, CHANGE); - - //PWM Receiver - #elif defined USE_PWM_RX - //Declare interrupt pins - pinMode(ch1Pin, INPUT_PULLUP); - pinMode(ch2Pin, INPUT_PULLUP); - pinMode(ch3Pin, INPUT_PULLUP); - pinMode(ch4Pin, INPUT_PULLUP); - pinMode(ch5Pin, INPUT_PULLUP); - pinMode(ch6Pin, INPUT_PULLUP); - delay(20); - //Attach interrupt and point to corresponding ISR functions - attachInterrupt(digitalPinToInterrupt(ch1Pin), getCh1, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch2Pin), getCh2, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch3Pin), getCh3, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch4Pin), getCh4, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch5Pin), getCh5, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch6Pin), getCh6, CHANGE); - delay(20); - - //SBUS Recevier - #elif defined USE_SBUS_RX - sbus.begin(); - - //DSM receiver - #elif defined USE_DSM_RX - Serial3.begin(115000); - #else - #error No RX type defined... - #endif -} - -unsigned long getRadioPWM(int ch_num) { - //DESCRIPTION: Get current radio commands from interrupt routines - unsigned long returnPWM = 0; +// unsigned long rising_edge_start_1, rising_edge_start_2, rising_edge_start_3, rising_edge_start_4, rising_edge_start_5, rising_edge_start_6; +// unsigned long channel_1_raw, channel_2_raw, channel_3_raw, channel_4_raw, channel_5_raw, channel_6_raw; +// int ppm_counter = 0; +// unsigned long time_ms = 0; + +// void radioSetup() { +// //PPM Receiver +// #if defined USE_PPM_RX +// //Declare interrupt pin +// pinMode(PPM_Pin, INPUT_PULLUP); +// delay(20); +// //Attach interrupt and point to corresponding ISR function +// attachInterrupt(digitalPinToInterrupt(PPM_Pin), getPPM, CHANGE); + +// //PWM Receiver +// #elif defined USE_PWM_RX +// //Declare interrupt pins +// pinMode(ch1Pin, INPUT_PULLUP); +// pinMode(ch2Pin, INPUT_PULLUP); +// pinMode(ch3Pin, INPUT_PULLUP); +// pinMode(ch4Pin, INPUT_PULLUP); +// pinMode(ch5Pin, INPUT_PULLUP); +// pinMode(ch6Pin, INPUT_PULLUP); +// delay(20); +// //Attach interrupt and point to corresponding ISR functions +// attachInterrupt(digitalPinToInterrupt(ch1Pin), getCh1, CHANGE); +// attachInterrupt(digitalPinToInterrupt(ch2Pin), getCh2, CHANGE); +// attachInterrupt(digitalPinToInterrupt(ch3Pin), getCh3, CHANGE); +// attachInterrupt(digitalPinToInterrupt(ch4Pin), getCh4, CHANGE); +// attachInterrupt(digitalPinToInterrupt(ch5Pin), getCh5, CHANGE); +// attachInterrupt(digitalPinToInterrupt(ch6Pin), getCh6, CHANGE); +// delay(20); + +// //SBUS Recevier +// #elif defined USE_SBUS_RX +// sbus.begin(); + +// //DSM receiver +// #elif defined USE_DSM_RX +// Serial3.begin(115000); +// #else +// #error No RX type defined... +// #endif +// } + +// unsigned long getRadioPWM(int ch_num) { +// //DESCRIPTION: Get current radio commands from interrupt routines +// unsigned long returnPWM = 0; - if (ch_num == 1) { - returnPWM = channel_1_raw; - } - else if (ch_num == 2) { - returnPWM = channel_2_raw; - } - else if (ch_num == 3) { - returnPWM = channel_3_raw; - } - else if (ch_num == 4) { - returnPWM = channel_4_raw; - } - else if (ch_num == 5) { - returnPWM = channel_5_raw; - } - else if (ch_num == 6) { - returnPWM = channel_6_raw; - } +// if (ch_num == 1) { +// returnPWM = channel_1_raw; +// } +// else if (ch_num == 2) { +// returnPWM = channel_2_raw; +// } +// else if (ch_num == 3) { +// returnPWM = channel_3_raw; +// } +// else if (ch_num == 4) { +// returnPWM = channel_4_raw; +// } +// else if (ch_num == 5) { +// returnPWM = channel_5_raw; +// } +// else if (ch_num == 6) { +// returnPWM = channel_6_raw; +// } - return returnPWM; -} +// return returnPWM; +// } //For DSM type receivers -void serialEvent3(void) -{ - #if defined USE_DSM_RX - while (Serial3.available()) { - DSM.handleSerialEvent(Serial3.read(), micros()); - } - #endif -} +// void serialEvent3(void) +// { +// #if defined USE_DSM_RX +// while (Serial3.available()) { +// DSM.handleSerialEvent(Serial3.read(), micros()); +// } +// #endif +// } @@ -97,102 +97,102 @@ void serialEvent3(void) //INTERRUPT SERVICE ROUTINES (for reading PWM and PPM) -void getPPM() { - unsigned long dt_ppm; - int trig = digitalRead(PPM_Pin); - if (trig==1) { //Only care about rising edge - dt_ppm = micros() - time_ms; - time_ms = micros(); +// void getPPM() { +// unsigned long dt_ppm; +// int trig = digitalRead(PPM_Pin); +// if (trig==1) { //Only care about rising edge +// dt_ppm = micros() - time_ms; +// time_ms = micros(); - if (dt_ppm > 5000) { //Waiting for long pulse to indicate a new pulse train has arrived - ppm_counter = 0; - } +// if (dt_ppm > 5000) { //Waiting for long pulse to indicate a new pulse train has arrived +// ppm_counter = 0; +// } - if (ppm_counter == 1) { //First pulse - channel_1_raw = dt_ppm; - } +// if (ppm_counter == 1) { //First pulse +// channel_1_raw = dt_ppm; +// } - if (ppm_counter == 2) { //Second pulse - channel_2_raw = dt_ppm; - } +// if (ppm_counter == 2) { //Second pulse +// channel_2_raw = dt_ppm; +// } - if (ppm_counter == 3) { //Third pulse - channel_3_raw = dt_ppm; - } +// if (ppm_counter == 3) { //Third pulse +// channel_3_raw = dt_ppm; +// } - if (ppm_counter == 4) { //Fourth pulse - channel_4_raw = dt_ppm; - } +// if (ppm_counter == 4) { //Fourth pulse +// channel_4_raw = dt_ppm; +// } - if (ppm_counter == 5) { //Fifth pulse - channel_5_raw = dt_ppm; - } +// if (ppm_counter == 5) { //Fifth pulse +// channel_5_raw = dt_ppm; +// } - if (ppm_counter == 6) { //Sixth pulse - channel_6_raw = dt_ppm; - } +// if (ppm_counter == 6) { //Sixth pulse +// channel_6_raw = dt_ppm; +// } - ppm_counter = ppm_counter + 1; - } -} - -void getCh1() { - int trigger = digitalRead(ch1Pin); - if(trigger == 1) { - rising_edge_start_1 = micros(); - } - else if(trigger == 0) { - channel_1_raw = micros() - rising_edge_start_1; - } -} - -void getCh2() { - int trigger = digitalRead(ch2Pin); - if(trigger == 1) { - rising_edge_start_2 = micros(); - } - else if(trigger == 0) { - channel_2_raw = micros() - rising_edge_start_2; - } -} - -void getCh3() { - int trigger = digitalRead(ch3Pin); - if(trigger == 1) { - rising_edge_start_3 = micros(); - } - else if(trigger == 0) { - channel_3_raw = micros() - rising_edge_start_3; - } -} - -void getCh4() { - int trigger = digitalRead(ch4Pin); - if(trigger == 1) { - rising_edge_start_4 = micros(); - } - else if(trigger == 0) { - channel_4_raw = micros() - rising_edge_start_4; - } -} - -void getCh5() { - int trigger = digitalRead(ch5Pin); - if(trigger == 1) { - rising_edge_start_5 = micros(); - } - else if(trigger == 0) { - channel_5_raw = micros() - rising_edge_start_5; - } -} - -void getCh6() { - int trigger = digitalRead(ch6Pin); - if(trigger == 1) { - rising_edge_start_6 = micros(); - } - else if(trigger == 0) { - channel_6_raw = micros() - rising_edge_start_6; - } -} +// ppm_counter = ppm_counter + 1; +// } +// } + +// void getCh1() { +// int trigger = digitalRead(ch1Pin); +// if(trigger == 1) { +// rising_edge_start_1 = micros(); +// } +// else if(trigger == 0) { +// channel_1_raw = micros() - rising_edge_start_1; +// } +// } + +// void getCh2() { +// int trigger = digitalRead(ch2Pin); +// if(trigger == 1) { +// rising_edge_start_2 = micros(); +// } +// else if(trigger == 0) { +// channel_2_raw = micros() - rising_edge_start_2; +// } +// } + +// void getCh3() { +// int trigger = digitalRead(ch3Pin); +// if(trigger == 1) { +// rising_edge_start_3 = micros(); +// } +// else if(trigger == 0) { +// channel_3_raw = micros() - rising_edge_start_3; +// } +// } + +// void getCh4() { +// int trigger = digitalRead(ch4Pin); +// if(trigger == 1) { +// rising_edge_start_4 = micros(); +// } +// else if(trigger == 0) { +// channel_4_raw = micros() - rising_edge_start_4; +// } +// } + +// void getCh5() { +// int trigger = digitalRead(ch5Pin); +// if(trigger == 1) { +// rising_edge_start_5 = micros(); +// } +// else if(trigger == 0) { +// channel_5_raw = micros() - rising_edge_start_5; +// } +// } + +// void getCh6() { +// int trigger = digitalRead(ch6Pin); +// if(trigger == 1) { +// rising_edge_start_6 = micros(); +// } +// else if(trigger == 0) { +// channel_6_raw = micros() - rising_edge_start_6; +// } +// } diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/keywords.txt b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/keywords.txt deleted file mode 100644 index d77fe459..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/keywords.txt +++ /dev/null @@ -1,34 +0,0 @@ -####################################### -# Syntax Coloring Map ArduinoLog library -####################################### - -####################################### -# Instances (KEYWORD1) -####################################### -#Logging KEYWORD1 -#Log KEYWORD1 -####################################### -# Datatypes (KEYWORD1) -####################################### - -####################################### -# Methods and Functions (KEYWORD2) -####################################### -IMUinit KEYWORD2 -getIMUdata KEYWORD2 -calculate_IMU_error KEYWORD2 - - -####################################### -# Instances (KEYWORD2) -####################################### -#Logging KEYWORD2 -#Log KEYWORD2 - -####################################### -# Constants (LITERAL1) -####################################### -GYRO_SCALE LITERAL1 Constants -GYRO_SCALE_FACTOR LITERAL1 Constants -ACCEL_SCALE LITERAL1 Constants -ACCEL_SCALE_FACTOR LITERAL1 Constants diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/IMU_PID.cpp b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/IMU_Wrapper.cpp similarity index 82% rename from Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/IMU_PID.cpp rename to Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/IMU_Wrapper.cpp index e3f2ebec..4065b35d 100644 --- a/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/IMU_PID.cpp +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/IMU_Wrapper.cpp @@ -1,15 +1,16 @@ -// IMU_PID.cpp +// IMU_Wrapper.cpp +// This is a wrapper for IMU chips supported by dRehmFlight. It consolidates all the IMU handling and PID subrutines into a single library that most people won't need to mess with. -#include "IMU_PID.h" +#include "IMU_Wrapper.h" -IMU::IMU(IMUType type): IMU(type, GYRO_250DPS, ACCEL_2G) {}; +IMU_Wrapper::IMU_Wrapper(IMUType type): IMU_Wrapper(type, GYRO_250DPS, ACCEL_2G) {}; -IMU::IMU(IMUType type, Gyro_DPS gyroDPS, Accel_G accelG) { +IMU_Wrapper::IMU_Wrapper(IMUType type, Gyro_DPS gyroDPS, Accel_G accelG) { _mpu6050 = nullptr; _mpu9250 = nullptr; _lsm6dsox = nullptr; @@ -23,15 +24,12 @@ IMU::IMU(IMUType type, Gyro_DPS gyroDPS, Accel_G accelG) { case GYRO_250DPS: _gyro_scale_factor = 131.0; break; - case GYRO_500DPS: _gyro_scale_factor = 65.5; break; - case GYRO_1000DPS: _gyro_scale_factor = 32.8; break; - case GYRO_2000DPS: _gyro_scale_factor = 16.4; break; @@ -41,15 +39,12 @@ IMU::IMU(IMUType type, Gyro_DPS gyroDPS, Accel_G accelG) { case ACCEL_2G: _accel_scale_factor = 16384.0; break; - case ACCEL_4G: _accel_scale_factor = 8192.0; break; - case ACCEL_8G: _accel_scale_factor = 4096.0; break; - case ACCEL_16G: _accel_scale_factor = 2048.0; break; @@ -66,7 +61,7 @@ IMU::IMU(IMUType type, Gyro_DPS gyroDPS, Accel_G accelG) { -bool IMU::begin() { +bool IMU_Wrapper::begin() { switch (_type) { case MPU6050_I2C: { @@ -168,9 +163,9 @@ bool IMU::begin() { break; } - _mpu9250->setMagCalX(MagErrorX, MagScaleX); - _mpu9250->setMagCalY(MagErrorY, MagScaleY); - _mpu9250->setMagCalZ(MagErrorZ, MagScaleZ); + _mpu9250->setMagCalX(_MagErrorX, _MagScaleX); + _mpu9250->setMagCalY(_MagErrorY, _MagScaleY); + _mpu9250->setMagCalZ(_MagErrorZ, _MagScaleZ); _mpu9250->setSrd(0); //sets gyro and accel read to 1khz, magnetometer read to 100hz return true; } @@ -178,19 +173,7 @@ bool IMU::begin() { case LSM6DSOX_SPI: { _lsm6dsox = new Adafruit_LSM6DSOX(); - - #define LSM_MISO 12 // MISO2/DO--red - #define LSM_MOSI 11 // MOSI2/SDA--black - #define LSM_CS 10 // CS2--green-yellow - #define LSM_SCK 13 // SCK2/SCL--white - - // #define LSM_MISO 34 // MISO2/DO--red - // #define LSM_MOSI 35 // MOSI2/SDA--black - // #define LSM_CS 36 // CS2--green-yellow - // #define LSM_SCK 37 // SCK2(SCL)--white - - - if (!_lsm6dsox->begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) { + if (!_lsm6dsox->begin_SPI(_cs_pin, _sck_pin, _miso_pin, _mosi_pin)) { Serial.println("Failed to find LSM6DSOX chip"); Serial.println("LSM6DSOX initialization unsuccessful"); Serial.println("Check LSM6DSOX wiring or try cycling power"); @@ -241,9 +224,19 @@ bool IMU::begin() { } +// Sets the Class SPI Pins +void IMU_Wrapper::setSPIpins(int8_t cs_pin, int8_t sck_pin, int8_t miso_pin, int8_t mosi_pin) { + _cs_pin = cs_pin; + _sck_pin = sck_pin; + _miso_pin = miso_pin; + _mosi_pin = mosi_pin; +} + + + -void IMU::getIMUdata() { +void IMU_Wrapper::getIMUdata() { //DESCRIPTION: Request full dataset from IMU and LP filter gyro, accelerometer, and magnetometer data /* * Reads accelerometer, gyro, and magnetometer data from IMU as AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, MagZ. @@ -252,6 +245,22 @@ void IMU::getIMUdata() { * off everything past 80Hz, but if your loop rate is not fast enough, the low pass filter will cause a lag in * the readings. The filter parameters B_gyro and B_accel are set to be good for a 2kHz loop rate. Finally, * the constant errors found in calculate_IMU_error() on startup are subtracted from the accelerometer and gyro readings. + * + * Notes from BrianJones: + * IN STANDARD MOUNTING FORM: + * MPU6060 + * Gyro + * Roll RIGHT shows + X activity + * Pitch FORWARD shows + Y activity + * Yaw LEFT shows + Z activity + * Accel + * Roll RIGHT shows +1 G on Y + * Pitch BACKWARD shows +1G on X + * +Z is normal gravity + * + * The LSMDSOX has the X and Y axis swapped, and the Y axis inverted. This is handled below to make it work like the original MPU6050 code from Nick + * I don't have a MPU9250 to test with. I've kept Nick's code unchanged. + * */ int16_t AcX = 0; @@ -303,11 +312,10 @@ void IMU::getIMUdata() { MagZ = MgZ/6.0; } - //Correct the outputs with the calculated error values - AccX = AccX - AccErrorX; - AccY = AccY - AccErrorY; - AccZ = AccZ - AccErrorZ; + AccX = AccX - _AccErrorX; + AccY = AccY - _AccErrorY; + AccZ = AccZ - _AccErrorZ; //LP filter accelerometer data AccX = (1.0 - B_accel)*AccX_prev + B_accel*AccX; AccY = (1.0 - B_accel)*AccY_prev + B_accel*AccY; @@ -317,9 +325,9 @@ void IMU::getIMUdata() { AccZ_prev = AccZ; //Correct the outputs with the calculated error values - GyroX = GyroX - GyroErrorX; - GyroY = GyroY - GyroErrorY; - GyroZ = GyroZ - GyroErrorZ; + GyroX = GyroX - _GyroErrorX; + GyroY = GyroY - _GyroErrorY; + GyroZ = GyroZ - _GyroErrorZ; //LP filter gyro data GyroX = (1.0 - B_gyro)*GyroX_prev + B_gyro*GyroX; GyroY = (1.0 - B_gyro)*GyroY_prev + B_gyro*GyroY; @@ -329,9 +337,9 @@ void IMU::getIMUdata() { GyroZ_prev = GyroZ; //Correct the outputs with the calculated error values - MagX = (MagX - MagErrorX)*MagScaleX; - MagY = (MagY - MagErrorY)*MagScaleY; - MagZ = (MagZ - MagErrorZ)*MagScaleZ; + MagX = (MagX - _MagErrorX)*_MagScaleX; + MagY = (MagY - _MagErrorY)*_MagScaleY; + MagZ = (MagZ - _MagErrorZ)*_MagScaleZ; //LP filter magnetometer data MagX = (1.0 - B_mag)*MagX_prev + B_mag*MagX; MagY = (1.0 - B_mag)*MagY_prev + B_mag*MagY; @@ -344,51 +352,60 @@ void IMU::getIMUdata() { -void IMU::calculate_IMU_error() { +void IMU_Wrapper::calculate_IMU_error() { //DESCRIPTION: Computes IMU accelerometer and gyro error on startup. Note: vehicle should be powered up on flat surface /* * Don't worry too much about what this is doing. The error values it computes are applied to the raw gyro and * accelerometer values AccX, AccY, AccZ, GyroX, GyroY, GyroZ in getIMUdata(). This eliminates drift in the * measurement. */ - #ifndef USE_LSM6DSOX_SPI - int16_t AcX = 0; - int16_t AcY = 0; - int16_t AcZ = 0; - int16_t GyX = 0; - int16_t GyY = 0; - int16_t GyZ = 0; - #endif - AccErrorX = 0.0; - AccErrorY = 0.0; - AccErrorZ = 0.0; - GyroErrorX = 0.0; - GyroErrorY= 0.0; - GyroErrorZ = 0.0; + int16_t AcX = 0; + int16_t AcY = 0; + int16_t AcZ = 0; + int16_t GyX = 0; + int16_t GyY = 0; + int16_t GyZ = 0; + int16_t MgX = 0; + int16_t MgY = 0; + int16_t MgZ = 0; + + _AccErrorX = 0.0; + _AccErrorY = 0.0; + _AccErrorZ = 0.0; + _GyroErrorX = 0.0; + _GyroErrorY= 0.0; + _GyroErrorZ = 0.0; + + Serial.println("Beginning gyro/accel calibration..."); + delay(5000); // Some IMUs (like the LSM6DSOX) take a bit to settle. I get way different readings for about the first 5-10s of power on. //Read IMU values 12000 times int c = 0; while (c < 12000) { - #if defined USE_MPU6050_I2C - mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); - #elif defined USE_MPU9250_SPI - mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); - #elif defined USE_LSM6DSOX_SPI - lsm6dsox.getEvent(&accel, &gyro, &temp); - - // THe LSM6DSOX has the X and Y reversed from the MPU6050 as well has having the X/Y accelerometers reversed and Y gyro reversed - AccX = accel.acceleration.y * MS2G; // Convert m/s to G-factor + if (_type == MPU6050_I2C) { + _mpu6050->getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); + } + else if (_type == MPU9250_SPI) { + _mpu9250->getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); + } + else if (_type == LSM6DSOX_SPI) { + _lsm6dsox->getEvent(&accel, &gyro, &temp); + + // THe LSM6DSOX has the X and Y reversed from the MPU6050 as well has having Y gyro flipped + AccX = accel.acceleration.y * MS2G; // Convert m/s to G-factor AccY = accel.acceleration.x * -MS2G; - AccZ = accel.acceleration.z * MS2G; - GyroX = gyro.gyro.y * RAD2DEG; // convert rad/s back to deg/sec + AccZ = accel.acceleration.z * MS2G; + GyroX = gyro.gyro.y * RAD2DEG; // convert rad/s back to deg/sec GyroY = gyro.gyro.x * -RAD2DEG; - GyroZ = gyro.gyro.z * RAD2DEG; - - #endif - - // The MPU6050 and MPU9250 IMUs have a different scale factor from the LSM6DSOX - #ifndef USE_LSM6DSOX_SPI - //Accelerometer + GyroZ = gyro.gyro.z * RAD2DEG; + } + else { + // should throw an error probably + } + + // The MPU6050 and MPU9250 IMUs have a different scale factor from the LSM6DSOX + if (_type == MPU6050_I2C || _type == MPU9250_SPI) { + //Accelerometer AccX = AcX / _accel_scale_factor; //G's AccY = AcY / _accel_scale_factor; AccZ = AcZ / _accel_scale_factor; @@ -397,53 +414,50 @@ void IMU::calculate_IMU_error() { GyroX = GyX / _gyro_scale_factor; //deg/sec GyroY = GyY / _gyro_scale_factor; GyroZ = GyZ / _gyro_scale_factor; - #endif + } //Sum all readings - AccErrorX = AccErrorX + AccX; - AccErrorY = AccErrorY + AccY; - AccErrorZ = AccErrorZ + AccZ; - GyroErrorX = GyroErrorX + GyroX; - GyroErrorY = GyroErrorY + GyroY; - GyroErrorZ = GyroErrorZ + GyroZ; + _AccErrorX = _AccErrorX + AccX; + _AccErrorY = _AccErrorY + AccY; + _AccErrorZ = _AccErrorZ + AccZ; + _GyroErrorX = _GyroErrorX + GyroX; + _GyroErrorY = _GyroErrorY + GyroY; + _GyroErrorZ = _GyroErrorZ + GyroZ; c++; } + //Divide the sum by 12000 to get the error value - AccErrorX = AccErrorX / c; - AccErrorY = AccErrorY / c; - AccErrorZ = AccErrorZ / c - 1.0; - GyroErrorX = GyroErrorX / c; - GyroErrorY = GyroErrorY / c; - GyroErrorZ = GyroErrorZ / c; - - Serial.print("float AccErrorX = "); - Serial.print(AccErrorX); - Serial.println(";"); - Serial.print("float AccErrorY = "); - Serial.print(AccErrorY); - Serial.println(";"); - Serial.print("float AccErrorZ = "); - Serial.print(AccErrorZ); - Serial.println(";"); - - Serial.print("float GyroErrorX = "); - Serial.print(GyroErrorX); - Serial.println(";"); - Serial.print("float GyroErrorY = "); - Serial.print(GyroErrorY); - Serial.println(";"); - Serial.print("float GyroErrorZ = "); - Serial.print(GyroErrorZ); - Serial.println(";"); - - Serial.println("Paste these values in user specified variables section and comment out calculate_IMU_error() in void setup."); + _AccErrorX = _AccErrorX / c; + _AccErrorY = _AccErrorY / c; + _AccErrorZ = _AccErrorZ / c - 1.0; + _GyroErrorX = _GyroErrorX / c; + _GyroErrorY = _GyroErrorY / c; + _GyroErrorZ = _GyroErrorZ / c; + + Serial.println("Paste these lines in void setup() and comment out imu.calculate_IMU_error()."); + Serial.print("imu.setAccError("); + Serial.print(_AccErrorX); + Serial.print(", "); + Serial.print(_AccErrorY); + Serial.print(", "); + Serial.print(_AccErrorZ); + Serial.println(");"); + + Serial.print("imu.setGyroError("); + Serial.print(_GyroErrorX); + Serial.print(", "); + Serial.print(_GyroErrorY); + Serial.print(", "); + Serial.print(_GyroErrorZ); + Serial.println(");"); + while(1); } -void IMU::calibrateMagnetometer() { - #if defined USE_MPU9250_SPI +void IMU_Wrapper::calibrateMagnetometer() { + if (_type == MPU9250_SPI) { float success; Serial.println("Beginning magnetometer calibration in"); Serial.println("3..."); @@ -454,45 +468,42 @@ void IMU::calibrateMagnetometer() { delay(1000); Serial.println("Rotate the IMU about all axes until complete."); Serial.println(" "); - success = mpu9250.calibrateMag(); + success = _mpu9250->calibrateMag(); if(success) { Serial.println("Calibration Successful!"); - Serial.println("Please comment out the calibrateMagnetometer() function and copy these values into the code:"); - Serial.print("float MagErrorX = "); - Serial.print(mpu9250.getMagBiasX_uT()); - Serial.println(";"); - Serial.print("float MagErrorY = "); - Serial.print(mpu9250.getMagBiasY_uT()); - Serial.println(";"); - Serial.print("float MagErrorZ = "); - Serial.print(mpu9250.getMagBiasZ_uT()); - Serial.println(";"); - Serial.print("float MagScaleX = "); - Serial.print(mpu9250.getMagScaleFactorX()); - Serial.println(";"); - Serial.print("float MagScaleY = "); - Serial.print(mpu9250.getMagScaleFactorY()); - Serial.println(";"); - Serial.print("float MagScaleZ = "); - Serial.print(mpu9250.getMagScaleFactorZ()); - Serial.println(";"); + Serial.println("Please comment out the calibrateMagnetometer() function and copy these lines into the code:"); + Serial.print("myIMU.setMagError("); + Serial.print(_mpu9250->getMagBiasX_uT()); + Serial.print(", "); + Serial.print(_mpu9250->getMagBiasY_uT()); + Serial.print(", "); + Serial.print(_mpu9250->getMagBiasZ_uT()); + Serial.println(");"); + Serial.print("myIMU.setMagScale("); + Serial.print(_mpu9250->getMagScaleFactorX()); + Serial.print(", "); + Serial.print(_mpu9250->getMagScaleFactorY()); + Serial.print(", "); + Serial.print(_mpu9250->getMagScaleFactorZ()); + Serial.println(");"); Serial.println(" "); Serial.println("If you are having trouble with your attitude estimate at a new flying location, repeat this process as needed."); } else { Serial.println("Calibration Unsuccessful. Please reset the board and try again."); + while(1); //Halt code so it won't enter main loop until this function commented out } - + } + else { + Serial.println("Error: MPU9250 not selected. Cannot calibrate non-existent magnetometer."); while(1); //Halt code so it won't enter main loop until this function commented out - #endif - Serial.println("Error: MPU9250 not selected. Cannot calibrate non-existent magnetometer."); - while(1); //Halt code so it won't enter main loop until this function commented out + } } -void IMU::Madgwick(float invSampleFreq) { +void IMU_Wrapper::Madgwick(float invSampleFreq) { float gx = GyroX; float gy = -GyroY; @@ -624,7 +635,7 @@ void IMU::Madgwick(float invSampleFreq) { yaw_IMU = -atan2(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3) *57.29577951; //degrees } -void IMU::Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, float invSampleFreq) { +void IMU_Wrapper::Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, float invSampleFreq) { //DESCRIPTION: Attitude estimation through sensor fusion - 6DOF /* * See description of Madgwick() for more information. This is a 6DOF implimentation for when magnetometer data is not @@ -710,7 +721,7 @@ void IMU::Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float a -void IMU::controlANGLE(float roll_des, float pitch_des, float yaw_des, float dt, unsigned long throttle) { +void IMU_Wrapper::controlANGLE(float roll_des, float pitch_des, float yaw_des, float dt, unsigned long throttle) { //DESCRIPTION: Computes control commands based on state error (angle) /* * Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des computed in @@ -762,7 +773,7 @@ void IMU::controlANGLE(float roll_des, float pitch_des, float yaw_des, float dt, integral_yaw_prev = integral_yaw; } -void IMU::controlANGLE2(float roll_des, float pitch_des, float yaw_des, float dt, unsigned long throttle) { +void IMU_Wrapper::controlANGLE2(float roll_des, float pitch_des, float yaw_des, float dt, unsigned long throttle) { //DESCRIPTION: Computes control commands based on state error (angle) in cascaded scheme /* * Gives better performance than controlANGLE() but requires much more tuning. Not reccommended for first-time setup. @@ -848,7 +859,7 @@ void IMU::controlANGLE2(float roll_des, float pitch_des, float yaw_des, float dt } -void IMU::controlRATE(float roll_des, float pitch_des, float yaw_des, float dt, unsigned long throttle) { +void IMU_Wrapper::controlRATE(float roll_des, float pitch_des, float yaw_des, float dt, unsigned long throttle) { //DESCRIPTION: Computes control commands based on state error (rate) /* * See explanation for controlANGLE(). Everything is the same here except the error is now the desired rate - raw gyro reading. @@ -898,7 +909,7 @@ void IMU::controlRATE(float roll_des, float pitch_des, float yaw_des, float dt, -float IMU::getGyro(char dir) { +float IMU_Wrapper::getGyro(char dir) { if (dir == 'X') { return GyroX; } @@ -911,7 +922,7 @@ float IMU::getGyro(char dir) { return 0.0; } -float IMU::getAccel(char dir) { +float IMU_Wrapper::getAccel(char dir) { if (dir == 'X') { return AccX; } @@ -925,7 +936,7 @@ float IMU::getAccel(char dir) { } -float IMU::getMag(char dir) { +float IMU_Wrapper::getMag(char dir) { if (dir == 'X') { return MagX; } @@ -939,48 +950,61 @@ float IMU::getMag(char dir) { } - void IMU::setMagError() { - + + void IMU_Wrapper::setAccError(float AccErrorX, float AccErrorY, float AccErrorZ) { + _AccErrorX = AccErrorX; + _AccErrorY = AccErrorY; + _AccErrorZ = AccErrorZ; + } + + void IMU_Wrapper::setGyroError(float GyroErrorX, float GyroErrorY, float GyroErrorZ) { + _GyroErrorX = GyroErrorX; + _GyroErrorY = GyroErrorY; + _GyroErrorZ = GyroErrorZ; } - void IMU::setMagScale() { + void IMU_Wrapper::setMagError(float MagErrorX, float MagErrorY, float MagErrorZ) { + _MagErrorX = MagErrorX; + _MagErrorY = MagErrorY; + _MagErrorZ = MagErrorZ; + } + void IMU_Wrapper::setMagScale(float MagScaleX, float MagScaleY, float MagScaleZ) { + _MagScaleX = MagScaleX; + _MagScaleY = MagScaleY; + _MagScaleZ = MagScaleZ; } - void IMU::setAccError() { - } - void IMU::setGryoError() { - } - void IMU::setRollAnglePID(float Kp, float Ki, float Kd) { + void IMU_Wrapper::setRollAnglePID(float Kp, float Ki, float Kd) { Kp_roll_angle = Kp; Ki_roll_angle = Ki; Kd_roll_angle = Kd; } - void IMU::setPitchAnglePID(float Kp, float Ki, float Kd) { + void IMU_Wrapper::setPitchAnglePID(float Kp, float Ki, float Kd) { Kp_pitch_angle = Kp; Ki_pitch_angle = Ki; Kd_pitch_angle = Kd; } - void IMU::setRollRatePID(float Kp, float Ki, float Kd) { + void IMU_Wrapper::setRollRatePID(float Kp, float Ki, float Kd) { Kp_roll_rate = Kp; Ki_roll_rate = Ki; Kd_roll_rate = Kd; } - void IMU::setPitchRatePID(float Kp, float Ki, float Kd) { + void IMU_Wrapper::setPitchRatePID(float Kp, float Ki, float Kd) { Kp_pitch_rate = Kp; Ki_pitch_rate = Ki; Kd_pitch_rate = Kd; } - void IMU::setYawRatePID(float Kp, float Ki, float Kd) { + void IMU_Wrapper::setYawRatePID(float Kp, float Ki, float Kd) { Kp_yaw = Kp; Ki_yaw = Ki; Kd_yaw = Kd; @@ -992,7 +1016,7 @@ float IMU::getMag(char dir) { //HELPER FUNCTIONS -float IMU::invSqrt(float x) { +float IMU_Wrapper::invSqrt(float x) { //Fast inverse sqrt for madgwick filter /* float halfx = 0.5f * x; diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/IMU_PID.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/IMU_Wrapper.h similarity index 81% rename from Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/IMU_PID.h rename to Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/IMU_Wrapper.h index 6c2d3c4a..fe7356b1 100644 --- a/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_PID/IMU_PID.h +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/IMU_Wrapper.h @@ -1,10 +1,12 @@ -// IMU_PID.h +// IMU_Wrapper.h +// This is a wrapper for IMU chips supported by dRehmFlight. It consolidates all the IMU handling and PID subrutines into a single library that most people won't need to mess with. -#ifndef IMU_PID_h -#define IMU_PID_h + +#ifndef IMU_Wrapper_h +#define IMU_Wrapper_h #include #include //I2c communication @@ -46,14 +48,17 @@ enum Accel_G { -class IMU { +class IMU_Wrapper { public: - IMU(IMUType type); - IMU(IMUType type, Gyro_DPS GyroDPS, Accel_G AccelG); + IMU_Wrapper(IMUType type); + IMU_Wrapper(IMUType type, Gyro_DPS GyroDPS, Accel_G AccelG); bool begin(); + void setSPIpins(int8_t cs_pin, int8_t sck_pin, int8_t miso_pin, int8_t mosi_pin); + void getIMUdata(); + void calculate_IMU_error(); void calibrateMagnetometer(); @@ -64,10 +69,11 @@ class IMU { void Madgwick(float invSampleFreq); void Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, float invSampleFreq); - void setMagError(); - void setMagScale(); - void setAccError(); - void setGryoError(); + void setAccError(float AccErrorX, float AccErrorY, float AccErrorZ); + void setGyroError(float GyroErrorX, float GyroErrorY, float GyroErrorZ); + + void setMagError(float MagErrorX, float MagErrorY, float MagErrorZ); + void setMagScale(float MagScaleX, float MagScaleY, float MagScaleZ); void setRollAnglePID(float Kp, float Ki, float Kd); void setPitchAnglePID(float Kp, float Ki, float Kd); @@ -112,6 +118,16 @@ class IMU { sensors_event_t temp; //unused, but could be interesting + // for SPI pins definitions. These are used by MPU9250 and LSMDSOX right now. Defaults are the back pins (SPI2) on the Teensy 4.0 + int8_t _cs_pin = 36; + int8_t _sck_pin = 37; + int8_t _miso_pin = 34; + int8_t _mosi_pin = 35; + + + + + float invSqrt(float x); float AccX_prev, AccY_prev, AccZ_prev; @@ -136,21 +152,23 @@ class IMU { float B_gyro = 0.1; //Gyro LP filter paramter, (MPU6050 default: 0.1. MPU9250 default: 0.17) float B_mag = 1.0; //Magnetometer LP filter parameter + //IMU calibration parameters - calibrate IMU using calculate_IMU_error() in the void setup() to get these values, then comment out calculate_IMU_error() + float _AccErrorX = 0.0; + float _AccErrorY = 0.0; + float _AccErrorZ = 0.0; + float _GyroErrorX = 0.0; + float _GyroErrorY = 0.0; + float _GyroErrorZ = 0.0; + + //Magnetometer calibration parameters - if using MPU9250, uncomment calibrateMagnetometer() in void setup() to get these values, else just ignore these - float MagErrorX = 0.0; - float MagErrorY = 0.0; - float MagErrorZ = 0.0; - float MagScaleX = 1.0; - float MagScaleY = 1.0; - float MagScaleZ = 1.0; + float _MagErrorX = 0.0; + float _MagErrorY = 0.0; + float _MagErrorZ = 0.0; + float _MagScaleX = 1.0; + float _MagScaleY = 1.0; + float _MagScaleZ = 1.0; - //IMU calibration parameters - calibrate IMU using calculate_IMU_error() in the void setup() to get these values, then comment out calculate_IMU_error() - float AccErrorX = -0.01; - float AccErrorY = 0.01; - float AccErrorZ = 0.04; - float GyroErrorX = 0.41; - float GyroErrorY = 0.17; - float GyroErrorZ = -0.67; // PIDs @@ -183,14 +201,4 @@ class IMU { }; - - - - - - - - - - #endif \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/keywords.txt b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/keywords.txt new file mode 100644 index 00000000..cfcbcc35 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/keywords.txt @@ -0,0 +1,59 @@ +####################################### +# Syntax Coloring Map ArduinoLog library +####################################### + +####################################### +# Instances (KEYWORD1) +####################################### +IMU_Wrapper KEYWORD1 + +####################################### +# Datatypes (KEYWORD1) +####################################### + +####################################### +# Methods and Functions (KEYWORD2) +####################################### +begin KEYWORD2 +setSPIpins KEYWORD2 +getIMUdata KEYWORD2 +calculate_IMU_error KEYWORD2 +calibrateMagnetometer KEYWORD2 + +controlANGLE KEYWORD2 +controlANGLE2 KEYWORD2 +controlRATE KEYWORD2 + +Madgwick KEYWORD2 +Madgwick6DOF KEYWORD2 + +setMagError KEYWORD2 +setMagScale KEYWORD2 +setAccError KEYWORD2 +setGyroError KEYWORD2 + +setRollAnglePID KEYWORD2 +setPitchAnglePID KEYWORD2 + +setRollRatePID KEYWORD2 +setPitchRatePID KEYWORD2 +setYawRatePID KEYWORD2 + +getGyro KEYWORD2 +getAccel KEYWORD2 +getMag KEYWORD2 + + +####################################### +# Instances (KEYWORD2) +####################################### +#Logging KEYWORD2 +#Log KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### +GYRO_SCALE LITERAL1 Constants +GYRO_SCALE_FACTOR LITERAL1 Constants +ACCEL_SCALE LITERAL1 Constants +ACCEL_SCALE_FACTOR LITERAL1 Constants diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.cpp b/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.cpp new file mode 100644 index 00000000..2523bdc4 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.cpp @@ -0,0 +1,181 @@ + +// RadioComm.cpp +// This is a wrapper for IMU chips supported by dRehmFlight. It consolidates all the IMU handling and PID subrutines into a single library that most people won't need to mess with. + + + +#include "RadioComm.h" + + + +RadioComm::RadioComm(RadioType type): RadioComm(type, 6) {}; + +RadioComm::RadioComm(RadioType type, uint8_t num_channels) { + _sbus = nullptr; + // _sbus_data = nullptr; + _dsm = nullptr; + _ppm = nullptr; + _pwm = nullptr; + + _type = type; + + _num_channels = num_channels; +} + + + + + + +void RadioComm::begin() { + if (_type == sBUS) { + // _sbus = new bfs::SbusRx(&Serial5, true, false); + // bfs::SbusData _sbus_data; + // _sbus->Begin(); + + _sbus = new SBUS(Serial5); + _sbus->begin(); + + + + + } + + else if (_type == DSM) { + _dsm = new DSM1024(); + Serial3.begin(115000); + + } + + else { + Serial.println("No Serial RX type defined..."); + while(1); + } +} + +void RadioComm::begin(int PPM_Pin) { + + if (_type == PPM) { + // _ppm = new PPM(); + // _ppm->begin(PPM_Pin, false); + // ppm.begin(PPM_Pin, false); + PPMReader _ppm(PPM_Pin, _num_channels); + + + } + + else { + Serial.println("No PPM type defined..."); + while(1); + } +} + + +void RadioComm::begin(int channelPins[]) { + byte i; + + // Assign channel pins into the Class variable. _num_channels MUST be set at invocation time or the default of 6 will be used. + for (i=0; i<_num_channels; i++) { + _pwm_channel_pins[i] = channelPins[i]; + } + + if (_type == PWM) { + RC_Receiver _pwm(_pwm_channel_pins[0], _pwm_channel_pins[1], _pwm_channel_pins[2], _pwm_channel_pins[3], + _pwm_channel_pins[4], _pwm_channel_pins[5], _pwm_channel_pins[6], _pwm_channel_pins[7]); + + + + } + + else { + Serial.println("No PWM type defined..."); + while(1); + } +} + + + + + +void RadioComm::getCommands(unsigned long int* returnArray) { + //DESCRIPTION: Get raw PWM values for every channel from the radio + /* + * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of + * the loop. If using a PWM or PPM receiver, the radio commands are retrieved from a function in the readPWM file separate from this one which + * is running a bunch of interrupts to continuously update the radio readings. If using an SBUS receiver, the alues are pulled from the SBUS library directly. + * The raw radio commands are filtered with a first order low-pass filter to eliminate any really high frequency noise. + */ + + int8_t i; + if (_type == sBUS) { + // _sbus->Read(); + + // /* Grab the received data */ + // _sbus_data = _sbus->data(); + // /* Display the received data */ + // for (i = 0; i < _num_channels; i++) { + // channel_pwm[i] = _sbus_data.ch[i]* _sbus_scale + _sbus_bias; + // } + // _sbusFailSafe = _sbus_data.failsafe; + // _sbusLostFrame = _sbus_data.lost_frame; + + + if (_sbus->read(_sbusChannels, &_sbusFailSafe, &_sbusLostFrame)) { + for (i = 0; i < _num_channels; i++) { + channel_pwm[i] = _sbusChannels[i] * _sbus_scale + _sbus_bias; + } + } + + + + + + + } + + + + + + +// #if defined USE_PPM_RX || defined USE_PWM_RX +// channel_1_pwm = getRadioPWM(1); +// channel_2_pwm = getRadioPWM(2); +// channel_3_pwm = getRadioPWM(3); +// channel_4_pwm = getRadioPWM(4); +// channel_5_pwm = getRadioPWM(5); +// channel_6_pwm = getRadioPWM(6); + +// #elif defined USE_DSM_RX +// if (DSM.timedOut(micros())) { +// //Serial.println("*** DSM RX TIMED OUT ***"); +// } +// else if (DSM.gotNewFrame()) { +// uint16_t values[num_DSM_channels]; +// DSM.getChannelValues(values, num_DSM_channels); + +// channel_1_pwm = values[0]; +// channel_2_pwm = values[1]; +// channel_3_pwm = values[2]; +// channel_4_pwm = values[3]; +// channel_5_pwm = values[4]; +// channel_6_pwm = values[5]; +// } +// #endif + + // Low-pass the critical commands and update previous values + float b = 0.7; //Lower=slower, higher=noiser + + for (i = 0; i < 4; i++) { + channel_pwm[i] = (1.0 - b)*_channel_pwm_prev[i] + b*channel_pwm[i]; + _channel_pwm_prev[i] = channel_pwm[i]; + } + + // return the values to the function + for (i = 0; i < _num_channels; i++) { + returnArray[i] = channel_pwm[i]; + } + +} + + diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.h new file mode 100644 index 00000000..9b1a03fc --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.h @@ -0,0 +1,82 @@ + + +// RadioComm.h +// This is a wrapper for IMU chips supported by dRehmFlight. It consolidates all the IMU handling and PID subrutines into a single library that most people won't need to mess with. + + +#ifndef RadioComm_h +#define RadioComm_h + +#include + +// Include all possible radio libraries +#include "../SBUS/SBUS.h" +// #include "sbus.h" // Bolder Flight Systems SBUS library +#include +#include "../DSMRX/DSMRX.h" +#include "RC_Receiver.h" + + +enum RadioType { + PWM, + PPM, + DSM, + sBUS +}; + + + +class RadioComm { +public: + RadioComm(RadioType type); + RadioComm(RadioType type, uint8_t num_channels); + + + + + void begin(); //DSM & //SBUS + void begin(int PPM_Pin); //PPM + void begin(int channelPins[]); //PWM + + void getCommands(unsigned long int* returnArray); + + + + unsigned long channel_pwm[16] = {0}; + + + + + + +private: + RadioType _type; + + // bfs::SbusRx* _sbus; + // bfs::SbusData _sbus_data; + SBUS* _sbus; + DSMRX* _dsm; + PPMReader* _ppm; + RC_Receiver* _pwm; + + uint8_t _num_channels = 6; + uint16_t _sbusChannels[16] = {0}; + int _pwm_channel_pins[8] = {0}; + unsigned long _channel_pwm_prev[4] = {0}; + + + + bool _sbusFailSafe; + bool _sbusLostFrame; + + //sBus scaling below is for Taranis-Plus and X4R-SB + const float _sbus_scale = 0.615; + const float _sbus_bias = 895.0; + + + + +}; + + +#endif \ No newline at end of file From d86268a8bdff4af3eaad752434177b5cbca846b0 Mon Sep 17 00:00:00 2001 From: Brian Jones Date: Sun, 10 Aug 2025 19:15:46 -0500 Subject: [PATCH 4/6] Added libraries to consolidate actuators. --- .../dRehmFlight_Teensy_BETA_2.0.ino | 784 ++++-------------- .../printFunctions.ino | 139 ++++ .../dRehmFlight_Teensy_BETA_2.0/radioComm.ino | 198 ----- .../src/Actuators/Actuators.cpp | 190 +++++ .../src/Actuators/Actuators.h | 58 ++ .../src/RadioComm/RadioComm.cpp | 148 ++-- .../src/RadioComm/RadioComm.h | 28 +- 7 files changed, 659 insertions(+), 886 deletions(-) create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/printFunctions.ino delete mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/radioComm.ino create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/Actuators/Actuators.cpp create mode 100644 Versions/dRehmFlight_Teensy_BETA_2.0/src/Actuators/Actuators.h diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino b/Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino index 09d58c85..5cbef128 100644 --- a/Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino @@ -31,35 +31,15 @@ Everyone that sends me pictures and videos of your flying creations! -Nick //REQUIRED LIBRARIES (included with download in main sketch folder) // #include "src/dRhemFlight_hardware.h" -#include //Commanding any extra actuators, installed with teensyduino installer #include "src/IMU_Wrapper/IMU_Wrapper.h" #include "src/RadioComm/RadioComm.h" - - - - - - - - - - - - - +#include "src/Actuators/Actuators.h" //========================================================================================================================// // USER-SPECIFIED VARIABLES // //========================================================================================================================// -//Radio failsafe values for every channel in the event that bad reciever data is detected. Recommended defaults: -unsigned long channel_1_fs = 1000; //thro -unsigned long channel_2_fs = 1500; //ail -unsigned long channel_3_fs = 1500; //elev -unsigned long channel_4_fs = 1500; //rudd -unsigned long channel_5_fs = 2000; //gear, greater than 1500 = throttle cut -unsigned long channel_6_fs = 2000; //aux1 //Controller parameters (take note of defaults before modifying!): @@ -68,63 +48,51 @@ float maxPitch = 30.0; //Max pitch angle in degrees for angle mode (maximum ~ float maxYaw = 160.0; //Max yaw rate in deg/sec + //========================================================================================================================// // DECLARE PINS // //========================================================================================================================// -//NOTE: Pin 13 is reserved for onboard LED, pins 18 and 19 are reserved for the MPU6050 IMU for default setup -//Radio: -//Note: If using SBUS, connect to pin 21 (RX5), if using DSM, connect to pin 15 (RX3) -const int ch1Pin = 15; //throttle -const int ch2Pin = 16; //ail -const int ch3Pin = 17; //ele -const int ch4Pin = 20; //rudd -const int ch5Pin = 21; //gear (throttle cut) -const int ch6Pin = 22; //aux1 (free aux channel) -const int PPM_Pin = 23; - - - - -//OneShot125 ESC pin outputs: -const int m1Pin = 0; -const int m2Pin = 1; -const int m3Pin = 2; -const int m4Pin = 3; -const int m5Pin = 4; -const int m6Pin = 5; -//PWM servo or ESC outputs: -const int servo1Pin = 6; -const int servo2Pin = 7; -const int servo3Pin = 8; -const int servo4Pin = 9; -// const int servo5Pin = 10; -// const int servo6Pin = 11; -// const int servo7Pin = 12; -PWMServo servo1; //Create servo objects to control a servo or ESC with PWM -PWMServo servo2; -PWMServo servo3; -PWMServo servo4; -// PWMServo servo5; -// PWMServo servo6; -// PWMServo servo7; +//NOTE: Pin 13 is reserved for onboard LED +//OneShot125 ESC pin outputs. Set the number of Servos you have and update the pin outputs: +const uint8_t numMotors = 6; +uint8_t motorPins[numMotors] = {0, 1, 2, 3, 4, 5}; +// Mixer commands from 0.0 to 1.0 (will be scaled in the library) +float m_command_scaled[numMotors] = {0.0}; // Initialize in idle +Actuators Motors(ONESHOT125, numMotors, motorPins); -//========================================================================================================================// -// declare IMU and pins +//PWM servo outputs. Set the number of Servos you have and update the pin outputs: +const uint8_t numServos = 2; +uint8_t servoPins[numServos] = {6, 7}; +// Mixer commands from 0.0 to 1.0 (will be scaled in the library) +float s_command_scaled[numServos] = {0.5}; // Default to centered. This can be customized. +Actuators Servos(SERVO, numServos, servoPins); + + +//PWM ESC outputs. Set the number of ESCs you have and update the pin outputs: +const uint8_t numESCs = 2; +uint8_t escPins[numESCs] = {8, 9}; +// Mixer commands from 0.0 to 1.0 (will be scaled in the library) +float e_command_scaled[numESCs] = {0.0}; // Initialize in idle +Actuators Escs(ESC, numESCs, escPins); -IMU_Wrapper imu(LSM6DSOX_SPI); + + +//========================================================================================================================// +// declare IMU and pins +IMU_Wrapper imu(MPU6050_I2C); // values are MPU6050_I2C (Default), MPU9250_SPI, LSM6DSOX_SPI // Defaults to GYRO_250DPS and ACCEL_2G. You can change the sensitivity by altering these in the call above. // example: IMU imu(LSM6DSOX_SPI, GYRO_1000DPS, ACCEL_8G); // Needed for MPU9250_SPI and LSM6DSOX_SPI. Comment these out for MPU6050_I2C -#define IMU_CS 10 // CS2--green-yellow -#define IMU_SCK 13 // SCK2/SCL--white -#define IMU_MISO 12 // MISO2/DO--red -#define IMU_MOSI 11 // MOSI2/SDA--black +// #define IMU_CS 10 // CS2--green-yellow +// #define IMU_SCK 13 // SCK2/SCL--white +// #define IMU_MISO 12 // MISO2/DO--red +// #define IMU_MOSI 11 // MOSI2/SDA--black /* IMU Pin locations for IMU: @@ -146,8 +114,49 @@ defaults for Teensy 4.0: MISO -> pin 34 MOSI -> pin 35 */ +//========================================================================================================================// + + + +//========================================================================================================================// +// Radio communication: +// How many input channels do you have? +// Max number of channels is 8 for PPM and 16 for SBUS. +#define NUM_CHANNELS 8 + +// Holder array for radio PWM data +unsigned int channel_pwm[NUM_CHANNELS]; + +// Note: If using SBUS, connect to pin 21 (RX5), if using DSM, connect to pin 15 (RX3) +// For PPM: +// const int PPM_Pin = 23; + +// Define the radio object +// Set the type of RC radio you are using +// Options options are: PPM, DSM, or sBUS. +RadioComm radio(sBUS, NUM_CHANNELS); + +//Radio failsafe values for every channel in the event that bad reciever data is detected. Recommended defaults: +unsigned int channel_fs[NUM_CHANNELS] = {1000, 1500, 1500, 1500, 2000, 1000, 1000, 1000}; +// thr, ail, ele, rud, arm, aux1, aux2, aux3 +//========================================================================================================================// + + + +// DECLARE PRINTING FUNCTIONS (IN printFunctions.ino) +void printRadioData(); //Prints radio pwm values (expected: 1000 to 2000) +void printDesiredState(); //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle) +void printGyroData(); //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest) +void printAccelData(); //Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level) +void printMagData(); //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300) +void printRollPitchYaw(); //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level) +void printPIDoutput(); //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1) +void printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250) +void printServoCommands(); //Prints the values being written to the servos (expected: 0 to 180) +void printLoopRate(); //Prints the time between loops in microseconds (expected: microseconds between loop iterations) + //DECLARE GLOBAL VARIABLES @@ -159,51 +168,21 @@ unsigned long print_counter, serial_counter; unsigned long blink_counter, blink_delay; bool blinkAlternate; -//Radio communication: -unsigned long channel_1_pwm, channel_2_pwm, channel_3_pwm, channel_4_pwm, channel_5_pwm, channel_6_pwm; -unsigned long channel_1_pwm_prev, channel_2_pwm_prev, channel_3_pwm_prev, channel_4_pwm_prev; +//Flight status +bool armedFly = false; -// How many input channels do you have? -#define NUM_CHANNELS 12 -// Holder array for radio PWM data -unsigned long channel_pwm[NUM_CHANNELS]; - -// Define the radio object -RadioComm radio(sBUS, NUM_CHANNELS); -// RadioComm radio(DSM, NUM_CHANNELS); -// RadioComm radio(PPM, NUM_CHANNELS); -// RadioComm radio(PWM, NUM_CHANNELS); -// Set the type of RC radio you are using and the number of input channels you are using -// Radio options are: PWM, PPM, DSM, or sBUS. -// Max number of channels is 8 for PPM/PWM and 16 for SBUS. The default is 6. -// example: RadioComm radio(PPM, 6); - -//Note: If using SBUS, connect to pin 21 (RX5), if using DSM, connect to pin 15 (RX3) -// const int ch1Pin = 15; //throttle -// const int ch2Pin = 16; //ail -// const int ch3Pin = 17; //ele -// const int ch4Pin = 20; //rudd -// const int ch5Pin = 21; //gear (throttle cut) -// const int ch6Pin = 22; //aux1 (free aux channel) -// const int PPM_Pin = 23; //Normalized desired state: +// NOTE: Additional controls, such as collective pitch, tilt servos, etc. can be added here. float thro_des, roll_des, pitch_des, yaw_des; float roll_passthru, pitch_passthru, yaw_passthru; -//Mixer -float m1_command_scaled, m2_command_scaled, m3_command_scaled, m4_command_scaled, m5_command_scaled, m6_command_scaled; -int m1_command_PWM, m2_command_PWM, m3_command_PWM, m4_command_PWM, m5_command_PWM, m6_command_PWM; -float s1_command_scaled, s2_command_scaled, s3_command_scaled, s4_command_scaled, s5_command_scaled, s6_command_scaled, s7_command_scaled; -int s1_command_PWM, s2_command_PWM, s3_command_PWM, s4_command_PWM, s5_command_PWM, s6_command_PWM, s7_command_PWM; -//Flight status -bool armedFly = false; //========================================================================================================================// // VOID SETUP // @@ -215,41 +194,32 @@ void setup() { //Initialize all pins pinMode(13, OUTPUT); //Pin 13 LED blinker on board, do not modify - pinMode(m1Pin, OUTPUT); - pinMode(m2Pin, OUTPUT); - pinMode(m3Pin, OUTPUT); - pinMode(m4Pin, OUTPUT); - pinMode(m5Pin, OUTPUT); - pinMode(m6Pin, OUTPUT); - servo1.attach(servo1Pin, 900, 2100); //Pin, min PWM value, max PWM value - servo2.attach(servo2Pin, 900, 2100); - servo3.attach(servo3Pin, 900, 2100); - servo4.attach(servo4Pin, 900, 2100); - // servo5.attach(servo5Pin, 900, 2100); - // servo6.attach(servo6Pin, 900, 2100); - // servo7.attach(servo7Pin, 900, 2100); + + //Initialize and arm servo channels + Motors.begin(); + Servos.begin(); + Escs.begin(); //Set built in LED to turn on to signal startup digitalWrite(13, HIGH); delay(500); + + //========================================================================================================================// //Initialize radio communication - // radioSetup(); - radio.begin(); + radio.begin(); //sBUS or DSM + // radio.begin(PPM_Pin); //PPM - //Set radio channels to default (safe) values before entering main loop - channel_1_pwm = channel_1_fs; - channel_2_pwm = channel_2_fs; - channel_3_pwm = channel_3_fs; - channel_4_pwm = channel_4_fs; - channel_5_pwm = channel_5_fs; - channel_6_pwm = channel_6_fs; + radio.setFailsafe(channel_fs); + //========================================================================================================================// + + //========================================================================================================================// //Initialize IMU communication // Needed for MPU9250_SPI and LSM6DSOX_SPI. Comment this out for MPU6050_I2C - imu.setSPIpins(IMU_CS, IMU_SCK, IMU_MISO, IMU_MOSI); + // imu.setSPIpins(IMU_CS, IMU_SCK, IMU_MISO, IMU_MOSI); if (!imu.begin()) { Serial.println("IMU init failed!"); @@ -263,11 +233,6 @@ void setup() { // Replace the following with the values from the calculate_IMU_error() output above: imu.setAccError(0.0, 0.0, 0.0); imu.setGyroError(0.0, 0.0, 0.0); - - // LSM6DSOX - // imu.setAccError(-0.02, 0.01, 0.03); - // imu.setGyroError(-0.24, -0.37, -0.42); - // If using MPU9250 IMU, uncomment for one-time magnetometer calibration (may need to repeat for new locations) // imu.calibrateMagnetometer(); //Generates magentometer error and scale factors to be pasted in user-specified variables section @@ -275,37 +240,16 @@ void setup() { // If using MPU9250 IMU, replace the below with the output from calibrateMagnetometer() above // imu.setMagError(0.0, 0.0, 0.0); // imu.setMagScale(1.0, 1.0, 1.0); + //========================================================================================================================// - - - - //Arm servo channels - servo1.write(0); //Command servo angle from 0-180 degrees (1000 to 2000 PWM) - servo2.write(0); //Set these to 90 for servos if you do not want them to briefly max out on startup - servo3.write(0); //Keep these at 0 if you are using servo outputs for motors - servo4.write(0); - // servo5.write(0); - // servo6.write(0); - // servo7.write(0); - delay(500); //calibrateESCs(); //PROPS OFF. Uncomment this to calibrate your ESCs by setting throttle stick to max, powering on, and lowering throttle to zero after the beeps //Code will not proceed past here if this function is uncommented! - //Arm OneShot125 motors - m1_command_PWM = 125; //Command OneShot125 ESC from 125 to 250us pulse length - m2_command_PWM = 125; - m3_command_PWM = 125; - m4_command_PWM = 125; - m5_command_PWM = 125; - m6_command_PWM = 125; - armMotors(); //Loop over commandMotors() until ESCs happily arm - + //Indicate entering main loop with 3 quick blinks setupBlink(3,160,70); //numBlinks, upTime (ms), downTime (ms) - - } @@ -323,54 +267,48 @@ void loop() { loopBlink(); //Indicate we are in main loop with short blink every 1.5 seconds //Print data at 100hz (uncomment one at a time for troubleshooting) - SELECT ONE: - printRadioData(); //Prints radio pwm values (expected: 1000 to 2000) + //printRadioData(); //Prints radio pwm values (expected: 1000 to 2000) //printDesiredState(); //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle) //printGyroData(); //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest) //printAccelData(); //Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level) //printMagData(); //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300) //printRollPitchYaw(); //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level) //printPIDoutput(); //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1) - //printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250) + printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250) //printServoCommands(); //Prints the values being written to the servos (expected: 0 to 180) //printLoopRate(); //Prints the time between loops in microseconds (expected: microseconds between loop iterations) // Get arming status - // armedStatus(); //Check if the throttle cut is off and throttle is low. + armedStatus(); //Check if the throttle cut is off and throttle is low. //Get vehicle state imu.getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise imu.Madgwick(dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU angle estimates (degrees) //Compute desired state - // getDesState(); //Convert raw commands to normalized values based on saturated control limits + getDesState(); //Convert raw commands to normalized values based on saturated control limits //PID Controller - SELECT ONE: - imu.controlANGLE(roll_des, pitch_des, yaw_des, dt, channel_1_pwm); //Stabilize on angle setpoint - //imu.controlANGLE2(roll_des, pitch_des, yaw_des, dt, channel_1_pwm); //Stabilize on angle setpoint using cascaded method. Rate controller must be tuned well first! - //imu.controlRATE(roll_des, pitch_des, yaw_des, dt, channel_1_pwm); //Stabilize on rate setpoint + imu.controlANGLE(roll_des, pitch_des, yaw_des, dt, channel_pwm[0]); //Stabilize on angle setpoint + //imu.controlANGLE2(roll_des, pitch_des, yaw_des, dt, channel_pwm[0]); //Stabilize on angle setpoint using cascaded method. Rate controller must be tuned well first! + //imu.controlRATE(roll_des, pitch_des, yaw_des, dt, channel_pwm[0]); //Stabilize on rate setpoint //Actuator mixing and scaling to PWM values - // controlMixer(); //Mixes PID outputs to scaled actuator commands -- custom mixing assignments done here - // scaleCommands(); //Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) + controlMixer(); //Mixes PID outputs to scaled actuator commands -- custom mixing assignments done here //Throttle cut check - // throttleCut(); //Directly sets motor commands to low based on state of ch5 - - //Command actuators - // commandMotors(); //Sends command pulses to each motor pin using OneShot125 protocol - // servo1.write(s1_command_PWM); //Writes PWM value to servo object - // servo2.write(s2_command_PWM); - // servo3.write(s3_command_PWM); - // servo4.write(s4_command_PWM); - // servo5.write(s5_command_PWM); - // servo6.write(s6_command_PWM); - // servo7.write(s7_command_PWM); - + throttleCut(); //Directly sets motor commands to low based on state of ch5 + + // Command actuators + // Write outputs to all servos, ESCs, and motors + Servos.commandAll(s_command_scaled); + Escs.commandAll(e_command_scaled); + Motors.commandMotors(m_command_scaled); + + //Get vehicle commands for next loop iteration - // getCommands(); //Pulls current available radio commands radio.getCommands(channel_pwm); //Pulls current available radio commands - - // failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup + radio.failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup //Regulate loop rate loopRate(2000); //Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default @@ -402,27 +340,38 @@ void controlMixer() { */ //Quad mixing - EXAMPLE - m1_command_scaled = thro_des - imu.pitch_PID + imu.roll_PID + imu.yaw_PID; //Front Left - m2_command_scaled = thro_des - imu.pitch_PID - imu.roll_PID - imu.yaw_PID; //Front Right - m3_command_scaled = thro_des + imu.pitch_PID - imu.roll_PID + imu.yaw_PID; //Back Right - m4_command_scaled = thro_des + imu.pitch_PID + imu.roll_PID - imu.yaw_PID; //Back Left - m5_command_scaled = 0; - m6_command_scaled = 0; + m_command_scaled[0] = thro_des - imu.pitch_PID + imu.roll_PID + imu.yaw_PID; //Front Left + m_command_scaled[1] = thro_des - imu.pitch_PID - imu.roll_PID - imu.yaw_PID; //Front Right + m_command_scaled[2] = thro_des + imu.pitch_PID - imu.roll_PID + imu.yaw_PID; //Back Right + m_command_scaled[3] = thro_des + imu.pitch_PID + imu.roll_PID - imu.yaw_PID; //Back Left + m_command_scaled[4] = 0; + m_command_scaled[5] = 0; + + s_command_scaled[0] = thro_des - imu.pitch_PID + imu.roll_PID + imu.yaw_PID; //Front Left + s_command_scaled[1] = thro_des - imu.pitch_PID + imu.roll_PID + imu.yaw_PID; //Front Left + + e_command_scaled[0] = thro_des; + e_command_scaled[1] = thro_des; + + //0.5 is centered servo, 0.0 is zero throttle if connecting to ESC for conventional PWM, 1.0 is max throttle - s1_command_scaled = 0; - s2_command_scaled = 0; - s3_command_scaled = 0; - s4_command_scaled = 0; - s5_command_scaled = 0; - s6_command_scaled = 0; - s7_command_scaled = 0; + // for (int i=0; i maxVal || channel_1_pwm < minVal) check1 = 1; - if (channel_2_pwm > maxVal || channel_2_pwm < minVal) check2 = 1; - if (channel_3_pwm > maxVal || channel_3_pwm < minVal) check3 = 1; - if (channel_4_pwm > maxVal || channel_4_pwm < minVal) check4 = 1; - if (channel_5_pwm > maxVal || channel_5_pwm < minVal) check5 = 1; - if (channel_6_pwm > maxVal || channel_6_pwm < minVal) check6 = 1; - #endif - - //If any failures, set to default failsafe values - #ifdef USE_SBUS_RX - if (sbusFailSafe) { - #else - if ((check1 + check2 + check3 + check4 + check5 + check6) > 0) { - #endif - channel_1_pwm = channel_1_fs; - channel_2_pwm = channel_2_fs; - channel_3_pwm = channel_3_fs; - channel_4_pwm = channel_4_fs; - channel_5_pwm = channel_5_fs; - channel_6_pwm = channel_6_fs; - } -} - -void commandMotors() { - //DESCRIPTION: Send pulses to motor pins, oneshot125 protocol - /* - * My crude implimentation of OneShot125 protocol which sends 125 - 250us pulses to the ESCs (mXPin). The pulselengths being - * sent are mX_command_PWM, computed in scaleCommands(). This may be replaced by something more efficient in the future. - */ - int wentLow = 0; - int pulseStart, timer; - int flagM1 = 0; - int flagM2 = 0; - int flagM3 = 0; - int flagM4 = 0; - int flagM5 = 0; - int flagM6 = 0; - - //Write all motor pins high - digitalWrite(m1Pin, HIGH); - digitalWrite(m2Pin, HIGH); - digitalWrite(m3Pin, HIGH); - digitalWrite(m4Pin, HIGH); - digitalWrite(m5Pin, HIGH); - digitalWrite(m6Pin, HIGH); - pulseStart = micros(); - - //Write each motor pin low as correct pulse length is reached - while (wentLow < 6 ) { //Keep going until final (6th) pulse is finished, then done - timer = micros(); - if ((m1_command_PWM <= timer - pulseStart) && (flagM1==0)) { - digitalWrite(m1Pin, LOW); - wentLow = wentLow + 1; - flagM1 = 1; - } - if ((m2_command_PWM <= timer - pulseStart) && (flagM2==0)) { - digitalWrite(m2Pin, LOW); - wentLow = wentLow + 1; - flagM2 = 1; - } - if ((m3_command_PWM <= timer - pulseStart) && (flagM3==0)) { - digitalWrite(m3Pin, LOW); - wentLow = wentLow + 1; - flagM3 = 1; - } - if ((m4_command_PWM <= timer - pulseStart) && (flagM4==0)) { - digitalWrite(m4Pin, LOW); - wentLow = wentLow + 1; - flagM4 = 1; - } - if ((m5_command_PWM <= timer - pulseStart) && (flagM5==0)) { - digitalWrite(m5Pin, LOW); - wentLow = wentLow + 1; - flagM5 = 1; - } - if ((m6_command_PWM <= timer - pulseStart) && (flagM6==0)) { - digitalWrite(m6Pin, LOW); - wentLow = wentLow + 1; - flagM6 = 1; - } - } -} - -void armMotors() { - //DESCRIPTION: Sends many command pulses to the motors, to be used to arm motors in the void setup() - /* - * Loops over the commandMotors() function 50 times with a delay in between, simulating how the commandMotors() - * function is used in the main loop. Ensures motors arm within the void setup() where there are some delays - * for other processes that sometimes prevent motors from arming. - */ - for (int i = 0; i <= 50; i++) { - commandMotors(); - delay(2); - } -} - void calibrateESCs() { //DESCRIPTION: Used in void setup() to allow standard ESC calibration procedure with the radio to take place. /* @@ -712,44 +440,47 @@ void calibrateESCs() { digitalWrite(13, HIGH); //LED on to indicate we are not in main loop // getCommands(); //Pulls current available radio commands - failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup + radio.failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup getDesState(); //Convert raw commands to normalized values based on saturated control limits imu.getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise imu.Madgwick(dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU (degrees) getDesState(); //Convert raw commands to normalized values based on saturated control limits - m1_command_scaled = thro_des; - m2_command_scaled = thro_des; - m3_command_scaled = thro_des; - m4_command_scaled = thro_des; - m5_command_scaled = thro_des; - m6_command_scaled = thro_des; - s1_command_scaled = thro_des; - s2_command_scaled = thro_des; - s3_command_scaled = thro_des; - s4_command_scaled = thro_des; - s5_command_scaled = thro_des; - s6_command_scaled = thro_des; - s7_command_scaled = thro_des; - scaleCommands(); //Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) + // m1_command_scaled = thro_des; + // m2_command_scaled = thro_des; + // m3_command_scaled = thro_des; + // m4_command_scaled = thro_des; + // m5_command_scaled = thro_des; + // m6_command_scaled = thro_des; + // s1_command_scaled = thro_des; + // s2_command_scaled = thro_des; + // s3_command_scaled = thro_des; + // s4_command_scaled = thro_des; + // s5_command_scaled = thro_des; + // s6_command_scaled = thro_des; + // s7_command_scaled = thro_des; + // scaleCommands(); //Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) //throttleCut(); //Directly sets motor commands to low based on state of ch5 + + Servos.commandAll(s_command_scaled); + Escs.commandAll(e_command_scaled); + Motors.commandMotors(m_command_scaled); - servo1.write(s1_command_PWM); - servo2.write(s2_command_PWM); - servo3.write(s3_command_PWM); - servo4.write(s4_command_PWM); - // servo5.write(s5_command_PWM); - // servo6.write(s6_command_PWM); - // servo7.write(s7_command_PWM); - commandMotors(); //Sends command pulses to each motor pin using OneShot125 protocol + + // commandMotors(); //Sends command pulses to each motor pin using OneShot125 protocol - //printRadioData(); //Radio pwm values (expected: 1000 to 2000) + // printRadioData(); //Radio pwm values (expected: 1000 to 2000) loopRate(2000); //Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default } } + + + + + float floatFaderLinear(float param, float param_min, float param_max, float fadeTime, int state, int loopFreq){ //DESCRIPTION: Linearly fades a float type variable between min and max bounds based on desired high or low state and time /* @@ -826,23 +557,21 @@ void throttleCut() { channel_5_pwm is LOW then throttle cut is OFF and throttle value can change. (ThrottleCut is DEACTIVATED) channel_5_pwm is HIGH then throttle cut is ON and throttle value = 120 only. (ThrottleCut is ACTIVATED), (drone is DISARMED) */ - if ((channel_5_pwm > 1500) || (armedFly == false)) { + if ((channel_pwm[4] > 1500) || (armedFly == false)) { + if (armedFly == true) Serial.println(F("DISARMED")); + armedFly = false; - m1_command_PWM = 120; - m2_command_PWM = 120; - m3_command_PWM = 120; - m4_command_PWM = 120; - m5_command_PWM = 120; - m6_command_PWM = 120; + + for (int i=0; i 10000) { - print_counter = micros(); - Serial.print(F("LOW:1000.0")); - Serial.print(F(" HIGH:2000.0")); - for (int8_t i = 0; i < NUM_CHANNELS; i++) { - Serial.print(F(" CH")); - Serial.print(i+1); - Serial.print(F(":")); - Serial.print(channel_pwm[i]); - } - Serial.println(F("")); - } -} - -void printDesiredState() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("LOW:-180.0")); - Serial.print(F(" HIGH:180.0")); - Serial.print(F(" thro_des:")); - Serial.print(thro_des); - Serial.print(F(" roll_des:")); - Serial.print(roll_des); - Serial.print(F(" pitch_des:")); - Serial.print(pitch_des); - Serial.print(F(" yaw_des:")); - Serial.println(yaw_des); - } -} - -void printGyroData() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("LOW:-300.0")); - Serial.print(F(" HIGH:300.0")); - Serial.print(F(" GyroX:")); - Serial.print(imu.GyroX); - Serial.print(F(" GyroY:")); - Serial.print(imu.GyroY); - Serial.print(F(" GyroZ:")); - Serial.println(imu.GyroZ); - } -} - -void printAccelData() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("LOW:-2.0")); - Serial.print(F(" HIGH:2.0")); - Serial.print(F(" AccX:")); - Serial.print(imu.AccX); - Serial.print(F(" AccY:")); - Serial.print(imu.AccY); - Serial.print(F(" AccZ:")); - Serial.println(imu.AccZ); - } -} - -void printMagData() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("LOW:-300.0")); - Serial.print(F(" HIGH:300.0")); - Serial.print(F(" MagX:")); - Serial.print(imu.MagX); - Serial.print(F(" MagY:")); - Serial.print(imu.MagY); - Serial.print(F(" MagZ:")); - Serial.println(imu.MagZ); - } -} - -void printRollPitchYaw() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("LOW:-180.0")); - Serial.print(F(" HIGH:180.0")); - Serial.print(F(" roll:")); - Serial.print(imu.roll_IMU); - Serial.print(F(" pitch:")); - Serial.print(imu.pitch_IMU); - Serial.print(F(" yaw:")); - Serial.println(imu.yaw_IMU); - } -} - -void printPIDoutput() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("LOW:-1.0")); - Serial.print(F(" HIGH:1.0")); - Serial.print(F(" roll_PID:")); - Serial.print(imu.roll_PID); - Serial.print(F(" pitch_PID:")); - Serial.print(imu.pitch_PID); - Serial.print(F(" yaw_PID:")); - Serial.println(imu.yaw_PID); - } -} - -void printMotorCommands() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("LOW:125.0")); - Serial.print(F(" HIGH:250.0")); - Serial.print(F(" m1_command:")); - Serial.print(m1_command_PWM); - Serial.print(F(" m2_command:")); - Serial.print(m2_command_PWM); - Serial.print(F(" m3_command:")); - Serial.print(m3_command_PWM); - Serial.print(F(" m4_command:")); - Serial.print(m4_command_PWM); - Serial.print(F(" m5_command:")); - Serial.print(m5_command_PWM); - Serial.print(F(" m6_command:")); - Serial.println(m6_command_PWM); - } -} - -void printServoCommands() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("LOW:0.0")); - Serial.print(F(" HIGH:180.0")); - Serial.print(F(" s1_command:")); - Serial.print(s1_command_PWM); - Serial.print(F(" s2_command:")); - Serial.print(s2_command_PWM); - Serial.print(F(" s3_command:")); - Serial.print(s3_command_PWM); - Serial.print(F(" s4_command:")); - Serial.print(s4_command_PWM); - Serial.print(F(" s5_command:")); - Serial.print(s5_command_PWM); - Serial.print(F(" s6_command:")); - Serial.print(s6_command_PWM); - Serial.print(F(" s7_command:")); - Serial.println(s7_command_PWM); - } -} - -void printLoopRate() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("dt:")); - Serial.println(dt*1000000.0); - } -} - diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/printFunctions.ino b/Versions/dRehmFlight_Teensy_BETA_2.0/printFunctions.ino new file mode 100644 index 00000000..3a3916b0 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/printFunctions.ino @@ -0,0 +1,139 @@ +void printRadioData() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:1000.0")); + Serial.print(F(" HIGH:2000.0")); + for (int8_t i = 0; i < NUM_CHANNELS; i++) { + Serial.print(F(" CH")); + Serial.print(i+1); + Serial.print(F(":")); + Serial.print(channel_pwm[i]); + } + Serial.println(F("")); + } +} + +void printDesiredState() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-180.0")); + Serial.print(F(" HIGH:180.0")); + Serial.print(F(" thro_des:")); + Serial.print(thro_des); + Serial.print(F(" roll_des:")); + Serial.print(roll_des); + Serial.print(F(" pitch_des:")); + Serial.print(pitch_des); + Serial.print(F(" yaw_des:")); + Serial.println(yaw_des); + } +} + +void printGyroData() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-300.0")); + Serial.print(F(" HIGH:300.0")); + Serial.print(F(" GyroX:")); + Serial.print(imu.GyroX); + Serial.print(F(" GyroY:")); + Serial.print(imu.GyroY); + Serial.print(F(" GyroZ:")); + Serial.println(imu.GyroZ); + } +} + +void printAccelData() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-2.0")); + Serial.print(F(" HIGH:2.0")); + Serial.print(F(" AccX:")); + Serial.print(imu.AccX); + Serial.print(F(" AccY:")); + Serial.print(imu.AccY); + Serial.print(F(" AccZ:")); + Serial.println(imu.AccZ); + } +} + +void printMagData() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-300.0")); + Serial.print(F(" HIGH:300.0")); + Serial.print(F(" MagX:")); + Serial.print(imu.MagX); + Serial.print(F(" MagY:")); + Serial.print(imu.MagY); + Serial.print(F(" MagZ:")); + Serial.println(imu.MagZ); + } +} + +void printRollPitchYaw() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-180.0")); + Serial.print(F(" HIGH:180.0")); + Serial.print(F(" roll:")); + Serial.print(imu.roll_IMU); + Serial.print(F(" pitch:")); + Serial.print(imu.pitch_IMU); + Serial.print(F(" yaw:")); + Serial.println(imu.yaw_IMU); + } +} + +void printPIDoutput() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:-1.0")); + Serial.print(F(" HIGH:1.0")); + Serial.print(F(" roll_PID:")); + Serial.print(imu.roll_PID); + Serial.print(F(" pitch_PID:")); + Serial.print(imu.pitch_PID); + Serial.print(F(" yaw_PID:")); + Serial.println(imu.yaw_PID); + } +} + +void printMotorCommands() { + if (current_time - print_counter > 10000) { + print_counter = micros(); + Serial.print(F("LOW:125.0")); + Serial.print(F(" HIGH:250.0")); + for (int i=0; i 10000) { + print_counter = micros(); + Serial.print(F("LOW:0.0")); + Serial.print(F(" HIGH:1.0")); + for (int i=0; i 10000) { + print_counter = micros(); + Serial.print(F("dt:")); + Serial.println(dt*1000000.0); + } +} + diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/radioComm.ino b/Versions/dRehmFlight_Teensy_BETA_2.0/radioComm.ino deleted file mode 100644 index c28709cb..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_2.0/radioComm.ino +++ /dev/null @@ -1,198 +0,0 @@ -//Arduino/Teensy Flight Controller - dRehmFlight -//Author: Nicholas Rehm -//Project Start: 1/6/2020 -//Last Updated: 7/29/2022 -//Version: Beta 1.3 - -//========================================================================================================================// - -//This file contains all necessary functions and code used for radio communication to avoid cluttering the main code - -// unsigned long rising_edge_start_1, rising_edge_start_2, rising_edge_start_3, rising_edge_start_4, rising_edge_start_5, rising_edge_start_6; -// unsigned long channel_1_raw, channel_2_raw, channel_3_raw, channel_4_raw, channel_5_raw, channel_6_raw; -// int ppm_counter = 0; -// unsigned long time_ms = 0; - -// void radioSetup() { -// //PPM Receiver -// #if defined USE_PPM_RX -// //Declare interrupt pin -// pinMode(PPM_Pin, INPUT_PULLUP); -// delay(20); -// //Attach interrupt and point to corresponding ISR function -// attachInterrupt(digitalPinToInterrupt(PPM_Pin), getPPM, CHANGE); - -// //PWM Receiver -// #elif defined USE_PWM_RX -// //Declare interrupt pins -// pinMode(ch1Pin, INPUT_PULLUP); -// pinMode(ch2Pin, INPUT_PULLUP); -// pinMode(ch3Pin, INPUT_PULLUP); -// pinMode(ch4Pin, INPUT_PULLUP); -// pinMode(ch5Pin, INPUT_PULLUP); -// pinMode(ch6Pin, INPUT_PULLUP); -// delay(20); -// //Attach interrupt and point to corresponding ISR functions -// attachInterrupt(digitalPinToInterrupt(ch1Pin), getCh1, CHANGE); -// attachInterrupt(digitalPinToInterrupt(ch2Pin), getCh2, CHANGE); -// attachInterrupt(digitalPinToInterrupt(ch3Pin), getCh3, CHANGE); -// attachInterrupt(digitalPinToInterrupt(ch4Pin), getCh4, CHANGE); -// attachInterrupt(digitalPinToInterrupt(ch5Pin), getCh5, CHANGE); -// attachInterrupt(digitalPinToInterrupt(ch6Pin), getCh6, CHANGE); -// delay(20); - -// //SBUS Recevier -// #elif defined USE_SBUS_RX -// sbus.begin(); - -// //DSM receiver -// #elif defined USE_DSM_RX -// Serial3.begin(115000); -// #else -// #error No RX type defined... -// #endif -// } - -// unsigned long getRadioPWM(int ch_num) { -// //DESCRIPTION: Get current radio commands from interrupt routines -// unsigned long returnPWM = 0; - -// if (ch_num == 1) { -// returnPWM = channel_1_raw; -// } -// else if (ch_num == 2) { -// returnPWM = channel_2_raw; -// } -// else if (ch_num == 3) { -// returnPWM = channel_3_raw; -// } -// else if (ch_num == 4) { -// returnPWM = channel_4_raw; -// } -// else if (ch_num == 5) { -// returnPWM = channel_5_raw; -// } -// else if (ch_num == 6) { -// returnPWM = channel_6_raw; -// } - -// return returnPWM; -// } - -//For DSM type receivers -// void serialEvent3(void) -// { -// #if defined USE_DSM_RX -// while (Serial3.available()) { -// DSM.handleSerialEvent(Serial3.read(), micros()); -// } -// #endif -// } - - - -//========================================================================================================================// - - - -//INTERRUPT SERVICE ROUTINES (for reading PWM and PPM) - -// void getPPM() { -// unsigned long dt_ppm; -// int trig = digitalRead(PPM_Pin); -// if (trig==1) { //Only care about rising edge -// dt_ppm = micros() - time_ms; -// time_ms = micros(); - - -// if (dt_ppm > 5000) { //Waiting for long pulse to indicate a new pulse train has arrived -// ppm_counter = 0; -// } - -// if (ppm_counter == 1) { //First pulse -// channel_1_raw = dt_ppm; -// } - -// if (ppm_counter == 2) { //Second pulse -// channel_2_raw = dt_ppm; -// } - -// if (ppm_counter == 3) { //Third pulse -// channel_3_raw = dt_ppm; -// } - -// if (ppm_counter == 4) { //Fourth pulse -// channel_4_raw = dt_ppm; -// } - -// if (ppm_counter == 5) { //Fifth pulse -// channel_5_raw = dt_ppm; -// } - -// if (ppm_counter == 6) { //Sixth pulse -// channel_6_raw = dt_ppm; -// } - -// ppm_counter = ppm_counter + 1; -// } -// } - -// void getCh1() { -// int trigger = digitalRead(ch1Pin); -// if(trigger == 1) { -// rising_edge_start_1 = micros(); -// } -// else if(trigger == 0) { -// channel_1_raw = micros() - rising_edge_start_1; -// } -// } - -// void getCh2() { -// int trigger = digitalRead(ch2Pin); -// if(trigger == 1) { -// rising_edge_start_2 = micros(); -// } -// else if(trigger == 0) { -// channel_2_raw = micros() - rising_edge_start_2; -// } -// } - -// void getCh3() { -// int trigger = digitalRead(ch3Pin); -// if(trigger == 1) { -// rising_edge_start_3 = micros(); -// } -// else if(trigger == 0) { -// channel_3_raw = micros() - rising_edge_start_3; -// } -// } - -// void getCh4() { -// int trigger = digitalRead(ch4Pin); -// if(trigger == 1) { -// rising_edge_start_4 = micros(); -// } -// else if(trigger == 0) { -// channel_4_raw = micros() - rising_edge_start_4; -// } -// } - -// void getCh5() { -// int trigger = digitalRead(ch5Pin); -// if(trigger == 1) { -// rising_edge_start_5 = micros(); -// } -// else if(trigger == 0) { -// channel_5_raw = micros() - rising_edge_start_5; -// } -// } - -// void getCh6() { -// int trigger = digitalRead(ch6Pin); -// if(trigger == 1) { -// rising_edge_start_6 = micros(); -// } -// else if(trigger == 0) { -// channel_6_raw = micros() - rising_edge_start_6; -// } -// } diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/Actuators/Actuators.cpp b/Versions/dRehmFlight_Teensy_BETA_2.0/src/Actuators/Actuators.cpp new file mode 100644 index 00000000..d6f3e635 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/Actuators/Actuators.cpp @@ -0,0 +1,190 @@ + + +// Actuators.cpp +// This is a wrapper for IMU chips supported by dRehmFlight. It consolidates all the IMU handling and PID subrutines into a single library that most people won't need to mess with. + + + +#include "Actuators.h" + + + +Actuators::Actuators(OutputType type, uint8_t numActuators, uint8_t* actuatorPins) { + _servos = nullptr; + // _escs = nullptr; + // _lsm6dsox = nullptr; + uint8_t i; + + _type = type; + _numActuators = numActuators; + for (i=0; i<_numActuators; i++) { + _actuatorPins[i] = actuatorPins[i]; + } + + if (_type == SERVO) { + _servos = new PWMServo[_numActuators]; + } + else if (_type == ESC) { + _servos = new PWMServo[_numActuators]; + } + else if (_type == ONESHOT125) { + for (i=0; i<_numActuators; i++) { + pinMode(_actuatorPins[i], OUTPUT); + } + } + else { + // error, unknown type + } +} + + + + + + + +bool Actuators::begin() { + uint8_t i; + + switch (_type) { + case SERVO: + { + for (i=0; i<_numActuators; i++) { + _servos[i].attach(_actuatorPins[i], SERVO_PWM_MIN, SERVO_PWM_MAX); //Pin, min PWM value, max PWM value + _servos[i].write(DEFAULT_SERVO_POS); + } + return true; + } + + case ESC: + { + for (i=0; i<_numActuators; i++) { + _servos[i].attach(_actuatorPins[i], SERVO_PWM_MIN, SERVO_PWM_MAX); //Pin, min PWM value, max PWM value + _servos[i].write(DEFAULT_ESC_POS); + } + return true; + } + + case ONESHOT125: + { + //Arm OneShot125 motors + float output[_numActuators] = {DEFAULT_OS125_POS}; + + //armMotors(); //Loop over commandMotors() until ESCs happily arm + for (int i = 0; i <= 50; i++) { + commandMotors(output); + delay(2); + } + + return true; + } + + default: + return false; + } +} + + + + + + +void Actuators::commandOne(uint8_t index, float output) { + if (index < _numActuators) { + int angle; + if (_type == SERVO || _type == ESC) { + //Scaled to 0-180 for servo library + angle = output * 180; + //Constrain commands to servos within servo library bounds + angle = constrain(angle, 0, 180); + + // Write output + _servos[index].write(angle); + } + else if (_type == ONESHOT125) { + + } + else { + // Error + } + } +} + +void Actuators::commandAll(float* output) { + int angle; + if (_type == SERVO || _type == ESC) { + for (uint8_t i = 0; i < _numActuators; i++) { + //Scaled to 0-180 for servo library + angle = output[i] * 180; + //Constrain commands to servos within servo library bounds + angle = constrain(angle, 0, 180); + + // Write output + _servos[i].write(angle); + } + } + else if (_type == ONESHOT125) { + + } + else { + // Error + } +} + +void Actuators::commandAllSame(float output) { + int angle; + if (_type == SERVO || _type == ESC) { + for (uint8_t i = 0; i < _numActuators; i++) { + //Scaled to 0-180 for servo library + angle = output * 180; + //Constrain commands to servos within servo library bounds + angle = constrain(angle, 0, 180); + + // Write output + _servos[i].write(angle); + } + } + else if (_type == ONESHOT125) { + + } + else { + // Error + } +} + + + +void Actuators::commandMotors(float* output) { + //DESCRIPTION: Send pulses to motor pins, oneshot125 protocol + /* + * My crude implimentation of OneShot125 protocol which sends 125 - 250us pulses to the ESCs (mXPin). The pulselengths being + * sent are mX_command_PWM, computed in scaleCommands(). This may be replaced by something more efficient in the future. + */ + int wentLow = 0; + int pulseStart, timer; + int flagM[_numActuators] = {0}; + int OSoutput = 125; + + //Write all motor pins high + for (uint8_t i = 0; i < _numActuators; i++) { + digitalWrite(_actuatorPins[i], HIGH); + } + pulseStart = micros(); + + //Write each motor pin low as correct pulse length is reached + while (wentLow < 6 ) { //Keep going until final (6th) pulse is finished, then done + timer = micros(); + for (uint8_t i = 0; i < _numActuators; i++) { + //Scaled to 125us - 250us for oneshot125 protocol + OSoutput = output[i]*125 + 125; + //Constrain commands to motors within oneshot125 bounds + OSoutput = constrain(OSoutput, 125, 250); + + if ((OSoutput <= timer - pulseStart) && (flagM[i]==0)) { + digitalWrite(_actuatorPins[i], LOW); + wentLow = wentLow + 1; + flagM[i] = 1; + } + } + } +} \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/Actuators/Actuators.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/Actuators/Actuators.h new file mode 100644 index 00000000..1d0f6c08 --- /dev/null +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/Actuators/Actuators.h @@ -0,0 +1,58 @@ +// Actuators.h +// This is a wrapper for IMU chips supported by dRehmFlight. It consolidates all the IMU handling and PID subrutines into a single library that most people won't need to mess with. + + +#ifndef Actuators_h +#define Actuators_h + +#include +#include //Commanding any extra actuators, installed with teensyduino installer + + +enum OutputType { + SERVO, + ESC, + ONESHOT125 +}; + +#define SERVO_PWM_MIN 900 +#define SERVO_PWM_MAX 2100 + +#define DEFAULT_SERVO_POS 90 +#define DEFAULT_ESC_POS 0 +#define DEFAULT_OS125_POS 0.0 + + + +class Actuators { +public: + + Actuators(OutputType type, uint8_t numActuators, uint8_t* actuatorPins); + + bool begin(); + + // Expect all of our outputs to be 0.0 to 1.0 from the Mixer. Will scale appropriately + void commandOne(uint8_t index, float output); + void commandAll(float* output); + void commandAllSame(float output); + void commandMotors(float* output); + + + +private: + + OutputType _type; + + PWMServo* _servos; + + + // oneshot _oneshot; + + uint8_t _numActuators = 1; + uint8_t _actuatorPins[]; +}; + + + + +#endif \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.cpp b/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.cpp index 2523bdc4..9e74615e 100644 --- a/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.cpp +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.cpp @@ -15,18 +15,14 @@ RadioComm::RadioComm(RadioType type, uint8_t num_channels) { // _sbus_data = nullptr; _dsm = nullptr; _ppm = nullptr; - _pwm = nullptr; _type = type; - _num_channels = num_channels; } - - void RadioComm::begin() { if (_type == sBUS) { // _sbus = new bfs::SbusRx(&Serial5, true, false); @@ -35,10 +31,6 @@ void RadioComm::begin() { _sbus = new SBUS(Serial5); _sbus->begin(); - - - - } else if (_type == DSM) { @@ -56,10 +48,10 @@ void RadioComm::begin() { void RadioComm::begin(int PPM_Pin) { if (_type == PPM) { - // _ppm = new PPM(); + _ppm = new PPMReader(PPM_Pin, _num_channels); // _ppm->begin(PPM_Pin, false); // ppm.begin(PPM_Pin, false); - PPMReader _ppm(PPM_Pin, _num_channels); + // PPMReader _ppm(PPM_Pin, _num_channels); } @@ -70,34 +62,12 @@ void RadioComm::begin(int PPM_Pin) { } } - -void RadioComm::begin(int channelPins[]) { - byte i; - - // Assign channel pins into the Class variable. _num_channels MUST be set at invocation time or the default of 6 will be used. - for (i=0; i<_num_channels; i++) { - _pwm_channel_pins[i] = channelPins[i]; - } - - if (_type == PWM) { - RC_Receiver _pwm(_pwm_channel_pins[0], _pwm_channel_pins[1], _pwm_channel_pins[2], _pwm_channel_pins[3], - _pwm_channel_pins[4], _pwm_channel_pins[5], _pwm_channel_pins[6], _pwm_channel_pins[7]); - - - - } - - else { - Serial.println("No PWM type defined..."); - while(1); - } -} -void RadioComm::getCommands(unsigned long int* returnArray) { +void RadioComm::getCommands(unsigned int* returnArray) { //DESCRIPTION: Get raw PWM values for every channel from the radio /* * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of @@ -109,7 +79,6 @@ void RadioComm::getCommands(unsigned long int* returnArray) { int8_t i; if (_type == sBUS) { // _sbus->Read(); - // /* Grab the received data */ // _sbus_data = _sbus->data(); // /* Display the received data */ @@ -125,43 +94,34 @@ void RadioComm::getCommands(unsigned long int* returnArray) { channel_pwm[i] = _sbusChannels[i] * _sbus_scale + _sbus_bias; } } - - - - - - } + else if (_type == PPM) { + for (i=0; i < _num_channels; i++) { + channel_pwm[i] = _ppm->latestValidChannelValue(i+1,1000); + } + } + else if (_type == DSM) { + if (_dsm->timedOut(micros())) { + //Serial.println("*** DSM RX TIMED OUT ***"); + } + else if (_dsm->gotNewFrame()) { + // uint16_t values[_num_channels]; + _dsm->getChannelValues(_sbusChannels, _num_channels); + for (i = 0; i < _num_channels; i++) { + channel_pwm[i] = _sbusChannels[i]; + } + } + } -// #if defined USE_PPM_RX || defined USE_PWM_RX -// channel_1_pwm = getRadioPWM(1); -// channel_2_pwm = getRadioPWM(2); -// channel_3_pwm = getRadioPWM(3); -// channel_4_pwm = getRadioPWM(4); -// channel_5_pwm = getRadioPWM(5); -// channel_6_pwm = getRadioPWM(6); - -// #elif defined USE_DSM_RX -// if (DSM.timedOut(micros())) { -// //Serial.println("*** DSM RX TIMED OUT ***"); -// } -// else if (DSM.gotNewFrame()) { -// uint16_t values[num_DSM_channels]; -// DSM.getChannelValues(values, num_DSM_channels); - -// channel_1_pwm = values[0]; -// channel_2_pwm = values[1]; -// channel_3_pwm = values[2]; -// channel_4_pwm = values[3]; -// channel_5_pwm = values[4]; -// channel_6_pwm = values[5]; -// } -// #endif + else { + // Should probably have an error here + } + // Low-pass the critical commands and update previous values float b = 0.7; //Lower=slower, higher=noiser @@ -175,7 +135,67 @@ void RadioComm::getCommands(unsigned long int* returnArray) { for (i = 0; i < _num_channels; i++) { returnArray[i] = channel_pwm[i]; } +} + + +void RadioComm::setFailsafe(unsigned int* failsafeArray) { + for (int8_t i=0; i<_num_channels; i++) { + _channel_fs[i] = failsafeArray[i]; + } +} + + + + +void RadioComm::failSafe() { + //DESCRIPTION: If radio gives garbage values, set all commands to default values + /* + * Radio connection failsafe used to check if the getCommands() function is returning acceptable pwm values. If any of + * the commands are lower than 800 or higher than 2200, then we can be certain that there is an issue with the radio + * connection (most likely hardware related). If any of the channels show this failure, then all of the radio commands + * channel_x_pwm are set to default failsafe values specified in the setup. Comment out this function when troubleshooting + * your radio connection in case any extreme values are triggering this function to overwrite the printed variables. + * + * When using SBUS, the library handles the detection of lost frames and failsafe. Some receivers handle failsafe differently. + * Testing with FrSky SBUS receivers immediately triggered failsafe when Tx disconnected. A Radiomaster RP3-H ELRS receiver would + * not trigger failsafe when set to "No Pulses", but would trigger when set to "Last Position" after a few seconds. + * Test your receiver's behavior! You can do this with the example sketch in the "Bolder Flight Systems SBUS" library. + */ + int8_t i; + + //If using SBUS, this will only check the boolean failsafe value from the SBUS library. If using PWM RX, it checks each channel. + if (_type == sBUS) { + if (_sbusFailSafe) { + for (i=0; i<_num_channels; i++) { + channel_pwm[i] = _channel_fs[i]; + } + } + } + + else { + unsigned minVal = 800; + unsigned maxVal = 2200; + int check[6] = {0}; + int sum = 0; + + //Triggers for failure criteria -- if the PWM value < minvalue or > maxvalue, set a flag + for (i=0; i<6; i++) { + if (channel_pwm[i] > maxVal || channel_pwm[i] < minVal) check[i] = 1; + } + + // Add up all the flags (if not failsafe, this will be 0) + for (i=0; i<6; i++) { + sum += check[i]; + } + + // If sum of all checks > 0, enable failsafe + if (sum > 0) { + for (i=0; i<_num_channels; i++) { + channel_pwm[i] = _channel_fs[i]; + } + } + } } diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.h index 9b1a03fc..dfb46f2d 100644 --- a/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.h +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.h @@ -14,11 +14,11 @@ // #include "sbus.h" // Bolder Flight Systems SBUS library #include #include "../DSMRX/DSMRX.h" -#include "RC_Receiver.h" + + enum RadioType { - PWM, PPM, DSM, sBUS @@ -31,24 +31,17 @@ class RadioComm { RadioComm(RadioType type); RadioComm(RadioType type, uint8_t num_channels); - - - void begin(); //DSM & //SBUS void begin(int PPM_Pin); //PPM - void begin(int channelPins[]); //PWM - void getCommands(unsigned long int* returnArray); + void getCommands(unsigned int* returnArray); + void setFailsafe(unsigned int* failsafeArray); + void failSafe(); - unsigned long channel_pwm[16] = {0}; - - - - private: RadioType _type; @@ -57,14 +50,11 @@ class RadioComm { SBUS* _sbus; DSMRX* _dsm; PPMReader* _ppm; - RC_Receiver* _pwm; uint8_t _num_channels = 6; uint16_t _sbusChannels[16] = {0}; - int _pwm_channel_pins[8] = {0}; - unsigned long _channel_pwm_prev[4] = {0}; - - + unsigned int _channel_pwm_prev[4] = {0}; + unsigned int _channel_fs[16] = {0}; bool _sbusFailSafe; bool _sbusLostFrame; @@ -72,10 +62,6 @@ class RadioComm { //sBus scaling below is for Taranis-Plus and X4R-SB const float _sbus_scale = 0.615; const float _sbus_bias = 895.0; - - - - }; From 6ddd894227becfbab8f44500b1ff1992036c2220 Mon Sep 17 00:00:00 2001 From: Brian Jones Date: Mon, 11 Aug 2025 09:18:27 -0500 Subject: [PATCH 5/6] General cleanup and documentation --- .../dRehmFlight_Teensy_BETA_2.0.ino | 250 +++++++++--------- .../src/Actuators/Actuators.cpp | 189 ++++++++----- .../src/Actuators/Actuators.h | 26 +- .../src/IMU_Wrapper/IMU_Wrapper.cpp | 165 +++++------- .../src/IMU_Wrapper/IMU_Wrapper.h | 38 +-- .../src/RadioComm/RadioComm.cpp | 54 ++-- .../src/RadioComm/RadioComm.h | 24 +- 7 files changed, 377 insertions(+), 369 deletions(-) diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino b/Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino index 5cbef128..8bc3ee66 100644 --- a/Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/dRehmFlight_Teensy_BETA_2.0.ino @@ -240,6 +240,19 @@ void setup() { // If using MPU9250 IMU, replace the below with the output from calibrateMagnetometer() above // imu.setMagError(0.0, 0.0, 0.0); // imu.setMagScale(1.0, 1.0, 1.0); + + // This sets the PID values for Rate and Angle modes based on Nick's Github defaults. These can be altered within the control loop as well for adjustments in flight either with fader variables, control inputs, etc. + // I put these here as an example of use. + imu.setRollAnglePID(0.2, 0.3, 0.05); + imu.setPitchAnglePID(0.2, 0.3, 0.05); + + imu.setRollRatePID(0.15, 0.2, 0.0002); + imu.setPitchRatePID(0.15, 0.2, 0.0002); + + imu.setYawRatePID(0.3, 0.05, 0.00015); + + + //========================================================================================================================// delay(500); @@ -303,7 +316,7 @@ void loop() { // Write outputs to all servos, ESCs, and motors Servos.commandAll(s_command_scaled); Escs.commandAll(e_command_scaled); - Motors.commandMotors(m_command_scaled); + Motors.commandAll(m_command_scaled); //Get vehicle commands for next loop iteration @@ -317,7 +330,7 @@ void loop() { //========================================================================================================================// -// FUNCTIONS // +// MIXER FUNCTIONS // //========================================================================================================================// @@ -366,118 +379,6 @@ void controlMixer() { } -void armedStatus() { - //DESCRIPTION: Check if the throttle cut is off and the throttle input is low to prepare for flight. - if ((channel_pwm[4] < 1500) && (channel_pwm[0] < 1050)) { - if (armedFly == false) { - Serial.println(F("ARMED!")); - } - armedFly = true; - } -} - -void calibrateAttitude() { - //DESCRIPTION: Used to warm up the main loop to allow the madwick filter to converge before commands can be sent to the actuators - //Assuming vehicle is powered up on level surface! - /* - * This function is used on startup to warm up the attitude estimation and is what causes startup to take a few seconds - * to boot. - */ - //Warm up IMU and madgwick filter in simulated main loop - for (int i = 0; i <= 10000; i++) { - prev_time = current_time; - current_time = micros(); - dt = (current_time - prev_time)/1000000.0; - imu.getIMUdata(); - imu.Madgwick(dt); - loopRate(2000); //do not exceed 2000Hz - } -} - -void getDesState() { - //DESCRIPTION: Normalizes desired control values to appropriate values - /* - * Updates the desired state variables thro_des, roll_des, pitch_des, and yaw_des. These are computed by using the raw - * RC pwm commands and scaling them to be within our limits defined in setup. thro_des stays within 0 to 1 range. - * roll_des and pitch_des are scaled to be within max roll/pitch amount in either degrees (angle mode) or degrees/sec - * (rate mode). yaw_des is scaled to be within max yaw in degrees/sec. Also creates roll_passthru, pitch_passthru, and - * yaw_passthru variables, to be used in commanding motors/servos with direct unstabilized commands in controlMixer(). - */ - - // Note that additional controls, such as collective pitch, tilt servos, etc. can be added here. - - thro_des = (channel_pwm[0] - 1000.0)/1000.0; //Between 0 and 1 - roll_des = (channel_pwm[1] - 1500.0)/500.0; //Between -1 and 1 - pitch_des = (channel_pwm[2] - 1500.0)/500.0; //Between -1 and 1 - yaw_des = (channel_pwm[3] - 1500.0)/500.0; //Between -1 and 1 - roll_passthru = roll_des/2.0; //Between -0.5 and 0.5 - pitch_passthru = pitch_des/2.0; //Between -0.5 and 0.5 - yaw_passthru = yaw_des/2.0; //Between -0.5 and 0.5 - - //Constrain within normalized bounds - thro_des = constrain(thro_des, 0.0, 1.0); //Between 0 and 1 - roll_des = constrain(roll_des, -1.0, 1.0)*maxRoll; //Between -maxRoll and +maxRoll - pitch_des = constrain(pitch_des, -1.0, 1.0)*maxPitch; //Between -maxPitch and +maxPitch - yaw_des = constrain(yaw_des, -1.0, 1.0)*maxYaw; //Between -maxYaw and +maxYaw - roll_passthru = constrain(roll_passthru, -0.5, 0.5); - pitch_passthru = constrain(pitch_passthru, -0.5, 0.5); - yaw_passthru = constrain(yaw_passthru, -0.5, 0.5); -} - - -void calibrateESCs() { - //DESCRIPTION: Used in void setup() to allow standard ESC calibration procedure with the radio to take place. - /* - * Simulates the void loop(), but only for the purpose of providing throttle pass through to the motors, so that you can - * power up with throttle at full, let ESCs begin arming sequence, and lower throttle to zero. This function should only be - * uncommented when performing an ESC calibration. - */ - while (true) { - prev_time = current_time; - current_time = micros(); - dt = (current_time - prev_time)/1000000.0; - - digitalWrite(13, HIGH); //LED on to indicate we are not in main loop - - // getCommands(); //Pulls current available radio commands - radio.failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup - getDesState(); //Convert raw commands to normalized values based on saturated control limits - imu.getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise - imu.Madgwick(dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU (degrees) - getDesState(); //Convert raw commands to normalized values based on saturated control limits - - // m1_command_scaled = thro_des; - // m2_command_scaled = thro_des; - // m3_command_scaled = thro_des; - // m4_command_scaled = thro_des; - // m5_command_scaled = thro_des; - // m6_command_scaled = thro_des; - // s1_command_scaled = thro_des; - // s2_command_scaled = thro_des; - // s3_command_scaled = thro_des; - // s4_command_scaled = thro_des; - // s5_command_scaled = thro_des; - // s6_command_scaled = thro_des; - // s7_command_scaled = thro_des; - // scaleCommands(); //Scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) - - //throttleCut(); //Directly sets motor commands to low based on state of ch5 - - Servos.commandAll(s_command_scaled); - Escs.commandAll(e_command_scaled); - Motors.commandMotors(m_command_scaled); - - - // commandMotors(); //Sends command pulses to each motor pin using OneShot125 protocol - - // printRadioData(); //Radio pwm values (expected: 1000 to 2000) - - loopRate(2000); //Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default - } -} - - - @@ -546,12 +447,122 @@ void switchRollYaw(int reverseRoll, int reverseYaw) { roll_des = reverseRoll*switch_holder; } + + + + +//========================================================================================================================// +// ADDITIONAL STARTUP / SUPPORT FUNCTIONS // +//========================================================================================================================// + +void getDesState() { + //DESCRIPTION: Normalizes desired control values to appropriate values + /* + * Updates the desired state variables thro_des, roll_des, pitch_des, and yaw_des. These are computed by using the raw + * RC pwm commands and scaling them to be within our limits defined in setup. thro_des stays within 0 to 1 range. + * roll_des and pitch_des are scaled to be within max roll/pitch amount in either degrees (angle mode) or degrees/sec + * (rate mode). yaw_des is scaled to be within max yaw in degrees/sec. Also creates roll_passthru, pitch_passthru, and + * yaw_passthru variables, to be used in commanding motors/servos with direct unstabilized commands in controlMixer(). + */ + + // Note that additional controls, such as collective pitch, tilt servos, etc. can be added here. + + thro_des = (channel_pwm[0] - 1000.0)/1000.0; //Between 0 and 1 + roll_des = (channel_pwm[1] - 1500.0)/500.0; //Between -1 and 1 + pitch_des = (channel_pwm[2] - 1500.0)/500.0; //Between -1 and 1 + yaw_des = (channel_pwm[3] - 1500.0)/500.0; //Between -1 and 1 + roll_passthru = roll_des/2.0; //Between -0.5 and 0.5 + pitch_passthru = pitch_des/2.0; //Between -0.5 and 0.5 + yaw_passthru = yaw_des/2.0; //Between -0.5 and 0.5 + + //Constrain within normalized bounds + thro_des = constrain(thro_des, 0.0, 1.0); //Between 0 and 1 + roll_des = constrain(roll_des, -1.0, 1.0)*maxRoll; //Between -maxRoll and +maxRoll + pitch_des = constrain(pitch_des, -1.0, 1.0)*maxPitch; //Between -maxPitch and +maxPitch + yaw_des = constrain(yaw_des, -1.0, 1.0)*maxYaw; //Between -maxYaw and +maxYaw + roll_passthru = constrain(roll_passthru, -0.5, 0.5); + pitch_passthru = constrain(pitch_passthru, -0.5, 0.5); + yaw_passthru = constrain(yaw_passthru, -0.5, 0.5); +} + +void armedStatus() { + //DESCRIPTION: Check if the throttle cut is off and the throttle input is low to prepare for flight. + if ((channel_pwm[4] < 1500) && (channel_pwm[0] < 1050)) { + if (armedFly == false) { + Serial.println(F("ARMED!")); + } + armedFly = true; + } +} + +void calibrateAttitude() { + //DESCRIPTION: Used to warm up the main loop to allow the madwick filter to converge before commands can be sent to the actuators + //Assuming vehicle is powered up on level surface! + /* + * This function is used on startup to warm up the attitude estimation and is what causes startup to take a few seconds + * to boot. + */ + //Warm up IMU and madgwick filter in simulated main loop + for (int i = 0; i <= 10000; i++) { + prev_time = current_time; + current_time = micros(); + dt = (current_time - prev_time)/1000000.0; + imu.getIMUdata(); + imu.Madgwick(dt); + loopRate(2000); //do not exceed 2000Hz + } +} + +void calibrateESCs() { + //DESCRIPTION: Used in void setup() to allow standard ESC calibration procedure with the radio to take place. + /* + * Simulates the void loop(), but only for the purpose of providing throttle pass through to the motors, so that you can + * power up with throttle at full, let ESCs begin arming sequence, and lower throttle to zero. This function should only be + * uncommented when performing an ESC calibration. + */ + while (true) { + prev_time = current_time; + current_time = micros(); + dt = (current_time - prev_time)/1000000.0; + + digitalWrite(13, HIGH); //LED on to indicate we are not in main loop + + radio.getCommands(channel_pwm); //Pulls current available radio commands + radio.failSafe(); //Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup + // getDesState(); //Convert raw commands to normalized values based on saturated control limits -- this was in here 2x. Not sure that's needed + imu.getIMUdata(); //Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise + imu.Madgwick(dt); //Updates roll_IMU, pitch_IMU, and yaw_IMU (degrees) + getDesState(); //Convert raw commands to normalized values based on saturated control limits + + // Set all outputs tied to the throttle input + for (int i=0; i #include //Commanding any extra actuators, installed with teensyduino installer - enum OutputType { SERVO, ESC, ONESHOT125 + // Add more types here }; #define SERVO_PWM_MIN 900 @@ -22,11 +29,8 @@ enum OutputType { #define DEFAULT_ESC_POS 0 #define DEFAULT_OS125_POS 0.0 - - class Actuators { public: - Actuators(OutputType type, uint8_t numActuators, uint8_t* actuatorPins); bool begin(); @@ -35,24 +39,14 @@ class Actuators { void commandOne(uint8_t index, float output); void commandAll(float* output); void commandAllSame(float output); - void commandMotors(float* output); - - private: - OutputType _type; PWMServo* _servos; - - // oneshot _oneshot; - uint8_t _numActuators = 1; uint8_t _actuatorPins[]; }; - - - #endif \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/IMU_Wrapper.cpp b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/IMU_Wrapper.cpp index 4065b35d..e56cb6a9 100644 --- a/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/IMU_Wrapper.cpp +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/IMU_Wrapper.cpp @@ -1,13 +1,16 @@ +//Arduino/Teensy Flight Controller - dRehmFlight +//Author: Nicholas Rehm +//Additional Author: Brian Jones +//Project Start: 1/6/2020 +//Last Updated: 8/11/2025 +//Version: Beta 2.0 +//========================================================================================================================// -// IMU_Wrapper.cpp // This is a wrapper for IMU chips supported by dRehmFlight. It consolidates all the IMU handling and PID subrutines into a single library that most people won't need to mess with. - - #include "IMU_Wrapper.h" - IMU_Wrapper::IMU_Wrapper(IMUType type): IMU_Wrapper(type, GYRO_250DPS, ACCEL_2G) {}; IMU_Wrapper::IMU_Wrapper(IMUType type, Gyro_DPS gyroDPS, Accel_G accelG) { @@ -52,15 +55,6 @@ IMU_Wrapper::IMU_Wrapper(IMUType type, Gyro_DPS gyroDPS, Accel_G accelG) { } - - - - - - - - - bool IMU_Wrapper::begin() { switch (_type) { case MPU6050_I2C: @@ -130,6 +124,9 @@ bool IMU_Wrapper::begin() { Serial.println(status); return false; } + else { + Serial.println("MPU9250 initialization successful"); + } //From the reset state all registers should be 0x00, so we should be at //max sample rate with digital low pass filter(s) off. All we need to @@ -180,7 +177,7 @@ bool IMU_Wrapper::begin() { return false; } else { - Serial.println("LSM6DSOX Found!"); + Serial.println("LSM6DSOX initialization successful"); } switch (_gyroDPS) { @@ -211,10 +208,6 @@ bool IMU_Wrapper::begin() { _lsm6dsox->setAccelRange(lsm6ds_accel_range_t::LSM6DS_ACCEL_RANGE_16_G); break; } - - // Set the data rate of the board higher than stock 104Hz - // lsm6dsox.setAccelDataRate(LSM6DS_RATE_6_66K_HZ); - // lsm6dsox.setGyroDataRate(LSM6DS_RATE_6_66K_HZ); return true; } @@ -233,9 +226,6 @@ void IMU_Wrapper::setSPIpins(int8_t cs_pin, int8_t sck_pin, int8_t miso_pin, int } - - - void IMU_Wrapper::getIMUdata() { //DESCRIPTION: Request full dataset from IMU and LP filter gyro, accelerometer, and magnetometer data /* @@ -350,8 +340,6 @@ void IMU_Wrapper::getIMUdata() { } - - void IMU_Wrapper::calculate_IMU_error() { //DESCRIPTION: Computes IMU accelerometer and gyro error on startup. Note: vehicle should be powered up on flat surface /* @@ -455,7 +443,6 @@ void IMU_Wrapper::calculate_IMU_error() { } - void IMU_Wrapper::calibrateMagnetometer() { if (_type == MPU9250_SPI) { float success; @@ -501,10 +488,7 @@ void IMU_Wrapper::calibrateMagnetometer() { } - - void IMU_Wrapper::Madgwick(float invSampleFreq) { - float gx = GyroX; float gy = -GyroY; float gz = -GyroZ; @@ -515,7 +499,6 @@ void IMU_Wrapper::Madgwick(float invSampleFreq) { float my = -MagX; float mz = MagZ; - //DESCRIPTION: Attitude estimation through sensor fusion - 9DOF /* * This function fuses the accelerometer gyro, and magnetometer readings AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, and MagZ for attitude estimation. @@ -718,9 +701,6 @@ void IMU_Wrapper::Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, } - - - void IMU_Wrapper::controlANGLE(float roll_des, float pitch_des, float yaw_des, float dt, unsigned long throttle) { //DESCRIPTION: Computes control commands based on state error (angle) /* @@ -773,6 +753,7 @@ void IMU_Wrapper::controlANGLE(float roll_des, float pitch_des, float yaw_des, f integral_yaw_prev = integral_yaw; } + void IMU_Wrapper::controlANGLE2(float roll_des, float pitch_des, float yaw_des, float dt, unsigned long throttle) { //DESCRIPTION: Computes control commands based on state error (angle) in cascaded scheme /* @@ -856,9 +837,9 @@ void IMU_Wrapper::controlANGLE2(float roll_des, float pitch_des, float yaw_des, //Update yaw variables error_yaw_prev = error_yaw; integral_yaw_prev = integral_yaw; - } + void IMU_Wrapper::controlRATE(float roll_des, float pitch_des, float yaw_des, float dt, unsigned long throttle) { //DESCRIPTION: Computes control commands based on state error (rate) /* @@ -908,113 +889,91 @@ void IMU_Wrapper::controlRATE(float roll_des, float pitch_des, float yaw_des, fl } - +// Functions to return individial Gyro, Accel, and Mag values. float IMU_Wrapper::getGyro(char dir) { - if (dir == 'X') { - return GyroX; - } - if (dir == 'Y') { - return GyroY; - } - if (dir == 'Z') { - return GyroZ; - } - return 0.0; + if (dir == 'X') return GyroX; + else if (dir == 'Y') return GyroY; + else if (dir == 'Z') return GyroZ; + else return 0.0; } float IMU_Wrapper::getAccel(char dir) { - if (dir == 'X') { - return AccX; - } - if (dir == 'Y') { - return AccY; - } - if (dir == 'Z') { - return AccZ; - } - return 0.0; + if (dir == 'X') return AccX; + else if (dir == 'Y') return AccY; + else if (dir == 'Z') return AccZ; + else return 0.0; } - float IMU_Wrapper::getMag(char dir) { - if (dir == 'X') { - return MagX; - } - if (dir == 'Y') { - return MagY; - } - if (dir == 'Z') { - return MagZ; - } - return 0.0; + if (dir == 'X') return MagX; + else if (dir == 'Y') return MagY; + else if (dir == 'Z') return MagZ; + else return 0.0; } - - void IMU_Wrapper::setAccError(float AccErrorX, float AccErrorY, float AccErrorZ) { - _AccErrorX = AccErrorX; - _AccErrorY = AccErrorY; - _AccErrorZ = AccErrorZ; - } - - void IMU_Wrapper::setGyroError(float GyroErrorX, float GyroErrorY, float GyroErrorZ) { - _GyroErrorX = GyroErrorX; - _GyroErrorY = GyroErrorY; - _GyroErrorZ = GyroErrorZ; - } - - void IMU_Wrapper::setMagError(float MagErrorX, float MagErrorY, float MagErrorZ) { - _MagErrorX = MagErrorX; - _MagErrorY = MagErrorY; - _MagErrorZ = MagErrorZ; - } - - void IMU_Wrapper::setMagScale(float MagScaleX, float MagScaleY, float MagScaleZ) { - _MagScaleX = MagScaleX; - _MagScaleY = MagScaleY; - _MagScaleZ = MagScaleZ; - } - - +// Functions to set the error correction values for each sensor: Gyro, Accel, and Mag +void IMU_Wrapper::setGyroError(float GyroErrorX, float GyroErrorY, float GyroErrorZ) { + _GyroErrorX = GyroErrorX; + _GyroErrorY = GyroErrorY; + _GyroErrorZ = GyroErrorZ; +} +void IMU_Wrapper::setAccError(float AccErrorX, float AccErrorY, float AccErrorZ) { + _AccErrorX = AccErrorX; + _AccErrorY = AccErrorY; + _AccErrorZ = AccErrorZ; +} +void IMU_Wrapper::setMagError(float MagErrorX, float MagErrorY, float MagErrorZ) { + _MagErrorX = MagErrorX; + _MagErrorY = MagErrorY; + _MagErrorZ = MagErrorZ; +} +void IMU_Wrapper::setMagScale(float MagScaleX, float MagScaleY, float MagScaleZ) { + _MagScaleX = MagScaleX; + _MagScaleY = MagScaleY; + _MagScaleZ = MagScaleZ; +} - void IMU_Wrapper::setRollAnglePID(float Kp, float Ki, float Kd) { +// Functions to set the PID values for each of the three axis (roll, pitch, yaw) for +// Angle mode and Rate mode. (Yaw only has rate mode) +void IMU_Wrapper::setRollAnglePID(float Kp, float Ki, float Kd) { Kp_roll_angle = Kp; Ki_roll_angle = Ki; Kd_roll_angle = Kd; - } - void IMU_Wrapper::setPitchAnglePID(float Kp, float Ki, float Kd) { +} + +void IMU_Wrapper::setPitchAnglePID(float Kp, float Ki, float Kd) { Kp_pitch_angle = Kp; Ki_pitch_angle = Ki; Kd_pitch_angle = Kd; - } +} - void IMU_Wrapper::setRollRatePID(float Kp, float Ki, float Kd) { +void IMU_Wrapper::setRollRatePID(float Kp, float Ki, float Kd) { Kp_roll_rate = Kp; Ki_roll_rate = Ki; Kd_roll_rate = Kd; - } +} - void IMU_Wrapper::setPitchRatePID(float Kp, float Ki, float Kd) { +void IMU_Wrapper::setPitchRatePID(float Kp, float Ki, float Kd) { Kp_pitch_rate = Kp; Ki_pitch_rate = Ki; Kd_pitch_rate = Kd; - } - void IMU_Wrapper::setYawRatePID(float Kp, float Ki, float Kd) { +} + +void IMU_Wrapper::setYawRatePID(float Kp, float Ki, float Kd) { Kp_yaw = Kp; Ki_yaw = Ki; Kd_yaw = Kd; - } - - +} - //HELPER FUNCTIONS +//HELPER FUNCTIONS float IMU_Wrapper::invSqrt(float x) { //Fast inverse sqrt for madgwick filter diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/IMU_Wrapper.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/IMU_Wrapper.h index fe7356b1..efdc5c7a 100644 --- a/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/IMU_Wrapper.h +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/IMU_Wrapper/IMU_Wrapper.h @@ -1,10 +1,14 @@ +//Arduino/Teensy Flight Controller - dRehmFlight +//Author: Nicholas Rehm +//Additional Author: Brian Jones +//Project Start: 1/6/2020 +//Last Updated: 8/11/2025 +//Version: Beta 2.0 +//========================================================================================================================// - -// IMU_Wrapper.h // This is a wrapper for IMU chips supported by dRehmFlight. It consolidates all the IMU handling and PID subrutines into a single library that most people won't need to mess with. - #ifndef IMU_Wrapper_h #define IMU_Wrapper_h @@ -46,11 +50,8 @@ enum Accel_G { #define MS2G 0.10197162129779283f - - class IMU_Wrapper { public: - IMU_Wrapper(IMUType type); IMU_Wrapper(IMUType type, Gyro_DPS GyroDPS, Accel_G AccelG); bool begin(); @@ -96,9 +97,6 @@ class IMU_Wrapper { float roll_PID = 0; float pitch_PID = 0; float yaw_PID = 0; - - - private: //IMU: IMUType _type; @@ -109,26 +107,21 @@ class IMU_Wrapper { MPU9250* _mpu9250; Adafruit_LSM6DSOX* _lsm6dsox; - float _gyro_scale_factor = 131.0; - float _accel_scale_factor = 16384.0; - // Used for LSM6DSOX sensors_event_t accel; sensors_event_t gyro; sensors_event_t temp; //unused, but could be interesting - + float invSqrt(float x); + // for SPI pins definitions. These are used by MPU9250 and LSMDSOX right now. Defaults are the back pins (SPI2) on the Teensy 4.0 int8_t _cs_pin = 36; int8_t _sck_pin = 37; int8_t _miso_pin = 34; int8_t _mosi_pin = 35; - - - - - float invSqrt(float x); + float _gyro_scale_factor = 131.0; + float _accel_scale_factor = 16384.0; float AccX_prev, AccY_prev, AccZ_prev; float GyroX_prev, GyroY_prev, GyroZ_prev; @@ -140,7 +133,6 @@ class IMU_Wrapper { float error_pitch, error_pitch_prev, pitch_des_prev, integral_pitch, integral_pitch_il, integral_pitch_ol, integral_pitch_prev, integral_pitch_prev_il, integral_pitch_prev_ol, derivative_pitch; float error_yaw, error_yaw_prev, integral_yaw, integral_yaw_prev, derivative_yaw; - float q0 = 1.0f; //Initialize quaternion for madgwick filter float q1 = 0.0f; float q2 = 0.0f; @@ -160,7 +152,6 @@ class IMU_Wrapper { float _GyroErrorY = 0.0; float _GyroErrorZ = 0.0; - //Magnetometer calibration parameters - if using MPU9250, uncomment calibrateMagnetometer() in void setup() to get these values, else just ignore these float _MagErrorX = 0.0; float _MagErrorY = 0.0; @@ -170,12 +161,8 @@ class IMU_Wrapper { float _MagScaleZ = 1.0; - // PIDs float i_limit = 25.0; //Integrator saturation level, mostly for safety (default 25.0) - // float maxRoll = 30.0; //Max roll angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode - // float maxPitch = 30.0; //Max pitch angle in degrees for angle mode (maximum ~70 degrees), deg/sec for rate mode - // float maxYaw = 160.0; //Max yaw rate in deg/sec float Kp_roll_angle = 0.2; //Roll P-gain - angle mode float Ki_roll_angle = 0.3; //Roll I-gain - angle mode @@ -196,9 +183,6 @@ class IMU_Wrapper { float Kp_yaw = 0.3; //Yaw P-gain float Ki_yaw = 0.05; //Yaw I-gain float Kd_yaw = 0.00015; //Yaw D-gain (be careful when increasing too high, motors will begin to overheat!) - - - }; #endif \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.cpp b/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.cpp index 9e74615e..f2b833c3 100644 --- a/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.cpp +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.cpp @@ -1,13 +1,16 @@ +//Arduino/Teensy Flight Controller - dRehmFlight +//Author: Nicholas Rehm +//Additional Author: Brian Jones +//Project Start: 1/6/2020 +//Last Updated: 8/11/2025 +//Version: Beta 2.0 -// RadioComm.cpp -// This is a wrapper for IMU chips supported by dRehmFlight. It consolidates all the IMU handling and PID subrutines into a single library that most people won't need to mess with. - +//========================================================================================================================// +//This file contains all necessary functions and code used for radio communication to avoid cluttering the main code #include "RadioComm.h" - - RadioComm::RadioComm(RadioType type): RadioComm(type, 6) {}; RadioComm::RadioComm(RadioType type, uint8_t num_channels) { @@ -21,8 +24,6 @@ RadioComm::RadioComm(RadioType type, uint8_t num_channels) { } - - void RadioComm::begin() { if (_type == sBUS) { // _sbus = new bfs::SbusRx(&Serial5, true, false); @@ -45,6 +46,7 @@ void RadioComm::begin() { } } + void RadioComm::begin(int PPM_Pin) { if (_type == PPM) { @@ -52,8 +54,6 @@ void RadioComm::begin(int PPM_Pin) { // _ppm->begin(PPM_Pin, false); // ppm.begin(PPM_Pin, false); // PPMReader _ppm(PPM_Pin, _num_channels); - - } else { @@ -62,19 +62,13 @@ void RadioComm::begin(int PPM_Pin) { } } - - - - void RadioComm::getCommands(unsigned int* returnArray) { - //DESCRIPTION: Get raw PWM values for every channel from the radio - /* - * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of - * the loop. If using a PWM or PPM receiver, the radio commands are retrieved from a function in the readPWM file separate from this one which - * is running a bunch of interrupts to continuously update the radio readings. If using an SBUS receiver, the alues are pulled from the SBUS library directly. - * The raw radio commands are filtered with a first order low-pass filter to eliminate any really high frequency noise. - */ + //DESCRIPTION: Get raw PWM values for every channel from the radio + /* + * Updates radio PWM commands in loop based on current available commands. The array channel_pwm[] is the raw command used in the rest of the loop. + * The raw radio commands of the first four control channels are filtered with a first order low-pass filter to eliminate any really high frequency noise. + */ int8_t i; if (_type == sBUS) { @@ -88,7 +82,6 @@ void RadioComm::getCommands(unsigned int* returnArray) { // _sbusFailSafe = _sbus_data.failsafe; // _sbusLostFrame = _sbus_data.lost_frame; - if (_sbus->read(_sbusChannels, &_sbusFailSafe, &_sbusLostFrame)) { for (i = 0; i < _num_channels; i++) { channel_pwm[i] = _sbusChannels[i] * _sbus_scale + _sbus_bias; @@ -116,8 +109,6 @@ void RadioComm::getCommands(unsigned int* returnArray) { } } - - else { // Should probably have an error here } @@ -139,15 +130,22 @@ void RadioComm::getCommands(unsigned int* returnArray) { void RadioComm::setFailsafe(unsigned int* failsafeArray) { + //DESCRIPTION: Used after initialization to set failsafe values for the receiver + /* + * This sets the failsafe values, typically throttle off and control surfaces neutral, so that in the event of a loss + * of radio signal, the craft will likely crash instead of fly away. + * + * Input values should be in the range of 1000-2000 microseconds, as normally delivered by the RC receiver. + * 1000 us == low throttle position + * 1500 us == center servo position + */ + for (int8_t i=0; i<_num_channels; i++) { _channel_fs[i] = failsafeArray[i]; } } - - - void RadioComm::failSafe() { //DESCRIPTION: If radio gives garbage values, set all commands to default values /* @@ -164,7 +162,7 @@ void RadioComm::failSafe() { */ int8_t i; - //If using SBUS, this will only check the boolean failsafe value from the SBUS library. If using PWM RX, it checks each channel. + //If using SBUS, this will only check the boolean failsafe value from the SBUS library. If using PPM/DSM RX, it checks each channel. if (_type == sBUS) { if (_sbusFailSafe) { for (i=0; i<_num_channels; i++) { @@ -197,5 +195,3 @@ void RadioComm::failSafe() { } } } - - diff --git a/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.h b/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.h index dfb46f2d..406fcd6c 100644 --- a/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.h +++ b/Versions/dRehmFlight_Teensy_BETA_2.0/src/RadioComm/RadioComm.h @@ -1,8 +1,13 @@ +//Arduino/Teensy Flight Controller - dRehmFlight +//Author: Nicholas Rehm +//Additional Author: Brian Jones +//Project Start: 1/6/2020 +//Last Updated: 8/11/2025 +//Version: Beta 2.0 +//========================================================================================================================// -// RadioComm.h -// This is a wrapper for IMU chips supported by dRehmFlight. It consolidates all the IMU handling and PID subrutines into a single library that most people won't need to mess with. - +//This file contains all necessary functions and code used for radio communication to avoid cluttering the main code #ifndef RadioComm_h #define RadioComm_h @@ -11,37 +16,30 @@ // Include all possible radio libraries #include "../SBUS/SBUS.h" -// #include "sbus.h" // Bolder Flight Systems SBUS library +// #include "sbus.h" // Bolder Flight Systems SBUS library -- had issues with loop timing using this library #include #include "../DSMRX/DSMRX.h" - - - enum RadioType { PPM, DSM, sBUS + // Removed PWM option because 1) it's not very popular anymore, and 2) The ISRs required to make it work were difficult to implement. }; - - class RadioComm { public: RadioComm(RadioType type); RadioComm(RadioType type, uint8_t num_channels); - void begin(); //DSM & //SBUS + void begin(); //DSM & //SBUS void begin(int PPM_Pin); //PPM void getCommands(unsigned int* returnArray); void setFailsafe(unsigned int* failsafeArray); void failSafe(); - unsigned long channel_pwm[16] = {0}; - - private: RadioType _type; From 136498fed29229878e93fe8ad05e56af7ba779b1 Mon Sep 17 00:00:00 2001 From: Brian Jones Date: Wed, 13 Aug 2025 08:22:01 -0500 Subject: [PATCH 6/6] Removed old versions --- .../dRehmFlight_Teensy_BETA_1.0/COPYING.txt | 674 ---- .../dRehmFlight_Teensy_BETA_1.0.ino | 1054 ------ .../dRehmFlight_Teensy_BETA_1.0/radioComm.ino | 173 - .../dRehmFlight_Teensy_BETA_1.1/COPYING.txt | 674 ---- .../dRehmFlight_Teensy_BETA_1.1.ino | 1218 ------ .../dRehmFlight_Teensy_BETA_1.1/radioComm.ino | 179 - .../dRehmFlight_Teensy_BETA_1.2/COPYING.txt | 674 ---- .../dRehmFlight_Teensy_BETA_1.2.ino | 1585 -------- .../dRehmFlight_Teensy_BETA_1.2/radioComm.ino | 184 - .../src/MPU6050/I2Cdev.cpp | 1468 -------- .../src/MPU6050/I2Cdev.h | 283 -- .../src/MPU6050/MPU6050.cpp | 3330 ----------------- .../src/MPU6050/MPU6050.h | 1041 ------ .../src/MPU6050/MPU6050_6Axis_MotionApps20.h | 617 --- .../MPU6050/MPU6050_6Axis_MotionApps_V6_12.h | 617 --- .../src/MPU6050/MPU6050_9Axis_MotionApps41.h | 887 ----- .../src/MPU6050/helper_3dmath.h | 216 -- .../src/MPU9250/MPU9250.cpp | 1202 ------ .../src/MPU9250/MPU9250.h | 313 -- .../src/SBUS/SBUS.cpp | 376 -- .../src/SBUS/SBUS.h | 82 - .../src/SBUS/elapsedMillis.h | 81 - 22 files changed, 16928 deletions(-) delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.0/COPYING.txt delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.0/dRehmFlight_Teensy_BETA_1.0.ino delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.0/radioComm.ino delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.1/COPYING.txt delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.1/dRehmFlight_Teensy_BETA_1.1.ino delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.1/radioComm.ino delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/COPYING.txt delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/dRehmFlight_Teensy_BETA_1.2.ino delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/radioComm.ino delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/I2Cdev.cpp delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/I2Cdev.h delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050.cpp delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050.h delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050_6Axis_MotionApps20.h delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050_9Axis_MotionApps41.h delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/helper_3dmath.h delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU9250/MPU9250.cpp delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU9250/MPU9250.h delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/src/SBUS/SBUS.cpp delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/src/SBUS/SBUS.h delete mode 100644 Versions/dRehmFlight_Teensy_BETA_1.2/src/SBUS/elapsedMillis.h diff --git a/Versions/dRehmFlight_Teensy_BETA_1.0/COPYING.txt b/Versions/dRehmFlight_Teensy_BETA_1.0/COPYING.txt deleted file mode 100644 index 80e659dc..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.0/COPYING.txt +++ /dev/null @@ -1,674 +0,0 @@ -GNU GENERAL PUBLIC LICENSE - Version 3, 29 June 2007 - - Copyright (C) 2007 Free Software Foundation, Inc. - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The GNU General Public License is a free, copyleft license for -software and other kinds of works. - - The licenses for most software and other practical works are designed -to take away your freedom to share and change the works. By contrast, -the GNU General Public License is intended to guarantee your freedom to -share and change all versions of a program--to make sure it remains free -software for all its users. We, the Free Software Foundation, use the -GNU General Public License for most of our software; it applies also to -any other work released this way by its authors. You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -them if you wish), that you receive source code or can get it if you -want it, that you can change the software or use pieces of it in new -free programs, and that you know you can do these things. - - To protect your rights, we need to prevent others from denying you -these rights or asking you to surrender the rights. Therefore, you have -certain responsibilities if you distribute copies of the software, or if -you modify it: responsibilities to respect the freedom of others. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must pass on to the recipients the same -freedoms that you received. You must make sure that they, too, receive -or can get the source code. And you must show them these terms so they -know their rights. - - Developers that use the GNU GPL protect your rights with two steps: -(1) assert copyright on the software, and (2) offer you this License -giving you legal permission to copy, distribute and/or modify it. - - For the developers' and authors' protection, the GPL clearly explains -that there is no warranty for this free software. For both users' and -authors' sake, the GPL requires that modified versions be marked as -changed, so that their problems will not be attributed erroneously to -authors of previous versions. - - Some devices are designed to deny users access to install or run -modified versions of the software inside them, although the manufacturer -can do so. This is fundamentally incompatible with the aim of -protecting users' freedom to change the software. The systematic -pattern of such abuse occurs in the area of products for individuals to -use, which is precisely where it is most unacceptable. Therefore, we -have designed this version of the GPL to prohibit the practice for those -products. If such problems arise substantially in other domains, we -stand ready to extend this provision to those domains in future versions -of the GPL, as needed to protect the freedom of users. - - Finally, every program is threatened constantly by software patents. -States should not allow patents to restrict development and use of -software on general-purpose computers, but in those that do, we wish to -avoid the special danger that patents applied to a free program could -make it effectively proprietary. To prevent this, the GPL assures that -patents cannot be used to render the program non-free. - - The precise terms and conditions for copying, distribution and -modification follow. - - TERMS AND CONDITIONS - - 0. Definitions. - - "This License" refers to version 3 of the GNU General Public License. - - "Copyright" also means copyright-like laws that apply to other kinds of -works, such as semiconductor masks. - - "The Program" refers to any copyrightable work licensed under this -License. Each licensee is addressed as "you". "Licensees" and -"recipients" may be individuals or organizations. - - To "modify" a work means to copy from or adapt all or part of the work -in a fashion requiring copyright permission, other than the making of an -exact copy. The resulting work is called a "modified version" of the -earlier work or a work "based on" the earlier work. - - A "covered work" means either the unmodified Program or a work based -on the Program. - - To "propagate" a work means to do anything with it that, without -permission, would make you directly or secondarily liable for -infringement under applicable copyright law, except executing it on a -computer or modifying a private copy. Propagation includes copying, -distribution (with or without modification), making available to the -public, and in some countries other activities as well. - - To "convey" a work means any kind of propagation that enables other -parties to make or receive copies. Mere interaction with a user through -a computer network, with no transfer of a copy, is not conveying. - - An interactive user interface displays "Appropriate Legal Notices" -to the extent that it includes a convenient and prominently visible -feature that (1) displays an appropriate copyright notice, and (2) -tells the user that there is no warranty for the work (except to the -extent that warranties are provided), that licensees may convey the -work under this License, and how to view a copy of this License. If -the interface presents a list of user commands or options, such as a -menu, a prominent item in the list meets this criterion. - - 1. Source Code. - - The "source code" for a work means the preferred form of the work -for making modifications to it. "Object code" means any non-source -form of a work. - - A "Standard Interface" means an interface that either is an official -standard defined by a recognized standards body, or, in the case of -interfaces specified for a particular programming language, one that -is widely used among developers working in that language. - - The "System Libraries" of an executable work include anything, other -than the work as a whole, that (a) is included in the normal form of -packaging a Major Component, but which is not part of that Major -Component, and (b) serves only to enable use of the work with that -Major Component, or to implement a Standard Interface for which an -implementation is available to the public in source code form. A -"Major Component", in this context, means a major essential component -(kernel, window system, and so on) of the specific operating system -(if any) on which the executable work runs, or a compiler used to -produce the work, or an object code interpreter used to run it. - - The "Corresponding Source" for a work in object code form means all -the source code needed to generate, install, and (for an executable -work) run the object code and to modify the work, including scripts to -control those activities. However, it does not include the work's -System Libraries, or general-purpose tools or generally available free -programs which are used unmodified in performing those activities but -which are not part of the work. For example, Corresponding Source -includes interface definition files associated with source files for -the work, and the source code for shared libraries and dynamically -linked subprograms that the work is specifically designed to require, -such as by intimate data communication or control flow between those -subprograms and other parts of the work. - - The Corresponding Source need not include anything that users -can regenerate automatically from other parts of the Corresponding -Source. - - The Corresponding Source for a work in source code form is that -same work. - - 2. Basic Permissions. - - All rights granted under this License are granted for the term of -copyright on the Program, and are irrevocable provided the stated -conditions are met. This License explicitly affirms your unlimited -permission to run the unmodified Program. The output from running a -covered work is covered by this License only if the output, given its -content, constitutes a covered work. This License acknowledges your -rights of fair use or other equivalent, as provided by copyright law. - - You may make, run and propagate covered works that you do not -convey, without conditions so long as your license otherwise remains -in force. You may convey covered works to others for the sole purpose -of having them make modifications exclusively for you, or provide you -with facilities for running those works, provided that you comply with -the terms of this License in conveying all material for which you do -not control copyright. Those thus making or running the covered works -for you must do so exclusively on your behalf, under your direction -and control, on terms that prohibit them from making any copies of -your copyrighted material outside their relationship with you. - - Conveying under any other circumstances is permitted solely under -the conditions stated below. Sublicensing is not allowed; section 10 -makes it unnecessary. - - 3. Protecting Users' Legal Rights From Anti-Circumvention Law. - - No covered work shall be deemed part of an effective technological -measure under any applicable law fulfilling obligations under article -11 of the WIPO copyright treaty adopted on 20 December 1996, or -similar laws prohibiting or restricting circumvention of such -measures. - - When you convey a covered work, you waive any legal power to forbid -circumvention of technological measures to the extent such circumvention -is effected by exercising rights under this License with respect to -the covered work, and you disclaim any intention to limit operation or -modification of the work as a means of enforcing, against the work's -users, your or third parties' legal rights to forbid circumvention of -technological measures. - - 4. Conveying Verbatim Copies. - - You may convey verbatim copies of the Program's source code as you -receive it, in any medium, provided that you conspicuously and -appropriately publish on each copy an appropriate copyright notice; -keep intact all notices stating that this License and any -non-permissive terms added in accord with section 7 apply to the code; -keep intact all notices of the absence of any warranty; and give all -recipients a copy of this License along with the Program. - - You may charge any price or no price for each copy that you convey, -and you may offer support or warranty protection for a fee. - - 5. Conveying Modified Source Versions. - - You may convey a work based on the Program, or the modifications to -produce it from the Program, in the form of source code under the -terms of section 4, provided that you also meet all of these conditions: - - a) The work must carry prominent notices stating that you modified - it, and giving a relevant date. - - b) The work must carry prominent notices stating that it is - released under this License and any conditions added under section - 7. This requirement modifies the requirement in section 4 to - "keep intact all notices". - - c) You must license the entire work, as a whole, under this - License to anyone who comes into possession of a copy. This - License will therefore apply, along with any applicable section 7 - additional terms, to the whole of the work, and all its parts, - regardless of how they are packaged. This License gives no - permission to license the work in any other way, but it does not - invalidate such permission if you have separately received it. - - d) If the work has interactive user interfaces, each must display - Appropriate Legal Notices; however, if the Program has interactive - interfaces that do not display Appropriate Legal Notices, your - work need not make them do so. - - A compilation of a covered work with other separate and independent -works, which are not by their nature extensions of the covered work, -and which are not combined with it such as to form a larger program, -in or on a volume of a storage or distribution medium, is called an -"aggregate" if the compilation and its resulting copyright are not -used to limit the access or legal rights of the compilation's users -beyond what the individual works permit. Inclusion of a covered work -in an aggregate does not cause this License to apply to the other -parts of the aggregate. - - 6. Conveying Non-Source Forms. - - You may convey a covered work in object code form under the terms -of sections 4 and 5, provided that you also convey the -machine-readable Corresponding Source under the terms of this License, -in one of these ways: - - a) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by the - Corresponding Source fixed on a durable physical medium - customarily used for software interchange. - - b) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by a - written offer, valid for at least three years and valid for as - long as you offer spare parts or customer support for that product - model, to give anyone who possesses the object code either (1) a - copy of the Corresponding Source for all the software in the - product that is covered by this License, on a durable physical - medium customarily used for software interchange, for a price no - more than your reasonable cost of physically performing this - conveying of source, or (2) access to copy the - Corresponding Source from a network server at no charge. - - c) Convey individual copies of the object code with a copy of the - written offer to provide the Corresponding Source. This - alternative is allowed only occasionally and noncommercially, and - only if you received the object code with such an offer, in accord - with subsection 6b. - - d) Convey the object code by offering access from a designated - place (gratis or for a charge), and offer equivalent access to the - Corresponding Source in the same way through the same place at no - further charge. You need not require recipients to copy the - Corresponding Source along with the object code. If the place to - copy the object code is a network server, the Corresponding Source - may be on a different server (operated by you or a third party) - that supports equivalent copying facilities, provided you maintain - clear directions next to the object code saying where to find the - Corresponding Source. Regardless of what server hosts the - Corresponding Source, you remain obligated to ensure that it is - available for as long as needed to satisfy these requirements. - - e) Convey the object code using peer-to-peer transmission, provided - you inform other peers where the object code and Corresponding - Source of the work are being offered to the general public at no - charge under subsection 6d. - - A separable portion of the object code, whose source code is excluded -from the Corresponding Source as a System Library, need not be -included in conveying the object code work. - - A "User Product" is either (1) a "consumer product", which means any -tangible personal property which is normally used for personal, family, -or household purposes, or (2) anything designed or sold for incorporation -into a dwelling. In determining whether a product is a consumer product, -doubtful cases shall be resolved in favor of coverage. For a particular -product received by a particular user, "normally used" refers to a -typical or common use of that class of product, regardless of the status -of the particular user or of the way in which the particular user -actually uses, or expects or is expected to use, the product. A product -is a consumer product regardless of whether the product has substantial -commercial, industrial or non-consumer uses, unless such uses represent -the only significant mode of use of the product. - - "Installation Information" for a User Product means any methods, -procedures, authorization keys, or other information required to install -and execute modified versions of a covered work in that User Product from -a modified version of its Corresponding Source. The information must -suffice to ensure that the continued functioning of the modified object -code is in no case prevented or interfered with solely because -modification has been made. - - If you convey an object code work under this section in, or with, or -specifically for use in, a User Product, and the conveying occurs as -part of a transaction in which the right of possession and use of the -User Product is transferred to the recipient in perpetuity or for a -fixed term (regardless of how the transaction is characterized), the -Corresponding Source conveyed under this section must be accompanied -by the Installation Information. But this requirement does not apply -if neither you nor any third party retains the ability to install -modified object code on the User Product (for example, the work has -been installed in ROM). - - The requirement to provide Installation Information does not include a -requirement to continue to provide support service, warranty, or updates -for a work that has been modified or installed by the recipient, or for -the User Product in which it has been modified or installed. Access to a -network may be denied when the modification itself materially and -adversely affects the operation of the network or violates the rules and -protocols for communication across the network. - - Corresponding Source conveyed, and Installation Information provided, -in accord with this section must be in a format that is publicly -documented (and with an implementation available to the public in -source code form), and must require no special password or key for -unpacking, reading or copying. - - 7. Additional Terms. - - "Additional permissions" are terms that supplement the terms of this -License by making exceptions from one or more of its conditions. -Additional permissions that are applicable to the entire Program shall -be treated as though they were included in this License, to the extent -that they are valid under applicable law. If additional permissions -apply only to part of the Program, that part may be used separately -under those permissions, but the entire Program remains governed by -this License without regard to the additional permissions. - - When you convey a copy of a covered work, you may at your option -remove any additional permissions from that copy, or from any part of -it. (Additional permissions may be written to require their own -removal in certain cases when you modify the work.) You may place -additional permissions on material, added by you to a covered work, -for which you have or can give appropriate copyright permission. - - Notwithstanding any other provision of this License, for material you -add to a covered work, you may (if authorized by the copyright holders of -that material) supplement the terms of this License with terms: - - a) Disclaiming warranty or limiting liability differently from the - terms of sections 15 and 16 of this License; or - - b) Requiring preservation of specified reasonable legal notices or - author attributions in that material or in the Appropriate Legal - Notices displayed by works containing it; or - - c) Prohibiting misrepresentation of the origin of that material, or - requiring that modified versions of such material be marked in - reasonable ways as different from the original version; or - - d) Limiting the use for publicity purposes of names of licensors or - authors of the material; or - - e) Declining to grant rights under trademark law for use of some - trade names, trademarks, or service marks; or - - f) Requiring indemnification of licensors and authors of that - material by anyone who conveys the material (or modified versions of - it) with contractual assumptions of liability to the recipient, for - any liability that these contractual assumptions directly impose on - those licensors and authors. - - All other non-permissive additional terms are considered "further -restrictions" within the meaning of section 10. If the Program as you -received it, or any part of it, contains a notice stating that it is -governed by this License along with a term that is a further -restriction, you may remove that term. If a license document contains -a further restriction but permits relicensing or conveying under this -License, you may add to a covered work material governed by the terms -of that license document, provided that the further restriction does -not survive such relicensing or conveying. - - If you add terms to a covered work in accord with this section, you -must place, in the relevant source files, a statement of the -additional terms that apply to those files, or a notice indicating -where to find the applicable terms. - - Additional terms, permissive or non-permissive, may be stated in the -form of a separately written license, or stated as exceptions; -the above requirements apply either way. - - 8. Termination. - - You may not propagate or modify a covered work except as expressly -provided under this License. Any attempt otherwise to propagate or -modify it is void, and will automatically terminate your rights under -this License (including any patent licenses granted under the third -paragraph of section 11). - - However, if you cease all violation of this License, then your -license from a particular copyright holder is reinstated (a) -provisionally, unless and until the copyright holder explicitly and -finally terminates your license, and (b) permanently, if the copyright -holder fails to notify you of the violation by some reasonable means -prior to 60 days after the cessation. - - Moreover, your license from a particular copyright holder is -reinstated permanently if the copyright holder notifies you of the -violation by some reasonable means, this is the first time you have -received notice of violation of this License (for any work) from that -copyright holder, and you cure the violation prior to 30 days after -your receipt of the notice. - - Termination of your rights under this section does not terminate the -licenses of parties who have received copies or rights from you under -this License. If your rights have been terminated and not permanently -reinstated, you do not qualify to receive new licenses for the same -material under section 10. - - 9. Acceptance Not Required for Having Copies. - - You are not required to accept this License in order to receive or -run a copy of the Program. Ancillary propagation of a covered work -occurring solely as a consequence of using peer-to-peer transmission -to receive a copy likewise does not require acceptance. However, -nothing other than this License grants you permission to propagate or -modify any covered work. These actions infringe copyright if you do -not accept this License. Therefore, by modifying or propagating a -covered work, you indicate your acceptance of this License to do so. - - 10. Automatic Licensing of Downstream Recipients. - - Each time you convey a covered work, the recipient automatically -receives a license from the original licensors, to run, modify and -propagate that work, subject to this License. You are not responsible -for enforcing compliance by third parties with this License. - - An "entity transaction" is a transaction transferring control of an -organization, or substantially all assets of one, or subdividing an -organization, or merging organizations. If propagation of a covered -work results from an entity transaction, each party to that -transaction who receives a copy of the work also receives whatever -licenses to the work the party's predecessor in interest had or could -give under the previous paragraph, plus a right to possession of the -Corresponding Source of the work from the predecessor in interest, if -the predecessor has it or can get it with reasonable efforts. - - You may not impose any further restrictions on the exercise of the -rights granted or affirmed under this License. For example, you may -not impose a license fee, royalty, or other charge for exercise of -rights granted under this License, and you may not initiate litigation -(including a cross-claim or counterclaim in a lawsuit) alleging that -any patent claim is infringed by making, using, selling, offering for -sale, or importing the Program or any portion of it. - - 11. Patents. - - A "contributor" is a copyright holder who authorizes use under this -License of the Program or a work on which the Program is based. The -work thus licensed is called the contributor's "contributor version". - - A contributor's "essential patent claims" are all patent claims -owned or controlled by the contributor, whether already acquired or -hereafter acquired, that would be infringed by some manner, permitted -by this License, of making, using, or selling its contributor version, -but do not include claims that would be infringed only as a -consequence of further modification of the contributor version. For -purposes of this definition, "control" includes the right to grant -patent sublicenses in a manner consistent with the requirements of -this License. - - Each contributor grants you a non-exclusive, worldwide, royalty-free -patent license under the contributor's essential patent claims, to -make, use, sell, offer for sale, import and otherwise run, modify and -propagate the contents of its contributor version. - - In the following three paragraphs, a "patent license" is any express -agreement or commitment, however denominated, not to enforce a patent -(such as an express permission to practice a patent or covenant not to -sue for patent infringement). To "grant" such a patent license to a -party means to make such an agreement or commitment not to enforce a -patent against the party. - - If you convey a covered work, knowingly relying on a patent license, -and the Corresponding Source of the work is not available for anyone -to copy, free of charge and under the terms of this License, through a -publicly available network server or other readily accessible means, -then you must either (1) cause the Corresponding Source to be so -available, or (2) arrange to deprive yourself of the benefit of the -patent license for this particular work, or (3) arrange, in a manner -consistent with the requirements of this License, to extend the patent -license to downstream recipients. "Knowingly relying" means you have -actual knowledge that, but for the patent license, your conveying the -covered work in a country, or your recipient's use of the covered work -in a country, would infringe one or more identifiable patents in that -country that you have reason to believe are valid. - - If, pursuant to or in connection with a single transaction or -arrangement, you convey, or propagate by procuring conveyance of, a -covered work, and grant a patent license to some of the parties -receiving the covered work authorizing them to use, propagate, modify -or convey a specific copy of the covered work, then the patent license -you grant is automatically extended to all recipients of the covered -work and works based on it. - - A patent license is "discriminatory" if it does not include within -the scope of its coverage, prohibits the exercise of, or is -conditioned on the non-exercise of one or more of the rights that are -specifically granted under this License. You may not convey a covered -work if you are a party to an arrangement with a third party that is -in the business of distributing software, under which you make payment -to the third party based on the extent of your activity of conveying -the work, and under which the third party grants, to any of the -parties who would receive the covered work from you, a discriminatory -patent license (a) in connection with copies of the covered work -conveyed by you (or copies made from those copies), or (b) primarily -for and in connection with specific products or compilations that -contain the covered work, unless you entered into that arrangement, -or that patent license was granted, prior to 28 March 2007. - - Nothing in this License shall be construed as excluding or limiting -any implied license or other defenses to infringement that may -otherwise be available to you under applicable patent law. - - 12. No Surrender of Others' Freedom. - - If conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot convey a -covered work so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you may -not convey it at all. For example, if you agree to terms that obligate you -to collect a royalty for further conveying from those to whom you convey -the Program, the only way you could satisfy both those terms and this -License would be to refrain entirely from conveying the Program. - - 13. Use with the GNU Affero General Public License. - - Notwithstanding any other provision of this License, you have -permission to link or combine any covered work with a work licensed -under version 3 of the GNU Affero General Public License into a single -combined work, and to convey the resulting work. The terms of this -License will continue to apply to the part which is the covered work, -but the special requirements of the GNU Affero General Public License, -section 13, concerning interaction through a network will apply to the -combination as such. - - 14. Revised Versions of this License. - - The Free Software Foundation may publish revised and/or new versions of -the GNU General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - - Each version is given a distinguishing version number. If the -Program specifies that a certain numbered version of the GNU General -Public License "or any later version" applies to it, you have the -option of following the terms and conditions either of that numbered -version or of any later version published by the Free Software -Foundation. If the Program does not specify a version number of the -GNU General Public License, you may choose any version ever published -by the Free Software Foundation. - - If the Program specifies that a proxy can decide which future -versions of the GNU General Public License can be used, that proxy's -public statement of acceptance of a version permanently authorizes you -to choose that version for the Program. - - Later license versions may give you additional or different -permissions. However, no additional obligations are imposed on any -author or copyright holder as a result of your choosing to follow a -later version. - - 15. Disclaimer of Warranty. - - THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY -APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT -HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY -OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, -THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM -IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF -ALL NECESSARY SERVICING, REPAIR OR CORRECTION. - - 16. Limitation of Liability. - - IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS -THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY -GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE -USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF -DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD -PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), -EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF -SUCH DAMAGES. - - 17. Interpretation of Sections 15 and 16. - - If the disclaimer of warranty and limitation of liability provided -above cannot be given local legal effect according to their terms, -reviewing courts shall apply local law that most closely approximates -an absolute waiver of all civil liability in connection with the -Program, unless a warranty or assumption of liability accompanies a -copy of the Program in return for a fee. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -state the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - - Copyright (C) - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - -Also add information on how to contact you by electronic and paper mail. - - If the program does terminal interaction, make it output a short -notice like this when it starts in an interactive mode: - - Copyright (C) - This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, your program's commands -might be different; for a GUI interface, you would use an "about box". - - You should also get your employer (if you work as a programmer) or school, -if any, to sign a "copyright disclaimer" for the program, if necessary. -For more information on this, and how to apply and follow the GNU GPL, see -. - - The GNU General Public License does not permit incorporating your program -into proprietary programs. If your program is a subroutine library, you -may consider it more useful to permit linking proprietary applications with -the library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. But first, please read -. \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_1.0/dRehmFlight_Teensy_BETA_1.0.ino b/Versions/dRehmFlight_Teensy_BETA_1.0/dRehmFlight_Teensy_BETA_1.0.ino deleted file mode 100644 index a6d8c478..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.0/dRehmFlight_Teensy_BETA_1.0.ino +++ /dev/null @@ -1,1054 +0,0 @@ -//Arduino/Teensy Flight Controller - dRehmFlight -//Author: Nicholas Rehm -//Project Start: 1/6/2020 -//Version: Beta 1.0 - -/* - * - * If you are using this for an academic or scholarly project, please credit me in any presentations or publications: - * - * Nicholas Rehm - * Department of Aerospace Engineering - * University of Maryland - * College Park 20742 - * Email: nrehm@umd.edu - * - */ - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -//CREDITS -/* -Some elements inspired by: -http://www.brokking.net/ymfc-al_downloads.html - -Skeleton code for reading and initializing MPU6050 borrowed from: -https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/ - -Madgwick filter function adapted from: -https://github.com/arduino-libraries/MadgwickAHRS/blob/master/src/MadgwickAHRS.cpp -*/ - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -//REQUIRED LIBRARIES -#include //I2c communication -#include //commanding any extra actuators, installed with teensyduino installer - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -//DECLARE PINS -//Radio: -const int ch1Pin = 9; -const int ch2Pin = 10; -const int ch3Pin = 11; -const int ch4Pin = 12; -const int ch5Pin = 22; -const int ch6Pin = 23; -const int PPM_Pin = 8; -//MPU6050: -const int MPU = 0x68; //MPU6050 I2C address -- pins A5 = SCL & A4 = SDA -//Motor pin outputs: -const int m1Pin = 2; -const int m2Pin = 3; -const int m3Pin = 4; -const int m4Pin = 5; -//PWM outputs: -const int servo1Pin = 6; -PWMServo servo1; //create servo object to control a servo - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -//DECLARE GLOBAL VARIABLES -//General stuff, DO NOT MODIFY: -float dt; -unsigned long current_time, prev_time; -unsigned long print_counter, serial_counter; -unsigned long blink_counter, blink_delay; -bool blinkAlternate; -//Radio comm: -unsigned long channel_1_pwm, channel_2_pwm, channel_3_pwm, channel_4_pwm, channel_5_pwm, channel_6_pwm; -unsigned long channel_1_pwm_prev, channel_2_pwm_prev, channel_3_pwm_prev, channel_4_pwm_prev; -int radio_command = 1; -//IMU: -float AccX, AccY, AccZ; -float AccX_prev, AccY_prev, AccZ_prev; -float GyroX, GyroY, GyroZ; -float GyroX_prev, GyroY_prev, GyroZ_prev; -float roll_IMU, pitch_IMU, yaw_IMU; -float roll_IMU_prev, pitch_IMU_prev; -float AccErrorX, AccErrorY, AccErrorZ, GyroErrorX, GyroErrorY, GyroErrorZ; -float roll_correction, pitch_correction; -float beta = 0.04; //madgwick filter parameter -float q0 = 1.0f; //initialize quaternion for madgwick filter -float q1 = 0.0f; -float q2 = 0.0f; -float q3 = 0.0f; -//Normalized desired state: -float thro_des, roll_des, pitch_des, yaw_des; -//Controller: -float error_roll, error_roll_prev, roll_des_prev, integral_roll, integral_roll_il, integral_roll_ol, integral_roll_prev, integral_roll_prev_il, integral_roll_prev_ol, derivative_roll, roll_PID = 0; -float error_pitch, error_pitch_prev, pitch_des_prev, integral_pitch, integral_pitch_il, integral_pitch_ol, integral_pitch_prev, integral_pitch_prev_il, integral_pitch_prev_ol, derivative_pitch, pitch_PID = 0; -float error_yaw, error_yaw_prev, integral_yaw, integral_yaw_prev, derivative_yaw, yaw_PID = 0; -//Mixer -float m1_command_scaled, m2_command_scaled, m3_command_scaled, m4_command_scaled; -int m1_command_PWM, m2_command_PWM, m3_command_PWM, m4_command_PWM; - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -//USER-SPECIFIED VARIABLES -//Radio failsafe values for every channel in the event that bad reciever data is detected. Recommended defaults: -unsigned long channel_1_fs = 1000; //thro -unsigned long channel_2_fs = 1500; //ail -unsigned long channel_3_fs = 1500; //elev -unsigned long channel_4_fs = 1500; //rudd -unsigned long channel_5_fs = 2000; //gear -unsigned long channel_6_fs = 2000; //aux2, high = throttle cut - -//Controller parameters: -float i_limit = 25.0; //integrator saturation level, mostly for safety (default 25.0) -float maxRoll = 20.0; //max roll angle in degrees for angle mode, deg/sec for rate mode -float maxPitch = 20.0; //max pitch angle in degrees for angle mode, deg/sec for rate mode -float maxYaw = 160.0; //max yaw rate in degrees/sec - -float Kp_roll_angle = 0.2; //Roll P-gain - angle mode -float Ki_roll_angle = 0.3; //Roll I-gain - angle mode -float Kd_roll_angle = 0.05; //Roll D-gain - angle mode (if using controlANGLE2, MUST be 0.0. Suggested default for controlANGLE is 0.05) -float Kp_pitch_angle = 0.2; //Pitch P-gain - angle mode -float Ki_pitch_angle = 0.3; //Pitch I-gain - angle mode -float Kd_pitch_angle = 0.05; //Pitch D-gain - angle mode (if using controlANGLE2, MUST be 0.0. Suggested default for controlANGLE is 0.05) - -float Kp_roll_rate = 0.15; //Roll P-gain - rate mode -float Ki_roll_rate = 0.2; //Roll I-gain - rate mode -float Kd_roll_rate = 0.0003; //Roll D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) -float Kp_pitch_rate = 0.15; //Pitch P-gain - rate mode -float Ki_pitch_rate = 0.2; //Pitch I-gain - rate mode -float Kd_pitch_rate = 0.0003; //Pitch D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) - -float Kp_yaw = 0.3; //Yaw P-gain -float Ki_yaw = 0.05; //Yaw I-gain -float Kd_yaw = 0.00015; //Yaw D-gain (be careful when increasing too high, motors will begin to overheat!) - - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -//SETUP -void setup() { - Serial.begin(500000); //usb serial - delay(1500); - - //Initialize all pins - pinMode(13, OUTPUT); //pin 13 LED blinker on board, do not modify - pinMode(m1Pin, OUTPUT); - pinMode(m2Pin, OUTPUT); - pinMode(m3Pin, OUTPUT); - pinMode(m4Pin, OUTPUT); - servo1.attach(servo1Pin, 1000, 2000); //pin, min value, max value - - //Set built in LED to high to signal startup & not to disturb vehicle during IMU calibration - digitalWrite(13, HIGH); - - delay(20); - - //Initialize radio communication - readPWM_setup(ch1Pin, ch2Pin, ch3Pin, ch4Pin, ch5Pin, ch6Pin); //6 channel pwm receiver - //readPPM_setup(PPM_Pin) //uncomment and assign pin to PPM_Pin in setup if using ppm receiver - - //Set radio channels to default (safe) values - channel_1_pwm = channel_1_fs; - channel_2_pwm = channel_2_fs; - channel_3_pwm = channel_3_fs; - channel_4_pwm = channel_4_fs; - channel_5_pwm = channel_5_fs; - channel_6_pwm = channel_6_fs; - - //Initialize IMU communication - IMUinit(); - - delay(20); - - //Get IMU error to calibrate attitude, assuming vehicle is level - calculate_IMU_error(); - calibrateAttitude(); //helps to warm up IMU and madgwick filter - - delay(20); - - //Arm servo channels - servo1.write(90); //command servo angle from 0-180 degrees - - delay(20); - - //Arm motors - m1_command_PWM = 125; - m2_command_PWM = 125; - m3_command_PWM = 125; - m4_command_PWM = 125; - commandMotors(); - - delay(200); - - //Indicate entering main loop - setupBlink(3,150,70); //numBlinks, upTime (ms), downTime (ms) -} - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -//MAIN LOOP -void loop() { - prev_time = current_time; - current_time = micros(); - dt = (current_time - prev_time)/1000000.0; - - loopBlink(); //looks cool - - //Print data at 100hz (uncomment one at a time for troubleshooting) - printRadioData(); //radio pwm values - //printDesiredState(); //desired vehicle state commanded in either degrees or degrees/sec - //printGyroData(); //prints filtered gyro data direct from IMU - //printAccelData(); //prints filtered accelerometer data direct from IMU - //printRollPitchYaw(); //prints roll, pitch, and yaw angles in degrees from madgwick filter - //printPIDoutput(); //prints computed stabilized PID variables from controller and desired setpoint - //printMotorCommands(); //prints the values being written to the motors - //printLoopRate(); //prints the time between loops in us - - //Get vehicle state - getIMUdata(); //pulls raw gyro and accel data from IMU and LP filters to remove noise - Madgwick(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, dt); //updates roll_IMU, pitch_IMU, and yaw_IMU (degrees) - - //Compute desired state - getDesState(); //convert raw commands to normalized values based on saturated limits - - //PID Controller - controlANGLE(); //stabilize on angle setpoint - //controlANGLE2(); //stabilize on angle setpoint using cascaded method - //controlRATE(); //stablize on rate setpoint - - //Actuator mixing and scaling to PWM values - controlMixer(); //mixes PID outputs to scaled actuator commands - scaleCommands(); //scales motor commands to 125-250 range (oneshot125 protocol) - - //Throttle cut check - throttleCut(); //directly sets motor commands to low based on state of ch6 - - //Command actuators - commandMotors(); //sends command pulses to each motor pin - servo1.write(90); //fixed for now, can add servo command variables similar to motors and assign desired values in controlMixer() - - //Get vehicle commands for next loop iteration - getCommands(); //pulls current available radio commands - failSafe(); //prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup - - //Regulate loop rate - loopRate(2000); //do not exceed 2000Hz -} - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -//FUNCTIONS - -void IMUinit() { - //DESCRIPTION: Initialize IMU I2C connection - /* - * Don't worry about how this works - */ - Wire.begin(); //Initialize communication - delay(20); - Wire.setClock(1000000); //should move this outside this function... - delay(20); - Wire.beginTransmission(MPU); //Start communication with MPU6050 // MPU=0x68 - Wire.write(0x6B); //Talk to the register 6B - Wire.write(0x00); //Make reset - place a 0 into the 6B register - Wire.endTransmission(true); //End the transmission -} - -void getIMUdata() { - //DESCRIPTION: Request full dataset from IMU and LP filter gyro and accelerometer data - /* - * Reads accelerometer and gyro data from IMU as AccX, AccY, AccZ, GyroX, GyroY, GyroZ. These values are scaled - * according to the IMU datasheet to put them into correct units of g's and rad/sec. A simple first-order - * low-pass filter is used to get rid of high frequency noise in these raw signals. Generally you want to cut - * off everything past 80Hz, but if your loop rate is not fast enough, the low pass filter will cause a lag in - * the readings. The filter parameters B_gyro and B_accel are set to be good for a 2kHz loop rate. Finally, - * the constant errors found in calculate_IMU_error() on startup are subtracted from the readings. - */ - int16_t AcX,AcY,AcZ,GyX,GyY,GyZ; - - //Get accel data - Wire.beginTransmission(MPU); - Wire.write(0x3B); //Start with register 0x3B (ACCEL_XOUT_H) - Wire.endTransmission(false); - Wire.requestFrom(MPU, 6, true); //Read 6 registers total, each axis value is stored in 2 registers - //For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet - AcX = (Wire.read() << 8 | Wire.read()); //X-axis value - AcY = (Wire.read() << 8 | Wire.read()); //Y-axis value - AcZ = (Wire.read() << 8 | Wire.read()); //Z-axis value - AccX = AcX / 16384.0; - AccY = AcY / 16384.0; - AccZ = AcZ / 16384.0; - //LP filter accelerometer data - float B_accel = 0.14; //0.01 - AccX = (1.0 - B_accel)*AccX_prev + B_accel*AccX; - AccY = (1.0 - B_accel)*AccY_prev + B_accel*AccY; - AccZ = (1.0 - B_accel)*AccZ_prev + B_accel*AccZ; - AccX_prev = AccX; - AccY_prev = AccY; - AccZ_prev = AccZ; - //Correct the outputs with the calculated error values - AccX = AccX - AccErrorX; - AccY = AccY - AccErrorY; - - //Get gyro data - Wire.beginTransmission(MPU); - Wire.write(0x43); //Gyro data first register address 0x43 - Wire.endTransmission(false); - Wire.requestFrom(MPU, 6, true); //Read 4 registers total, each axis value is stored in 2 registers - //For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet - GyX = (Wire.read() << 8 | Wire.read()); - GyY = (Wire.read() << 8 | Wire.read()); - GyZ = (Wire.read() << 8 | Wire.read()); - GyroX = GyX / 131.0; - GyroY = GyY / 131.0; - GyroZ = GyZ / 131.0; - //LP filter gyro data - float B_gyro = 0.1; //0.13 sets cutoff just past 80Hz for about 3000Hz loop rate - GyroX = (1.0 - B_gyro)*GyroX_prev + B_gyro*GyroX; - GyroY = (1.0 - B_gyro)*GyroY_prev + B_gyro*GyroY; - GyroZ = (1.0 - B_gyro)*GyroZ_prev + B_gyro*GyroZ; - GyroX_prev = GyroX; - GyroY_prev = GyroY; - GyroZ_prev = GyroZ; - //Correct the outputs with the calculated error values - GyroX = GyroX - GyroErrorX; - GyroY = GyroY - GyroErrorY; - GyroZ = GyroZ - GyroErrorZ; -} - -void calculate_IMU_error() { - //DESCRIPTION: Computes IMU error on startup. Note: vehicle should be powered up on flat surface - /* - * Don't worry too much about what this is doing. The error values it computes are applied to the raw gyro and - * accelerometer values AccX, AccY, AccZ, GyroX, GyroY, GyroZ in getIMUdata(). This eliminates drift in the - * measurement. - */ - int16_t AcX,AcY,AcZ,GyX,GyY,GyZ; - - //Read accelerometer values 12000 times - int c = 0; - while (c < 12000) { - Wire.beginTransmission(MPU); - Wire.write(0x3B); - Wire.endTransmission(false); - Wire.requestFrom(MPU, 6, true); - AcX = (Wire.read() << 8 | Wire.read()); - AcY = (Wire.read() << 8 | Wire.read()); - AcZ = (Wire.read() << 8 | Wire.read()); - AccX = AcX / 16384.0; - AccY = AcY / 16384.0; - AccZ = AcZ / 16384.0; - // Sum all readings - AccErrorX = AccErrorX + AccX; - AccErrorY = AccErrorY + AccY; - AccErrorZ = AccErrorZ + AccZ; - c++; - } - //Divide the sum by 12000 to get the error value - AccErrorX = AccErrorX / 12000.0; - AccErrorY = AccErrorY / 12000.0; - AccErrorZ = AccErrorZ / 12000.0; - c = 0; - //Read gyro values 12000 times - while (c < 12000) { - Wire.beginTransmission(MPU); - Wire.write(0x43); - Wire.endTransmission(false); - Wire.requestFrom(MPU, 6, true); - GyX = Wire.read() << 8 | Wire.read(); - GyY = Wire.read() << 8 | Wire.read(); - GyZ = Wire.read() << 8 | Wire.read(); - GyroX = GyX / 131.0; - GyroY = GyY / 131.0; - GyroZ = GyZ / 131.0; - // Sum all readings - GyroErrorX = GyroErrorX + GyroX; - GyroErrorY = GyroErrorY + GyroY; - GyroErrorZ = GyroErrorZ + GyroZ; - c++; - } - //Divide the sum by 12000 to get the error value - GyroErrorX = GyroErrorX / 12000.0; - GyroErrorY = GyroErrorY / 12000.0; - GyroErrorZ = GyroErrorZ / 12000.0; -} - -void calibrateAttitude() { - //DESCRIPTION: Extra function to calibrate IMU attitude estimate on startup, can be used to warm up everything before entering main loop - //Assuming vehicle is powered up on level surface! - /* - * This function is used on startup to warm up the attitude estimation. Originally used to eliminate additional error in the signal - * but no longer used for that purpose as it is not needed on the Teensy. This function is what causes startup to take a few seconds - * to boot. The roll_correction and pitch_correction values can be applied to the roll and pitch attitude estimates using - * correctRollPitch() in the main loop after the madgwick filter function. Again, we don't use this for that purpose but just to warm - * up the IMU and attitude estimation algorithm. - */ - //Warm up IMU and madgwick filter - for (int i = 0; i <= 10000; i++) { - prev_time = current_time; - current_time = micros(); - dt = (current_time - prev_time)/1000000.0; - getIMUdata(); - Madgwick(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, dt); - loopRate(2000); //do not exceed 2000Hz - } - //Grab mean roll and pitch values after everything is warmed up - for (int j = 1; j <= 2000; j++) { - prev_time = current_time; - current_time = micros(); - dt = (current_time - prev_time)/1000000.0; - getIMUdata(); - Madgwick(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, dt); - roll_correction = roll_IMU + roll_correction; - pitch_correction = pitch_IMU + pitch_correction; - loopRate(2000); //do not exceed 2000Hz - } - //These are applied to roll and pitch after Madgwick filter in main loop if desired using correctRollPitch() - roll_correction = roll_correction/2000.0; - pitch_correction = pitch_correction/2000.0; -} - -void correctRollPitch() { - //DESCRIPTION: Correct roll and pitch values with corrections found in calibrateAttitude() on startup - /* - * Not in use. Delete later. - */ - roll_IMU = roll_IMU - roll_correction; - pitch_IMU = pitch_IMU - pitch_correction; -} - -void Madgwick(float gx, float gy, float gz, float ax, float ay, float az, float invSampleFreq) { - //DESCRIPTION: Attitude estimation through sensor fusion - /* - * This function fuses the accelerometer and gyro readings AccX, AccY, AccZ, GyroX, GyroY, GyroZ for attitude estimation. - * Don't worry about the math. There is a tunable parameter called beta in the variable declaring section which basically - * adjusts the weight of accelerometer and gyro data in the state estimate. Higher beta leads to noisier estimate, lower - * beta leads to slower to respond estimate. It is currently tuned for 2kHz loop rate. This function updates the roll_IMU, - * pitch_IMU, and yaw_IMU variables which are in degrees. - */ - float recipNorm; - float s0, s1, s2, s3; - float qDot1, qDot2, qDot3, qDot4; - float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; - - //Convert gyroscope degrees/sec to radians/sec - gx *= 0.0174533f; - gy *= 0.0174533f; - gz *= 0.0174533f; - - //Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); - qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); - qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); - qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); - - //Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - //Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - //Auxiliary variables to avoid repeated arithmetic - _2q0 = 2.0f * q0; - _2q1 = 2.0f * q1; - _2q2 = 2.0f * q2; - _2q3 = 2.0f * q3; - _4q0 = 4.0f * q0; - _4q1 = 4.0f * q1; - _4q2 = 4.0f * q2; - _8q1 = 8.0f * q1; - _8q2 = 8.0f * q2; - q0q0 = q0 * q0; - q1q1 = q1 * q1; - q2q2 = q2 * q2; - q3q3 = q3 * q3; - - //Gradient decent algorithm corrective step - s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; - s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; - s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; - s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); //normalise step magnitude - s0 *= recipNorm; - s1 *= recipNorm; - s2 *= recipNorm; - s3 *= recipNorm; - - //Apply feedback step - qDot1 -= beta * s0; - qDot2 -= beta * s1; - qDot3 -= beta * s2; - qDot4 -= beta * s3; - } - - //Integrate rate of change of quaternion to yield quaternion - q0 += qDot1 * invSampleFreq; - q1 += qDot2 * invSampleFreq; - q2 += qDot3 * invSampleFreq; - q3 += qDot4 * invSampleFreq; - - //Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - - //compute angles - roll_IMU = atan2(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)*57.29577951; - pitch_IMU = asin(-2.0f * (q1*q3 - q0*q2))*57.29577951; - yaw_IMU = atan2(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3)*57.29577951; -} - -void getDesState() { - //DESCRIPTION: Normalizes desired control values to appropriate values - /* - * Updates the desired state variables thro_des, roll_des, pitch_des, and yaw_des. These are computed by using the raw - * RC pwm commands and scaling them to be within our limits defined in setup. thro_des stays within 0 to 1 range. - * roll_des and pitch_des are scaled to be within max roll/pitch amount in either degrees (angle mode) or degrees/sec - * (rate mode). yaw_des is scaled to be within max yaw in degrees/sec. - */ - thro_des = (channel_1_pwm - 1000.0)/1000.0; //between 0 and 1 - roll_des = (channel_2_pwm - 1500.0)/500.0; //between -1 and 1 - pitch_des = (channel_3_pwm - 1500.0)/500.0; //between -1 and 1 - yaw_des = (channel_4_pwm - 1500.0)/500.0; //between -1 and 1 - //Constrain within normalized bounds - thro_des = constrain(thro_des, 0.0, 1.0); //between 0 and 1 - roll_des = constrain(roll_des, -1.0, 1.0)*maxRoll; //between -maxRoll and +maxRoll - pitch_des = constrain(pitch_des, -1.0, 1.0)*maxPitch; //between -maxPitch and +maxPitch - yaw_des = constrain(yaw_des, -1.0, 1.0)*maxYaw; //between -maxYaw and +maxYaw -} - -void controlANGLE() { - //DESCRIPTION: Computes control commands based on state error (angle) - /* - * Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des computed in - * getDesState(). Error is simply the desired state minus the actual state (ex. roll_des - roll_IMU). Two safety features - * are implimented here regarding the I terms. The I terms are saturated within specified limits on startup to prevent - * excessive buildup. This can be seen by holding the vehicle at an angle and seeing the motors ramp up on one side until - * they've maxed out throttle...saturating I to a specified limit fixes this. The second feature defaults the I terms to 0 - * if the throttle is at the minimum setting. This means the motors will not start spooling up on the ground, and the I - * terms will always start from 0 on takeoff. This function updates the variables roll_PID, pitch_PID, and yaw_PID which - * can be thought of as 1D stablized signals. They are mixed to the configuration of the vehicle in controlMixer(). - */ - - //Roll - error_roll = roll_des - roll_IMU; - integral_roll = integral_roll_prev + error_roll*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_roll = 0; - } - integral_roll = constrain(integral_roll, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_roll = GyroX; - roll_PID = Kp_roll_angle*error_roll + Ki_roll_angle*integral_roll - Kd_roll_angle*derivative_roll; - - //Pitch - error_pitch = pitch_des - pitch_IMU; - integral_pitch = integral_pitch_prev + error_pitch*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_pitch = 0; - } - integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_pitch = GyroY; - pitch_PID = Kp_pitch_angle*error_pitch + Ki_pitch_angle*integral_pitch - Kd_pitch_angle*derivative_pitch; - - //Yaw, stablize on rate from GyroZ - error_yaw = yaw_des - GyroZ; - integral_yaw = integral_yaw_prev + error_yaw*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_yaw = 0; - } - integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_yaw = (error_yaw - error_yaw_prev)/dt; - yaw_PID = Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw; - - //Update roll variables - integral_roll_prev = integral_roll; - //Update pitch variables - integral_pitch_prev = integral_pitch; - //Update yaw variables - error_yaw_prev = error_yaw; - integral_yaw_prev = integral_yaw; -} - -void controlANGLE2() { - //DESCRIPTION: Computes control commands based on state error (angle) in cascaded scheme - /* - * Gives better performance than controlANGLE() but requires much more tuning. Not reccommended for general use at the moment. - */ - //Outer loop - PID on angle - float roll_des_ol, pitch_des_ol; - //Roll - error_roll = roll_des - roll_IMU; - integral_roll_ol = integral_roll_prev_ol + error_roll*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_roll_ol = 0; - } - integral_roll_ol = constrain(integral_roll_ol, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_roll = (roll_IMU - roll_IMU_prev)/dt; - roll_des_ol = Kp_roll_angle*error_roll + Ki_roll_angle*integral_roll_ol - Kd_roll_angle*derivative_roll; - - //Pitch - error_pitch = pitch_des - pitch_IMU; - integral_pitch_ol = integral_pitch_prev_ol + error_pitch*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_pitch_ol = 0; - } - integral_pitch_ol = constrain(integral_pitch_ol, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_pitch = (pitch_IMU - pitch_IMU_prev)/dt; - pitch_des_ol = Kp_pitch_angle*error_pitch + Ki_pitch_angle*integral_pitch_ol - Kd_pitch_angle*derivative_pitch; - - //Apply loop gain, constrain, and filter - float Kl = 30.0; - roll_des_ol = Kl*roll_des_ol; - pitch_des_ol = Kl*pitch_des_ol; - roll_des_ol = constrain(roll_des_ol, -240.0, 240.0); - pitch_des_ol = constrain(pitch_des_ol, -240.0, 240.0); - float B_loop = 0.9; //LP filter parameter for outer to inner loop, acts as damping term - roll_des_ol = (1.0 - B_loop)*roll_des_prev + B_loop*roll_des_ol; - pitch_des_ol = (1.0 - B_loop)*pitch_des_prev + B_loop*pitch_des_ol; - - //Inner loop - PID on rate - //Roll - error_roll = roll_des_ol - GyroX; - integral_roll_il = integral_roll_prev_il + error_roll*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_roll_il = 0; - } - integral_roll_il = constrain(integral_roll_il, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_roll = (error_roll - error_roll_prev)/dt; - roll_PID = Kp_roll_rate*error_roll + Ki_roll_rate*integral_roll_il + Kd_roll_rate*derivative_roll; - - //Pitch - error_pitch = pitch_des_ol - GyroY; - integral_pitch_il = integral_pitch_prev_il + error_pitch*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_pitch_il = 0; - } - integral_pitch_il = constrain(integral_pitch_il, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_pitch = (error_pitch - error_pitch_prev)/dt; - pitch_PID = Kp_pitch_rate*error_pitch + Ki_pitch_rate*integral_pitch_il + Kd_pitch_rate*derivative_pitch; - - //Yaw - error_yaw = yaw_des - GyroZ; - integral_yaw = integral_yaw_prev + error_yaw*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_yaw = 0; - } - integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_yaw = (error_yaw - error_yaw_prev)/dt; - yaw_PID = Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw; - - //Update roll variables - integral_roll_prev_ol = integral_roll_ol; - integral_roll_prev_il = integral_roll_il; - error_roll_prev = error_roll; - roll_IMU_prev = roll_IMU; - roll_des_prev = roll_des_ol; - //Update pitch variables - integral_pitch_prev_ol = integral_pitch_ol; - integral_pitch_prev_il = integral_pitch_il; - error_pitch_prev = error_pitch; - pitch_IMU_prev = pitch_IMU; - pitch_des_prev = pitch_des_ol; - //Update yaw variables - error_yaw_prev = error_yaw; - integral_yaw_prev = integral_yaw; - -} - -void controlRATE() { - //DESCRIPTION: Computes control commands based on state error (rate) - /* - * See explanation for controlANGLE(). Everything is the same here except the error is now the desired rate - raw gyro reading. - */ - //Roll - error_roll = roll_des - GyroX; - integral_roll = integral_roll_prev + error_roll*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_roll = 0; - } - integral_roll = constrain(integral_roll, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_roll = (error_roll - error_roll_prev)/dt; - roll_PID = Kp_roll_rate*error_roll + Ki_roll_rate*integral_roll + Kd_roll_rate*derivative_roll; - - //Pitch - error_pitch = pitch_des - GyroY; - integral_pitch = integral_pitch_prev + error_pitch*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_pitch = 0; - } - integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_pitch = (error_pitch - error_pitch_prev)/dt; - pitch_PID = Kp_pitch_rate*error_pitch + Ki_pitch_rate*integral_pitch + Kd_pitch_rate*derivative_pitch; - - //Yaw, stablize on rate from GyroZ - error_yaw = yaw_des - GyroZ; - integral_yaw = integral_yaw_prev + error_yaw*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_yaw = 0; - } - integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_yaw = (error_yaw - error_yaw_prev)/dt; - yaw_PID = Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw; - - //Update roll variables - error_roll_prev = error_roll; - integral_roll_prev = integral_roll; - GyroX_prev = GyroX; - //Update pitch variables - error_pitch_prev = error_pitch; - integral_pitch_prev = integral_pitch; - GyroY_prev = GyroY; - //Update yaw variables - error_yaw_prev = error_yaw; - integral_yaw_prev = integral_yaw; -} - -void controlMixer() { - //DESCRIPTION: Mixes scaled commands from PID controller to actuator outputs based on vehicle configuration - /* - * Takes roll_PID, pitch_PID, and yaw_PID computed from the PID controller and appropriately mixes them for the desired - * vehicle configuration. For example on a quadcopter, the left two motors should have +roll_PID while the right two motors - * should have -roll_PID. Front two should have -pitch_PID and the back two should have +pitch_PID etc... every motor has - * normalized (0 - 1) thro_des command for throttle control. PID parameters are scaled by 0.01 to put them into the general - * area of 0 - 1 range. Don't know why I decided to do this here, should be done in the PID function. This is why every - * flight controller software has different gain settings... because the PID commands are scaled uniquely. My method is - * the best. mX_command_scaled variables are used in scaleCommands() in preparation to be sent to the motors. - */ - //Quad mixing - //m1 = front left, m2 = front right, m3 = back right, m4 = back left - m1_command_scaled = thro_des - 0.01*pitch_PID + 0.01*roll_PID + 0.01*yaw_PID; - m2_command_scaled = thro_des - 0.01*pitch_PID - 0.01*roll_PID - 0.01*yaw_PID; - m3_command_scaled = thro_des + 0.01*pitch_PID - 0.01*roll_PID + 0.01*yaw_PID; - m4_command_scaled = thro_des + 0.01*pitch_PID + 0.01*roll_PID - 0.01*yaw_PID; - -} - -void scaleCommands() { - //DESCRIPTION: Scale normalized actuator commands to values for ESC protocol - /* - * mX_command_scaled variables from the mixer function are scaled up to whatever ESC protocol you are using. Right now is - * configured for oneshot125 which is a 125 - 250us pulse. If you are using standard PWM, change this to 1000 - 2000us. - * mX_command_PWM are updated here which are used to command the motors in commandMotors(). - */ - //Scaled to 125us - 250us for oneshot125 protocol - m1_command_PWM = m1_command_scaled*125 + 125; - m2_command_PWM = m2_command_scaled*125 + 125; - m3_command_PWM = m3_command_scaled*125 + 125; - m4_command_PWM = m4_command_scaled*125 + 125; - //Constrain commands to motors within oneshot125 bounds - m1_command_PWM = constrain(m1_command_PWM, 125, 250); - m2_command_PWM = constrain(m2_command_PWM, 125, 250); - m3_command_PWM = constrain(m3_command_PWM, 125, 250); - m4_command_PWM = constrain(m4_command_PWM, 125, 250); -} - -void getCommands() { - //DESCRIPTION: Get raw PWM values for every channel from the radio - /* - * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of - * the loop. The radio commands are retrieved from a function in the readPWM - * file separate from this one which is running a bunch of interrupts to continually update the radio readings. - * The raw radio commands are being filtered with a first order low-pass filter to eliminate any really high frequency noise. - */ - - radio_command = 1; - channel_1_pwm = getRadioPWM(1); - channel_2_pwm = getRadioPWM(2); - channel_3_pwm = getRadioPWM(3); - channel_4_pwm = getRadioPWM(4); - channel_5_pwm = getRadioPWM(5); - channel_6_pwm = getRadioPWM(6); - - //Low-pass the critical commands and update previous values - float b = 0.2; - channel_1_pwm = (1.0 - b)*channel_1_pwm_prev + b*channel_1_pwm; - channel_2_pwm = (1.0 - b)*channel_2_pwm_prev + b*channel_2_pwm; - channel_3_pwm = (1.0 - b)*channel_3_pwm_prev + b*channel_3_pwm; - channel_4_pwm = (1.0 - b)*channel_4_pwm_prev + b*channel_4_pwm; - channel_1_pwm_prev = channel_1_pwm; - channel_2_pwm_prev = channel_2_pwm; - channel_3_pwm_prev = channel_3_pwm; - channel_4_pwm_prev = channel_4_pwm; -} - -void failSafe() { - //DESCRIPTION: If radio gives garbage values, set all commands to default values - /* - * Radio connection failsafe used to check if the getCommands() function is returning acceptable pwm values. If any of - * the commands are lower than 800 or higher than 2200, then we can be certain that there is an issue with the radio - * connection (most likely hardware related). If any of the channels show this failure, then all of the radio commands - * channel_x_pwm are set to default failsafe values specified in the setup. - */ - unsigned minVal = 800; - unsigned maxVal = 2200; - int check1 = 0; - int check2 = 0; - int check3 = 0; - int check4 = 0; - int check5 = 0; - int check6 = 0; - - //Triggers for failure criteria - if (channel_1_pwm > maxVal || channel_1_pwm < minVal) check1 = 1; - if (channel_2_pwm > maxVal || channel_2_pwm < minVal) check2 = 1; - if (channel_3_pwm > maxVal || channel_3_pwm < minVal) check3 = 1; - if (channel_4_pwm > maxVal || channel_4_pwm < minVal) check4 = 1; - if (channel_5_pwm > maxVal || channel_5_pwm < minVal) check5 = 1; - if (channel_6_pwm > maxVal || channel_6_pwm < minVal) check6 = 1; - - //If any failures, set to default failsafe values - if ((check1 + check2 + check3 + check4 + check5 + check6) > 0) { - channel_1_pwm = channel_1_fs; - channel_2_pwm = channel_2_fs; - channel_3_pwm = channel_3_fs; - channel_4_pwm = channel_4_fs; - channel_5_pwm = channel_5_fs; - channel_6_pwm = channel_6_fs; - } -} - -void commandMotors() { - //DESCRIPTION: Send pulses to motor pins, oneshot125 protocol - /* - * My crude implimentation of oneshot125 protocol which sends 125 - 250us pulses to the ESCs (mxPin). The pulselengths being - * sent are mx_command_PWM, computed in scaleCommands(). - */ - int wentLow = 0; - int pulseStart, timer; - int flagM1 = 0; - int flagM2 = 0; - int flagM3 = 0; - int flagM4 = 0; - - //Write all motor pins high - digitalWrite(m1Pin, HIGH); - digitalWrite(m2Pin, HIGH); - digitalWrite(m3Pin, HIGH); - digitalWrite(m4Pin, HIGH); - pulseStart = micros(); - - //Write each motor pin low as correct pulse length is reached - while (wentLow < 4 ) { //keep going until final (4th) pulse is finished, then done - timer = micros(); - if ((m1_command_PWM <= timer - pulseStart) && (flagM1==0)) { - digitalWrite(m1Pin, LOW); - wentLow = wentLow + 1; - flagM1 = 1; - } - if ((m2_command_PWM <= timer - pulseStart) && (flagM2==0)) { - digitalWrite(m2Pin, LOW); - wentLow = wentLow + 1; - flagM2 = 1; - } - if ((m3_command_PWM <= timer - pulseStart) && (flagM3==0)) { - digitalWrite(m3Pin, LOW); - wentLow = wentLow + 1; - flagM3 = 1; - } - if ((m4_command_PWM <= timer - pulseStart) && (flagM4==0)) { - digitalWrite(m4Pin, LOW); - wentLow = wentLow + 1; - flagM4 = 1; - } - } -} - -void throttleCut() { - //DESCRIPTION: Directly set actuator outputs to minimum value if triggered - /* - * Monitors the state of radio command channel_6_pwm and directly sets the mx_command_PWM values to minimum (120 is - * minimum for oneshot125 protocol, 1000 is minimum for standard PWM) if channel 6 is high. This is the last function - * called before commandMotors() is called so that the last thing checked is if the user is giving permission to command - * the motors to anything other than minimum value. Safety first. - */ - if (channel_6_pwm > 1500) { - m1_command_PWM = 120; - m2_command_PWM = 120; - m3_command_PWM = 120; - m4_command_PWM = 120; - } -} - -void loopRate(int freq) { - //DESCRIPTION: Regulate main loop rate to specified frequency in Hz - /* - * It's good to operate at a constant loop rate for filters to remain stable and whatnot. Interrupt routines running in the - * background cause the loop rate to fluctuate. This function basically just waits at the end of every loop iteration until - * the correct time has passed since the start of the current loop for the desired loop rate in Hz. 2kHz is a good rate to - * be at because the loop nominally will run between 2.8kHz - 4.2kHz. This lets us have a little room to add extra computations - * and remain above 2kHz, without needing to retune all of our filtering parameters scattered around this piece of shit code. - */ - float invFreq = 1.0/freq*1000000.0; - unsigned long checker = micros(); - - //Sit in loop until appropriate time has passed - while (invFreq > (checker - current_time)) { - checker = micros(); - } -} - -void loopBlink() { - //DESCRIPTION: Blink LED on board to indicate main loop is running - /* - * It looks cool. - */ - if (current_time - blink_counter > blink_delay) { - blink_counter = micros(); - digitalWrite(13, blinkAlternate); //pin 13 is built in LED - - if (blinkAlternate == 1) { - blinkAlternate = 0; - blink_delay = 100000; - } - else if (blinkAlternate == 0) { - blinkAlternate = 1; - blink_delay = 2000000; - } - } -} - -void setupBlink(int numBlinks,int upTime, int downTime) { - //DESCRIPTION: Simple function to make LED on board blink as desired - for (int j = 1; j<= numBlinks; j++) { - digitalWrite(13, LOW); - delay(downTime); - digitalWrite(13, HIGH); - delay(upTime); - } -} - -void printRadioData() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F(" CH1: ")); - Serial.print(channel_1_pwm); - Serial.print(F(" CH2: ")); - Serial.print(channel_2_pwm); - Serial.print(F(" CH3: ")); - Serial.print(channel_3_pwm); - Serial.print(F(" CH4: ")); - Serial.print(channel_4_pwm); - Serial.print(F(" CH5: ")); - Serial.print(channel_5_pwm); - Serial.print(F(" CH6: ")); - Serial.println(channel_6_pwm); - } -} - -void printDesiredState() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("thro_des: ")); - Serial.print(thro_des); - Serial.print(F(" roll_des: ")); - Serial.print(roll_des); - Serial.print(F(" pitch_des: ")); - Serial.print(pitch_des); - Serial.print(F(" yaw_des: ")); - Serial.println(yaw_des); - } -} - -void printGyroData() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("GyroX: ")); - Serial.print(GyroX); - Serial.print(F(" GyroY: ")); - Serial.print(GyroY); - Serial.print(F(" GyroZ: ")); - Serial.println(GyroZ); - } -} - -void printAccelData() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("AccX: ")); - Serial.print(AccX); - Serial.print(F(" AccY: ")); - Serial.print(AccY); - Serial.print(F(" AccZ: ")); - Serial.println(AccZ); - } -} - -void printRollPitchYaw() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("roll: ")); - Serial.print(roll_IMU); - Serial.print(F(" pitch: ")); - Serial.print(pitch_IMU); - Serial.print(F(" yaw: ")); - Serial.println(yaw_IMU); - } -} - -void printPIDoutput() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("roll_PID: ")); - Serial.print(roll_PID); - Serial.print(F(" pitch_PID: ")); - Serial.print(pitch_PID); - Serial.print(F(" yaw_PID: ")); - Serial.println(yaw_PID); - } -} - -void printMotorCommands() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("m1_command: ")); - Serial.print(m1_command_PWM); - Serial.print(F(" m2_command: ")); - Serial.print(m2_command_PWM); - Serial.print(F(" m3_command: ")); - Serial.print(m3_command_PWM); - Serial.print(F(" m4_command: ")); - Serial.println(m4_command_PWM); - } -} - -void printLoopRate() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("dt = ")); - Serial.println(dt*1000000.0); - } -} - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -//HELPER FUNCTIONS - -float invSqrt(float x) { - //Fast inverse sqrt - /* - float halfx = 0.5f * x; - float y = x; - long i = *(long*)&y; - i = 0x5f3759df - (i>>1); - y = *(float*)&i; - y = y * (1.5f - (halfx * y * y)); - y = y * (1.5f - (halfx * y * y)); - return y; - */ - //alternate form: - unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1); - float tmp = *(float*)&i; - float y = tmp * (1.69000231f - 0.714158168f * x * tmp * tmp); - return y; -} diff --git a/Versions/dRehmFlight_Teensy_BETA_1.0/radioComm.ino b/Versions/dRehmFlight_Teensy_BETA_1.0/radioComm.ino deleted file mode 100644 index f80fdca0..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.0/radioComm.ino +++ /dev/null @@ -1,173 +0,0 @@ -//Arduino/Teensy Flight Controller - dRehmFlight -//Author: Nicholas Rehm -//Project Start: 1/6/2020 -//Version: Beta 1.0 - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -unsigned long rising_edge_start_1, rising_edge_start_2, rising_edge_start_3, rising_edge_start_4, rising_edge_start_5, rising_edge_start_6; -unsigned long channel_1_raw, channel_2_raw, channel_3_raw, channel_4_raw, channel_5_raw, channel_6_raw; -int ppm_counter = 0; -unsigned long time_ms = 0; - - -void readPPM_setup(int pin) { - // DESCRIPTION: Initialize software interrupts on radio channel pins - // Declare interrupt pins - pinMode(pin, INPUT_PULLUP); - delay(20); - //Attach interrupt and point to corresponding functions - attachInterrupt(digitalPinToInterrupt(pin), getPPM, CHANGE); -} - - -void readPWM_setup(int ch1, int ch2, int ch3, int ch4, int ch5, int ch6) { - //DESCRIPTION: Initialize software interrupts on radio channel pins - //Declare interrupt pins - pinMode(ch1, INPUT_PULLUP); - pinMode(ch2, INPUT_PULLUP); - pinMode(ch3, INPUT_PULLUP); - pinMode(ch4, INPUT_PULLUP); - pinMode(ch5, INPUT_PULLUP); - pinMode(ch6, INPUT_PULLUP); - delay(20); - //Attach interrupt and point to corresponding functions - attachInterrupt(digitalPinToInterrupt(ch1), getCh1, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch2), getCh2, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch3), getCh3, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch4), getCh4, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch5), getCh5, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch6), getCh6, CHANGE); - delay(20); -} - -unsigned long getRadioPWM(int ch_num) { - //DESCRIPTION: Get current radio commands from interrupt routines - unsigned long returnPWM = 0; - - if (ch_num == 1) { - returnPWM = channel_1_raw; - } - else if (ch_num == 2) { - returnPWM = channel_2_raw; - } - else if (ch_num == 3) { - returnPWM = channel_3_raw; - } - else if (ch_num == 4) { - returnPWM = channel_4_raw; - } - else if (ch_num == 5) { - returnPWM = channel_5_raw; - } - else if (ch_num == 6) { - returnPWM = channel_6_raw; - } - - return returnPWM; -} - - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -//INTERRUPT SERVICE ROUTINES - -void getPPM() { - unsigned long dt_ppm; - int trig = digitalRead(PPM_Pin); - - if (trig==1) { //only care about rising edge - dt_ppm = micros() - time_ms; - time_ms = micros(); - - if (dt_ppm > 5000) { //ms - ppm_counter = 0; - } - - if (ppm_counter == 1) { - channel_2_raw = dt_ppm; - } - - if (ppm_counter == 2) { - channel_3_raw = dt_ppm; - } - - if (ppm_counter == 3) { - channel_1_raw = dt_ppm; - } - - if (ppm_counter == 4) { - channel_4_raw = dt_ppm; - } - - if (ppm_counter == 5) { - channel_6_raw = dt_ppm; - } - - if (ppm_counter == 6) { - channel_5_raw = dt_ppm; - } - - ppm_counter = ppm_counter + 1; - } -} - -void getCh1() { - int trigger = digitalRead(ch1Pin); - if(trigger == 1) { - rising_edge_start_1 = micros(); - } - else if(trigger == 0) { - channel_1_raw = micros() - rising_edge_start_1; - } -} - -void getCh2() { - int trigger = digitalRead(ch2Pin); - if(trigger == 1) { - rising_edge_start_2 = micros(); - } - else if(trigger == 0) { - channel_2_raw = micros() - rising_edge_start_2; - } -} - -void getCh3() { - int trigger = digitalRead(ch3Pin); - if(trigger == 1) { - rising_edge_start_3 = micros(); - } - else if(trigger == 0) { - channel_3_raw = micros() - rising_edge_start_3; - } -} - -void getCh4() { - int trigger = digitalRead(ch4Pin); - if(trigger == 1) { - rising_edge_start_4 = micros(); - } - else if(trigger == 0) { - channel_4_raw = micros() - rising_edge_start_4; - } -} - -void getCh5() { - int trigger = digitalRead(ch5Pin); - if(trigger == 1) { - rising_edge_start_5 = micros(); - } - else if(trigger == 0) { - channel_5_raw = micros() - rising_edge_start_5; - } -} - -void getCh6() { - int trigger = digitalRead(ch6Pin); - if(trigger == 1) { - rising_edge_start_6 = micros(); - } - else if(trigger == 0) { - channel_6_raw = micros() - rising_edge_start_6; - } -} diff --git a/Versions/dRehmFlight_Teensy_BETA_1.1/COPYING.txt b/Versions/dRehmFlight_Teensy_BETA_1.1/COPYING.txt deleted file mode 100644 index 18b14822..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.1/COPYING.txt +++ /dev/null @@ -1,674 +0,0 @@ -GNU GENERAL PUBLIC LICENSE - Version 3, 29 June 2007 - - Copyright (C) 2007 Free Software Foundation, Inc. - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The GNU General Public License is a free, copyleft license for -software and other kinds of works. - - The licenses for most software and other practical works are designed -to take away your freedom to share and change the works. By contrast, -the GNU General Public License is intended to guarantee your freedom to -share and change all versions of a program--to make sure it remains free -software for all its users. We, the Free Software Foundation, use the -GNU General Public License for most of our software; it applies also to -any other work released this way by its authors. You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -them if you wish), that you receive source code or can get it if you -want it, that you can change the software or use pieces of it in new -free programs, and that you know you can do these things. - - To protect your rights, we need to prevent others from denying you -these rights or asking you to surrender the rights. Therefore, you have -certain responsibilities if you distribute copies of the software, or if -you modify it: responsibilities to respect the freedom of others. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must pass on to the recipients the same -freedoms that you received. You must make sure that they, too, receive -or can get the source code. And you must show them these terms so they -know their rights. - - Developers that use the GNU GPL protect your rights with two steps: -(1) assert copyright on the software, and (2) offer you this License -giving you legal permission to copy, distribute and/or modify it. - - For the developers' and authors' protection, the GPL clearly explains -that there is no warranty for this free software. For both users' and -authors' sake, the GPL requires that modified versions be marked as -changed, so that their problems will not be attributed erroneously to -authors of previous versions. - - Some devices are designed to deny users access to install or run -modified versions of the software inside them, although the manufacturer -can do so. This is fundamentally incompatible with the aim of -protecting users' freedom to change the software. The systematic -pattern of such abuse occurs in the area of products for individuals to -use, which is precisely where it is most unacceptable. Therefore, we -have designed this version of the GPL to prohibit the practice for those -products. If such problems arise substantially in other domains, we -stand ready to extend this provision to those domains in future versions -of the GPL, as needed to protect the freedom of users. - - Finally, every program is threatened constantly by software patents. -States should not allow patents to restrict development and use of -software on general-purpose computers, but in those that do, we wish to -avoid the special danger that patents applied to a free program could -make it effectively proprietary. To prevent this, the GPL assures that -patents cannot be used to render the program non-free. - - The precise terms and conditions for copying, distribution and -modification follow. - - TERMS AND CONDITIONS - - 0. Definitions. - - "This License" refers to version 3 of the GNU General Public License. - - "Copyright" also means copyright-like laws that apply to other kinds of -works, such as semiconductor masks. - - "The Program" refers to any copyrightable work licensed under this -License. Each licensee is addressed as "you". "Licensees" and -"recipients" may be individuals or organizations. - - To "modify" a work means to copy from or adapt all or part of the work -in a fashion requiring copyright permission, other than the making of an -exact copy. The resulting work is called a "modified version" of the -earlier work or a work "based on" the earlier work. - - A "covered work" means either the unmodified Program or a work based -on the Program. - - To "propagate" a work means to do anything with it that, without -permission, would make you directly or secondarily liable for -infringement under applicable copyright law, except executing it on a -computer or modifying a private copy. Propagation includes copying, -distribution (with or without modification), making available to the -public, and in some countries other activities as well. - - To "convey" a work means any kind of propagation that enables other -parties to make or receive copies. Mere interaction with a user through -a computer network, with no transfer of a copy, is not conveying. - - An interactive user interface displays "Appropriate Legal Notices" -to the extent that it includes a convenient and prominently visible -feature that (1) displays an appropriate copyright notice, and (2) -tells the user that there is no warranty for the work (except to the -extent that warranties are provided), that licensees may convey the -work under this License, and how to view a copy of this License. If -the interface presents a list of user commands or options, such as a -menu, a prominent item in the list meets this criterion. - - 1. Source Code. - - The "source code" for a work means the preferred form of the work -for making modifications to it. "Object code" means any non-source -form of a work. - - A "Standard Interface" means an interface that either is an official -standard defined by a recognized standards body, or, in the case of -interfaces specified for a particular programming language, one that -is widely used among developers working in that language. - - The "System Libraries" of an executable work include anything, other -than the work as a whole, that (a) is included in the normal form of -packaging a Major Component, but which is not part of that Major -Component, and (b) serves only to enable use of the work with that -Major Component, or to implement a Standard Interface for which an -implementation is available to the public in source code form. A -"Major Component", in this context, means a major essential component -(kernel, window system, and so on) of the specific operating system -(if any) on which the executable work runs, or a compiler used to -produce the work, or an object code interpreter used to run it. - - The "Corresponding Source" for a work in object code form means all -the source code needed to generate, install, and (for an executable -work) run the object code and to modify the work, including scripts to -control those activities. However, it does not include the work's -System Libraries, or general-purpose tools or generally available free -programs which are used unmodified in performing those activities but -which are not part of the work. For example, Corresponding Source -includes interface definition files associated with source files for -the work, and the source code for shared libraries and dynamically -linked subprograms that the work is specifically designed to require, -such as by intimate data communication or control flow between those -subprograms and other parts of the work. - - The Corresponding Source need not include anything that users -can regenerate automatically from other parts of the Corresponding -Source. - - The Corresponding Source for a work in source code form is that -same work. - - 2. Basic Permissions. - - All rights granted under this License are granted for the term of -copyright on the Program, and are irrevocable provided the stated -conditions are met. This License explicitly affirms your unlimited -permission to run the unmodified Program. The output from running a -covered work is covered by this License only if the output, given its -content, constitutes a covered work. This License acknowledges your -rights of fair use or other equivalent, as provided by copyright law. - - You may make, run and propagate covered works that you do not -convey, without conditions so long as your license otherwise remains -in force. You may convey covered works to others for the sole purpose -of having them make modifications exclusively for you, or provide you -with facilities for running those works, provided that you comply with -the terms of this License in conveying all material for which you do -not control copyright. Those thus making or running the covered works -for you must do so exclusively on your behalf, under your direction -and control, on terms that prohibit them from making any copies of -your copyrighted material outside their relationship with you. - - Conveying under any other circumstances is permitted solely under -the conditions stated below. Sublicensing is not allowed; section 10 -makes it unnecessary. - - 3. Protecting Users' Legal Rights From Anti-Circumvention Law. - - No covered work shall be deemed part of an effective technological -measure under any applicable law fulfilling obligations under article -11 of the WIPO copyright treaty adopted on 20 December 1996, or -similar laws prohibiting or restricting circumvention of such -measures. - - When you convey a covered work, you waive any legal power to forbid -circumvention of technological measures to the extent such circumvention -is effected by exercising rights under this License with respect to -the covered work, and you disclaim any intention to limit operation or -modification of the work as a means of enforcing, against the work's -users, your or third parties' legal rights to forbid circumvention of -technological measures. - - 4. Conveying Verbatim Copies. - - You may convey verbatim copies of the Program's source code as you -receive it, in any medium, provided that you conspicuously and -appropriately publish on each copy an appropriate copyright notice; -keep intact all notices stating that this License and any -non-permissive terms added in accord with section 7 apply to the code; -keep intact all notices of the absence of any warranty; and give all -recipients a copy of this License along with the Program. - - You may charge any price or no price for each copy that you convey, -and you may offer support or warranty protection for a fee. - - 5. Conveying Modified Source Versions. - - You may convey a work based on the Program, or the modifications to -produce it from the Program, in the form of source code under the -terms of section 4, provided that you also meet all of these conditions: - - a) The work must carry prominent notices stating that you modified - it, and giving a relevant date. - - b) The work must carry prominent notices stating that it is - released under this License and any conditions added under section - 7. This requirement modifies the requirement in section 4 to - "keep intact all notices". - - c) You must license the entire work, as a whole, under this - License to anyone who comes into possession of a copy. This - License will therefore apply, along with any applicable section 7 - additional terms, to the whole of the work, and all its parts, - regardless of how they are packaged. This License gives no - permission to license the work in any other way, but it does not - invalidate such permission if you have separately received it. - - d) If the work has interactive user interfaces, each must display - Appropriate Legal Notices; however, if the Program has interactive - interfaces that do not display Appropriate Legal Notices, your - work need not make them do so. - - A compilation of a covered work with other separate and independent -works, which are not by their nature extensions of the covered work, -and which are not combined with it such as to form a larger program, -in or on a volume of a storage or distribution medium, is called an -"aggregate" if the compilation and its resulting copyright are not -used to limit the access or legal rights of the compilation's users -beyond what the individual works permit. Inclusion of a covered work -in an aggregate does not cause this License to apply to the other -parts of the aggregate. - - 6. Conveying Non-Source Forms. - - You may convey a covered work in object code form under the terms -of sections 4 and 5, provided that you also convey the -machine-readable Corresponding Source under the terms of this License, -in one of these ways: - - a) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by the - Corresponding Source fixed on a durable physical medium - customarily used for software interchange. - - b) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by a - written offer, valid for at least three years and valid for as - long as you offer spare parts or customer support for that product - model, to give anyone who possesses the object code either (1) a - copy of the Corresponding Source for all the software in the - product that is covered by this License, on a durable physical - medium customarily used for software interchange, for a price no - more than your reasonable cost of physically performing this - conveying of source, or (2) access to copy the - Corresponding Source from a network server at no charge. - - c) Convey individual copies of the object code with a copy of the - written offer to provide the Corresponding Source. This - alternative is allowed only occasionally and noncommercially, and - only if you received the object code with such an offer, in accord - with subsection 6b. - - d) Convey the object code by offering access from a designated - place (gratis or for a charge), and offer equivalent access to the - Corresponding Source in the same way through the same place at no - further charge. You need not require recipients to copy the - Corresponding Source along with the object code. If the place to - copy the object code is a network server, the Corresponding Source - may be on a different server (operated by you or a third party) - that supports equivalent copying facilities, provided you maintain - clear directions next to the object code saying where to find the - Corresponding Source. Regardless of what server hosts the - Corresponding Source, you remain obligated to ensure that it is - available for as long as needed to satisfy these requirements. - - e) Convey the object code using peer-to-peer transmission, provided - you inform other peers where the object code and Corresponding - Source of the work are being offered to the general public at no - charge under subsection 6d. - - A separable portion of the object code, whose source code is excluded -from the Corresponding Source as a System Library, need not be -included in conveying the object code work. - - A "User Product" is either (1) a "consumer product", which means any -tangible personal property which is normally used for personal, family, -or household purposes, or (2) anything designed or sold for incorporation -into a dwelling. In determining whether a product is a consumer product, -doubtful cases shall be resolved in favor of coverage. For a particular -product received by a particular user, "normally used" refers to a -typical or common use of that class of product, regardless of the status -of the particular user or of the way in which the particular user -actually uses, or expects or is expected to use, the product. A product -is a consumer product regardless of whether the product has substantial -commercial, industrial or non-consumer uses, unless such uses represent -the only significant mode of use of the product. - - "Installation Information" for a User Product means any methods, -procedures, authorization keys, or other information required to install -and execute modified versions of a covered work in that User Product from -a modified version of its Corresponding Source. The information must -suffice to ensure that the continued functioning of the modified object -code is in no case prevented or interfered with solely because -modification has been made. - - If you convey an object code work under this section in, or with, or -specifically for use in, a User Product, and the conveying occurs as -part of a transaction in which the right of possession and use of the -User Product is transferred to the recipient in perpetuity or for a -fixed term (regardless of how the transaction is characterized), the -Corresponding Source conveyed under this section must be accompanied -by the Installation Information. But this requirement does not apply -if neither you nor any third party retains the ability to install -modified object code on the User Product (for example, the work has -been installed in ROM). - - The requirement to provide Installation Information does not include a -requirement to continue to provide support service, warranty, or updates -for a work that has been modified or installed by the recipient, or for -the User Product in which it has been modified or installed. Access to a -network may be denied when the modification itself materially and -adversely affects the operation of the network or violates the rules and -protocols for communication across the network. - - Corresponding Source conveyed, and Installation Information provided, -in accord with this section must be in a format that is publicly -documented (and with an implementation available to the public in -source code form), and must require no special password or key for -unpacking, reading or copying. - - 7. Additional Terms. - - "Additional permissions" are terms that supplement the terms of this -License by making exceptions from one or more of its conditions. -Additional permissions that are applicable to the entire Program shall -be treated as though they were included in this License, to the extent -that they are valid under applicable law. If additional permissions -apply only to part of the Program, that part may be used separately -under those permissions, but the entire Program remains governed by -this License without regard to the additional permissions. - - When you convey a copy of a covered work, you may at your option -remove any additional permissions from that copy, or from any part of -it. (Additional permissions may be written to require their own -removal in certain cases when you modify the work.) You may place -additional permissions on material, added by you to a covered work, -for which you have or can give appropriate copyright permission. - - Notwithstanding any other provision of this License, for material you -add to a covered work, you may (if authorized by the copyright holders of -that material) supplement the terms of this License with terms: - - a) Disclaiming warranty or limiting liability differently from the - terms of sections 15 and 16 of this License; or - - b) Requiring preservation of specified reasonable legal notices or - author attributions in that material or in the Appropriate Legal - Notices displayed by works containing it; or - - c) Prohibiting misrepresentation of the origin of that material, or - requiring that modified versions of such material be marked in - reasonable ways as different from the original version; or - - d) Limiting the use for publicity purposes of names of licensors or - authors of the material; or - - e) Declining to grant rights under trademark law for use of some - trade names, trademarks, or service marks; or - - f) Requiring indemnification of licensors and authors of that - material by anyone who conveys the material (or modified versions of - it) with contractual assumptions of liability to the recipient, for - any liability that these contractual assumptions directly impose on - those licensors and authors. - - All other non-permissive additional terms are considered "further -restrictions" within the meaning of section 10. If the Program as you -received it, or any part of it, contains a notice stating that it is -governed by this License along with a term that is a further -restriction, you may remove that term. If a license document contains -a further restriction but permits relicensing or conveying under this -License, you may add to a covered work material governed by the terms -of that license document, provided that the further restriction does -not survive such relicensing or conveying. - - If you add terms to a covered work in accord with this section, you -must place, in the relevant source files, a statement of the -additional terms that apply to those files, or a notice indicating -where to find the applicable terms. - - Additional terms, permissive or non-permissive, may be stated in the -form of a separately written license, or stated as exceptions; -the above requirements apply either way. - - 8. Termination. - - You may not propagate or modify a covered work except as expressly -provided under this License. Any attempt otherwise to propagate or -modify it is void, and will automatically terminate your rights under -this License (including any patent licenses granted under the third -paragraph of section 11). - - However, if you cease all violation of this License, then your -license from a particular copyright holder is reinstated (a) -provisionally, unless and until the copyright holder explicitly and -finally terminates your license, and (b) permanently, if the copyright -holder fails to notify you of the violation by some reasonable means -prior to 60 days after the cessation. - - Moreover, your license from a particular copyright holder is -reinstated permanently if the copyright holder notifies you of the -violation by some reasonable means, this is the first time you have -received notice of violation of this License (for any work) from that -copyright holder, and you cure the violation prior to 30 days after -your receipt of the notice. - - Termination of your rights under this section does not terminate the -licenses of parties who have received copies or rights from you under -this License. If your rights have been terminated and not permanently -reinstated, you do not qualify to receive new licenses for the same -material under section 10. - - 9. Acceptance Not Required for Having Copies. - - You are not required to accept this License in order to receive or -run a copy of the Program. Ancillary propagation of a covered work -occurring solely as a consequence of using peer-to-peer transmission -to receive a copy likewise does not require acceptance. However, -nothing other than this License grants you permission to propagate or -modify any covered work. These actions infringe copyright if you do -not accept this License. Therefore, by modifying or propagating a -covered work, you indicate your acceptance of this License to do so. - - 10. Automatic Licensing of Downstream Recipients. - - Each time you convey a covered work, the recipient automatically -receives a license from the original licensors, to run, modify and -propagate that work, subject to this License. You are not responsible -for enforcing compliance by third parties with this License. - - An "entity transaction" is a transaction transferring control of an -organization, or substantially all assets of one, or subdividing an -organization, or merging organizations. If propagation of a covered -work results from an entity transaction, each party to that -transaction who receives a copy of the work also receives whatever -licenses to the work the party's predecessor in interest had or could -give under the previous paragraph, plus a right to possession of the -Corresponding Source of the work from the predecessor in interest, if -the predecessor has it or can get it with reasonable efforts. - - You may not impose any further restrictions on the exercise of the -rights granted or affirmed under this License. For example, you may -not impose a license fee, royalty, or other charge for exercise of -rights granted under this License, and you may not initiate litigation -(including a cross-claim or counterclaim in a lawsuit) alleging that -any patent claim is infringed by making, using, selling, offering for -sale, or importing the Program or any portion of it. - - 11. Patents. - - A "contributor" is a copyright holder who authorizes use under this -License of the Program or a work on which the Program is based. The -work thus licensed is called the contributor's "contributor version". - - A contributor's "essential patent claims" are all patent claims -owned or controlled by the contributor, whether already acquired or -hereafter acquired, that would be infringed by some manner, permitted -by this License, of making, using, or selling its contributor version, -but do not include claims that would be infringed only as a -consequence of further modification of the contributor version. For -purposes of this definition, "control" includes the right to grant -patent sublicenses in a manner consistent with the requirements of -this License. - - Each contributor grants you a non-exclusive, worldwide, royalty-free -patent license under the contributor's essential patent claims, to -make, use, sell, offer for sale, import and otherwise run, modify and -propagate the contents of its contributor version. - - In the following three paragraphs, a "patent license" is any express -agreement or commitment, however denominated, not to enforce a patent -(such as an express permission to practice a patent or covenant not to -sue for patent infringement). To "grant" such a patent license to a -party means to make such an agreement or commitment not to enforce a -patent against the party. - - If you convey a covered work, knowingly relying on a patent license, -and the Corresponding Source of the work is not available for anyone -to copy, free of charge and under the terms of this License, through a -publicly available network server or other readily accessible means, -then you must either (1) cause the Corresponding Source to be so -available, or (2) arrange to deprive yourself of the benefit of the -patent license for this particular work, or (3) arrange, in a manner -consistent with the requirements of this License, to extend the patent -license to downstream recipients. "Knowingly relying" means you have -actual knowledge that, but for the patent license, your conveying the -covered work in a country, or your recipient's use of the covered work -in a country, would infringe one or more identifiable patents in that -country that you have reason to believe are valid. - - If, pursuant to or in connection with a single transaction or -arrangement, you convey, or propagate by procuring conveyance of, a -covered work, and grant a patent license to some of the parties -receiving the covered work authorizing them to use, propagate, modify -or convey a specific copy of the covered work, then the patent license -you grant is automatically extended to all recipients of the covered -work and works based on it. - - A patent license is "discriminatory" if it does not include within -the scope of its coverage, prohibits the exercise of, or is -conditioned on the non-exercise of one or more of the rights that are -specifically granted under this License. You may not convey a covered -work if you are a party to an arrangement with a third party that is -in the business of distributing software, under which you make payment -to the third party based on the extent of your activity of conveying -the work, and under which the third party grants, to any of the -parties who would receive the covered work from you, a discriminatory -patent license (a) in connection with copies of the covered work -conveyed by you (or copies made from those copies), or (b) primarily -for and in connection with specific products or compilations that -contain the covered work, unless you entered into that arrangement, -or that patent license was granted, prior to 28 March 2007. - - Nothing in this License shall be construed as excluding or limiting -any implied license or other defenses to infringement that may -otherwise be available to you under applicable patent law. - - 12. No Surrender of Others' Freedom. - - If conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot convey a -covered work so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you may -not convey it at all. For example, if you agree to terms that obligate you -to collect a royalty for further conveying from those to whom you convey -the Program, the only way you could satisfy both those terms and this -License would be to refrain entirely from conveying the Program. - - 13. Use with the GNU Affero General Public License. - - Notwithstanding any other provision of this License, you have -permission to link or combine any covered work with a work licensed -under version 3 of the GNU Affero General Public License into a single -combined work, and to convey the resulting work. The terms of this -License will continue to apply to the part which is the covered work, -but the special requirements of the GNU Affero General Public License, -section 13, concerning interaction through a network will apply to the -combination as such. - - 14. Revised Versions of this License. - - The Free Software Foundation may publish revised and/or new versions of -the GNU General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - - Each version is given a distinguishing version number. If the -Program specifies that a certain numbered version of the GNU General -Public License "or any later version" applies to it, you have the -option of following the terms and conditions either of that numbered -version or of any later version published by the Free Software -Foundation. If the Program does not specify a version number of the -GNU General Public License, you may choose any version ever published -by the Free Software Foundation. - - If the Program specifies that a proxy can decide which future -versions of the GNU General Public License can be used, that proxy's -public statement of acceptance of a version permanently authorizes you -to choose that version for the Program. - - Later license versions may give you additional or different -permissions. However, no additional obligations are imposed on any -author or copyright holder as a result of your choosing to follow a -later version. - - 15. Disclaimer of Warranty. - - THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY -APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT -HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY -OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, -THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM -IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF -ALL NECESSARY SERVICING, REPAIR OR CORRECTION. - - 16. Limitation of Liability. - - IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS -THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY -GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE -USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF -DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD -PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), -EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF -SUCH DAMAGES. - - 17. Interpretation of Sections 15 and 16. - - If the disclaimer of warranty and limitation of liability provided -above cannot be given local legal effect according to their terms, -reviewing courts shall apply local law that most closely approximates -an absolute waiver of all civil liability in connection with the -Program, unless a warranty or assumption of liability accompanies a -copy of the Program in return for a fee. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -state the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - - Copyright (C) - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - -Also add information on how to contact you by electronic and paper mail. - - If the program does terminal interaction, make it output a short -notice like this when it starts in an interactive mode: - - Copyright (C) - This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, your program's commands -might be different; for a GUI interface, you would use an "about box". - - You should also get your employer (if you work as a programmer) or school, -if any, to sign a "copyright disclaimer" for the program, if necessary. -For more information on this, and how to apply and follow the GNU GPL, see -. - - The GNU General Public License does not permit incorporating your program -into proprietary programs. If your program is a subroutine library, you -may consider it more useful to permit linking proprietary applications with -the library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. But first, please read -. diff --git a/Versions/dRehmFlight_Teensy_BETA_1.1/dRehmFlight_Teensy_BETA_1.1.ino b/Versions/dRehmFlight_Teensy_BETA_1.1/dRehmFlight_Teensy_BETA_1.1.ino deleted file mode 100644 index 4e66c289..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.1/dRehmFlight_Teensy_BETA_1.1.ino +++ /dev/null @@ -1,1218 +0,0 @@ -//Arduino/Teensy Flight Controller - dRehmFlight -//Author: Nicholas Rehm -//Project Start: 1/6/2020 -//Version: Beta 1.1 - -/* - * - * If you are using this for an academic or scholarly project, please credit me in any presentations or publications: - * - * Nicholas Rehm - * Department of Aerospace Engineering - * University of Maryland - * College Park 20742 - * Email: nrehm@umd.edu - * - */ - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -//CREDITS -/* -Some elements inspired by: -http://www.brokking.net/ymfc-32_main.html - -Skeleton code for reading and initializing MPU6050 borrowed from: -https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/ - -Madgwick filter function adapted from: -https://github.com/arduino-libraries/MadgwickAHRS -*/ - - - - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - - - - -//REQUIRED LIBRARIES -#include //I2c communication -#include //commanding any extra actuators, installed with teensyduino installer - - - - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - - - - -//DECLARE PINS -//NOTE: Pin 13 is reserved for onboard LED, pins 18 and 19 are reserved for the IMU -//Radio: -const int ch1Pin = 15; //throttle -const int ch2Pin = 16; //ail -const int ch3Pin = 17; //ele -const int ch4Pin = 20; //rudd -const int ch5Pin = 21; //gear (throttle cut) -const int ch6Pin = 22; //aux1 (free aux channel) -const int PPM_Pin = 23; -//MPU6050: -const int MPU = 0x68; //MPU6050 I2C address -- pins 19 = SCL & 18 = SDA -//Motor pin outputs: -const int m1Pin = 0; -const int m2Pin = 1; -const int m3Pin = 2; -const int m4Pin = 3; -const int m5Pin = 4; -const int m6Pin = 5; -//PWM outputs: -const int servo1Pin = 6; -const int servo2Pin = 7; -const int servo3Pin = 8; -const int servo4Pin = 9; -const int servo5Pin = 10; -const int servo6Pin = 11; -const int servo7Pin = 12; -PWMServo servo1; //create servo object to control a servo -PWMServo servo2; -PWMServo servo3; -PWMServo servo4; -PWMServo servo5; -PWMServo servo6; -PWMServo servo7; - - - - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - - - - -//DECLARE GLOBAL VARIABLES -//General stuff -float dt; -unsigned long current_time, prev_time; -unsigned long print_counter, serial_counter; -unsigned long blink_counter, blink_delay; -bool blinkAlternate; -//Radio comm: -unsigned long channel_1_pwm, channel_2_pwm, channel_3_pwm, channel_4_pwm, channel_5_pwm, channel_6_pwm; -unsigned long channel_1_pwm_prev, channel_2_pwm_prev, channel_3_pwm_prev, channel_4_pwm_prev; -int radio_command = 1; -//IMU: -float AccX, AccY, AccZ; -float AccX_prev, AccY_prev, AccZ_prev; -float GyroX, GyroY, GyroZ; -float GyroX_prev, GyroY_prev, GyroZ_prev; -float roll_IMU, pitch_IMU, yaw_IMU; -float roll_IMU_prev, pitch_IMU_prev; -float AccErrorX, AccErrorY, AccErrorZ, GyroErrorX, GyroErrorY, GyroErrorZ; -float roll_correction, pitch_correction; -float beta = 0.04; //madgwick filter parameter -float q0 = 1.0f; //initialize quaternion for madgwick filter -float q1 = 0.0f; -float q2 = 0.0f; -float q3 = 0.0f; -//Normalized desired state: -float thro_des, roll_des, pitch_des, yaw_des; -float roll_passthru, pitch_passthru, yaw_passthru; -//Controller: -float error_roll, error_roll_prev, roll_des_prev, integral_roll, integral_roll_il, integral_roll_ol, integral_roll_prev, integral_roll_prev_il, integral_roll_prev_ol, derivative_roll, roll_PID = 0; -float error_pitch, error_pitch_prev, pitch_des_prev, integral_pitch, integral_pitch_il, integral_pitch_ol, integral_pitch_prev, integral_pitch_prev_il, integral_pitch_prev_ol, derivative_pitch, pitch_PID = 0; -float error_yaw, error_yaw_prev, integral_yaw, integral_yaw_prev, derivative_yaw, yaw_PID = 0; -//Mixer -float m1_command_scaled, m2_command_scaled, m3_command_scaled, m4_command_scaled, m5_command_scaled, m6_command_scaled; -int m1_command_PWM, m2_command_PWM, m3_command_PWM, m4_command_PWM, m5_command_PWM, m6_command_PWM; -float s1_command_scaled, s2_command_scaled, s3_command_scaled, s4_command_scaled, s5_command_scaled, s6_command_scaled, s7_command_scaled; -int s1_command_PWM, s2_command_PWM, s3_command_PWM, s4_command_PWM, s5_command_PWM, s6_command_PWM, s7_command_PWM; - - - - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - - - - -//USER-SPECIFIED VARIABLES -//Radio failsafe values for every channel in the event that bad reciever data is detected. Recommended defaults: -unsigned long channel_1_fs = 1000; //thro -unsigned long channel_2_fs = 1500; //ail -unsigned long channel_3_fs = 1500; //elev -unsigned long channel_4_fs = 1500; //rudd -unsigned long channel_5_fs = 2000; //gear, greater than 1500 = throttle cut -unsigned long channel_6_fs = 2000; //aux1 - -//Controller parameters (take note of defaults before modifying!): -float i_limit = 25.0; //integrator saturation level, mostly for safety (default 25.0) -float maxRoll = 30.0; //max roll angle in degrees for angle mode (maximum 60 degrees), deg/sec for rate mode (maximum ~400) -float maxPitch = 30.0; //max pitch angle in degrees for angle mode (maximum 60 degrees), deg/sec for rate mode (maximum ~400) -float maxYaw = 160.0; //max yaw rate in deg/sec (maximum ~400) - -float Kp_roll_angle = 0.2; //Roll P-gain - angle mode -float Ki_roll_angle = 0.3; //Roll I-gain - angle mode -float Kd_roll_angle = 0.05; //Roll D-gain - angle mode (if using controlANGLE2, MUST be 0.0. Suggested default for controlANGLE is 0.05) -float Kp_pitch_angle = 0.2; //Pitch P-gain - angle mode -float Ki_pitch_angle = 0.3; //Pitch I-gain - angle mode -float Kd_pitch_angle = 0.05; //Pitch D-gain - angle mode (if using controlANGLE2, MUST be 0.0. Suggested default for controlANGLE is 0.05) - -float Kp_roll_rate = 0.15; //Roll P-gain - rate mode -float Ki_roll_rate = 0.2; //Roll I-gain - rate mode -float Kd_roll_rate = 0.0002; //Roll D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) -float Kp_pitch_rate = 0.15; //Pitch P-gain - rate mode -float Ki_pitch_rate = 0.2; //Pitch I-gain - rate mode -float Kd_pitch_rate = 0.0002; //Pitch D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) - -float Kp_yaw = 0.3; //Yaw P-gain -float Ki_yaw = 0.05; //Yaw I-gain -float Kd_yaw = 0.00015; //Yaw D-gain (be careful when increasing too high, motors will begin to overheat!) - - - - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - - - - -//SETUP -void setup() { - Serial.begin(500000); //usb serial - delay(1500); - - //Initialize all pins - pinMode(13, OUTPUT); //pin 13 LED blinker on board, do not modify - pinMode(m1Pin, OUTPUT); - pinMode(m2Pin, OUTPUT); - pinMode(m3Pin, OUTPUT); - pinMode(m4Pin, OUTPUT); - pinMode(m5Pin, OUTPUT); - pinMode(m6Pin, OUTPUT); - servo1.attach(servo1Pin, 900, 2100); //pin, min PWM value, max PWM value - servo2.attach(servo2Pin, 900, 2100); - servo3.attach(servo3Pin, 900, 2100); - servo4.attach(servo4Pin, 900, 2100); - servo5.attach(servo5Pin, 900, 2100); - servo6.attach(servo6Pin, 900, 2100); - servo7.attach(servo7Pin, 900, 2100); - - //Set built in LED to turn on to signal startup & not to disturb vehicle during IMU calibration - digitalWrite(13, HIGH); - - delay(20); - - //Initialize radio communication - SELECT ONE - readPWM_setup(ch1Pin, ch2Pin, ch3Pin, ch4Pin, ch5Pin, ch6Pin); //uncomment if using 6 channel pwm receiver - //readPPM_setup(PPM_Pin) //uncomment if using ppm receiver - - //Set radio channels to default (safe) values - channel_1_pwm = channel_1_fs; - channel_2_pwm = channel_2_fs; - channel_3_pwm = channel_3_fs; - channel_4_pwm = channel_4_fs; - channel_5_pwm = channel_5_fs; - channel_6_pwm = channel_6_fs; - - //Initialize IMU communication - IMUinit(); - - delay(20); - - //Get IMU error to calibrate attitude, assuming vehicle is level - calculate_IMU_error(); - calibrateAttitude(); //helps to warm up IMU and Madgwick filter - - delay(20); - - //Arm servo channels - servo1.write(0); //command servo angle from 0-180 degrees (1000 - 2000 PWM) - servo2.write(0); - servo3.write(0); - servo4.write(0); - servo5.write(0); - servo6.write(0); - servo7.write(0); - - delay(20); - - //Arm motors - m1_command_PWM = 125; - m2_command_PWM = 125; - m3_command_PWM = 125; - m4_command_PWM = 125; - m5_command_PWM = 125; - m6_command_PWM = 125; - commandMotors(); - - delay(200); - - //Indicate entering main loop with 3 quick blinks - setupBlink(3,150,70); //numBlinks, upTime (ms), downTime (ms) -- 3 quick blinks indicates entering main loop! -} - - - - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - - - - -//MAIN LOOP -void loop() { - prev_time = current_time; - current_time = micros(); - dt = (current_time - prev_time)/1000000.0; - - loopBlink(); //indicate we are in main loop with short blink every 1.5 seconds - - //Print data at 100hz (uncomment one at a time for troubleshooting) - SELECT ONE - //printRadioData(); //radio pwm values - //printDesiredState(); //desired vehicle state commanded in either degrees or degrees/sec - //printGyroData(); //prints filtered gyro data direct from IMU - //printAccelData(); //prints filtered accelerometer data direct from IMU - //printRollPitchYaw(); //prints roll, pitch, and yaw angles in degrees from Madgwick filter - //printPIDoutput(); //prints computed stabilized PID variables from controller and desired setpoint - //printMotorCommands(); //prints the values being written to the motors - //printLoopRate(); //prints the time between loops in microseconds - - //Get vehicle state - getIMUdata(); //pulls raw gyro and accel data from IMU and LP filters to remove noise - Madgwick(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, dt); //updates roll_IMU, pitch_IMU, and yaw_IMU (degrees) - - //Compute desired state - getDesState(); //convert raw commands to normalized values based on saturated control limits - - //PID Controller - SELECT ONE - controlANGLE(); //stabilize on angle setpoint - //controlANGLE2(); //stabilize on angle setpoint using cascaded method - //controlRATE(); //stabilize on rate setpoint - - //Actuator mixing and scaling to PWM values - controlMixer(); //mixes PID outputs to scaled actuator commands -- custom mixing assignments done here - scaleCommands(); //scales motor commands to 125-250 range (oneshot125 protocol) and servo commands to 0-180 (for servo library) - - //Throttle cut check - throttleCut(); //directly sets motor commands to low based on state of ch5 - - //Command actuators - commandMotors(); //sends command pulses to each motor pin using OneShot125 protocol - servo1.write(s1_command_PWM); - servo2.write(s2_command_PWM); - servo3.write(s3_command_PWM); - servo4.write(s4_command_PWM); - servo5.write(s5_command_PWM); - servo6.write(s6_command_PWM); - servo7.write(s7_command_PWM); - - //Get vehicle commands for next loop iteration - getCommands(); //pulls current available radio commands - failSafe(); //prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup - - //Regulate loop rate - loopRate(2000); //do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default -} - - - - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - - - - -//FUNCTIONS - -void IMUinit() { - //DESCRIPTION: Initialize IMU I2C connection - /* - * Don't worry about how this works - */ - Wire.begin(); //Initialize communication - delay(20); - Wire.setClock(1000000); - delay(20); - Wire.beginTransmission(MPU); //Start communication with MPU6050 // MPU=0x68 - Wire.write(0x6B); //Talk to the register 6B - Wire.write(0x00); //Make reset - place a 0 into the 6B register - Wire.endTransmission(true); //End the transmission -} - -void getIMUdata() { - //DESCRIPTION: Request full dataset from IMU and LP filter gyro and accelerometer data - /* - * Reads accelerometer and gyro data from IMU as AccX, AccY, AccZ, GyroX, GyroY, GyroZ. These values are scaled - * according to the IMU datasheet to put them into correct units of g's and degree/sec. A simple first-order - * low-pass filter is used to get rid of high frequency noise in these raw signals. Generally you want to cut - * off everything past 80Hz, but if your loop rate is not fast enough, the low pass filter will cause a lag in - * the readings. The filter parameters B_gyro and B_accel are set to be good for a 2kHz loop rate. Finally, - * the constant errors found in calculate_IMU_error() on startup are subtracted from the readings. - */ - int16_t AcX,AcY,AcZ,GyX,GyY,GyZ; - - //Get accel data - Wire.beginTransmission(MPU); - Wire.write(0x3B); //Start with register 0x3B (ACCEL_XOUT_H) - Wire.endTransmission(false); - Wire.requestFrom(MPU, 6, true); //Read 6 registers total, each axis value is stored in 2 registers - //For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet - AcX = (Wire.read() << 8 | Wire.read()); //X-axis value - AcY = (Wire.read() << 8 | Wire.read()); //Y-axis value - AcZ = (Wire.read() << 8 | Wire.read()); //Z-axis value - AccX = AcX / 16384.0; - AccY = AcY / 16384.0; - AccZ = AcZ / 16384.0; - //LP filter accelerometer data - float B_accel = 0.14; //0.01 - AccX = (1.0 - B_accel)*AccX_prev + B_accel*AccX; - AccY = (1.0 - B_accel)*AccY_prev + B_accel*AccY; - AccZ = (1.0 - B_accel)*AccZ_prev + B_accel*AccZ; - AccX_prev = AccX; - AccY_prev = AccY; - AccZ_prev = AccZ; - //Correct the outputs with the calculated error values - AccX = AccX - AccErrorX; - AccY = AccY - AccErrorY; - AccZ = AccZ - AccErrorZ; - - //Get gyro data - Wire.beginTransmission(MPU); - Wire.write(0x43); //Gyro data first register address 0x43 - Wire.endTransmission(false); - Wire.requestFrom(MPU, 6, true); //Read 4 registers total, each axis value is stored in 2 registers - //For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet - GyX = (Wire.read() << 8 | Wire.read()); - GyY = (Wire.read() << 8 | Wire.read()); - GyZ = (Wire.read() << 8 | Wire.read()); - GyroX = GyX / 131.0; - GyroY = GyY / 131.0; - GyroZ = GyZ / 131.0; - //LP filter gyro data - float B_gyro = 0.1; //0.13 sets cutoff just past 80Hz for about 3000Hz loop rate - GyroX = (1.0 - B_gyro)*GyroX_prev + B_gyro*GyroX; - GyroY = (1.0 - B_gyro)*GyroY_prev + B_gyro*GyroY; - GyroZ = (1.0 - B_gyro)*GyroZ_prev + B_gyro*GyroZ; - GyroX_prev = GyroX; - GyroY_prev = GyroY; - GyroZ_prev = GyroZ; - //Correct the outputs with the calculated error values - GyroX = GyroX - GyroErrorX; - GyroY = GyroY - GyroErrorY; - GyroZ = GyroZ - GyroErrorZ; -} - -void calculate_IMU_error() { - //DESCRIPTION: Computes IMU error on startup. Note: vehicle should be powered up on flat surface - /* - * Don't worry too much about what this is doing. The error values it computes are applied to the raw gyro and - * accelerometer values AccX, AccY, AccZ, GyroX, GyroY, GyroZ in getIMUdata(). This eliminates drift in the - * measurement. - */ - int16_t AcX,AcY,AcZ,GyX,GyY,GyZ; - - //Read accelerometer values 12000 times - int c = 0; - while (c < 12000) { - Wire.beginTransmission(MPU); - Wire.write(0x3B); - Wire.endTransmission(false); - Wire.requestFrom(MPU, 6, true); - AcX = (Wire.read() << 8 | Wire.read()); - AcY = (Wire.read() << 8 | Wire.read()); - AcZ = (Wire.read() << 8 | Wire.read()); - AccX = AcX / 16384.0; - AccY = AcY / 16384.0; - AccZ = AcZ / 16384.0; - // Sum all readings - AccErrorX = AccErrorX + AccX; - AccErrorY = AccErrorY + AccY; - AccErrorZ = AccErrorZ + AccZ; - c++; - } - //Divide the sum by 12000 to get the error value - AccErrorX = AccErrorX / 12000.0; - AccErrorY = AccErrorY / 12000.0; - AccErrorZ = AccErrorZ / 12000.0 - 1.0; - c = 0; - //Read gyro values 12000 times - while (c < 12000) { - Wire.beginTransmission(MPU); - Wire.write(0x43); - Wire.endTransmission(false); - Wire.requestFrom(MPU, 6, true); - GyX = Wire.read() << 8 | Wire.read(); - GyY = Wire.read() << 8 | Wire.read(); - GyZ = Wire.read() << 8 | Wire.read(); - GyroX = GyX / 131.0; - GyroY = GyY / 131.0; - GyroZ = GyZ / 131.0; - // Sum all readings - GyroErrorX = GyroErrorX + GyroX; - GyroErrorY = GyroErrorY + GyroY; - GyroErrorZ = GyroErrorZ + GyroZ; - c++; - } - //Divide the sum by 12000 to get the error value - GyroErrorX = GyroErrorX / 12000.0; - GyroErrorY = GyroErrorY / 12000.0; - GyroErrorZ = GyroErrorZ / 12000.0; -} - -void calibrateAttitude() { - //DESCRIPTION: Extra function to calibrate IMU attitude estimate on startup, can be used to warm up everything before entering main loop - //Assuming vehicle is powered up on level surface! - /* - * This function is used on startup to warm up the attitude estimation. Originally used to eliminate additional error in the signal - * but no longer used for that purpose as it is not needed on the Teensy. This function is what causes startup to take a few seconds - * to boot. The roll_correction and pitch_correction values can be applied to the roll and pitch attitude estimates using - * correctRollPitch() in the main loop after the madgwick filter function. Again, we don't use this for that purpose but just to warm - * up the IMU and attitude estimation algorithm. - */ - //Warm up IMU and madgwick filter - for (int i = 0; i <= 10000; i++) { - prev_time = current_time; - current_time = micros(); - dt = (current_time - prev_time)/1000000.0; - getIMUdata(); - Madgwick(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, dt); - loopRate(2000); //do not exceed 2000Hz - } - //Grab mean roll and pitch values after everything is warmed up - for (int j = 1; j <= 2000; j++) { - prev_time = current_time; - current_time = micros(); - dt = (current_time - prev_time)/1000000.0; - getIMUdata(); - Madgwick(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, dt); - roll_correction = roll_IMU + roll_correction; - pitch_correction = pitch_IMU + pitch_correction; - loopRate(2000); //do not exceed 2000Hz - } - //These are applied to roll and pitch after Madgwick filter in main loop if desired using correctRollPitch() - roll_correction = roll_correction/2000.0; - pitch_correction = pitch_correction/2000.0; -} - -void Madgwick(float gx, float gy, float gz, float ax, float ay, float az, float invSampleFreq) { - //DESCRIPTION: Attitude estimation through sensor fusion - /* - * This function fuses the accelerometer and gyro readings AccX, AccY, AccZ, GyroX, GyroY, GyroZ for attitude estimation. - * Don't worry about the math. There is a tunable parameter called beta in the variable declaration section which basically - * adjusts the weight of accelerometer and gyro data in the state estimate. Higher beta leads to noisier estimate, lower - * beta leads to slower to respond estimate. It is currently tuned for 2kHz loop rate. This function updates the roll_IMU, - * pitch_IMU, and yaw_IMU variables which are in degrees. - */ - float recipNorm; - float s0, s1, s2, s3; - float qDot1, qDot2, qDot3, qDot4; - float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; - - //Convert gyroscope degrees/sec to radians/sec - gx *= 0.0174533f; - gy *= 0.0174533f; - gz *= 0.0174533f; - - //Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); - qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); - qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); - qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); - - //Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - //Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - //Auxiliary variables to avoid repeated arithmetic - _2q0 = 2.0f * q0; - _2q1 = 2.0f * q1; - _2q2 = 2.0f * q2; - _2q3 = 2.0f * q3; - _4q0 = 4.0f * q0; - _4q1 = 4.0f * q1; - _4q2 = 4.0f * q2; - _8q1 = 8.0f * q1; - _8q2 = 8.0f * q2; - q0q0 = q0 * q0; - q1q1 = q1 * q1; - q2q2 = q2 * q2; - q3q3 = q3 * q3; - - //Gradient decent algorithm corrective step - s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; - s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; - s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; - s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); //normalise step magnitude - s0 *= recipNorm; - s1 *= recipNorm; - s2 *= recipNorm; - s3 *= recipNorm; - - //Apply feedback step - qDot1 -= beta * s0; - qDot2 -= beta * s1; - qDot3 -= beta * s2; - qDot4 -= beta * s3; - } - - //Integrate rate of change of quaternion to yield quaternion - q0 += qDot1 * invSampleFreq; - q1 += qDot2 * invSampleFreq; - q2 += qDot3 * invSampleFreq; - q3 += qDot4 * invSampleFreq; - - //Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - - //compute angles - roll_IMU = atan2(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)*57.29577951; //degrees - pitch_IMU = asin(-2.0f * (q1*q3 - q0*q2))*57.29577951; //degrees - yaw_IMU = atan2(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3)*57.29577951; //degrees -} - -void getDesState() { - //DESCRIPTION: Normalizes desired control values to appropriate values - /* - * Updates the desired state variables thro_des, roll_des, pitch_des, and yaw_des. These are computed by using the raw - * RC pwm commands and scaling them to be within our limits defined in setup. thro_des stays within 0 to 1 range. - * roll_des and pitch_des are scaled to be within max roll/pitch amount in either degrees (angle mode) or degrees/sec - * (rate mode). yaw_des is scaled to be within max yaw in degrees/sec. Also creates roll_passthru, pitch_passthru, and - * yaw_passthru variables, to be used in commanding motors/servos with direct unstabilized commands in controlMixer(). - */ - thro_des = (channel_1_pwm - 1000.0)/1000.0; //between 0 and 1 - roll_des = (channel_2_pwm - 1500.0)/500.0; //between -1 and 1 - pitch_des = (channel_3_pwm - 1500.0)/500.0; //between -1 and 1 - yaw_des = (channel_4_pwm - 1500.0)/500.0; //between -1 and 1 - //Constrain within normalized bounds - thro_des = constrain(thro_des, 0.0, 1.0); //between 0 and 1 - roll_des = constrain(roll_des, -1.0, 1.0)*maxRoll; //between -maxRoll and +maxRoll - pitch_des = constrain(pitch_des, -1.0, 1.0)*maxPitch; //between -maxPitch and +maxPitch - yaw_des = constrain(yaw_des, -1.0, 1.0)*maxYaw; //between -maxYaw and +maxYaw - - roll_passthru = roll_des/(2*maxRoll); - pitch_passthru = pitch_des/(2*maxPitch); - yaw_passthru = yaw_des/(2*maxYaw); -} - -void controlANGLE() { - //DESCRIPTION: Computes control commands based on state error (angle) - /* - * Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des computed in - * getDesState(). Error is simply the desired state minus the actual state (ex. roll_des - roll_IMU). Two safety features - * are implimented here regarding the I terms. The I terms are saturated within specified limits on startup to prevent - * excessive buildup. This can be seen by holding the vehicle at an angle and seeing the motors ramp up on one side until - * they've maxed out throttle...saturating I to a specified limit fixes this. The second feature defaults the I terms to 0 - * if the throttle is at the minimum setting. This means the motors will not start spooling up on the ground, and the I - * terms will always start from 0 on takeoff. This function updates the variables roll_PID, pitch_PID, and yaw_PID which - * can be thought of as 1-D stablized signals. They are mixed to the configuration of the vehicle in controlMixer(). - */ - - //Roll - error_roll = roll_des - roll_IMU; - integral_roll = integral_roll_prev + error_roll*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_roll = 0; - } - integral_roll = constrain(integral_roll, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_roll = GyroX; - roll_PID = 0.01*(Kp_roll_angle*error_roll + Ki_roll_angle*integral_roll - Kd_roll_angle*derivative_roll); //scaled by .01 to bring within -1 to 1 range - - //Pitch - error_pitch = pitch_des - pitch_IMU; - integral_pitch = integral_pitch_prev + error_pitch*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_pitch = 0; - } - integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_pitch = GyroY; - pitch_PID = .01*(Kp_pitch_angle*error_pitch + Ki_pitch_angle*integral_pitch - Kd_pitch_angle*derivative_pitch); //scaled by .01 to bring within -1 to 1 range - - //Yaw, stablize on rate from GyroZ - error_yaw = yaw_des - GyroZ; - integral_yaw = integral_yaw_prev + error_yaw*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_yaw = 0; - } - integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_yaw = (error_yaw - error_yaw_prev)/dt; - yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //scaled by .01 to bring within -1 to 1 range - - //Update roll variables - integral_roll_prev = integral_roll; - //Update pitch variables - integral_pitch_prev = integral_pitch; - //Update yaw variables - error_yaw_prev = error_yaw; - integral_yaw_prev = integral_yaw; -} - -void controlANGLE2() { - //DESCRIPTION: Computes control commands based on state error (angle) in cascaded scheme - /* - * Gives better performance than controlANGLE() but requires much more tuning. Not reccommended for first-time setup. - */ - //Outer loop - PID on angle - float roll_des_ol, pitch_des_ol; - //Roll - error_roll = roll_des - roll_IMU; - integral_roll_ol = integral_roll_prev_ol + error_roll*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_roll_ol = 0; - } - integral_roll_ol = constrain(integral_roll_ol, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_roll = (roll_IMU - roll_IMU_prev)/dt; - roll_des_ol = Kp_roll_angle*error_roll + Ki_roll_angle*integral_roll_ol - Kd_roll_angle*derivative_roll; - - //Pitch - error_pitch = pitch_des - pitch_IMU; - integral_pitch_ol = integral_pitch_prev_ol + error_pitch*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_pitch_ol = 0; - } - integral_pitch_ol = constrain(integral_pitch_ol, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_pitch = (pitch_IMU - pitch_IMU_prev)/dt; - pitch_des_ol = Kp_pitch_angle*error_pitch + Ki_pitch_angle*integral_pitch_ol - Kd_pitch_angle*derivative_pitch; - - //Apply loop gain, constrain, and filter - float Kl = 30.0; - roll_des_ol = Kl*roll_des_ol; - pitch_des_ol = Kl*pitch_des_ol; - roll_des_ol = constrain(roll_des_ol, -240.0, 240.0); - pitch_des_ol = constrain(pitch_des_ol, -240.0, 240.0); - float B_loop = 0.9; //LP filter parameter for outer to inner loop, acts as damping term - roll_des_ol = (1.0 - B_loop)*roll_des_prev + B_loop*roll_des_ol; - pitch_des_ol = (1.0 - B_loop)*pitch_des_prev + B_loop*pitch_des_ol; - - //Inner loop - PID on rate - //Roll - error_roll = roll_des_ol - GyroX; - integral_roll_il = integral_roll_prev_il + error_roll*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_roll_il = 0; - } - integral_roll_il = constrain(integral_roll_il, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_roll = (error_roll - error_roll_prev)/dt; - roll_PID = .01*(Kp_roll_rate*error_roll + Ki_roll_rate*integral_roll_il + Kd_roll_rate*derivative_roll); //scaled by .01 to bring within -1 to 1 range - - //Pitch - error_pitch = pitch_des_ol - GyroY; - integral_pitch_il = integral_pitch_prev_il + error_pitch*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_pitch_il = 0; - } - integral_pitch_il = constrain(integral_pitch_il, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_pitch = (error_pitch - error_pitch_prev)/dt; - pitch_PID = .01*(Kp_pitch_rate*error_pitch + Ki_pitch_rate*integral_pitch_il + Kd_pitch_rate*derivative_pitch); //scaled by .01 to bring within -1 to 1 range - - //Yaw - error_yaw = yaw_des - GyroZ; - integral_yaw = integral_yaw_prev + error_yaw*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_yaw = 0; - } - integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_yaw = (error_yaw - error_yaw_prev)/dt; - yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //scaled by .01 to bring within -1 to 1 range - - //Update roll variables - integral_roll_prev_ol = integral_roll_ol; - integral_roll_prev_il = integral_roll_il; - error_roll_prev = error_roll; - roll_IMU_prev = roll_IMU; - roll_des_prev = roll_des_ol; - //Update pitch variables - integral_pitch_prev_ol = integral_pitch_ol; - integral_pitch_prev_il = integral_pitch_il; - error_pitch_prev = error_pitch; - pitch_IMU_prev = pitch_IMU; - pitch_des_prev = pitch_des_ol; - //Update yaw variables - error_yaw_prev = error_yaw; - integral_yaw_prev = integral_yaw; - -} - -void controlRATE() { - //DESCRIPTION: Computes control commands based on state error (rate) - /* - * See explanation for controlANGLE(). Everything is the same here except the error is now the desired rate - raw gyro reading. - */ - //Roll - error_roll = roll_des - GyroX; - integral_roll = integral_roll_prev + error_roll*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_roll = 0; - } - integral_roll = constrain(integral_roll, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_roll = (error_roll - error_roll_prev)/dt; - roll_PID = .01*(Kp_roll_rate*error_roll + Ki_roll_rate*integral_roll + Kd_roll_rate*derivative_roll); //scaled by .01 to bring within -1 to 1 range - - //Pitch - error_pitch = pitch_des - GyroY; - integral_pitch = integral_pitch_prev + error_pitch*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_pitch = 0; - } - integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_pitch = (error_pitch - error_pitch_prev)/dt; - pitch_PID = .01*(Kp_pitch_rate*error_pitch + Ki_pitch_rate*integral_pitch + Kd_pitch_rate*derivative_pitch); //scaled by .01 to bring within -1 to 1 range - - //Yaw, stablize on rate from GyroZ - error_yaw = yaw_des - GyroZ; - integral_yaw = integral_yaw_prev + error_yaw*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_yaw = 0; - } - integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_yaw = (error_yaw - error_yaw_prev)/dt; - yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //scaled by .01 to bring within -1 to 1 range - - //Update roll variables - error_roll_prev = error_roll; - integral_roll_prev = integral_roll; - GyroX_prev = GyroX; - //Update pitch variables - error_pitch_prev = error_pitch; - integral_pitch_prev = integral_pitch; - GyroY_prev = GyroY; - //Update yaw variables - error_yaw_prev = error_yaw; - integral_yaw_prev = integral_yaw; -} - -void controlMixer() { - //DESCRIPTION: Mixes scaled commands from PID controller to actuator outputs based on vehicle configuration - /* - * Takes roll_PID, pitch_PID, and yaw_PID computed from the PID controller and appropriately mixes them for the desired - * vehicle configuration. For example on a quadcopter, the left two motors should have +roll_PID while the right two motors - * should have -roll_PID. Front two should have -pitch_PID and the back two should have +pitch_PID etc... every motor has - * normalized (0 - 1) thro_des command for throttle control. Can also apply direct unstabilized commands from the transmitter with - * roll_passthru, pitch_passthru, and yaw_passthu. mX_command_scaled and sX_command scaled variables are used in scaleCommands() - * in preparation to be sent to the motors and servos. - */ - //Quad mixing - //m1 = front left, m2 = front right, m3 = back right, m4 = back left - m1_command_scaled = thro_des - pitch_PID + roll_PID + yaw_PID; - m2_command_scaled = thro_des - pitch_PID - roll_PID - yaw_PID; - m3_command_scaled = thro_des + pitch_PID - roll_PID + yaw_PID; - m4_command_scaled = thro_des + pitch_PID + roll_PID - yaw_PID; - m5_command_scaled = 0; - m6_command_scaled = 0; - - //0.5 is centered servo, 0 is zero throttle if connecting to ESC for conventional PWM, 1 is max throttle - s1_command_scaled = 0; - s2_command_scaled = 0; - s3_command_scaled = 0; - s4_command_scaled = 0; - s5_command_scaled = 0; - s6_command_scaled = 0; - s7_command_scaled = 0; - - //Example use of the linear fader for float type variables. Linearly interpolates between minimum and maximum values for Kp_pitch_rate variable based on state of channel 6 - /* - if (channel_6_pwm > 1500){ - Kp_pitch_rate = floatFaderLinear(Kp_pitch_rate, 0.1, 0.3, 5.5, 1, 2000); //parameter, minimum value, maximum value, fadeTime (seconds), state (0 min or 1 max), loop frequency - } - else if (channel_6_pwm < 1500) { - Kp_pitch_rate = floatFaderLinear(Kp_pitch_rate, 0.1, 0.3, 2.5, 0, 2000); //parameter, minimum value, maximum value, fadeTime, state (0 min or 1 max), loop frequency - } - */ -} - -void scaleCommands() { - //DESCRIPTION: Scale normalized actuator commands to values for ESC protocol - /* - * mX_command_scaled variables from the mixer function are scaled to 125-250us for OneShot125 protocol. sX_command_scaled variables from - * the mixer function are scaled to 0-180 for the servo library using standard PWM. - * mX_command_PWM are updated here which are used to command the motors in commandMotors(). sX_command_PWM are updated - * which are used to command the servos. - */ - //Scaled to 125us - 250us for oneshot125 protocol - m1_command_PWM = m1_command_scaled*125 + 125; - m2_command_PWM = m2_command_scaled*125 + 125; - m3_command_PWM = m3_command_scaled*125 + 125; - m4_command_PWM = m4_command_scaled*125 + 125; - m5_command_PWM = m5_command_scaled*125 + 125; - m6_command_PWM = m6_command_scaled*125 + 125; - //Constrain commands to motors within oneshot125 bounds - m1_command_PWM = constrain(m1_command_PWM, 125, 250); - m2_command_PWM = constrain(m2_command_PWM, 125, 250); - m3_command_PWM = constrain(m3_command_PWM, 125, 250); - m4_command_PWM = constrain(m4_command_PWM, 125, 250); - m5_command_PWM = constrain(m5_command_PWM, 125, 250); - m6_command_PWM = constrain(m6_command_PWM, 125, 250); - - //Scaled to 0-180 for servo library - s1_command_PWM = s1_command_scaled*180; - s2_command_PWM = s2_command_scaled*180; - s3_command_PWM = s3_command_scaled*180; - s4_command_PWM = s4_command_scaled*180; - s5_command_PWM = s5_command_scaled*180; - s6_command_PWM = s6_command_scaled*180; - s7_command_PWM = s7_command_scaled*180; - //Constrain commands to servos within servo library bounds - s1_command_PWM = constrain(s1_command_PWM, 0, 180); - s2_command_PWM = constrain(s2_command_PWM, 0, 180); - s3_command_PWM = constrain(s3_command_PWM, 0, 180); - s4_command_PWM = constrain(s4_command_PWM, 0, 180); - s5_command_PWM = constrain(s5_command_PWM, 0, 180); - s6_command_PWM = constrain(s6_command_PWM, 0, 180); - s7_command_PWM = constrain(s7_command_PWM, 0, 180); - -} - -void getCommands() { - //DESCRIPTION: Get raw PWM values for every channel from the radio - /* - * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of - * the loop. The radio commands are retrieved from a function in the readPWM - * file separate from this one which is running a bunch of interrupts to continuously update the radio readings. - * The raw radio commands are being filtered with a first order low-pass filter to eliminate any really high frequency noise. - */ - - radio_command = 1; - channel_1_pwm = getRadioPWM(1); - channel_2_pwm = getRadioPWM(2); - channel_3_pwm = getRadioPWM(3); - channel_4_pwm = getRadioPWM(4); - channel_5_pwm = getRadioPWM(5); - channel_6_pwm = getRadioPWM(6); - - //Low-pass the critical commands and update previous values - float b = 0.2; - channel_1_pwm = (1.0 - b)*channel_1_pwm_prev + b*channel_1_pwm; - channel_2_pwm = (1.0 - b)*channel_2_pwm_prev + b*channel_2_pwm; - channel_3_pwm = (1.0 - b)*channel_3_pwm_prev + b*channel_3_pwm; - channel_4_pwm = (1.0 - b)*channel_4_pwm_prev + b*channel_4_pwm; - channel_1_pwm_prev = channel_1_pwm; - channel_2_pwm_prev = channel_2_pwm; - channel_3_pwm_prev = channel_3_pwm; - channel_4_pwm_prev = channel_4_pwm; -} - -void failSafe() { - //DESCRIPTION: If radio gives garbage values, set all commands to default values - /* - * Radio connection failsafe used to check if the getCommands() function is returning acceptable pwm values. If any of - * the commands are lower than 800 or higher than 2200, then we can be certain that there is an issue with the radio - * connection (most likely hardware related). If any of the channels show this failure, then all of the radio commands - * channel_x_pwm are set to default failsafe values specified in the setup. - */ - unsigned minVal = 800; - unsigned maxVal = 2200; - int check1 = 0; - int check2 = 0; - int check3 = 0; - int check4 = 0; - int check5 = 0; - int check6 = 0; - - //Triggers for failure criteria - if (channel_1_pwm > maxVal || channel_1_pwm < minVal) check1 = 1; - if (channel_2_pwm > maxVal || channel_2_pwm < minVal) check2 = 1; - if (channel_3_pwm > maxVal || channel_3_pwm < minVal) check3 = 1; - if (channel_4_pwm > maxVal || channel_4_pwm < minVal) check4 = 1; - if (channel_5_pwm > maxVal || channel_5_pwm < minVal) check5 = 1; - if (channel_6_pwm > maxVal || channel_6_pwm < minVal) check6 = 1; - - //If any failures, set to default failsafe values - if ((check1 + check2 + check3 + check4 + check5 + check6) > 0) { - channel_1_pwm = channel_1_fs; - channel_2_pwm = channel_2_fs; - channel_3_pwm = channel_3_fs; - channel_4_pwm = channel_4_fs; - channel_5_pwm = channel_5_fs; - channel_6_pwm = channel_6_fs; - } -} - -void commandMotors() { - //DESCRIPTION: Send pulses to motor pins, oneshot125 protocol - /* - * My crude implimentation of OneShot125 protocol which sends 125 - 250us pulses to the ESCs (mXPin). The pulselengths being - * sent are mX_command_PWM, computed in scaleCommands(). - */ - int wentLow = 0; - int pulseStart, timer; - int flagM1 = 0; - int flagM2 = 0; - int flagM3 = 0; - int flagM4 = 0; - int flagM5 = 0; - int flagM6 = 0; - - //Write all motor pins high - digitalWrite(m1Pin, HIGH); - digitalWrite(m2Pin, HIGH); - digitalWrite(m3Pin, HIGH); - digitalWrite(m4Pin, HIGH); - digitalWrite(m5Pin, HIGH); - digitalWrite(m6Pin, HIGH); - pulseStart = micros(); - - //Write each motor pin low as correct pulse length is reached - while (wentLow < 6 ) { //keep going until final (6th) pulse is finished, then done - timer = micros(); - if ((m1_command_PWM <= timer - pulseStart) && (flagM1==0)) { - digitalWrite(m1Pin, LOW); - wentLow = wentLow + 1; - flagM1 = 1; - } - if ((m2_command_PWM <= timer - pulseStart) && (flagM2==0)) { - digitalWrite(m2Pin, LOW); - wentLow = wentLow + 1; - flagM2 = 1; - } - if ((m3_command_PWM <= timer - pulseStart) && (flagM3==0)) { - digitalWrite(m3Pin, LOW); - wentLow = wentLow + 1; - flagM3 = 1; - } - if ((m4_command_PWM <= timer - pulseStart) && (flagM4==0)) { - digitalWrite(m4Pin, LOW); - wentLow = wentLow + 1; - flagM4 = 1; - } - if ((m5_command_PWM <= timer - pulseStart) && (flagM5==0)) { - digitalWrite(m5Pin, LOW); - wentLow = wentLow + 1; - flagM5 = 1; - } - if ((m6_command_PWM <= timer - pulseStart) && (flagM6==0)) { - digitalWrite(m6Pin, LOW); - wentLow = wentLow + 1; - flagM6 = 1; - } - } -} - -float floatFaderLinear(float param, float param_min, float param_max, float fadeTime, int state, int loopFreq){ - //DESCRIPTION: Linearly fades a float type variable between min and max bounds based on desired high or low state and time - /* Takes in a float variable, desired minimum and maximum bounds, fade time, high or low desired state, and the loop frequency - * and linearly interpolates that param variable between the maximum and minimum bounds. This function can be called in controlMixer() - * and high/low states can be determined by monitoring the state of an auxillarly radio channel. For example, if channel_6_pwm is being - * monitored to switch between two dynamic configurations (hover and forward flight), this function can be called within the logical - * statements in order to fade controller gains, for example between the two dynamic configurations. The 'state' (1 or 0) can be used - * to designate the two final options for that control gain based on the dynamic configuration assignment to the auxillary radio channel. - */ - float diffParam = (param_max - param_min)/(fadeTime*loopFreq); //difference to add or subtract from param for each loop iteration for desired fadeTime - - if (state == 1) { //maximum param bound desired, increase param by diffParam for each loop iteration - param = param + diffParam; - } - else if (state == 0) { //minimum param bound desired, decrease param by diffParam for each loop iteration - param = param - diffParam; - } - - param = constrain(param, param_min, param_max); //constrain param within max bounds - - return param; -} - - -void throttleCut() { - //DESCRIPTION: Directly set actuator outputs to minimum value if triggered - /* - * Monitors the state of radio command channel_5_pwm and directly sets the mx_command_PWM values to minimum (120 is - * minimum for oneshot125 protocol, 1000 is minimum for standard PWM) if channel 5 is high. This is the last function - * called before commandMotors() is called so that the last thing checked is if the user is giving permission to command - * the motors to anything other than minimum value. Safety first. - */ - if (channel_5_pwm > 1500) { - m1_command_PWM = 120; - m2_command_PWM = 120; - m3_command_PWM = 120; - m4_command_PWM = 120; - m5_command_PWM = 120; - m6_command_PWM = 120; - } -} - -void loopRate(int freq) { - //DESCRIPTION: Regulate main loop rate to specified frequency in Hz - /* - * It's good to operate at a constant loop rate for filters to remain stable and whatnot. Interrupt routines running in the - * background cause the loop rate to fluctuate. This function basically just waits at the end of every loop iteration until - * the correct time has passed since the start of the current loop for the desired loop rate in Hz. 2kHz is a good rate to - * be at because the loop nominally will run between 2.8kHz - 4.2kHz. This lets us have a little room to add extra computations - * and remain above 2kHz, without needing to retune all of our filtering parameters scattered around this code. - */ - float invFreq = 1.0/freq*1000000.0; - unsigned long checker = micros(); - - //Sit in loop until appropriate time has passed - while (invFreq > (checker - current_time)) { - checker = micros(); - } -} - -void loopBlink() { - //DESCRIPTION: Blink LED on board to indicate main loop is running - /* - * It looks cool. - */ - if (current_time - blink_counter > blink_delay) { - blink_counter = micros(); - digitalWrite(13, blinkAlternate); //pin 13 is built in LED - - if (blinkAlternate == 1) { - blinkAlternate = 0; - blink_delay = 100000; - } - else if (blinkAlternate == 0) { - blinkAlternate = 1; - blink_delay = 2000000; - } - } -} - -void setupBlink(int numBlinks,int upTime, int downTime) { - //DESCRIPTION: Simple function to make LED on board blink as desired - for (int j = 1; j<= numBlinks; j++) { - digitalWrite(13, LOW); - delay(downTime); - digitalWrite(13, HIGH); - delay(upTime); - } -} - -void printRadioData() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F(" CH1: ")); - Serial.print(channel_1_pwm); - Serial.print(F(" CH2: ")); - Serial.print(channel_2_pwm); - Serial.print(F(" CH3: ")); - Serial.print(channel_3_pwm); - Serial.print(F(" CH4: ")); - Serial.print(channel_4_pwm); - Serial.print(F(" CH5: ")); - Serial.print(channel_5_pwm); - Serial.print(F(" CH6: ")); - Serial.println(channel_6_pwm); - } -} - -void printDesiredState() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("thro_des: ")); - Serial.print(thro_des); - Serial.print(F(" roll_des: ")); - Serial.print(roll_des); - Serial.print(F(" pitch_des: ")); - Serial.print(pitch_des); - Serial.print(F(" yaw_des: ")); - Serial.println(yaw_des); - } -} - -void printGyroData() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("GyroX: ")); - Serial.print(GyroX); - Serial.print(F(" GyroY: ")); - Serial.print(GyroY); - Serial.print(F(" GyroZ: ")); - Serial.println(GyroZ); - } -} - -void printAccelData() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("AccX: ")); - Serial.print(AccX); - Serial.print(F(" AccY: ")); - Serial.print(AccY); - Serial.print(F(" AccZ: ")); - Serial.println(AccZ); - } -} - -void printRollPitchYaw() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("roll: ")); - Serial.print(roll_IMU); - Serial.print(F(" pitch: ")); - Serial.print(pitch_IMU); - Serial.print(F(" yaw: ")); - Serial.println(yaw_IMU); - } -} - -void printPIDoutput() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("roll_PID: ")); - Serial.print(roll_PID); - Serial.print(F(" pitch_PID: ")); - Serial.print(pitch_PID); - Serial.print(F(" yaw_PID: ")); - Serial.println(yaw_PID); - } -} - -void printMotorCommands() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("m1_command: ")); - Serial.print(m1_command_PWM); - Serial.print(F(" m2_command: ")); - Serial.print(m2_command_PWM); - Serial.print(F(" m3_command: ")); - Serial.print(m3_command_PWM); - Serial.print(F(" m4_command: ")); - Serial.print(m4_command_PWM); - Serial.print(F(" m5_command: ")); - Serial.print(m5_command_PWM); - Serial.print(F(" m6_command: ")); - Serial.println(m6_command_PWM); - } -} - -void printLoopRate() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("dt = ")); - Serial.println(dt*1000000.0); - } -} - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -//HELPER FUNCTIONS - -float invSqrt(float x) { - //Fast inverse sqrt - /* - float halfx = 0.5f * x; - float y = x; - long i = *(long*)&y; - i = 0x5f3759df - (i>>1); - y = *(float*)&i; - y = y * (1.5f - (halfx * y * y)); - y = y * (1.5f - (halfx * y * y)); - return y; - */ - //alternate form: - unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1); - float tmp = *(float*)&i; - float y = tmp * (1.69000231f - 0.714158168f * x * tmp * tmp); - return y; -} diff --git a/Versions/dRehmFlight_Teensy_BETA_1.1/radioComm.ino b/Versions/dRehmFlight_Teensy_BETA_1.1/radioComm.ino deleted file mode 100644 index 73ed4280..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.1/radioComm.ino +++ /dev/null @@ -1,179 +0,0 @@ -//Arduino/Teensy Flight Controller - dRehmFlight -//Author: Nicholas Rehm -//Project Start: 1/6/2020 -//Version: Beta 1.1 - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -unsigned long rising_edge_start_1, rising_edge_start_2, rising_edge_start_3, rising_edge_start_4, rising_edge_start_5, rising_edge_start_6; -unsigned long channel_1_raw, channel_2_raw, channel_3_raw, channel_4_raw, channel_5_raw, channel_6_raw; -int ppm_counter = 0; -unsigned long time_ms = 0; - - -void readPPM_setup(int pin) { - // DESCRIPTION: Initialize software interrupts on radio channel pin for PPM type receiver - // Declare interrupt pins - pinMode(pin, INPUT_PULLUP); - delay(20); - //Attach interrupt and point to corresponding ISR function - attachInterrupt(digitalPinToInterrupt(pin), getPPM, CHANGE); -} - - -void readPWM_setup(int ch1, int ch2, int ch3, int ch4, int ch5, int ch6) { - //DESCRIPTION: Initialize software interrupts on radio channel pins for PWM tpye receiver - //Declare interrupt pins - pinMode(ch1, INPUT_PULLUP); - pinMode(ch2, INPUT_PULLUP); - pinMode(ch3, INPUT_PULLUP); - pinMode(ch4, INPUT_PULLUP); - pinMode(ch5, INPUT_PULLUP); - pinMode(ch6, INPUT_PULLUP); - delay(20); - //Attach interrupt and point to corresponding ISR functions - attachInterrupt(digitalPinToInterrupt(ch1), getCh1, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch2), getCh2, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch3), getCh3, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch4), getCh4, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch5), getCh5, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch6), getCh6, CHANGE); - delay(20); -} - -unsigned long getRadioPWM(int ch_num) { - //DESCRIPTION: Get current radio commands from interrupt routines - unsigned long returnPWM = 0; - - if (ch_num == 1) { - returnPWM = channel_1_raw; - } - else if (ch_num == 2) { - returnPWM = channel_2_raw; - } - else if (ch_num == 3) { - returnPWM = channel_3_raw; - } - else if (ch_num == 4) { - returnPWM = channel_4_raw; - } - else if (ch_num == 5) { - returnPWM = channel_5_raw; - } - else if (ch_num == 6) { - returnPWM = channel_6_raw; - } - - return returnPWM; -} - - - - -//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - - - - -//INTERRUPT SERVICE ROUTINES - -void getPPM() { - unsigned long dt_ppm; - int trig = digitalRead(PPM_Pin); - - if (trig==1) { //only care about rising edge - dt_ppm = micros() - time_ms; - time_ms = micros(); - - - if (dt_ppm > 5000) { //waiting for long pulse to indicate a new pulse train has arrived - ppm_counter = 0; - } - - if (ppm_counter == 1) { //first pulse - channel_1_raw = dt_ppm; - } - - if (ppm_counter == 2) { //second pulse - channel_2_raw = dt_ppm; - } - - if (ppm_counter == 3) { //third pulse - channel_3_raw = dt_ppm; - } - - if (ppm_counter == 4) { //fourth pulse - channel_4_raw = dt_ppm; - } - - if (ppm_counter == 5) { //fifth pulse - channel_5_raw = dt_ppm; - } - - if (ppm_counter == 6) { //sixth pulse - channel_6_raw = dt_ppm; - } - - ppm_counter = ppm_counter + 1; - } -} - -void getCh1() { - int trigger = digitalRead(ch1Pin); - if(trigger == 1) { - rising_edge_start_1 = micros(); - } - else if(trigger == 0) { - channel_1_raw = micros() - rising_edge_start_1; - } -} - -void getCh2() { - int trigger = digitalRead(ch2Pin); - if(trigger == 1) { - rising_edge_start_2 = micros(); - } - else if(trigger == 0) { - channel_2_raw = micros() - rising_edge_start_2; - } -} - -void getCh3() { - int trigger = digitalRead(ch3Pin); - if(trigger == 1) { - rising_edge_start_3 = micros(); - } - else if(trigger == 0) { - channel_3_raw = micros() - rising_edge_start_3; - } -} - -void getCh4() { - int trigger = digitalRead(ch4Pin); - if(trigger == 1) { - rising_edge_start_4 = micros(); - } - else if(trigger == 0) { - channel_4_raw = micros() - rising_edge_start_4; - } -} - -void getCh5() { - int trigger = digitalRead(ch5Pin); - if(trigger == 1) { - rising_edge_start_5 = micros(); - } - else if(trigger == 0) { - channel_5_raw = micros() - rising_edge_start_5; - } -} - -void getCh6() { - int trigger = digitalRead(ch6Pin); - if(trigger == 1) { - rising_edge_start_6 = micros(); - } - else if(trigger == 0) { - channel_6_raw = micros() - rising_edge_start_6; - } -} diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/COPYING.txt b/Versions/dRehmFlight_Teensy_BETA_1.2/COPYING.txt deleted file mode 100644 index 61d18602..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/COPYING.txt +++ /dev/null @@ -1,674 +0,0 @@ -GNU GENERAL PUBLIC LICENSE - Version 3, 29 June 2007 - - Copyright (C) 2007 Free Software Foundation, Inc. - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The GNU General Public License is a free, copyleft license for -software and other kinds of works. - - The licenses for most software and other practical works are designed -to take away your freedom to share and change the works. By contrast, -the GNU General Public License is intended to guarantee your freedom to -share and change all versions of a program--to make sure it remains free -software for all its users. We, the Free Software Foundation, use the -GNU General Public License for most of our software; it applies also to -any other work released this way by its authors. You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -them if you wish), that you receive source code or can get it if you -want it, that you can change the software or use pieces of it in new -free programs, and that you know you can do these things. - - To protect your rights, we need to prevent others from denying you -these rights or asking you to surrender the rights. Therefore, you have -certain responsibilities if you distribute copies of the software, or if -you modify it: responsibilities to respect the freedom of others. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must pass on to the recipients the same -freedoms that you received. You must make sure that they, too, receive -or can get the source code. And you must show them these terms so they -know their rights. - - Developers that use the GNU GPL protect your rights with two steps: -(1) assert copyright on the software, and (2) offer you this License -giving you legal permission to copy, distribute and/or modify it. - - For the developers' and authors' protection, the GPL clearly explains -that there is no warranty for this free software. For both users' and -authors' sake, the GPL requires that modified versions be marked as -changed, so that their problems will not be attributed erroneously to -authors of previous versions. - - Some devices are designed to deny users access to install or run -modified versions of the software inside them, although the manufacturer -can do so. This is fundamentally incompatible with the aim of -protecting users' freedom to change the software. The systematic -pattern of such abuse occurs in the area of products for individuals to -use, which is precisely where it is most unacceptable. Therefore, we -have designed this version of the GPL to prohibit the practice for those -products. If such problems arise substantially in other domains, we -stand ready to extend this provision to those domains in future versions -of the GPL, as needed to protect the freedom of users. - - Finally, every program is threatened constantly by software patents. -States should not allow patents to restrict development and use of -software on general-purpose computers, but in those that do, we wish to -avoid the special danger that patents applied to a free program could -make it effectively proprietary. To prevent this, the GPL assures that -patents cannot be used to render the program non-free. - - The precise terms and conditions for copying, distribution and -modification follow. - - TERMS AND CONDITIONS - - 0. Definitions. - - "This License" refers to version 3 of the GNU General Public License. - - "Copyright" also means copyright-like laws that apply to other kinds of -works, such as semiconductor masks. - - "The Program" refers to any copyrightable work licensed under this -License. Each licensee is addressed as "you". "Licensees" and -"recipients" may be individuals or organizations. - - To "modify" a work means to copy from or adapt all or part of the work -in a fashion requiring copyright permission, other than the making of an -exact copy. The resulting work is called a "modified version" of the -earlier work or a work "based on" the earlier work. - - A "covered work" means either the unmodified Program or a work based -on the Program. - - To "propagate" a work means to do anything with it that, without -permission, would make you directly or secondarily liable for -infringement under applicable copyright law, except executing it on a -computer or modifying a private copy. Propagation includes copying, -distribution (with or without modification), making available to the -public, and in some countries other activities as well. - - To "convey" a work means any kind of propagation that enables other -parties to make or receive copies. Mere interaction with a user through -a computer network, with no transfer of a copy, is not conveying. - - An interactive user interface displays "Appropriate Legal Notices" -to the extent that it includes a convenient and prominently visible -feature that (1) displays an appropriate copyright notice, and (2) -tells the user that there is no warranty for the work (except to the -extent that warranties are provided), that licensees may convey the -work under this License, and how to view a copy of this License. If -the interface presents a list of user commands or options, such as a -menu, a prominent item in the list meets this criterion. - - 1. Source Code. - - The "source code" for a work means the preferred form of the work -for making modifications to it. "Object code" means any non-source -form of a work. - - A "Standard Interface" means an interface that either is an official -standard defined by a recognized standards body, or, in the case of -interfaces specified for a particular programming language, one that -is widely used among developers working in that language. - - The "System Libraries" of an executable work include anything, other -than the work as a whole, that (a) is included in the normal form of -packaging a Major Component, but which is not part of that Major -Component, and (b) serves only to enable use of the work with that -Major Component, or to implement a Standard Interface for which an -implementation is available to the public in source code form. A -"Major Component", in this context, means a major essential component -(kernel, window system, and so on) of the specific operating system -(if any) on which the executable work runs, or a compiler used to -produce the work, or an object code interpreter used to run it. - - The "Corresponding Source" for a work in object code form means all -the source code needed to generate, install, and (for an executable -work) run the object code and to modify the work, including scripts to -control those activities. However, it does not include the work's -System Libraries, or general-purpose tools or generally available free -programs which are used unmodified in performing those activities but -which are not part of the work. For example, Corresponding Source -includes interface definition files associated with source files for -the work, and the source code for shared libraries and dynamically -linked subprograms that the work is specifically designed to require, -such as by intimate data communication or control flow between those -subprograms and other parts of the work. - - The Corresponding Source need not include anything that users -can regenerate automatically from other parts of the Corresponding -Source. - - The Corresponding Source for a work in source code form is that -same work. - - 2. Basic Permissions. - - All rights granted under this License are granted for the term of -copyright on the Program, and are irrevocable provided the stated -conditions are met. This License explicitly affirms your unlimited -permission to run the unmodified Program. The output from running a -covered work is covered by this License only if the output, given its -content, constitutes a covered work. This License acknowledges your -rights of fair use or other equivalent, as provided by copyright law. - - You may make, run and propagate covered works that you do not -convey, without conditions so long as your license otherwise remains -in force. You may convey covered works to others for the sole purpose -of having them make modifications exclusively for you, or provide you -with facilities for running those works, provided that you comply with -the terms of this License in conveying all material for which you do -not control copyright. Those thus making or running the covered works -for you must do so exclusively on your behalf, under your direction -and control, on terms that prohibit them from making any copies of -your copyrighted material outside their relationship with you. - - Conveying under any other circumstances is permitted solely under -the conditions stated below. Sublicensing is not allowed; section 10 -makes it unnecessary. - - 3. Protecting Users' Legal Rights From Anti-Circumvention Law. - - No covered work shall be deemed part of an effective technological -measure under any applicable law fulfilling obligations under article -11 of the WIPO copyright treaty adopted on 20 December 1996, or -similar laws prohibiting or restricting circumvention of such -measures. - - When you convey a covered work, you waive any legal power to forbid -circumvention of technological measures to the extent such circumvention -is effected by exercising rights under this License with respect to -the covered work, and you disclaim any intention to limit operation or -modification of the work as a means of enforcing, against the work's -users, your or third parties' legal rights to forbid circumvention of -technological measures. - - 4. Conveying Verbatim Copies. - - You may convey verbatim copies of the Program's source code as you -receive it, in any medium, provided that you conspicuously and -appropriately publish on each copy an appropriate copyright notice; -keep intact all notices stating that this License and any -non-permissive terms added in accord with section 7 apply to the code; -keep intact all notices of the absence of any warranty; and give all -recipients a copy of this License along with the Program. - - You may charge any price or no price for each copy that you convey, -and you may offer support or warranty protection for a fee. - - 5. Conveying Modified Source Versions. - - You may convey a work based on the Program, or the modifications to -produce it from the Program, in the form of source code under the -terms of section 4, provided that you also meet all of these conditions: - - a) The work must carry prominent notices stating that you modified - it, and giving a relevant date. - - b) The work must carry prominent notices stating that it is - released under this License and any conditions added under section - 7. This requirement modifies the requirement in section 4 to - "keep intact all notices". - - c) You must license the entire work, as a whole, under this - License to anyone who comes into possession of a copy. This - License will therefore apply, along with any applicable section 7 - additional terms, to the whole of the work, and all its parts, - regardless of how they are packaged. This License gives no - permission to license the work in any other way, but it does not - invalidate such permission if you have separately received it. - - d) If the work has interactive user interfaces, each must display - Appropriate Legal Notices; however, if the Program has interactive - interfaces that do not display Appropriate Legal Notices, your - work need not make them do so. - - A compilation of a covered work with other separate and independent -works, which are not by their nature extensions of the covered work, -and which are not combined with it such as to form a larger program, -in or on a volume of a storage or distribution medium, is called an -"aggregate" if the compilation and its resulting copyright are not -used to limit the access or legal rights of the compilation's users -beyond what the individual works permit. Inclusion of a covered work -in an aggregate does not cause this License to apply to the other -parts of the aggregate. - - 6. Conveying Non-Source Forms. - - You may convey a covered work in object code form under the terms -of sections 4 and 5, provided that you also convey the -machine-readable Corresponding Source under the terms of this License, -in one of these ways: - - a) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by the - Corresponding Source fixed on a durable physical medium - customarily used for software interchange. - - b) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by a - written offer, valid for at least three years and valid for as - long as you offer spare parts or customer support for that product - model, to give anyone who possesses the object code either (1) a - copy of the Corresponding Source for all the software in the - product that is covered by this License, on a durable physical - medium customarily used for software interchange, for a price no - more than your reasonable cost of physically performing this - conveying of source, or (2) access to copy the - Corresponding Source from a network server at no charge. - - c) Convey individual copies of the object code with a copy of the - written offer to provide the Corresponding Source. This - alternative is allowed only occasionally and noncommercially, and - only if you received the object code with such an offer, in accord - with subsection 6b. - - d) Convey the object code by offering access from a designated - place (gratis or for a charge), and offer equivalent access to the - Corresponding Source in the same way through the same place at no - further charge. You need not require recipients to copy the - Corresponding Source along with the object code. If the place to - copy the object code is a network server, the Corresponding Source - may be on a different server (operated by you or a third party) - that supports equivalent copying facilities, provided you maintain - clear directions next to the object code saying where to find the - Corresponding Source. Regardless of what server hosts the - Corresponding Source, you remain obligated to ensure that it is - available for as long as needed to satisfy these requirements. - - e) Convey the object code using peer-to-peer transmission, provided - you inform other peers where the object code and Corresponding - Source of the work are being offered to the general public at no - charge under subsection 6d. - - A separable portion of the object code, whose source code is excluded -from the Corresponding Source as a System Library, need not be -included in conveying the object code work. - - A "User Product" is either (1) a "consumer product", which means any -tangible personal property which is normally used for personal, family, -or household purposes, or (2) anything designed or sold for incorporation -into a dwelling. In determining whether a product is a consumer product, -doubtful cases shall be resolved in favor of coverage. For a particular -product received by a particular user, "normally used" refers to a -typical or common use of that class of product, regardless of the status -of the particular user or of the way in which the particular user -actually uses, or expects or is expected to use, the product. A product -is a consumer product regardless of whether the product has substantial -commercial, industrial or non-consumer uses, unless such uses represent -the only significant mode of use of the product. - - "Installation Information" for a User Product means any methods, -procedures, authorization keys, or other information required to install -and execute modified versions of a covered work in that User Product from -a modified version of its Corresponding Source. The information must -suffice to ensure that the continued functioning of the modified object -code is in no case prevented or interfered with solely because -modification has been made. - - If you convey an object code work under this section in, or with, or -specifically for use in, a User Product, and the conveying occurs as -part of a transaction in which the right of possession and use of the -User Product is transferred to the recipient in perpetuity or for a -fixed term (regardless of how the transaction is characterized), the -Corresponding Source conveyed under this section must be accompanied -by the Installation Information. But this requirement does not apply -if neither you nor any third party retains the ability to install -modified object code on the User Product (for example, the work has -been installed in ROM). - - The requirement to provide Installation Information does not include a -requirement to continue to provide support service, warranty, or updates -for a work that has been modified or installed by the recipient, or for -the User Product in which it has been modified or installed. Access to a -network may be denied when the modification itself materially and -adversely affects the operation of the network or violates the rules and -protocols for communication across the network. - - Corresponding Source conveyed, and Installation Information provided, -in accord with this section must be in a format that is publicly -documented (and with an implementation available to the public in -source code form), and must require no special password or key for -unpacking, reading or copying. - - 7. Additional Terms. - - "Additional permissions" are terms that supplement the terms of this -License by making exceptions from one or more of its conditions. -Additional permissions that are applicable to the entire Program shall -be treated as though they were included in this License, to the extent -that they are valid under applicable law. If additional permissions -apply only to part of the Program, that part may be used separately -under those permissions, but the entire Program remains governed by -this License without regard to the additional permissions. - - When you convey a copy of a covered work, you may at your option -remove any additional permissions from that copy, or from any part of -it. (Additional permissions may be written to require their own -removal in certain cases when you modify the work.) You may place -additional permissions on material, added by you to a covered work, -for which you have or can give appropriate copyright permission. - - Notwithstanding any other provision of this License, for material you -add to a covered work, you may (if authorized by the copyright holders of -that material) supplement the terms of this License with terms: - - a) Disclaiming warranty or limiting liability differently from the - terms of sections 15 and 16 of this License; or - - b) Requiring preservation of specified reasonable legal notices or - author attributions in that material or in the Appropriate Legal - Notices displayed by works containing it; or - - c) Prohibiting misrepresentation of the origin of that material, or - requiring that modified versions of such material be marked in - reasonable ways as different from the original version; or - - d) Limiting the use for publicity purposes of names of licensors or - authors of the material; or - - e) Declining to grant rights under trademark law for use of some - trade names, trademarks, or service marks; or - - f) Requiring indemnification of licensors and authors of that - material by anyone who conveys the material (or modified versions of - it) with contractual assumptions of liability to the recipient, for - any liability that these contractual assumptions directly impose on - those licensors and authors. - - All other non-permissive additional terms are considered "further -restrictions" within the meaning of section 10. If the Program as you -received it, or any part of it, contains a notice stating that it is -governed by this License along with a term that is a further -restriction, you may remove that term. If a license document contains -a further restriction but permits relicensing or conveying under this -License, you may add to a covered work material governed by the terms -of that license document, provided that the further restriction does -not survive such relicensing or conveying. - - If you add terms to a covered work in accord with this section, you -must place, in the relevant source files, a statement of the -additional terms that apply to those files, or a notice indicating -where to find the applicable terms. - - Additional terms, permissive or non-permissive, may be stated in the -form of a separately written license, or stated as exceptions; -the above requirements apply either way. - - 8. Termination. - - You may not propagate or modify a covered work except as expressly -provided under this License. Any attempt otherwise to propagate or -modify it is void, and will automatically terminate your rights under -this License (including any patent licenses granted under the third -paragraph of section 11). - - However, if you cease all violation of this License, then your -license from a particular copyright holder is reinstated (a) -provisionally, unless and until the copyright holder explicitly and -finally terminates your license, and (b) permanently, if the copyright -holder fails to notify you of the violation by some reasonable means -prior to 60 days after the cessation. - - Moreover, your license from a particular copyright holder is -reinstated permanently if the copyright holder notifies you of the -violation by some reasonable means, this is the first time you have -received notice of violation of this License (for any work) from that -copyright holder, and you cure the violation prior to 30 days after -your receipt of the notice. - - Termination of your rights under this section does not terminate the -licenses of parties who have received copies or rights from you under -this License. If your rights have been terminated and not permanently -reinstated, you do not qualify to receive new licenses for the same -material under section 10. - - 9. Acceptance Not Required for Having Copies. - - You are not required to accept this License in order to receive or -run a copy of the Program. Ancillary propagation of a covered work -occurring solely as a consequence of using peer-to-peer transmission -to receive a copy likewise does not require acceptance. However, -nothing other than this License grants you permission to propagate or -modify any covered work. These actions infringe copyright if you do -not accept this License. Therefore, by modifying or propagating a -covered work, you indicate your acceptance of this License to do so. - - 10. Automatic Licensing of Downstream Recipients. - - Each time you convey a covered work, the recipient automatically -receives a license from the original licensors, to run, modify and -propagate that work, subject to this License. You are not responsible -for enforcing compliance by third parties with this License. - - An "entity transaction" is a transaction transferring control of an -organization, or substantially all assets of one, or subdividing an -organization, or merging organizations. If propagation of a covered -work results from an entity transaction, each party to that -transaction who receives a copy of the work also receives whatever -licenses to the work the party's predecessor in interest had or could -give under the previous paragraph, plus a right to possession of the -Corresponding Source of the work from the predecessor in interest, if -the predecessor has it or can get it with reasonable efforts. - - You may not impose any further restrictions on the exercise of the -rights granted or affirmed under this License. For example, you may -not impose a license fee, royalty, or other charge for exercise of -rights granted under this License, and you may not initiate litigation -(including a cross-claim or counterclaim in a lawsuit) alleging that -any patent claim is infringed by making, using, selling, offering for -sale, or importing the Program or any portion of it. - - 11. Patents. - - A "contributor" is a copyright holder who authorizes use under this -License of the Program or a work on which the Program is based. The -work thus licensed is called the contributor's "contributor version". - - A contributor's "essential patent claims" are all patent claims -owned or controlled by the contributor, whether already acquired or -hereafter acquired, that would be infringed by some manner, permitted -by this License, of making, using, or selling its contributor version, -but do not include claims that would be infringed only as a -consequence of further modification of the contributor version. For -purposes of this definition, "control" includes the right to grant -patent sublicenses in a manner consistent with the requirements of -this License. - - Each contributor grants you a non-exclusive, worldwide, royalty-free -patent license under the contributor's essential patent claims, to -make, use, sell, offer for sale, import and otherwise run, modify and -propagate the contents of its contributor version. - - In the following three paragraphs, a "patent license" is any express -agreement or commitment, however denominated, not to enforce a patent -(such as an express permission to practice a patent or covenant not to -sue for patent infringement). To "grant" such a patent license to a -party means to make such an agreement or commitment not to enforce a -patent against the party. - - If you convey a covered work, knowingly relying on a patent license, -and the Corresponding Source of the work is not available for anyone -to copy, free of charge and under the terms of this License, through a -publicly available network server or other readily accessible means, -then you must either (1) cause the Corresponding Source to be so -available, or (2) arrange to deprive yourself of the benefit of the -patent license for this particular work, or (3) arrange, in a manner -consistent with the requirements of this License, to extend the patent -license to downstream recipients. "Knowingly relying" means you have -actual knowledge that, but for the patent license, your conveying the -covered work in a country, or your recipient's use of the covered work -in a country, would infringe one or more identifiable patents in that -country that you have reason to believe are valid. - - If, pursuant to or in connection with a single transaction or -arrangement, you convey, or propagate by procuring conveyance of, a -covered work, and grant a patent license to some of the parties -receiving the covered work authorizing them to use, propagate, modify -or convey a specific copy of the covered work, then the patent license -you grant is automatically extended to all recipients of the covered -work and works based on it. - - A patent license is "discriminatory" if it does not include within -the scope of its coverage, prohibits the exercise of, or is -conditioned on the non-exercise of one or more of the rights that are -specifically granted under this License. You may not convey a covered -work if you are a party to an arrangement with a third party that is -in the business of distributing software, under which you make payment -to the third party based on the extent of your activity of conveying -the work, and under which the third party grants, to any of the -parties who would receive the covered work from you, a discriminatory -patent license (a) in connection with copies of the covered work -conveyed by you (or copies made from those copies), or (b) primarily -for and in connection with specific products or compilations that -contain the covered work, unless you entered into that arrangement, -or that patent license was granted, prior to 28 March 2007. - - Nothing in this License shall be construed as excluding or limiting -any implied license or other defenses to infringement that may -otherwise be available to you under applicable patent law. - - 12. No Surrender of Others' Freedom. - - If conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot convey a -covered work so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you may -not convey it at all. For example, if you agree to terms that obligate you -to collect a royalty for further conveying from those to whom you convey -the Program, the only way you could satisfy both those terms and this -License would be to refrain entirely from conveying the Program. - - 13. Use with the GNU Affero General Public License. - - Notwithstanding any other provision of this License, you have -permission to link or combine any covered work with a work licensed -under version 3 of the GNU Affero General Public License into a single -combined work, and to convey the resulting work. The terms of this -License will continue to apply to the part which is the covered work, -but the special requirements of the GNU Affero General Public License, -section 13, concerning interaction through a network will apply to the -combination as such. - - 14. Revised Versions of this License. - - The Free Software Foundation may publish revised and/or new versions of -the GNU General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - - Each version is given a distinguishing version number. If the -Program specifies that a certain numbered version of the GNU General -Public License "or any later version" applies to it, you have the -option of following the terms and conditions either of that numbered -version or of any later version published by the Free Software -Foundation. If the Program does not specify a version number of the -GNU General Public License, you may choose any version ever published -by the Free Software Foundation. - - If the Program specifies that a proxy can decide which future -versions of the GNU General Public License can be used, that proxy's -public statement of acceptance of a version permanently authorizes you -to choose that version for the Program. - - Later license versions may give you additional or different -permissions. However, no additional obligations are imposed on any -author or copyright holder as a result of your choosing to follow a -later version. - - 15. Disclaimer of Warranty. - - THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY -APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT -HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY -OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, -THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM -IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF -ALL NECESSARY SERVICING, REPAIR OR CORRECTION. - - 16. Limitation of Liability. - - IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS -THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY -GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE -USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF -DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD -PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), -EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF -SUCH DAMAGES. - - 17. Interpretation of Sections 15 and 16. - - If the disclaimer of warranty and limitation of liability provided -above cannot be given local legal effect according to their terms, -reviewing courts shall apply local law that most closely approximates -an absolute waiver of all civil liability in connection with the -Program, unless a warranty or assumption of liability accompanies a -copy of the Program in return for a fee. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -state the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - - Copyright (C) - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - -Also add information on how to contact you by electronic and paper mail. - - If the program does terminal interaction, make it output a short -notice like this when it starts in an interactive mode: - - Copyright (C) - This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, your program's commands -might be different; for a GUI interface, you would use an "about box". - - You should also get your employer (if you work as a programmer) or school, -if any, to sign a "copyright disclaimer" for the program, if necessary. -For more information on this, and how to apply and follow the GNU GPL, see -. - - The GNU General Public License does not permit incorporating your program -into proprietary programs. If your program is a subroutine library, you -may consider it more useful to permit linking proprietary applications with -the library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. But first, please read -. \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/dRehmFlight_Teensy_BETA_1.2.ino b/Versions/dRehmFlight_Teensy_BETA_1.2/dRehmFlight_Teensy_BETA_1.2.ino deleted file mode 100644 index d35ecd74..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/dRehmFlight_Teensy_BETA_1.2.ino +++ /dev/null @@ -1,1585 +0,0 @@ -//Arduino/Teensy Flight Controller - dRehmFlight -//Author: Nicholas Rehm -//Project Start: 1/6/2020 -//Version: Beta 1.2 - -/* - * - * If you are using this for an academic or scholarly project, please credit me in any presentations or publications: - * - * Nicholas Rehm - * Department of Aerospace Engineering - * University of Maryland - * College Park 20742 - * Email: nrehm@umd.edu - * - */ - -//========================================================================================================================// - -//CREDITS + SPECIAL THANKS -/* -Some elements inspired by: -http://www.brokking.net/ymfc-32_main.html - -Skeleton code for reading and initializing MPU6050 borrowed from: -https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/ - -Madgwick filter function adapted from: -https://github.com/arduino-libraries/MadgwickAHRS - -MPU9250 implementation based on MPU9250 library by -brian.taylor@bolderflight.com -http://www.bolderflight.com - - -Thank you to: - -RcGroups 'jihlein' - IMU implementation overhaul + SBUS implementation - -*/ - - - -//========================================================================================================================// -// USER-SPECIFIED DEFINES // -//========================================================================================================================// - -//Uncomment only one receiver type -#define USE_PWM_RX -//#define USE_PPM_RX -//#define USE_SBUS_RX - -//Uncomment only one IMU -#define USE_MPU6050_I2C //default -//#define USE_MPU9250_SPI - -//Uncomment only one full scale gyro range (deg/sec) -#define GYRO_250DPS //default -//#define GYRO_500DPS -//#define GYRO_1000DPS -//#define GYRO_2000DPS - -//Uncomment only one full scale accelerometer range (G's) -#define ACCEL_2G //default -//#define ACCEL_4G -//#define ACCEL_8G -//#define ACCEL_16G - - - -//========================================================================================================================// - - - -//REQUIRED LIBRARIES (included with download in main sketch folder) - -#include //I2c communication -#include //SPI communication -#include //commanding any extra actuators, installed with teensyduino installer - -#if defined USE_SBUS_RX - #include "src/SBUS/SBUS.h" //sBus interface -#endif - -#if defined USE_MPU6050_I2C - #include "src/MPU6050/MPU6050.h" - MPU6050 mpu6050; -#elif defined USE_MPU9250_SPI - #include "src/MPU9250/MPU9250.h" - MPU9250 mpu9250(SPI2,36); -#else - #error No MPU defined... -#endif - - - -//========================================================================================================================// - - - -//Setup gyro and accel full scale value selection and scale factor - -#if defined USE_MPU6050_I2C - #define GYRO_FS_SEL_250 MPU6050_GYRO_FS_250 - #define GYRO_FS_SEL_500 MPU6050_GYRO_FS_500 - #define GYRO_FS_SEL_1000 MPU6050_GYRO_FS_1000 - #define GYRO_FS_SEL_2000 MPU6050_GYRO_FS_2000 - #define ACCEL_FS_SEL_2 MPU6050_ACCEL_FS_2 - #define ACCEL_FS_SEL_4 MPU6050_ACCEL_FS_4 - #define ACCEL_FS_SEL_8 MPU6050_ACCEL_FS_8 - #define ACCEL_FS_SEL_16 MPU6050_ACCEL_FS_16 -#elif defined USE_MPU9250_SPI - #define GYRO_FS_SEL_250 mpu9250.GYRO_RANGE_250DPS - #define GYRO_FS_SEL_500 mpu9250.GYRO_RANGE_500DPS - #define GYRO_FS_SEL_1000 mpu9250.GYRO_RANGE_1000DPS - #define GYRO_FS_SEL_2000 mpu9250.GYRO_RANGE_2000DPS - #define ACCEL_FS_SEL_2 mpu9250.ACCEL_RANGE_2G - #define ACCEL_FS_SEL_4 mpu9250.ACCEL_RANGE_4G - #define ACCEL_FS_SEL_8 mpu9250.ACCEL_RANGE_8G - #define ACCEL_FS_SEL_16 mpu9250.ACCEL_RANGE_16G -#endif - -#if defined GYRO_250DPS - #define GYRO_SCALE GYRO_FS_SEL_250 - #define GYRO_SCALE_FACTOR 131.0 -#elif defined GYRO_500DPS - #define GYRO_SCALE GYRO_FS_SEL_500 - #define GYRO_SCALE_FACTOR 65.5 -#elif defined GYRO_1000DPS - #define GYRO_SCALE GYRO_FS_SEL_1000 - #define GYRO_SCALE_FACTOR 32.8 -#elif defined GYRO_2000DPS - #define GYRO_SCALE GYRO_FS_SEL_2000 - #define GYRO_SCALE_FACTOR 16.4 -#endif - -#if defined ACCEL_2G - #define ACCEL_SCALE ACCEL_FS_SEL_2 - #define ACCEL_SCALE_FACTOR 16384.0 -#elif defined ACCEL_4G - #define ACCEL_SCALE ACCEL_FS_SEL_4 - #define ACCEL_SCALE_FACTOR 8192.0 -#elif defined ACCEL_8G - #define ACCEL_SCALE ACCEL_FS_SEL_8 - #define ACCEL_SCALE_FACTOR 4096.0 -#elif defined ACCEL_16G - #define ACCEL_SCALE ACCEL_FS_SEL_16 - #define ACCEL_SCALE_FACTOR 2048.0 -#endif - - - -//========================================================================================================================// -// USER-SPECIFIED VARIABLES // -//========================================================================================================================// - -//Radio failsafe values for every channel in the event that bad reciever data is detected. Recommended defaults: -unsigned long channel_1_fs = 1000; //thro -unsigned long channel_2_fs = 1500; //ail -unsigned long channel_3_fs = 1500; //elev -unsigned long channel_4_fs = 1500; //rudd -unsigned long channel_5_fs = 2000; //gear, greater than 1500 = throttle cut -unsigned long channel_6_fs = 2000; //aux1 - -//Filter parameters - Defaults tuned for 2kHz loop rate; Do not touch unless you know what you are doing: -float B_madgwick = 0.04; //Madgwick filter parameter -float B_accel = 0.14; //Accelerometer LP filter paramter, (MPU6050 default: 0.14. MPU9250 default: 0.2) -float B_gyro = 0.1; //Gyro LP filter paramter, (MPU6050 default: 0.1. MPU9250 default: 0.17) -float B_mag = 1.0; //Magnetometer LP filter parameter - -//Magnetometer calibration parameters - if using MPU9250, uncomment calibrateMagnetometer() in void setup() to get these values, else just ignore these -float MagErrorX = 0.0; -float MagErrorY = 0.0; -float MagErrorZ = 0.0; -float MagScaleX = 1.0; -float MagScaleY = 1.0; -float MagScaleZ = 1.0; - -//Controller parameters (take note of defaults before modifying!): -float i_limit = 25.0; //Integrator saturation level, mostly for safety (default 25.0) -float maxRoll = 30.0; //Max roll angle in degrees for angle mode (maximum 60 degrees), deg/sec for rate mode -float maxPitch = 30.0; //Max pitch angle in degrees for angle mode (maximum 60 degrees), deg/sec for rate mode -float maxYaw = 160.0; //Max yaw rate in deg/sec - -float Kp_roll_angle = 0.2; //Roll P-gain - angle mode -float Ki_roll_angle = 0.3; //Roll I-gain - angle mode -float Kd_roll_angle = 0.05; //Roll D-gain - angle mode (if using controlANGLE2(), set to 0.0) -float B_loop_roll = 0.9; //Roll damping term for controlANGLE2(), lower is more damping (must be between 0 to 1) -float Kp_pitch_angle = 0.2; //Pitch P-gain - angle mode -float Ki_pitch_angle = 0.3; //Pitch I-gain - angle mode -float Kd_pitch_angle = 0.05; //Pitch D-gain - angle mode (if using controlANGLE2(), set to 0.0) -float B_loop_pitch = 0.9; //Pitch damping term for controlANGLE2(), lower is more damping (must be between 0 to 1) - -float Kp_roll_rate = 0.15; //Roll P-gain - rate mode -float Ki_roll_rate = 0.2; //Roll I-gain - rate mode -float Kd_roll_rate = 0.0002; //Roll D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) -float Kp_pitch_rate = 0.15; //Pitch P-gain - rate mode -float Ki_pitch_rate = 0.2; //Pitch I-gain - rate mode -float Kd_pitch_rate = 0.0002; //Pitch D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) - -float Kp_yaw = 0.3; //Yaw P-gain -float Ki_yaw = 0.05; //Yaw I-gain -float Kd_yaw = 0.00015; //Yaw D-gain (be careful when increasing too high, motors will begin to overheat!) - - - -//========================================================================================================================// -// DECLARE PINS // -//========================================================================================================================// - -//NOTE: Pin 13 is reserved for onboard LED, pins 18 and 19 are reserved for the MPU6050 IMU for default setup -//Radio: -//Note: If using SBUS, connect to pin 21 (RX5) -const int ch1Pin = 15; //throttle -const int ch2Pin = 16; //ail -const int ch3Pin = 17; //ele -const int ch4Pin = 20; //rudd -const int ch5Pin = 21; //gear (throttle cut) -const int ch6Pin = 22; //aux1 (free aux channel) -const int PPM_Pin = 23; -//OneShot125 ESC pin outputs: -const int m1Pin = 0; -const int m2Pin = 1; -const int m3Pin = 2; -const int m4Pin = 3; -const int m5Pin = 4; -const int m6Pin = 5; -//PWM servo or ESC outputs: -const int servo1Pin = 6; -const int servo2Pin = 7; -const int servo3Pin = 8; -const int servo4Pin = 9; -const int servo5Pin = 10; -const int servo6Pin = 11; -const int servo7Pin = 12; -PWMServo servo1; //create servo object to control a servo or ESC with PWM -PWMServo servo2; -PWMServo servo3; -PWMServo servo4; -PWMServo servo5; -PWMServo servo6; -PWMServo servo7; - - - -//========================================================================================================================// - - - -//DECLARE GLOBAL VARIABLES - -//General stuff -float dt; -unsigned long current_time, prev_time; -unsigned long print_counter, serial_counter; -unsigned long blink_counter, blink_delay; -bool blinkAlternate; - -//Radio comm: -unsigned long channel_1_pwm, channel_2_pwm, channel_3_pwm, channel_4_pwm, channel_5_pwm, channel_6_pwm; -unsigned long channel_1_pwm_prev, channel_2_pwm_prev, channel_3_pwm_prev, channel_4_pwm_prev; - -#if defined USE_SBUS_RX - SBUS sbus(Serial5); - uint16_t sbusChannels[16]; - bool sbusFailSafe; - bool sbusLostFrame; -#endif - -//IMU: -float AccX, AccY, AccZ; -float AccX_prev, AccY_prev, AccZ_prev; -float GyroX, GyroY, GyroZ; -float GyroX_prev, GyroY_prev, GyroZ_prev; -float MagX, MagY, MagZ; -float MagX_prev, MagY_prev, MagZ_prev; -float roll_IMU, pitch_IMU, yaw_IMU; -float roll_IMU_prev, pitch_IMU_prev; -float AccErrorX, AccErrorY, AccErrorZ, GyroErrorX, GyroErrorY, GyroErrorZ; -float q0 = 1.0f; //initialize quaternion for madgwick filter -float q1 = 0.0f; -float q2 = 0.0f; -float q3 = 0.0f; - -//Normalized desired state: -float thro_des, roll_des, pitch_des, yaw_des; -float roll_passthru, pitch_passthru, yaw_passthru; - -//Controller: -float error_roll, error_roll_prev, roll_des_prev, integral_roll, integral_roll_il, integral_roll_ol, integral_roll_prev, integral_roll_prev_il, integral_roll_prev_ol, derivative_roll, roll_PID = 0; -float error_pitch, error_pitch_prev, pitch_des_prev, integral_pitch, integral_pitch_il, integral_pitch_ol, integral_pitch_prev, integral_pitch_prev_il, integral_pitch_prev_ol, derivative_pitch, pitch_PID = 0; -float error_yaw, error_yaw_prev, integral_yaw, integral_yaw_prev, derivative_yaw, yaw_PID = 0; - -//Mixer -float m1_command_scaled, m2_command_scaled, m3_command_scaled, m4_command_scaled, m5_command_scaled, m6_command_scaled; -int m1_command_PWM, m2_command_PWM, m3_command_PWM, m4_command_PWM, m5_command_PWM, m6_command_PWM; -float s1_command_scaled, s2_command_scaled, s3_command_scaled, s4_command_scaled, s5_command_scaled, s6_command_scaled, s7_command_scaled; -int s1_command_PWM, s2_command_PWM, s3_command_PWM, s4_command_PWM, s5_command_PWM, s6_command_PWM, s7_command_PWM; - - - -//========================================================================================================================// -// VOID SETUP // -//========================================================================================================================// - -void setup() { - Serial.begin(500000); //usb serial - delay(3000); //3 second delay for plugging in battery before IMU calibration begins, feel free to comment this out to reduce boot time - - //Initialize all pins - pinMode(13, OUTPUT); //pin 13 LED blinker on board, do not modify - pinMode(m1Pin, OUTPUT); - pinMode(m2Pin, OUTPUT); - pinMode(m3Pin, OUTPUT); - pinMode(m4Pin, OUTPUT); - pinMode(m5Pin, OUTPUT); - pinMode(m6Pin, OUTPUT); - servo1.attach(servo1Pin, 900, 2100); //pin, min PWM value, max PWM value - servo2.attach(servo2Pin, 900, 2100); - servo3.attach(servo3Pin, 900, 2100); - servo4.attach(servo4Pin, 900, 2100); - servo5.attach(servo5Pin, 900, 2100); - servo6.attach(servo6Pin, 900, 2100); - servo7.attach(servo7Pin, 900, 2100); - - //Set built in LED to turn on to signal startup & not to disturb vehicle during IMU calibration - digitalWrite(13, HIGH); - - delay(10); - - //Initialize radio communication - radioSetup(); - - //Set radio channels to default (safe) values before entering main loop - channel_1_pwm = channel_1_fs; - channel_2_pwm = channel_2_fs; - channel_3_pwm = channel_3_fs; - channel_4_pwm = channel_4_fs; - channel_5_pwm = channel_5_fs; - channel_6_pwm = channel_6_fs; - - //Initialize IMU communication - IMUinit(); - - delay(10); - - //Get IMU error to zero accelerometer and gyro readings, assuming vehicle is level - calculate_IMU_error(); - - delay(10); - - //Arm servo channels - servo1.write(0); //command servo angle from 0-180 degrees (1000 to 2000 PWM) - servo2.write(0); - servo3.write(0); - servo4.write(0); - servo5.write(0); - servo6.write(0); - servo7.write(0); - - delay(10); - - //Arm OneShot125 motors - m1_command_PWM = 125; //command OneShot125 ESC from 125 to 250us pulse length - m2_command_PWM = 125; - m3_command_PWM = 125; - m4_command_PWM = 125; - m5_command_PWM = 125; - m6_command_PWM = 125; - commandMotors(); - - delay(100); - - //Warm up the loop - calibrateAttitude(); //helps to warm up IMU and Madgwick filter before finally entering main loop - - //Indicate entering main loop with 3 quick blinks - setupBlink(3,160,70); //numBlinks, upTime (ms), downTime (ms) - - //If using MPU9250 IMU, uncomment for one-time magnetometer calibration (may need to repeat for new locations) - //calibrateMagnetometer(); //generates magentometer error and scale factors - -} - - - -//========================================================================================================================// -// MAIN LOOP // -//========================================================================================================================// - -void loop() { - prev_time = current_time; - current_time = micros(); - dt = (current_time - prev_time)/1000000.0; - - loopBlink(); //indicate we are in main loop with short blink every 1.5 seconds - - //Print data at 100hz (uncomment one at a time for troubleshooting) - SELECT ONE: - //printRadioData(); //radio pwm values (expected: 1000 to 2000) - //printDesiredState(); //prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle) - //printGyroData(); //prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest) - //printAccelData(); //prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level) - //printMagData(); //prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300) - //printRollPitchYaw(); //prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level) - //printPIDoutput(); //prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1) - //printMotorCommands(); //prints the values being written to the motors (expected: 120 to 250) - //printServoCommands(); //prints the values being written to the servos (expected: 0 to 180) - //printLoopRate(); //prints the time between loops in microseconds (expected: microseconds between loop iterations) - - //Get vehicle state - getIMUdata(); //pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise - Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); //updates roll_IMU, pitch_IMU, and yaw_IMU (degrees) - - //Compute desired state - getDesState(); //convert raw commands to normalized values based on saturated control limits - - //PID Controller - SELECT ONE: - controlANGLE(); //stabilize on angle setpoint - //controlANGLE2(); //stabilize on angle setpoint using cascaded method - //controlRATE(); //stabilize on rate setpoint - - //Actuator mixing and scaling to PWM values - controlMixer(); //mixes PID outputs to scaled actuator commands -- custom mixing assignments done here - scaleCommands(); //scales motor commands to 125 to 250 range (oneshot125 protocol) and servo PWM commands to 0 to 180 (for servo library) - - //Throttle cut check - throttleCut(); //directly sets motor commands to low based on state of ch5 - - //Command actuators - commandMotors(); //sends command pulses to each motor pin using OneShot125 protocol - servo1.write(s1_command_PWM); - servo2.write(s2_command_PWM); - servo3.write(s3_command_PWM); - servo4.write(s4_command_PWM); - servo5.write(s5_command_PWM); - servo6.write(s6_command_PWM); - servo7.write(s7_command_PWM); - - //Get vehicle commands for next loop iteration - getCommands(); //pulls current available radio commands - failSafe(); //prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup - - //Regulate loop rate - loopRate(2000); //do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default -} - - - -//========================================================================================================================// -// FUNCTIONS // -//========================================================================================================================// - -void IMUinit() { - //DESCRIPTION: Initialize IMU - /* - * Don't worry about how this works - */ - #if defined USE_MPU6050_I2C - Wire.begin(); - Wire.setClock(1000000); //Note this is 2.5 times the spec sheet 400 kHz max... - - mpu6050.initialize(); - - if (mpu6050.testConnection() == false) { - Serial.println("MPU6050 initialization unsuccessful"); - Serial.println("Check MPU6050 wiring or try cycling power"); - while(1) {} - } - - //From the reset state all registers should be 0x00, so we should be at - //max sample rate with digital low pass filter(s) off. All we need to - //do is set the desired fullscale ranges - mpu6050.setFullScaleGyroRange(GYRO_SCALE); - mpu6050.setFullScaleAccelRange(ACCEL_SCALE); - - #elif defined USE_MPU9250_SPI - int status = mpu9250.begin(); - - if (status < 0) { - Serial.println("MPU9250 initialization unsuccessful"); - Serial.println("Check MPU9250 wiring or try cycling power"); - Serial.print("Status: "); - Serial.println(status); - while(1) {} - } - - //From the reset state all registers should be 0x00, so we should be at - //max sample rate with digital low pass filter(s) off. All we need to - //do is set the desired fullscale ranges - mpu9250.setGyroRange(GYRO_SCALE); - mpu9250.setAccelRange(ACCEL_SCALE); - mpu9250.setMagCalX(MagErrorX, MagScaleX); - mpu9250.setMagCalY(MagErrorY, MagScaleY); - mpu9250.setMagCalZ(MagErrorZ, MagScaleZ); - mpu9250.setSrd(0); //sets gyro and accel read to 1khz, magnetometer read to 100hz - #endif -} - -void getIMUdata() { - //DESCRIPTION: Request full dataset from IMU and LP filter gyro, accelerometer, and magnetometer data - /* - * Reads accelerometer, gyro, and magnetometer data from IMU as AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, MagZ. - * These values are scaled according to the IMU datasheet to put them into correct units of g's, deg/sec, and uT. A simple first-order - * low-pass filter is used to get rid of high frequency noise in these raw signals. Generally you want to cut - * off everything past 80Hz, but if your loop rate is not fast enough, the low pass filter will cause a lag in - * the readings. The filter parameters B_gyro and B_accel are set to be good for a 2kHz loop rate. Finally, - * the constant errors found in calculate_IMU_error() on startup are subtracted from the accelerometer and gyro readings. - */ - int16_t AcX,AcY,AcZ,GyX,GyY,GyZ,MgX,MgY,MgZ; - - #if defined USE_MPU6050_I2C - mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); - #elif defined USE_MPU9250_SPI - mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); - #endif - - //Accelerometer - AccX = AcX / ACCEL_SCALE_FACTOR; //G's - AccY = AcY / ACCEL_SCALE_FACTOR; - AccZ = AcZ / ACCEL_SCALE_FACTOR; - //Correct the outputs with the calculated error values - AccX = AccX - AccErrorX; - AccY = AccY - AccErrorY; - AccZ = AccZ - AccErrorZ; - //LP filter accelerometer data - AccX = (1.0 - B_accel)*AccX_prev + B_accel*AccX; - AccY = (1.0 - B_accel)*AccY_prev + B_accel*AccY; - AccZ = (1.0 - B_accel)*AccZ_prev + B_accel*AccZ; - AccX_prev = AccX; - AccY_prev = AccY; - AccZ_prev = AccZ; - - //Gyro - GyroX = GyX / GYRO_SCALE_FACTOR; //deg/sec - GyroY = GyY / GYRO_SCALE_FACTOR; - GyroZ = GyZ / GYRO_SCALE_FACTOR; - //Correct the outputs with the calculated error values - GyroX = GyroX - GyroErrorX; - GyroY = GyroY - GyroErrorY; - GyroZ = GyroZ - GyroErrorZ; - //LP filter gyro data - GyroX = (1.0 - B_gyro)*GyroX_prev + B_gyro*GyroX; - GyroY = (1.0 - B_gyro)*GyroY_prev + B_gyro*GyroY; - GyroZ = (1.0 - B_gyro)*GyroZ_prev + B_gyro*GyroZ; - GyroX_prev = GyroX; - GyroY_prev = GyroY; - GyroZ_prev = GyroZ; - - //Magnetometer - MagX = MgX/6.0; //uT - MagY = MgY/6.0; - MagZ = MgZ/6.0; - //Correct the outputs with the calculated error values - MagX = (MagX - MagErrorX)*MagScaleX; - MagY = (MagY - MagErrorY)*MagScaleY; - MagZ = (MagZ - MagErrorZ)*MagScaleZ; - //LP filter magnetometer data - MagX = (1.0 - B_mag)*MagX_prev + B_mag*MagX; - MagY = (1.0 - B_mag)*MagY_prev + B_mag*MagY; - MagZ = (1.0 - B_mag)*MagZ_prev + B_mag*MagZ; - MagX_prev = MagX; - MagY_prev = MagY; - MagZ_prev = MagZ; -} - -void calculate_IMU_error() { - //DESCRIPTION: Computes IMU accelerometer and gyro error on startup. Note: vehicle should be powered up on flat surface - /* - * Don't worry too much about what this is doing. The error values it computes are applied to the raw gyro and - * accelerometer values AccX, AccY, AccZ, GyroX, GyroY, GyroZ in getIMUdata(). This eliminates drift in the - * measurement. - */ - int16_t AcX,AcY,AcZ,GyX,GyY,GyZ,MgX,MgY,MgZ; - - //Read IMU values 12000 times - int c = 0; - while (c < 12000) { - #if defined USE_MPU6050_I2C - mpu6050.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); - #elif defined USE_MPU9250_SPI - mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ); - #endif - - AccX = AcX / ACCEL_SCALE_FACTOR; - AccY = AcY / ACCEL_SCALE_FACTOR; - AccZ = AcZ / ACCEL_SCALE_FACTOR; - GyroX = GyX / GYRO_SCALE_FACTOR; - GyroY = GyY / GYRO_SCALE_FACTOR; - GyroZ = GyZ / GYRO_SCALE_FACTOR; - - //Sum all readings - AccErrorX = AccErrorX + AccX; - AccErrorY = AccErrorY + AccY; - AccErrorZ = AccErrorZ + AccZ; - GyroErrorX = GyroErrorX + GyroX; - GyroErrorY = GyroErrorY + GyroY; - GyroErrorZ = GyroErrorZ + GyroZ; - c++; - } - //Divide the sum by 12000 to get the error value - AccErrorX = AccErrorX / c; - AccErrorY = AccErrorY / c; - AccErrorZ = AccErrorZ / c - 1.0; - GyroErrorX = GyroErrorX / c; - GyroErrorY = GyroErrorY / c; - GyroErrorZ = GyroErrorZ / c; -} - -void calibrateAttitude() { - //DESCRIPTION: Used to warm up the main loop to allow the madwick filter to converge before commands can be sent to the actuators - //Assuming vehicle is powered up on level surface! - /* - * This function is used on startup to warm up the attitude estimation and is what causes startup to take a few seconds - * to boot. - */ - //Warm up IMU and madgwick filter in simulated main loop - for (int i = 0; i <= 10000; i++) { - prev_time = current_time; - current_time = micros(); - dt = (current_time - prev_time)/1000000.0; - getIMUdata(); - Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); - loopRate(2000); //do not exceed 2000Hz - } -} - -void Madgwick(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float invSampleFreq) { - //DESCRIPTION: Attitude estimation through sensor fusion - 9DOF - /* - * This function fuses the accelerometer gyro, and magnetometer readings AccX, AccY, AccZ, GyroX, GyroY, GyroZ, MagX, MagY, and MagZ for attitude estimation. - * Don't worry about the math. There is a tunable parameter B_madgwick in the user specified variable section which basically - * adjusts the weight of gyro data in the state estimate. Higher beta leads to noisier estimate, lower - * beta leads to slower to respond estimate. It is currently tuned for 2kHz loop rate. This function updates the roll_IMU, - * pitch_IMU, and yaw_IMU variables which are in degrees. If magnetometer data is not available, this function calls Madgwick6DOF() instead. - */ - float recipNorm; - float s0, s1, s2, s3; - float qDot1, qDot2, qDot3, qDot4; - float hx, hy; - float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; - float mholder; - - //use 6DOF algorithm if MPU6050 is being used - #if defined USE_MPU6050_I2C - Madgwick6DOF(gx, gy, gz, ax, ay, az, invSampleFreq); - return; - #endif - - //Use 6DOF algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) - if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { - Madgwick6DOF(gx, gy, gz, ax, ay, az, invSampleFreq); - return; - } - - //Convert gyroscope degrees/sec to radians/sec - gx *= 0.0174533f; - gy *= 0.0174533f; - gz *= 0.0174533f; - - //Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); - qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); - qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); - qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); - - //Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - - //Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - //Normalise magnetometer measurement - recipNorm = invSqrt(mx * mx + my * my + mz * mz); - mx *= recipNorm; - my *= recipNorm; - mz *= recipNorm; - - //Auxiliary variables to avoid repeated arithmetic - _2q0mx = 2.0f * q0 * mx; - _2q0my = 2.0f * q0 * my; - _2q0mz = 2.0f * q0 * mz; - _2q1mx = 2.0f * q1 * mx; - _2q0 = 2.0f * q0; - _2q1 = 2.0f * q1; - _2q2 = 2.0f * q2; - _2q3 = 2.0f * q3; - _2q0q2 = 2.0f * q0 * q2; - _2q2q3 = 2.0f * q2 * q3; - q0q0 = q0 * q0; - q0q1 = q0 * q1; - q0q2 = q0 * q2; - q0q3 = q0 * q3; - q1q1 = q1 * q1; - q1q2 = q1 * q2; - q1q3 = q1 * q3; - q2q2 = q2 * q2; - q2q3 = q2 * q3; - q3q3 = q3 * q3; - - //Reference direction of Earth's magnetic field - hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; - hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; - _2bx = sqrtf(hx * hx + hy * hy); - _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; - _4bx = 2.0f * _2bx; - _4bz = 2.0f * _2bz; - - //Gradient decent algorithm corrective step - s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude - s0 *= recipNorm; - s1 *= recipNorm; - s2 *= recipNorm; - s3 *= recipNorm; - - //Apply feedback step - qDot1 -= B_madgwick * s0; - qDot2 -= B_madgwick * s1; - qDot3 -= B_madgwick * s2; - qDot4 -= B_madgwick * s3; - } - - //Integrate rate of change of quaternion to yield quaternion - q0 += qDot1 * invSampleFreq; - q1 += qDot2 * invSampleFreq; - q2 += qDot3 * invSampleFreq; - q3 += qDot4 * invSampleFreq; - - //Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - - //compute angles - NWU - roll_IMU = atan2(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)*57.29577951; //degrees - pitch_IMU = -asin(-2.0f * (q1*q3 - q0*q2))*57.29577951; //degrees - yaw_IMU = -atan2(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3)*57.29577951; //degrees -} - -void Madgwick6DOF(float gx, float gy, float gz, float ax, float ay, float az, float invSampleFreq) { - //DESCRIPTION: Attitude estimation through sensor fusion - 6DOF - /* - * See description of Madgwick() for more information. This is a 6DOF implimentation for when magnetometer data is not - * available (for example when using the recommended MPU6050 IMU for the default setup). - */ - float recipNorm; - float s0, s1, s2, s3; - float qDot1, qDot2, qDot3, qDot4; - float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; - - //Convert gyroscope degrees/sec to radians/sec - gx *= 0.0174533f; - gy *= 0.0174533f; - gz *= 0.0174533f; - - //Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); - qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); - qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); - qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); - - //Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - //Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - //Auxiliary variables to avoid repeated arithmetic - _2q0 = 2.0f * q0; - _2q1 = 2.0f * q1; - _2q2 = 2.0f * q2; - _2q3 = 2.0f * q3; - _4q0 = 4.0f * q0; - _4q1 = 4.0f * q1; - _4q2 = 4.0f * q2; - _8q1 = 8.0f * q1; - _8q2 = 8.0f * q2; - q0q0 = q0 * q0; - q1q1 = q1 * q1; - q2q2 = q2 * q2; - q3q3 = q3 * q3; - - //Gradient decent algorithm corrective step - s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; - s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; - s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; - s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); //normalise step magnitude - s0 *= recipNorm; - s1 *= recipNorm; - s2 *= recipNorm; - s3 *= recipNorm; - - //Apply feedback step - qDot1 -= B_madgwick * s0; - qDot2 -= B_madgwick * s1; - qDot3 -= B_madgwick * s2; - qDot4 -= B_madgwick * s3; - } - - //Integrate rate of change of quaternion to yield quaternion - q0 += qDot1 * invSampleFreq; - q1 += qDot2 * invSampleFreq; - q2 += qDot3 * invSampleFreq; - q3 += qDot4 * invSampleFreq; - - //Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - - //compute angles - roll_IMU = atan2(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)*57.29577951; //degrees - pitch_IMU = -asin(-2.0f * (q1*q3 - q0*q2))*57.29577951; //degrees - yaw_IMU = -atan2(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3)*57.29577951; //degrees -} - -void getDesState() { - //DESCRIPTION: Normalizes desired control values to appropriate values - /* - * Updates the desired state variables thro_des, roll_des, pitch_des, and yaw_des. These are computed by using the raw - * RC pwm commands and scaling them to be within our limits defined in setup. thro_des stays within 0 to 1 range. - * roll_des and pitch_des are scaled to be within max roll/pitch amount in either degrees (angle mode) or degrees/sec - * (rate mode). yaw_des is scaled to be within max yaw in degrees/sec. Also creates roll_passthru, pitch_passthru, and - * yaw_passthru variables, to be used in commanding motors/servos with direct unstabilized commands in controlMixer(). - */ - thro_des = (channel_1_pwm - 1000.0)/1000.0; //between 0 and 1 - roll_des = (channel_2_pwm - 1500.0)/500.0; //between -1 and 1 - pitch_des = (channel_3_pwm - 1500.0)/500.0; //between -1 and 1 - yaw_des = (channel_4_pwm - 1500.0)/500.0; //between -1 and 1 - //Constrain within normalized bounds - thro_des = constrain(thro_des, 0.0, 1.0); //between 0 and 1 - roll_des = constrain(roll_des, -1.0, 1.0)*maxRoll; //between -maxRoll and +maxRoll - pitch_des = constrain(pitch_des, -1.0, 1.0)*maxPitch; //between -maxPitch and +maxPitch - yaw_des = constrain(yaw_des, -1.0, 1.0)*maxYaw; //between -maxYaw and +maxYaw - - roll_passthru = roll_des/(2*maxRoll); - pitch_passthru = pitch_des/(2*maxPitch); - yaw_passthru = yaw_des/(2*maxYaw); -} - -void controlANGLE() { - //DESCRIPTION: Computes control commands based on state error (angle) - /* - * Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des computed in - * getDesState(). Error is simply the desired state minus the actual state (ex. roll_des - roll_IMU). Two safety features - * are implimented here regarding the I terms. The I terms are saturated within specified limits on startup to prevent - * excessive buildup. This can be seen by holding the vehicle at an angle and seeing the motors ramp up on one side until - * they've maxed out throttle...saturating I to a specified limit fixes this. The second feature defaults the I terms to 0 - * if the throttle is at the minimum setting. This means the motors will not start spooling up on the ground, and the I - * terms will always start from 0 on takeoff. This function updates the variables roll_PID, pitch_PID, and yaw_PID which - * can be thought of as 1-D stablized signals. They are mixed to the configuration of the vehicle in controlMixer(). - */ - - //Roll - error_roll = roll_des - roll_IMU; - integral_roll = integral_roll_prev + error_roll*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_roll = 0; - } - integral_roll = constrain(integral_roll, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_roll = GyroX; - roll_PID = 0.01*(Kp_roll_angle*error_roll + Ki_roll_angle*integral_roll - Kd_roll_angle*derivative_roll); //scaled by .01 to bring within -1 to 1 range - - //Pitch - error_pitch = pitch_des - pitch_IMU; - integral_pitch = integral_pitch_prev + error_pitch*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_pitch = 0; - } - integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_pitch = GyroY; - pitch_PID = .01*(Kp_pitch_angle*error_pitch + Ki_pitch_angle*integral_pitch - Kd_pitch_angle*derivative_pitch); //scaled by .01 to bring within -1 to 1 range - - //Yaw, stablize on rate from GyroZ - error_yaw = yaw_des - GyroZ; - integral_yaw = integral_yaw_prev + error_yaw*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_yaw = 0; - } - integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_yaw = (error_yaw - error_yaw_prev)/dt; - yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //scaled by .01 to bring within -1 to 1 range - - //Update roll variables - integral_roll_prev = integral_roll; - //Update pitch variables - integral_pitch_prev = integral_pitch; - //Update yaw variables - error_yaw_prev = error_yaw; - integral_yaw_prev = integral_yaw; -} - -void controlANGLE2() { - //DESCRIPTION: Computes control commands based on state error (angle) in cascaded scheme - /* - * Gives better performance than controlANGLE() but requires much more tuning. Not reccommended for first-time setup. - * See the documentation for tuning this controller. - */ - //Outer loop - PID on angle - float roll_des_ol, pitch_des_ol; - //Roll - error_roll = roll_des - roll_IMU; - integral_roll_ol = integral_roll_prev_ol + error_roll*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_roll_ol = 0; - } - integral_roll_ol = constrain(integral_roll_ol, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_roll = (roll_IMU - roll_IMU_prev)/dt; - roll_des_ol = Kp_roll_angle*error_roll + Ki_roll_angle*integral_roll_ol - Kd_roll_angle*derivative_roll; - - //Pitch - error_pitch = pitch_des - pitch_IMU; - integral_pitch_ol = integral_pitch_prev_ol + error_pitch*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_pitch_ol = 0; - } - integral_pitch_ol = constrain(integral_pitch_ol, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_pitch = (pitch_IMU - pitch_IMU_prev)/dt; - pitch_des_ol = Kp_pitch_angle*error_pitch + Ki_pitch_angle*integral_pitch_ol - Kd_pitch_angle*derivative_pitch; - - //Apply loop gain, constrain, and LP filter for artificial damping - float Kl = 30.0; - roll_des_ol = Kl*roll_des_ol; - pitch_des_ol = Kl*pitch_des_ol; - roll_des_ol = constrain(roll_des_ol, -240.0, 240.0); - pitch_des_ol = constrain(pitch_des_ol, -240.0, 240.0); - roll_des_ol = (1.0 - B_loop_roll)*roll_des_prev + B_loop_roll*roll_des_ol; - pitch_des_ol = (1.0 - B_loop_pitch)*pitch_des_prev + B_loop_pitch*pitch_des_ol; - - //Inner loop - PID on rate - //Roll - error_roll = roll_des_ol - GyroX; - integral_roll_il = integral_roll_prev_il + error_roll*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_roll_il = 0; - } - integral_roll_il = constrain(integral_roll_il, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_roll = (error_roll - error_roll_prev)/dt; - roll_PID = .01*(Kp_roll_rate*error_roll + Ki_roll_rate*integral_roll_il + Kd_roll_rate*derivative_roll); //scaled by .01 to bring within -1 to 1 range - - //Pitch - error_pitch = pitch_des_ol - GyroY; - integral_pitch_il = integral_pitch_prev_il + error_pitch*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_pitch_il = 0; - } - integral_pitch_il = constrain(integral_pitch_il, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_pitch = (error_pitch - error_pitch_prev)/dt; - pitch_PID = .01*(Kp_pitch_rate*error_pitch + Ki_pitch_rate*integral_pitch_il + Kd_pitch_rate*derivative_pitch); //scaled by .01 to bring within -1 to 1 range - - //Yaw - error_yaw = yaw_des - GyroZ; - integral_yaw = integral_yaw_prev + error_yaw*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_yaw = 0; - } - integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_yaw = (error_yaw - error_yaw_prev)/dt; - yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //scaled by .01 to bring within -1 to 1 range - - //Update roll variables - integral_roll_prev_ol = integral_roll_ol; - integral_roll_prev_il = integral_roll_il; - error_roll_prev = error_roll; - roll_IMU_prev = roll_IMU; - roll_des_prev = roll_des_ol; - //Update pitch variables - integral_pitch_prev_ol = integral_pitch_ol; - integral_pitch_prev_il = integral_pitch_il; - error_pitch_prev = error_pitch; - pitch_IMU_prev = pitch_IMU; - pitch_des_prev = pitch_des_ol; - //Update yaw variables - error_yaw_prev = error_yaw; - integral_yaw_prev = integral_yaw; - -} - -void controlRATE() { - //DESCRIPTION: Computes control commands based on state error (rate) - /* - * See explanation for controlANGLE(). Everything is the same here except the error is now the desired rate - raw gyro reading. - */ - //Roll - error_roll = roll_des - GyroX; - integral_roll = integral_roll_prev + error_roll*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_roll = 0; - } - integral_roll = constrain(integral_roll, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_roll = (error_roll - error_roll_prev)/dt; - roll_PID = .01*(Kp_roll_rate*error_roll + Ki_roll_rate*integral_roll + Kd_roll_rate*derivative_roll); //scaled by .01 to bring within -1 to 1 range - - //Pitch - error_pitch = pitch_des - GyroY; - integral_pitch = integral_pitch_prev + error_pitch*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_pitch = 0; - } - integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_pitch = (error_pitch - error_pitch_prev)/dt; - pitch_PID = .01*(Kp_pitch_rate*error_pitch + Ki_pitch_rate*integral_pitch + Kd_pitch_rate*derivative_pitch); //scaled by .01 to bring within -1 to 1 range - - //Yaw, stablize on rate from GyroZ - error_yaw = yaw_des - GyroZ; - integral_yaw = integral_yaw_prev + error_yaw*dt; - if (channel_1_pwm < 1060) { //don't let integrator build if throttle is too low - integral_yaw = 0; - } - integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //saturate integrator to prevent unsafe buildup - derivative_yaw = (error_yaw - error_yaw_prev)/dt; - yaw_PID = .01*(Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //scaled by .01 to bring within -1 to 1 range - - //Update roll variables - error_roll_prev = error_roll; - integral_roll_prev = integral_roll; - GyroX_prev = GyroX; - //Update pitch variables - error_pitch_prev = error_pitch; - integral_pitch_prev = integral_pitch; - GyroY_prev = GyroY; - //Update yaw variables - error_yaw_prev = error_yaw; - integral_yaw_prev = integral_yaw; -} - -void controlMixer() { - //DESCRIPTION: Mixes scaled commands from PID controller to actuator outputs based on vehicle configuration - /* - * Takes roll_PID, pitch_PID, and yaw_PID computed from the PID controller and appropriately mixes them for the desired - * vehicle configuration. For example on a quadcopter, the left two motors should have +roll_PID while the right two motors - * should have -roll_PID. Front two should have -pitch_PID and the back two should have +pitch_PID etc... every motor has - * normalized (0 to 1) thro_des command for throttle control. Can also apply direct unstabilized commands from the transmitter with - * roll_passthru, pitch_passthru, and yaw_passthu. mX_command_scaled and sX_command scaled variables are used in scaleCommands() - * in preparation to be sent to the motor ESCs and servos. - */ - //Quad mixing - //m1 = front left, m2 = front right, m3 = back right, m4 = back left - m1_command_scaled = thro_des - pitch_PID + roll_PID + yaw_PID; - m2_command_scaled = thro_des - pitch_PID - roll_PID - yaw_PID; - m3_command_scaled = thro_des + pitch_PID - roll_PID + yaw_PID; - m4_command_scaled = thro_des + pitch_PID + roll_PID - yaw_PID; - m5_command_scaled = 0; - m6_command_scaled = 0; - - //0.5 is centered servo, 0 is zero throttle if connecting to ESC for conventional PWM, 1 is max throttle - s1_command_scaled = 0; - s2_command_scaled = 0; - s3_command_scaled = 0; - s4_command_scaled = 0; - s5_command_scaled = 0; - s6_command_scaled = 0; - s7_command_scaled = 0; - - //Example use of the linear fader for float type variables. Linearly interpolate between minimum and maximum values for Kp_pitch_rate variable based on state of channel 6: - /* - if (channel_6_pwm > 1500){ //go to max specified value in 5.5 seconds - Kp_pitch_rate = floatFaderLinear(Kp_pitch_rate, 0.1, 0.3, 5.5, 1, 2000); //parameter, minimum value, maximum value, fadeTime (seconds), state (0 min or 1 max), loop frequency - } - if (channel_6_pwm < 1500) { //go to min specified value in 2.5 seconds - Kp_pitch_rate = floatFaderLinear(Kp_pitch_rate, 0.1, 0.3, 2.5, 0, 2000); //parameter, minimum value, maximum value, fadeTime, state (0 min or 1 max), loop frequency - } - */ -} - -void scaleCommands() { - //DESCRIPTION: Scale normalized actuator commands to values for ESC/Servo protocol - /* - * mX_command_scaled variables from the mixer function are scaled to 125-250us for OneShot125 protocol. sX_command_scaled variables from - * the mixer function are scaled to 0-180 for the servo library using standard PWM. - * mX_command_PWM are updated here which are used to command the motors in commandMotors(). sX_command_PWM are updated - * which are used to command the servos. - */ - //Scaled to 125us - 250us for oneshot125 protocol - m1_command_PWM = m1_command_scaled*125 + 125; - m2_command_PWM = m2_command_scaled*125 + 125; - m3_command_PWM = m3_command_scaled*125 + 125; - m4_command_PWM = m4_command_scaled*125 + 125; - m5_command_PWM = m5_command_scaled*125 + 125; - m6_command_PWM = m6_command_scaled*125 + 125; - //Constrain commands to motors within oneshot125 bounds - m1_command_PWM = constrain(m1_command_PWM, 125, 250); - m2_command_PWM = constrain(m2_command_PWM, 125, 250); - m3_command_PWM = constrain(m3_command_PWM, 125, 250); - m4_command_PWM = constrain(m4_command_PWM, 125, 250); - m5_command_PWM = constrain(m5_command_PWM, 125, 250); - m6_command_PWM = constrain(m6_command_PWM, 125, 250); - - //Scaled to 0-180 for servo library - s1_command_PWM = s1_command_scaled*180; - s2_command_PWM = s2_command_scaled*180; - s3_command_PWM = s3_command_scaled*180; - s4_command_PWM = s4_command_scaled*180; - s5_command_PWM = s5_command_scaled*180; - s6_command_PWM = s6_command_scaled*180; - s7_command_PWM = s7_command_scaled*180; - //Constrain commands to servos within servo library bounds - s1_command_PWM = constrain(s1_command_PWM, 0, 180); - s2_command_PWM = constrain(s2_command_PWM, 0, 180); - s3_command_PWM = constrain(s3_command_PWM, 0, 180); - s4_command_PWM = constrain(s4_command_PWM, 0, 180); - s5_command_PWM = constrain(s5_command_PWM, 0, 180); - s6_command_PWM = constrain(s6_command_PWM, 0, 180); - s7_command_PWM = constrain(s7_command_PWM, 0, 180); - -} - -void getCommands() { - //DESCRIPTION: Get raw PWM values for every channel from the radio - /* - * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of - * the loop. If using a PWM or PPM receiver, the radio commands are retrieved from a function in the readPWM file separate from this one which - * is running a bunch of interrupts to continuously update the radio readings. If using an SBUS receiver, the alues are pulled from the SBUS library directly. - * The raw radio commands are filtered with a first order low-pass filter to eliminate any really high frequency noise. - */ - - #if defined USE_PPM_RX || defined USE_PWM_RX - channel_1_pwm = getRadioPWM(1); - channel_2_pwm = getRadioPWM(2); - channel_3_pwm = getRadioPWM(3); - channel_4_pwm = getRadioPWM(4); - channel_5_pwm = getRadioPWM(5); - channel_6_pwm = getRadioPWM(6); - - #elif defined USE_SBUS_RX - if (sbus.read(&sbusChannels[0], &sbusFailSafe, &sbusLostFrame)) - { - //sBus scaling below is for Taranis-Plus and X4R-SB - float scale = 0.615; - float bias = 895.0; - channel_1_pwm = sbusChannels[0] * scale + bias; - channel_2_pwm = sbusChannels[1] * scale + bias; - channel_3_pwm = sbusChannels[2] * scale + bias; - channel_4_pwm = sbusChannels[3] * scale + bias; - channel_5_pwm = sbusChannels[4] * scale + bias; - channel_6_pwm = sbusChannels[5] * scale + bias; - } - #endif - - //Low-pass the critical commands and update previous values - float b = 0.2; //lower=slower, higher=noiser - channel_1_pwm = (1.0 - b)*channel_1_pwm_prev + b*channel_1_pwm; - channel_2_pwm = (1.0 - b)*channel_2_pwm_prev + b*channel_2_pwm; - channel_3_pwm = (1.0 - b)*channel_3_pwm_prev + b*channel_3_pwm; - channel_4_pwm = (1.0 - b)*channel_4_pwm_prev + b*channel_4_pwm; - channel_1_pwm_prev = channel_1_pwm; - channel_2_pwm_prev = channel_2_pwm; - channel_3_pwm_prev = channel_3_pwm; - channel_4_pwm_prev = channel_4_pwm; -} - -void failSafe() { - //DESCRIPTION: If radio gives garbage values, set all commands to default values - /* - * Radio connection failsafe used to check if the getCommands() function is returning acceptable pwm values. If any of - * the commands are lower than 800 or higher than 2200, then we can be certain that there is an issue with the radio - * connection (most likely hardware related). If any of the channels show this failure, then all of the radio commands - * channel_x_pwm are set to default failsafe values specified in the setup. Comment out this function when troubleshooting - * your radio connection in case any extreme values are triggering this function to overwrite the printed variables. - */ - unsigned minVal = 800; - unsigned maxVal = 2200; - int check1 = 0; - int check2 = 0; - int check3 = 0; - int check4 = 0; - int check5 = 0; - int check6 = 0; - - //Triggers for failure criteria - if (channel_1_pwm > maxVal || channel_1_pwm < minVal) check1 = 1; - if (channel_2_pwm > maxVal || channel_2_pwm < minVal) check2 = 1; - if (channel_3_pwm > maxVal || channel_3_pwm < minVal) check3 = 1; - if (channel_4_pwm > maxVal || channel_4_pwm < minVal) check4 = 1; - if (channel_5_pwm > maxVal || channel_5_pwm < minVal) check5 = 1; - if (channel_6_pwm > maxVal || channel_6_pwm < minVal) check6 = 1; - - //If any failures, set to default failsafe values - if ((check1 + check2 + check3 + check4 + check5 + check6) > 0) { - channel_1_pwm = channel_1_fs; - channel_2_pwm = channel_2_fs; - channel_3_pwm = channel_3_fs; - channel_4_pwm = channel_4_fs; - channel_5_pwm = channel_5_fs; - channel_6_pwm = channel_6_fs; - } -} - -void commandMotors() { - //DESCRIPTION: Send pulses to motor pins, oneshot125 protocol - /* - * My crude implimentation of OneShot125 protocol which sends 125 - 250us pulses to the ESCs (mXPin). The pulselengths being - * sent are mX_command_PWM, computed in scaleCommands(). This may be replaced by something more efficient in the future. - */ - int wentLow = 0; - int pulseStart, timer; - int flagM1 = 0; - int flagM2 = 0; - int flagM3 = 0; - int flagM4 = 0; - int flagM5 = 0; - int flagM6 = 0; - - //Write all motor pins high - digitalWrite(m1Pin, HIGH); - digitalWrite(m2Pin, HIGH); - digitalWrite(m3Pin, HIGH); - digitalWrite(m4Pin, HIGH); - digitalWrite(m5Pin, HIGH); - digitalWrite(m6Pin, HIGH); - pulseStart = micros(); - - //Write each motor pin low as correct pulse length is reached - while (wentLow < 6 ) { //keep going until final (6th) pulse is finished, then done - timer = micros(); - if ((m1_command_PWM <= timer - pulseStart) && (flagM1==0)) { - digitalWrite(m1Pin, LOW); - wentLow = wentLow + 1; - flagM1 = 1; - } - if ((m2_command_PWM <= timer - pulseStart) && (flagM2==0)) { - digitalWrite(m2Pin, LOW); - wentLow = wentLow + 1; - flagM2 = 1; - } - if ((m3_command_PWM <= timer - pulseStart) && (flagM3==0)) { - digitalWrite(m3Pin, LOW); - wentLow = wentLow + 1; - flagM3 = 1; - } - if ((m4_command_PWM <= timer - pulseStart) && (flagM4==0)) { - digitalWrite(m4Pin, LOW); - wentLow = wentLow + 1; - flagM4 = 1; - } - if ((m5_command_PWM <= timer - pulseStart) && (flagM5==0)) { - digitalWrite(m5Pin, LOW); - wentLow = wentLow + 1; - flagM5 = 1; - } - if ((m6_command_PWM <= timer - pulseStart) && (flagM6==0)) { - digitalWrite(m6Pin, LOW); - wentLow = wentLow + 1; - flagM6 = 1; - } - } -} - -float floatFaderLinear(float param, float param_min, float param_max, float fadeTime, int state, int loopFreq){ - //DESCRIPTION: Linearly fades a float type variable between min and max bounds based on desired high or low state and time - /* - * Takes in a float variable, desired minimum and maximum bounds, fade time, high or low desired state, and the loop frequency - * and linearly interpolates that param variable between the maximum and minimum bounds. This function can be called in controlMixer() - * and high/low states can be determined by monitoring the state of an auxillarly radio channel. For example, if channel_6_pwm is being - * monitored to switch between two dynamic configurations (hover and forward flight), this function can be called within the logical - * statements in order to fade controller gains, for example between the two dynamic configurations. The 'state' (1 or 0) can be used - * to designate the two final options for that control gain based on the dynamic configuration assignment to the auxillary radio channel. - * - */ - float diffParam = (param_max - param_min)/(fadeTime*loopFreq); //difference to add or subtract from param for each loop iteration for desired fadeTime - - if (state == 1) { //maximum param bound desired, increase param by diffParam for each loop iteration - param = param + diffParam; - } - else if (state == 0) { //minimum param bound desired, decrease param by diffParam for each loop iteration - param = param - diffParam; - } - - param = constrain(param, param_min, param_max); //constrain param within max bounds - - return param; -} - -float switchRollYaw(int reverseRoll, int reverseYaw) { - //DESCRIPTION: Switches roll_des and yaw_des variables for tailsitter-type configurations - /* - * Takes in two integers (either 1 or -1) corresponding to the desired reversing of the roll axis and yaw axis, respectively. - * Reversing of the roll or yaw axis may be needed when switching between the two for some dynamic configurations. Inputs of 1, 1 does not - * reverse either of them, while -1, 1 will reverse the output corresponding to the new roll axis. - * This function may be replaced in the future by a function that switches the IMU data instead (so that angle can also be estimated with the - * IMU tilted 90 degrees from default level). - */ - float switch_holder; - - switch_holder = yaw_des; - yaw_des = reverseYaw*roll_des; - roll_des = reverseRoll*switch_holder; -} - - -void throttleCut() { - //DESCRIPTION: Directly set actuator outputs to minimum value if triggered - /* - * Monitors the state of radio command channel_5_pwm and directly sets the mx_command_PWM values to minimum (120 is - * minimum for oneshot125 protocol, 0 is minimum for standard PWM servo library used) if channel 5 is high. This is the last function - * called before commandMotors() is called so that the last thing checked is if the user is giving permission to command - * the motors to anything other than minimum value. Safety first. - */ - if (channel_5_pwm > 1500) { - m1_command_PWM = 120; - m2_command_PWM = 120; - m3_command_PWM = 120; - m4_command_PWM = 120; - m5_command_PWM = 120; - m6_command_PWM = 120; - - //uncomment if using servo PWM variables to control motor ESCs - //s1_command_PWM = 0; - //s2_command_PWM = 0; - //s3_command_PWM = 0; - //s4_command_PWM = 0; - //s5_command_PWM = 0; - //s6_command_PWM = 0; - //s7_command_PWM = 0; - } -} - -void calibrateMagnetometer() { - #if defined USE_MPU9250_SPI - float success; - Serial.println("Beginning magnetometer calibration in"); - Serial.println("3..."); - delay(1000); - Serial.println("2..."); - delay(1000); - Serial.println("1..."); - delay(1000); - Serial.println("Rotate the IMU about all axes until complete."); - Serial.println(" "); - success = mpu9250.calibrateMag(); - if(success) { - Serial.println("Calibration Successful!"); - Serial.println("Please comment out the calibrateMagnetometer() function and copy these values into the code:"); - Serial.print("float MagErrorX = "); - Serial.print(mpu9250.getMagBiasX_uT()); - Serial.println(";"); - Serial.print("float MagErrorY = "); - Serial.print(mpu9250.getMagBiasY_uT()); - Serial.println(";"); - Serial.print("float MagErrorZ = "); - Serial.print(mpu9250.getMagBiasZ_uT()); - Serial.println(";"); - Serial.print("float MagScaleX = "); - Serial.print(mpu9250.getMagScaleFactorX()); - Serial.println(";"); - Serial.print("float MagScaleY = "); - Serial.print(mpu9250.getMagScaleFactorY()); - Serial.println(";"); - Serial.print("float MagScaleZ = "); - Serial.print(mpu9250.getMagScaleFactorZ()); - Serial.println(";"); - Serial.println(" "); - Serial.println("If you are having trouble with your attitude estimate at a new flying location, repeat this process as needed."); - } - else { - Serial.println("Calibration Unsuccessful. Please reset the board and try again."); - } - - while(1); //halt code so it won't enter main loop until this function commented out - #endif - Serial.println("Error: MPU9250 not selected. Cannot calibrate non-existent magnetometer."); - while(1); //halt code so it won't enter main loop until this function commented out -} - -void loopRate(int freq) { - //DESCRIPTION: Regulate main loop rate to specified frequency in Hz - /* - * It's good to operate at a constant loop rate for filters to remain stable and whatnot. Interrupt routines running in the - * background cause the loop rate to fluctuate. This function basically just waits at the end of every loop iteration until - * the correct time has passed since the start of the current loop for the desired loop rate in Hz. 2kHz is a good rate to - * be at because the loop nominally will run between 2.8kHz - 4.2kHz. This lets us have a little room to add extra computations - * and remain above 2kHz, without needing to retune all of our filtering parameters. - */ - float invFreq = 1.0/freq*1000000.0; - unsigned long checker = micros(); - - //Sit in loop until appropriate time has passed - while (invFreq > (checker - current_time)) { - checker = micros(); - } -} - -void loopBlink() { - //DESCRIPTION: Blink LED on board to indicate main loop is running - /* - * It looks cool. - */ - if (current_time - blink_counter > blink_delay) { - blink_counter = micros(); - digitalWrite(13, blinkAlternate); //pin 13 is built in LED - - if (blinkAlternate == 1) { - blinkAlternate = 0; - blink_delay = 100000; - } - else if (blinkAlternate == 0) { - blinkAlternate = 1; - blink_delay = 2000000; - } - } -} - -void setupBlink(int numBlinks,int upTime, int downTime) { - //DESCRIPTION: Simple function to make LED on board blink as desired - for (int j = 1; j<= numBlinks; j++) { - digitalWrite(13, LOW); - delay(downTime); - digitalWrite(13, HIGH); - delay(upTime); - } -} - -void printRadioData() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F(" CH1: ")); - Serial.print(channel_1_pwm); - Serial.print(F(" CH2: ")); - Serial.print(channel_2_pwm); - Serial.print(F(" CH3: ")); - Serial.print(channel_3_pwm); - Serial.print(F(" CH4: ")); - Serial.print(channel_4_pwm); - Serial.print(F(" CH5: ")); - Serial.print(channel_5_pwm); - Serial.print(F(" CH6: ")); - Serial.println(channel_6_pwm); - } -} - -void printDesiredState() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("thro_des: ")); - Serial.print(thro_des); - Serial.print(F(" roll_des: ")); - Serial.print(roll_des); - Serial.print(F(" pitch_des: ")); - Serial.print(pitch_des); - Serial.print(F(" yaw_des: ")); - Serial.println(yaw_des); - } -} - -void printGyroData() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("GyroX: ")); - Serial.print(GyroX); - Serial.print(F(" GyroY: ")); - Serial.print(GyroY); - Serial.print(F(" GyroZ: ")); - Serial.println(GyroZ); - } -} - -void printAccelData() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("AccX: ")); - Serial.print(AccX); - Serial.print(F(" AccY: ")); - Serial.print(AccY); - Serial.print(F(" AccZ: ")); - Serial.println(AccZ); - } -} - -void printMagData() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("MagX: ")); - Serial.print(MagX); - Serial.print(F(" MagY: ")); - Serial.print(MagY); - Serial.print(F(" MagZ: ")); - Serial.println(MagZ); - } -} - -void printRollPitchYaw() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("roll: ")); - Serial.print(roll_IMU); - Serial.print(F(" pitch: ")); - Serial.print(pitch_IMU); - Serial.print(F(" yaw: ")); - Serial.println(yaw_IMU); - } -} - -void printPIDoutput() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("roll_PID: ")); - Serial.print(roll_PID); - Serial.print(F(" pitch_PID: ")); - Serial.print(pitch_PID); - Serial.print(F(" yaw_PID: ")); - Serial.println(yaw_PID); - } -} - -void printMotorCommands() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("m1_command: ")); - Serial.print(m1_command_PWM); - Serial.print(F(" m2_command: ")); - Serial.print(m2_command_PWM); - Serial.print(F(" m3_command: ")); - Serial.print(m3_command_PWM); - Serial.print(F(" m4_command: ")); - Serial.print(m4_command_PWM); - Serial.print(F(" m5_command: ")); - Serial.print(m5_command_PWM); - Serial.print(F(" m6_command: ")); - Serial.println(m6_command_PWM); - } -} - -void printServoCommands() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("s1_command: ")); - Serial.print(s1_command_PWM); - Serial.print(F(" s2_command: ")); - Serial.print(s2_command_PWM); - Serial.print(F(" s3_command: ")); - Serial.print(s3_command_PWM); - Serial.print(F(" s4_command: ")); - Serial.print(s4_command_PWM); - Serial.print(F(" s5_command: ")); - Serial.print(s5_command_PWM); - Serial.print(F(" s6_command: ")); - Serial.print(s6_command_PWM); - Serial.print(F(" s7_command: ")); - Serial.println(s7_command_PWM); - } -} - -void printLoopRate() { - if (current_time - print_counter > 10000) { - print_counter = micros(); - Serial.print(F("dt = ")); - Serial.println(dt*1000000.0); - } -} - -//=========================================================================================// - -//HELPER FUNCTIONS - -float invSqrt(float x) { - //Fast inverse sqrt for madgwick filter - /* - float halfx = 0.5f * x; - float y = x; - long i = *(long*)&y; - i = 0x5f3759df - (i>>1); - y = *(float*)&i; - y = y * (1.5f - (halfx * y * y)); - y = y * (1.5f - (halfx * y * y)); - return y; - */ - //alternate form: - unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1); - float tmp = *(float*)&i; - float y = tmp * (1.69000231f - 0.714158168f * x * tmp * tmp); - return y; -} diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/radioComm.ino b/Versions/dRehmFlight_Teensy_BETA_1.2/radioComm.ino deleted file mode 100644 index 30b28bd9..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/radioComm.ino +++ /dev/null @@ -1,184 +0,0 @@ -//Arduino/Teensy Flight Controller - dRehmFlight -//Author: Nicholas Rehm -//Project Start: 1/6/2020 -//Version: Beta 1.2 - -//========================================================================================================================// - -//This file contains all necessary functions and code used for radio communication to avoid cluttering the main code - -unsigned long rising_edge_start_1, rising_edge_start_2, rising_edge_start_3, rising_edge_start_4, rising_edge_start_5, rising_edge_start_6; -unsigned long channel_1_raw, channel_2_raw, channel_3_raw, channel_4_raw, channel_5_raw, channel_6_raw; -int ppm_counter = 0; -unsigned long time_ms = 0; - -void radioSetup() { - //PPM Receiver - #if defined USE_PPM_RX - //Declare interrupt pin - pinMode(PPM_Pin, INPUT_PULLUP); - delay(20); - //Attach interrupt and point to corresponding ISR function - attachInterrupt(digitalPinToInterrupt(PPM_Pin), getPPM, CHANGE); - - //PWM Receiver - #elif defined USE_PWM_RX - //Declare interrupt pins - pinMode(ch1Pin, INPUT_PULLUP); - pinMode(ch2Pin, INPUT_PULLUP); - pinMode(ch3Pin, INPUT_PULLUP); - pinMode(ch4Pin, INPUT_PULLUP); - pinMode(ch5Pin, INPUT_PULLUP); - pinMode(ch6Pin, INPUT_PULLUP); - delay(20); - //Attach interrupt and point to corresponding ISR functions - attachInterrupt(digitalPinToInterrupt(ch1Pin), getCh1, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch2Pin), getCh2, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch3Pin), getCh3, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch4Pin), getCh4, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch5Pin), getCh5, CHANGE); - attachInterrupt(digitalPinToInterrupt(ch6Pin), getCh6, CHANGE); - delay(20); - - //SBUS Recevier - #elif defined USE_SBUS_RX - sbus.begin(); - - #else - #error No RX type defined... - #endif -} - -unsigned long getRadioPWM(int ch_num) { - //DESCRIPTION: Get current radio commands from interrupt routines - unsigned long returnPWM; - - if (ch_num == 1) { - returnPWM = channel_1_raw; - } - else if (ch_num == 2) { - returnPWM = channel_2_raw; - } - else if (ch_num == 3) { - returnPWM = channel_3_raw; - } - else if (ch_num == 4) { - returnPWM = channel_4_raw; - } - else if (ch_num == 5) { - returnPWM = channel_5_raw; - } - else if (ch_num == 6) { - returnPWM = channel_6_raw; - } - - return returnPWM; -} - - - -//========================================================================================================================// - - - -//INTERRUPT SERVICE ROUTINES (for reading PWM and PPM) - -void getPPM() { - unsigned long dt_ppm; - int trig = digitalRead(PPM_Pin); - if (trig==1) { //only care about rising edge - dt_ppm = micros() - time_ms; - time_ms = micros(); - - - if (dt_ppm > 5000) { //waiting for long pulse to indicate a new pulse train has arrived - ppm_counter = 0; - } - - if (ppm_counter == 1) { //first pulse - channel_1_raw = dt_ppm; - } - - if (ppm_counter == 2) { //second pulse - channel_2_raw = dt_ppm; - } - - if (ppm_counter == 3) { //third pulse - channel_3_raw = dt_ppm; - } - - if (ppm_counter == 4) { //fourth pulse - channel_4_raw = dt_ppm; - } - - if (ppm_counter == 5) { //fifth pulse - channel_5_raw = dt_ppm; - } - - if (ppm_counter == 6) { //sixth pulse - channel_6_raw = dt_ppm; - } - - ppm_counter = ppm_counter + 1; - } -} - -void getCh1() { - int trigger = digitalRead(ch1Pin); - if(trigger == 1) { - rising_edge_start_1 = micros(); - } - else if(trigger == 0) { - channel_1_raw = micros() - rising_edge_start_1; - } -} - -void getCh2() { - int trigger = digitalRead(ch2Pin); - if(trigger == 1) { - rising_edge_start_2 = micros(); - } - else if(trigger == 0) { - channel_2_raw = micros() - rising_edge_start_2; - } -} - -void getCh3() { - int trigger = digitalRead(ch3Pin); - if(trigger == 1) { - rising_edge_start_3 = micros(); - } - else if(trigger == 0) { - channel_3_raw = micros() - rising_edge_start_3; - } -} - -void getCh4() { - int trigger = digitalRead(ch4Pin); - if(trigger == 1) { - rising_edge_start_4 = micros(); - } - else if(trigger == 0) { - channel_4_raw = micros() - rising_edge_start_4; - } -} - -void getCh5() { - int trigger = digitalRead(ch5Pin); - if(trigger == 1) { - rising_edge_start_5 = micros(); - } - else if(trigger == 0) { - channel_5_raw = micros() - rising_edge_start_5; - } -} - -void getCh6() { - int trigger = digitalRead(ch6Pin); - if(trigger == 1) { - rising_edge_start_6 = micros(); - } - else if(trigger == 0) { - channel_6_raw = micros() - rising_edge_start_6; - } -} diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/I2Cdev.cpp b/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/I2Cdev.cpp deleted file mode 100644 index a22474dd..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/I2Cdev.cpp +++ /dev/null @@ -1,1468 +0,0 @@ -// I2Cdev library collection - Main I2C device class -// Abstracts bit and byte I2C R/W functions into a convenient class -// 2013-06-05 by Jeff Rowberg -// -// Changelog: -// 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications -// 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) -// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire -// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation -// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) -// 2011-10-03 - added automatic Arduino version detection for ease of use -// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications -// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) -// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default -// 2011-08-02 - added support for 16-bit registers -// - fixed incorrect Doxygen comments on some methods -// - added timeout value for read operations (thanks mem @ Arduino forums) -// 2011-07-30 - changed read/write function structures to return success or byte counts -// - made all methods static for multi-device memory savings -// 2011-07-28 - initial release - -/* ============================================ -I2Cdev device library code is placed under the MIT license -Copyright (c) 2013 Jeff Rowberg - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. -=============================================== -*/ - -#include "I2Cdev.h" - -#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE - - #ifdef I2CDEV_IMPLEMENTATION_WARNINGS - #if ARDUINO < 100 - #warning Using outdated Arduino IDE with Wire library is functionally limiting. - #warning Arduino IDE v1.6.5+ with I2Cdev Fastwire implementation is recommended. - #warning This I2Cdev implementation does not support: - #warning - Repeated starts conditions - #warning - Timeout detection (some Wire requests block forever) - #elif ARDUINO == 100 - #warning Using outdated Arduino IDE with Wire library is functionally limiting. - #warning Arduino IDE v1.6.5+ with I2Cdev Fastwire implementation is recommended. - #warning This I2Cdev implementation does not support: - #warning - Repeated starts conditions - #warning - Timeout detection (some Wire requests block forever) - #elif ARDUINO > 100 - /*#warning Using current Arduino IDE with Wire library is functionally limiting. - #warning Arduino IDE v1.6.5+ with I2CDEV_BUILTIN_FASTWIRE implementation is recommended. - #warning This I2Cdev implementation does not support: - #warning - Timeout detection (some Wire requests block forever)*/ - #endif - #endif - -#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE - - //#error The I2CDEV_BUILTIN_FASTWIRE implementation is known to be broken right now. Patience, Iago! - -#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE - - #ifdef I2CDEV_IMPLEMENTATION_WARNINGS - #warning Using I2CDEV_BUILTIN_NBWIRE implementation may adversely affect interrupt detection. - #warning This I2Cdev implementation does not support: - #warning - Repeated starts conditions - #endif - - // NBWire implementation based heavily on code by Gene Knight - // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html - // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html - TwoWire Wire; - -#endif - -#ifndef BUFFER_LENGTH -// band-aid fix for platforms without Wire-defined BUFFER_LENGTH (removed from some official implementations) -#define BUFFER_LENGTH 32 -#endif - -/** Default constructor. - */ -I2Cdev::I2Cdev() { -} - -/** Read a single bit from an 8-bit device register. - * @param devAddr I2C slave device address - * @param regAddr Register regAddr to read from - * @param bitNum Bit position to read (0-7) - * @param data Container for single bit value - * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) - * @return Status of read operation (true = success) - */ -int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) { - uint8_t b; - uint8_t count = readByte(devAddr, regAddr, &b, timeout); - *data = b & (1 << bitNum); - return count; -} - -/** Read a single bit from a 16-bit device register. - * @param devAddr I2C slave device address - * @param regAddr Register regAddr to read from - * @param bitNum Bit position to read (0-15) - * @param data Container for single bit value - * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) - * @return Status of read operation (true = success) - */ -int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) { - uint16_t b; - uint8_t count = readWord(devAddr, regAddr, &b, timeout); - *data = b & (1 << bitNum); - return count; -} - -/** Read multiple bits from an 8-bit device register. - * @param devAddr I2C slave device address - * @param regAddr Register regAddr to read from - * @param bitStart First bit position to read (0-7) - * @param length Number of bits to read (not more than 8) - * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) - * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) - * @return Status of read operation (true = success) - */ -int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) { - // 01101001 read byte - // 76543210 bit numbers - // xxx args: bitStart=4, length=3 - // 010 masked - // -> 010 shifted - uint8_t count, b; - if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) { - uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); - b &= mask; - b >>= (bitStart - length + 1); - *data = b; - } - return count; -} - -/** Read multiple bits from a 16-bit device register. - * @param devAddr I2C slave device address - * @param regAddr Register regAddr to read from - * @param bitStart First bit position to read (0-15) - * @param length Number of bits to read (not more than 16) - * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) - * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) - * @return Status of read operation (1 = success, 0 = failure, -1 = timeout) - */ -int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) { - // 1101011001101001 read byte - // fedcba9876543210 bit numbers - // xxx args: bitStart=12, length=3 - // 010 masked - // -> 010 shifted - uint8_t count; - uint16_t w; - if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) { - uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); - w &= mask; - w >>= (bitStart - length + 1); - *data = w; - } - return count; -} - -/** Read single byte from an 8-bit device register. - * @param devAddr I2C slave device address - * @param regAddr Register regAddr to read from - * @param data Container for byte value read from device - * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) - * @return Status of read operation (true = success) - */ -int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) { - return readBytes(devAddr, regAddr, 1, data, timeout); -} - -/** Read single word from a 16-bit device register. - * @param devAddr I2C slave device address - * @param regAddr Register regAddr to read from - * @param data Container for word value read from device - * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) - * @return Status of read operation (true = success) - */ -int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) { - return readWords(devAddr, regAddr, 1, data, timeout); -} - -/** Read multiple bytes from an 8-bit device register. - * @param devAddr I2C slave device address - * @param regAddr First register regAddr to read from - * @param length Number of bytes to read - * @param data Buffer to store read data in - * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) - * @return Number of bytes read (-1 indicates failure) - */ -int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) { - #ifdef I2CDEV_SERIAL_DEBUG - Serial.print("I2C (0x"); - Serial.print(devAddr, HEX); - Serial.print(") reading "); - Serial.print(length, DEC); - Serial.print(" bytes from 0x"); - Serial.print(regAddr, HEX); - Serial.print("..."); - #endif - - int8_t count = 0; - uint32_t t1 = millis(); - - #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE) - - #if (ARDUINO < 100) - // Arduino v00xx (before v1.0), Wire library - - // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in - // smaller chunks instead of all at once - for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.send(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); - - for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { - data[count] = Wire.receive(); - #ifdef I2CDEV_SERIAL_DEBUG - Serial.print(data[count], HEX); - if (count + 1 < length) Serial.print(" "); - #endif - } - - Wire.endTransmission(); - } - #elif (ARDUINO == 100) - // Arduino v1.0.0, Wire library - // Adds standardized write() and read() stream methods instead of send() and receive() - - // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in - // smaller chunks instead of all at once - for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.write(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); - - for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { - data[count] = Wire.read(); - #ifdef I2CDEV_SERIAL_DEBUG - Serial.print(data[count], HEX); - if (count + 1 < length) Serial.print(" "); - #endif - } - - Wire.endTransmission(); - } - #elif (ARDUINO > 100) - // Arduino v1.0.1+, Wire library - // Adds official support for repeated start condition, yay! - - // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in - // smaller chunks instead of all at once - for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.write(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); - - for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { - data[count] = Wire.read(); - #ifdef I2CDEV_SERIAL_DEBUG - Serial.print(data[count], HEX); - if (count + 1 < length) Serial.print(" "); - #endif - } - } - #endif - - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) - - // Fastwire library - // no loop required for fastwire - uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, data, length); - if (status == 0) { - count = length; // success - } else { - count = -1; // error - } - - #endif - - // check for timeout - if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout - - #ifdef I2CDEV_SERIAL_DEBUG - Serial.print(". Done ("); - Serial.print(count, DEC); - Serial.println(" read)."); - #endif - - return count; -} - -/** Read multiple words from a 16-bit device register. - * @param devAddr I2C slave device address - * @param regAddr First register regAddr to read from - * @param length Number of words to read - * @param data Buffer to store read data in - * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) - * @return Number of words read (-1 indicates failure) - */ -int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) { - #ifdef I2CDEV_SERIAL_DEBUG - Serial.print("I2C (0x"); - Serial.print(devAddr, HEX); - Serial.print(") reading "); - Serial.print(length, DEC); - Serial.print(" words from 0x"); - Serial.print(regAddr, HEX); - Serial.print("..."); - #endif - - int8_t count = 0; - uint32_t t1 = millis(); - -#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE - - #if (ARDUINO < 100) - // Arduino v00xx (before v1.0), Wire library - - // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in - // smaller chunks instead of all at once - for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.send(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes - - bool msb = true; // starts with MSB, then LSB - for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { - if (msb) { - // first byte is bits 15-8 (MSb=15) - data[count] = Wire.receive() << 8; - } else { - // second byte is bits 7-0 (LSb=0) - data[count] |= Wire.receive(); - #ifdef I2CDEV_SERIAL_DEBUG - Serial.print(data[count], HEX); - if (count + 1 < length) Serial.print(" "); - #endif - count++; - } - msb = !msb; - } - - Wire.endTransmission(); - } - #elif (ARDUINO == 100) - // Arduino v1.0.0, Wire library - // Adds standardized write() and read() stream methods instead of send() and receive() - - // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in - // smaller chunks instead of all at once - for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.write(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes - - bool msb = true; // starts with MSB, then LSB - for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { - if (msb) { - // first byte is bits 15-8 (MSb=15) - data[count] = Wire.read() << 8; - } else { - // second byte is bits 7-0 (LSb=0) - data[count] |= Wire.read(); - #ifdef I2CDEV_SERIAL_DEBUG - Serial.print(data[count], HEX); - if (count + 1 < length) Serial.print(" "); - #endif - count++; - } - msb = !msb; - } - - Wire.endTransmission(); - } - #elif (ARDUINO > 100) - // Arduino v1.0.1+, Wire library - // Adds official support for repeated start condition, yay! - - // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in - // smaller chunks instead of all at once - for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.write(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes - - bool msb = true; // starts with MSB, then LSB - for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { - if (msb) { - // first byte is bits 15-8 (MSb=15) - data[count] = Wire.read() << 8; - } else { - // second byte is bits 7-0 (LSb=0) - data[count] |= Wire.read(); - #ifdef I2CDEV_SERIAL_DEBUG - Serial.print(data[count], HEX); - if (count + 1 < length) Serial.print(" "); - #endif - count++; - } - msb = !msb; - } - - Wire.endTransmission(); - } - #endif - - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) - - // Fastwire library - // no loop required for fastwire - uint16_t intermediate[(uint8_t)length]; - uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, (uint8_t *)intermediate, (uint8_t)(length * 2)); - if (status == 0) { - count = length; // success - for (uint8_t i = 0; i < length; i++) { - data[i] = (intermediate[2*i] << 8) | intermediate[2*i + 1]; - } - } else { - count = -1; // error - } - - #endif - - if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout - - #ifdef I2CDEV_SERIAL_DEBUG - Serial.print(". Done ("); - Serial.print(count, DEC); - Serial.println(" read)."); - #endif - - return count; -} - -/** write a single bit in an 8-bit device register. - * @param devAddr I2C slave device address - * @param regAddr Register regAddr to write to - * @param bitNum Bit position to write (0-7) - * @param value New bit value to write - * @return Status of operation (true = success) - */ -bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) { - uint8_t b; - readByte(devAddr, regAddr, &b); - b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); - return writeByte(devAddr, regAddr, b); -} - -/** write a single bit in a 16-bit device register. - * @param devAddr I2C slave device address - * @param regAddr Register regAddr to write to - * @param bitNum Bit position to write (0-15) - * @param value New bit value to write - * @return Status of operation (true = success) - */ -bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) { - uint16_t w; - readWord(devAddr, regAddr, &w); - w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum)); - return writeWord(devAddr, regAddr, w); -} - -/** Write multiple bits in an 8-bit device register. - * @param devAddr I2C slave device address - * @param regAddr Register regAddr to write to - * @param bitStart First bit position to write (0-7) - * @param length Number of bits to write (not more than 8) - * @param data Right-aligned value to write - * @return Status of operation (true = success) - */ -bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { - // 010 value to write - // 76543210 bit numbers - // xxx args: bitStart=4, length=3 - // 00011100 mask byte - // 10101111 original value (sample) - // 10100011 original & ~mask - // 10101011 masked | value - uint8_t b; - if (readByte(devAddr, regAddr, &b) != 0) { - uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); - data <<= (bitStart - length + 1); // shift data into correct position - data &= mask; // zero all non-important bits in data - b &= ~(mask); // zero all important bits in existing byte - b |= data; // combine data with existing byte - return writeByte(devAddr, regAddr, b); - } else { - return false; - } -} - -/** Write multiple bits in a 16-bit device register. - * @param devAddr I2C slave device address - * @param regAddr Register regAddr to write to - * @param bitStart First bit position to write (0-15) - * @param length Number of bits to write (not more than 16) - * @param data Right-aligned value to write - * @return Status of operation (true = success) - */ -bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) { - // 010 value to write - // fedcba9876543210 bit numbers - // xxx args: bitStart=12, length=3 - // 0001110000000000 mask word - // 1010111110010110 original value (sample) - // 1010001110010110 original & ~mask - // 1010101110010110 masked | value - uint16_t w; - if (readWord(devAddr, regAddr, &w) != 0) { - uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); - data <<= (bitStart - length + 1); // shift data into correct position - data &= mask; // zero all non-important bits in data - w &= ~(mask); // zero all important bits in existing word - w |= data; // combine data with existing word - return writeWord(devAddr, regAddr, w); - } else { - return false; - } -} - -/** Write single byte to an 8-bit device register. - * @param devAddr I2C slave device address - * @param regAddr Register address to write to - * @param data New byte value to write - * @return Status of operation (true = success) - */ -bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) { - return writeBytes(devAddr, regAddr, 1, &data); -} - -/** Write single word to a 16-bit device register. - * @param devAddr I2C slave device address - * @param regAddr Register address to write to - * @param data New word value to write - * @return Status of operation (true = success) - */ -bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) { - return writeWords(devAddr, regAddr, 1, &data); -} - -/** Write multiple bytes to an 8-bit device register. - * @param devAddr I2C slave device address - * @param regAddr First register address to write to - * @param length Number of bytes to write - * @param data Buffer to copy new data from - * @return Status of operation (true = success) - */ -bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) { - #ifdef I2CDEV_SERIAL_DEBUG - Serial.print("I2C (0x"); - Serial.print(devAddr, HEX); - Serial.print(") writing "); - Serial.print(length, DEC); - Serial.print(" bytes to 0x"); - Serial.print(regAddr, HEX); - Serial.print("..."); - #endif - uint8_t status = 0; - #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.beginTransmission(devAddr); - Wire.send((uint8_t) regAddr); // send address - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ - || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) - Wire.beginTransmission(devAddr); - Wire.write((uint8_t) regAddr); // send address - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) - Fastwire::beginTransmission(devAddr); - Fastwire::write(regAddr); - #endif - for (uint8_t i = 0; i < length; i++) { - #ifdef I2CDEV_SERIAL_DEBUG - Serial.print(data[i], HEX); - if (i + 1 < length) Serial.print(" "); - #endif - #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.send((uint8_t) data[i]); - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ - || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) - Wire.write((uint8_t) data[i]); - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) - Fastwire::write((uint8_t) data[i]); - #endif - } - #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.endTransmission(); - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ - || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) - status = Wire.endTransmission(); - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) - Fastwire::stop(); - //status = Fastwire::endTransmission(); - #endif - #ifdef I2CDEV_SERIAL_DEBUG - Serial.println(". Done."); - #endif - return status == 0; -} - -/** Write multiple words to a 16-bit device register. - * @param devAddr I2C slave device address - * @param regAddr First register address to write to - * @param length Number of words to write - * @param data Buffer to copy new data from - * @return Status of operation (true = success) - */ -bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) { - #ifdef I2CDEV_SERIAL_DEBUG - Serial.print("I2C (0x"); - Serial.print(devAddr, HEX); - Serial.print(") writing "); - Serial.print(length, DEC); - Serial.print(" words to 0x"); - Serial.print(regAddr, HEX); - Serial.print("..."); - #endif - uint8_t status = 0; - #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.beginTransmission(devAddr); - Wire.send(regAddr); // send address - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ - || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) - Wire.beginTransmission(devAddr); - Wire.write(regAddr); // send address - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) - Fastwire::beginTransmission(devAddr); - Fastwire::write(regAddr); - #endif - for (uint8_t i = 0; i < length; i++) { - #ifdef I2CDEV_SERIAL_DEBUG - Serial.print(data[i], HEX); - if (i + 1 < length) Serial.print(" "); - #endif - #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.send((uint8_t)(data[i] >> 8)); // send MSB - Wire.send((uint8_t)data[i]); // send LSB - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ - || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) - Wire.write((uint8_t)(data[i] >> 8)); // send MSB - Wire.write((uint8_t)data[i]); // send LSB - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) - Fastwire::write((uint8_t)(data[i] >> 8)); // send MSB - status = Fastwire::write((uint8_t)data[i]); // send LSB - if (status != 0) break; - #endif - } - #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.endTransmission(); - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \ - || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) - status = Wire.endTransmission(); - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) - Fastwire::stop(); - //status = Fastwire::endTransmission(); - #endif - #ifdef I2CDEV_SERIAL_DEBUG - Serial.println(". Done."); - #endif - return status == 0; -} - -/** Default timeout value for read operations. - * Set this to 0 to disable timeout detection. - */ -uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; - -#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE - // I2C library - ////////////////////// - // Copyright(C) 2012 - // Francesco Ferrara - // ferrara[at]libero[point]it - ////////////////////// - - /* - FastWire - - 0.24 added stop - - 0.23 added reset - - This is a library to help faster programs to read I2C devices. - Copyright(C) 2012 Francesco Ferrara - occhiobello at gmail dot com - [used by Jeff Rowberg for I2Cdevlib with permission] - */ - - boolean Fastwire::waitInt() { - int l = 250; - while (!(TWCR & (1 << TWINT)) && l-- > 0); - return l > 0; - } - - void Fastwire::setup(int khz, boolean pullup) { - TWCR = 0; - #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__) - // activate internal pull-ups for twi (PORTC bits 4 & 5) - // as per note from atmega8 manual pg167 - if (pullup) PORTC |= ((1 << 4) | (1 << 5)); - else PORTC &= ~((1 << 4) | (1 << 5)); - #elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) - // activate internal pull-ups for twi (PORTC bits 0 & 1) - if (pullup) PORTC |= ((1 << 0) | (1 << 1)); - else PORTC &= ~((1 << 0) | (1 << 1)); - #else - // activate internal pull-ups for twi (PORTD bits 0 & 1) - // as per note from atmega128 manual pg204 - if (pullup) PORTD |= ((1 << 0) | (1 << 1)); - else PORTD &= ~((1 << 0) | (1 << 1)); - #endif - - TWSR = 0; // no prescaler => prescaler = 1 - TWBR = ((16000L / khz) - 16) / 2; // change the I2C clock rate - TWCR = 1 << TWEN; // enable twi module, no interrupt - } - - // added by Jeff Rowberg 2013-05-07: - // Arduino Wire-style "beginTransmission" function - // (takes 7-bit device address like the Wire method, NOT 8-bit: 0x68, not 0xD0/0xD1) - byte Fastwire::beginTransmission(byte device) { - byte twst, retry; - retry = 2; - do { - TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); - if (!waitInt()) return 1; - twst = TWSR & 0xF8; - if (twst != TW_START && twst != TW_REP_START) return 2; - - //Serial.print(device, HEX); - //Serial.print(" "); - TWDR = device << 1; // send device address without read bit (1) - TWCR = (1 << TWINT) | (1 << TWEN); - if (!waitInt()) return 3; - twst = TWSR & 0xF8; - } while (twst == TW_MT_SLA_NACK && retry-- > 0); - if (twst != TW_MT_SLA_ACK) return 4; - return 0; - } - - byte Fastwire::writeBuf(byte device, byte address, byte *data, byte num) { - byte twst, retry; - - retry = 2; - do { - TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); - if (!waitInt()) return 1; - twst = TWSR & 0xF8; - if (twst != TW_START && twst != TW_REP_START) return 2; - - //Serial.print(device, HEX); - //Serial.print(" "); - TWDR = device & 0xFE; // send device address without read bit (1) - TWCR = (1 << TWINT) | (1 << TWEN); - if (!waitInt()) return 3; - twst = TWSR & 0xF8; - } while (twst == TW_MT_SLA_NACK && retry-- > 0); - if (twst != TW_MT_SLA_ACK) return 4; - - //Serial.print(address, HEX); - //Serial.print(" "); - TWDR = address; // send data to the previously addressed device - TWCR = (1 << TWINT) | (1 << TWEN); - if (!waitInt()) return 5; - twst = TWSR & 0xF8; - if (twst != TW_MT_DATA_ACK) return 6; - - for (byte i = 0; i < num; i++) { - //Serial.print(data[i], HEX); - //Serial.print(" "); - TWDR = data[i]; // send data to the previously addressed device - TWCR = (1 << TWINT) | (1 << TWEN); - if (!waitInt()) return 7; - twst = TWSR & 0xF8; - if (twst != TW_MT_DATA_ACK) return 8; - } - //Serial.print("\n"); - - return 0; - } - - byte Fastwire::write(byte value) { - byte twst; - //Serial.println(value, HEX); - TWDR = value; // send data - TWCR = (1 << TWINT) | (1 << TWEN); - if (!waitInt()) return 1; - twst = TWSR & 0xF8; - if (twst != TW_MT_DATA_ACK) return 2; - return 0; - } - - byte Fastwire::readBuf(byte device, byte address, byte *data, byte num) { - byte twst, retry; - - retry = 2; - do { - TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); - if (!waitInt()) return 16; - twst = TWSR & 0xF8; - if (twst != TW_START && twst != TW_REP_START) return 17; - - //Serial.print(device, HEX); - //Serial.print(" "); - TWDR = device & 0xfe; // send device address to write - TWCR = (1 << TWINT) | (1 << TWEN); - if (!waitInt()) return 18; - twst = TWSR & 0xF8; - } while (twst == TW_MT_SLA_NACK && retry-- > 0); - if (twst != TW_MT_SLA_ACK) return 19; - - //Serial.print(address, HEX); - //Serial.print(" "); - TWDR = address; // send data to the previously addressed device - TWCR = (1 << TWINT) | (1 << TWEN); - if (!waitInt()) return 20; - twst = TWSR & 0xF8; - if (twst != TW_MT_DATA_ACK) return 21; - - /***/ - - retry = 2; - do { - TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); - if (!waitInt()) return 22; - twst = TWSR & 0xF8; - if (twst != TW_START && twst != TW_REP_START) return 23; - - //Serial.print(device, HEX); - //Serial.print(" "); - TWDR = device | 0x01; // send device address with the read bit (1) - TWCR = (1 << TWINT) | (1 << TWEN); - if (!waitInt()) return 24; - twst = TWSR & 0xF8; - } while (twst == TW_MR_SLA_NACK && retry-- > 0); - if (twst != TW_MR_SLA_ACK) return 25; - - for (uint8_t i = 0; i < num; i++) { - if (i == num - 1) - TWCR = (1 << TWINT) | (1 << TWEN); - else - TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA); - if (!waitInt()) return 26; - twst = TWSR & 0xF8; - if (twst != TW_MR_DATA_ACK && twst != TW_MR_DATA_NACK) return twst; - data[i] = TWDR; - //Serial.print(data[i], HEX); - //Serial.print(" "); - } - //Serial.print("\n"); - stop(); - - return 0; - } - - void Fastwire::reset() { - TWCR = 0; - } - - byte Fastwire::stop() { - TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO); - if (!waitInt()) return 1; - return 0; - } -#endif - -#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE - // NBWire implementation based heavily on code by Gene Knight - // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html - // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html - - /* - call this version 1.0 - - Offhand, the only funky part that I can think of is in nbrequestFrom, where the buffer - length and index are set *before* the data is actually read. The problem is that these - are variables local to the TwoWire object, and by the time we actually have read the - data, and know what the length actually is, we have no simple access to the object's - variables. The actual bytes read *is* given to the callback function, though. - - The ISR code for a slave receiver is commented out. I don't have that setup, and can't - verify it at this time. Save it for 2.0! - - The handling of the read and write processes here is much like in the demo sketch code: - the process is broken down into sequential functions, where each registers the next as a - callback, essentially. - - For example, for the Read process, twi_read00 just returns if TWI is not yet in a - ready state. When there's another interrupt, and the interface *is* ready, then it - sets up the read, starts it, and registers twi_read01 as the function to call after - the *next* interrupt. twi_read01, then, just returns if the interface is still in a - "reading" state. When the reading is done, it copies the information to the buffer, - cleans up, and calls the user-requested callback function with the actual number of - bytes read. - - The writing is similar. - - Questions, comments and problems can go to Gene@Telobot.com. - - Thumbs Up! - Gene Knight - - */ - - uint8_t TwoWire::rxBuffer[NBWIRE_BUFFER_LENGTH]; - uint8_t TwoWire::rxBufferIndex = 0; - uint8_t TwoWire::rxBufferLength = 0; - - uint8_t TwoWire::txAddress = 0; - uint8_t TwoWire::txBuffer[NBWIRE_BUFFER_LENGTH]; - uint8_t TwoWire::txBufferIndex = 0; - uint8_t TwoWire::txBufferLength = 0; - - //uint8_t TwoWire::transmitting = 0; - void (*TwoWire::user_onRequest)(void); - void (*TwoWire::user_onReceive)(int); - - static volatile uint8_t twi_transmitting; - static volatile uint8_t twi_state; - static uint8_t twi_slarw; - static volatile uint8_t twi_error; - static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH]; - static volatile uint8_t twi_masterBufferIndex; - static uint8_t twi_masterBufferLength; - static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH]; - static volatile uint8_t twi_rxBufferIndex; - //static volatile uint8_t twi_Interrupt_Continue_Command; - static volatile uint8_t twi_Return_Value; - static volatile uint8_t twi_Done; - void (*twi_cbendTransmissionDone)(int); - void (*twi_cbreadFromDone)(int); - - void twi_init() { - // initialize state - twi_state = TWI_READY; - - // activate internal pull-ups for twi - // as per note from atmega8 manual pg167 - sbi(PORTC, 4); - sbi(PORTC, 5); - - // initialize twi prescaler and bit rate - cbi(TWSR, TWPS0); // TWI Status Register - Prescaler bits - cbi(TWSR, TWPS1); - - /* twi bit rate formula from atmega128 manual pg 204 - SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR)) - note: TWBR should be 10 or higher for master mode - It is 72 for a 16mhz Wiring board with 100kHz TWI */ - - TWBR = ((CPU_FREQ / TWI_FREQ) - 16) / 2; // bitrate register - // enable twi module, acks, and twi interrupt - - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); - - /* TWEN - TWI Enable Bit - TWIE - TWI Interrupt Enable - TWEA - TWI Enable Acknowledge Bit - TWINT - TWI Interrupt Flag - TWSTA - TWI Start Condition - */ - } - - typedef struct { - uint8_t address; - uint8_t* data; - uint8_t length; - uint8_t wait; - uint8_t i; - } twi_Write_Vars; - - twi_Write_Vars *ptwv = 0; - static void (*fNextInterruptFunction)(void) = 0; - - void twi_Finish(byte bRetVal) { - if (ptwv) { - free(ptwv); - ptwv = 0; - } - twi_Done = 0xFF; - twi_Return_Value = bRetVal; - fNextInterruptFunction = 0; - } - - uint8_t twii_WaitForDone(uint16_t timeout) { - uint32_t endMillis = millis() + timeout; - while (!twi_Done && (timeout == 0 || millis() < endMillis)) continue; - return twi_Return_Value; - } - - void twii_SetState(uint8_t ucState) { - twi_state = ucState; - } - - void twii_SetError(uint8_t ucError) { - twi_error = ucError ; - } - - void twii_InitBuffer(uint8_t ucPos, uint8_t ucLength) { - twi_masterBufferIndex = 0; - twi_masterBufferLength = ucLength; - } - - void twii_CopyToBuf(uint8_t* pData, uint8_t ucLength) { - uint8_t i; - for (i = 0; i < ucLength; ++i) { - twi_masterBuffer[i] = pData[i]; - } - } - - void twii_CopyFromBuf(uint8_t *pData, uint8_t ucLength) { - uint8_t i; - for (i = 0; i < ucLength; ++i) { - pData[i] = twi_masterBuffer[i]; - } - } - - void twii_SetSlaRW(uint8_t ucSlaRW) { - twi_slarw = ucSlaRW; - } - - void twii_SetStart() { - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA); - } - - void twi_write01() { - if (TWI_MTX == twi_state) return; // blocking test - twi_transmitting = 0 ; - if (twi_error == 0xFF) - twi_Finish (0); // success - else if (twi_error == TW_MT_SLA_NACK) - twi_Finish (2); // error: address send, nack received - else if (twi_error == TW_MT_DATA_NACK) - twi_Finish (3); // error: data send, nack received - else - twi_Finish (4); // other twi error - if (twi_cbendTransmissionDone) return twi_cbendTransmissionDone(twi_Return_Value); - return; - } - - - void twi_write00() { - if (TWI_READY != twi_state) return; // blocking test - if (TWI_BUFFER_LENGTH < ptwv -> length) { - twi_Finish(1); // end write with error 1 - return; - } - twi_Done = 0x00; // show as working - twii_SetState(TWI_MTX); // to transmitting - twii_SetError(0xFF); // to No Error - twii_InitBuffer(0, ptwv -> length); // pointer and length - twii_CopyToBuf(ptwv -> data, ptwv -> length); // get the data - twii_SetSlaRW((ptwv -> address << 1) | TW_WRITE); // write command - twii_SetStart(); // start the cycle - fNextInterruptFunction = twi_write01; // next routine - return twi_write01(); - } - - void twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait) { - uint8_t i; - ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars)); - ptwv -> address = address; - ptwv -> data = data; - ptwv -> length = length; - ptwv -> wait = wait; - fNextInterruptFunction = twi_write00; - return twi_write00(); - } - - void twi_read01() { - if (TWI_MRX == twi_state) return; // blocking test - if (twi_masterBufferIndex < ptwv -> length) ptwv -> length = twi_masterBufferIndex; - twii_CopyFromBuf(ptwv -> data, ptwv -> length); - twi_Finish(ptwv -> length); - if (twi_cbreadFromDone) return twi_cbreadFromDone(twi_Return_Value); - return; - } - - void twi_read00() { - if (TWI_READY != twi_state) return; // blocking test - if (TWI_BUFFER_LENGTH < ptwv -> length) twi_Finish(0); // error return - twi_Done = 0x00; // show as working - twii_SetState(TWI_MRX); // reading - twii_SetError(0xFF); // reset error - twii_InitBuffer(0, ptwv -> length - 1); // init to one less than length - twii_SetSlaRW((ptwv -> address << 1) | TW_READ); // read command - twii_SetStart(); // start cycle - fNextInterruptFunction = twi_read01; - return twi_read01(); - } - - void twi_readFrom(uint8_t address, uint8_t* data, uint8_t length) { - uint8_t i; - - ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars)); - ptwv -> address = address; - ptwv -> data = data; - ptwv -> length = length; - fNextInterruptFunction = twi_read00; - return twi_read00(); - } - - void twi_reply(uint8_t ack) { - // transmit master read ready signal, with or without ack - if (ack){ - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA); - } else { - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT); - } - } - - void twi_stop(void) { - // send stop condition - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO); - - // wait for stop condition to be exectued on bus - // TWINT is not set after a stop condition! - while (TWCR & _BV(TWSTO)) { - continue; - } - - // update twi state - twi_state = TWI_READY; - } - - void twi_releaseBus(void) { - // release bus - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT); - - // update twi state - twi_state = TWI_READY; - } - - SIGNAL(TWI_vect) { - switch (TW_STATUS) { - // All Master - case TW_START: // sent start condition - case TW_REP_START: // sent repeated start condition - // copy device address and r/w bit to output register and ack - TWDR = twi_slarw; - twi_reply(1); - break; - - // Master Transmitter - case TW_MT_SLA_ACK: // slave receiver acked address - case TW_MT_DATA_ACK: // slave receiver acked data - // if there is data to send, send it, otherwise stop - if (twi_masterBufferIndex < twi_masterBufferLength) { - // copy data to output register and ack - TWDR = twi_masterBuffer[twi_masterBufferIndex++]; - twi_reply(1); - } else { - twi_stop(); - } - break; - - case TW_MT_SLA_NACK: // address sent, nack received - twi_error = TW_MT_SLA_NACK; - twi_stop(); - break; - - case TW_MT_DATA_NACK: // data sent, nack received - twi_error = TW_MT_DATA_NACK; - twi_stop(); - break; - - case TW_MT_ARB_LOST: // lost bus arbitration - twi_error = TW_MT_ARB_LOST; - twi_releaseBus(); - break; - - // Master Receiver - case TW_MR_DATA_ACK: // data received, ack sent - // put byte into buffer - twi_masterBuffer[twi_masterBufferIndex++] = TWDR; - - case TW_MR_SLA_ACK: // address sent, ack received - // ack if more bytes are expected, otherwise nack - if (twi_masterBufferIndex < twi_masterBufferLength) { - twi_reply(1); - } else { - twi_reply(0); - } - break; - - case TW_MR_DATA_NACK: // data received, nack sent - // put final byte into buffer - twi_masterBuffer[twi_masterBufferIndex++] = TWDR; - - case TW_MR_SLA_NACK: // address sent, nack received - twi_stop(); - break; - - // TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case - - // Slave Receiver (NOT IMPLEMENTED YET) - /* - case TW_SR_SLA_ACK: // addressed, returned ack - case TW_SR_GCALL_ACK: // addressed generally, returned ack - case TW_SR_ARB_LOST_SLA_ACK: // lost arbitration, returned ack - case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack - // enter slave receiver mode - twi_state = TWI_SRX; - - // indicate that rx buffer can be overwritten and ack - twi_rxBufferIndex = 0; - twi_reply(1); - break; - - case TW_SR_DATA_ACK: // data received, returned ack - case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack - // if there is still room in the rx buffer - if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) { - // put byte in buffer and ack - twi_rxBuffer[twi_rxBufferIndex++] = TWDR; - twi_reply(1); - } else { - // otherwise nack - twi_reply(0); - } - break; - - case TW_SR_STOP: // stop or repeated start condition received - // put a null char after data if there's room - if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) { - twi_rxBuffer[twi_rxBufferIndex] = 0; - } - - // sends ack and stops interface for clock stretching - twi_stop(); - - // callback to user defined callback - twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex); - - // since we submit rx buffer to "wire" library, we can reset it - twi_rxBufferIndex = 0; - - // ack future responses and leave slave receiver state - twi_releaseBus(); - break; - - case TW_SR_DATA_NACK: // data received, returned nack - case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack - // nack back at master - twi_reply(0); - break; - - // Slave Transmitter - case TW_ST_SLA_ACK: // addressed, returned ack - case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack - // enter slave transmitter mode - twi_state = TWI_STX; - - // ready the tx buffer index for iteration - twi_txBufferIndex = 0; - - // set tx buffer length to be zero, to verify if user changes it - twi_txBufferLength = 0; - - // request for txBuffer to be filled and length to be set - // note: user must call twi_transmit(bytes, length) to do this - twi_onSlaveTransmit(); - - // if they didn't change buffer & length, initialize it - if (0 == twi_txBufferLength) { - twi_txBufferLength = 1; - twi_txBuffer[0] = 0x00; - } - - // transmit first byte from buffer, fall through - - case TW_ST_DATA_ACK: // byte sent, ack returned - // copy data to output register - TWDR = twi_txBuffer[twi_txBufferIndex++]; - - // if there is more to send, ack, otherwise nack - if (twi_txBufferIndex < twi_txBufferLength) { - twi_reply(1); - } else { - twi_reply(0); - } - break; - - case TW_ST_DATA_NACK: // received nack, we are done - case TW_ST_LAST_DATA: // received ack, but we are done already! - // ack future responses - twi_reply(1); - // leave slave receiver state - twi_state = TWI_READY; - break; - */ - - // all - case TW_NO_INFO: // no state information - break; - - case TW_BUS_ERROR: // bus error, illegal stop/start - twi_error = TW_BUS_ERROR; - twi_stop(); - break; - } - - if (fNextInterruptFunction) return fNextInterruptFunction(); - } - - TwoWire::TwoWire() { } - - void TwoWire::begin(void) { - rxBufferIndex = 0; - rxBufferLength = 0; - - txBufferIndex = 0; - txBufferLength = 0; - - twi_init(); - } - - void TwoWire::beginTransmission(uint8_t address) { - //beginTransmission((uint8_t)address); - - // indicate that we are transmitting - twi_transmitting = 1; - - // set address of targeted slave - txAddress = address; - - // reset tx buffer iterator vars - txBufferIndex = 0; - txBufferLength = 0; - } - - uint8_t TwoWire::endTransmission(uint16_t timeout) { - // transmit buffer (blocking) - //int8_t ret = - twi_cbendTransmissionDone = NULL; - twi_writeTo(txAddress, txBuffer, txBufferLength, 1); - int8_t ret = twii_WaitForDone(timeout); - - // reset tx buffer iterator vars - txBufferIndex = 0; - txBufferLength = 0; - - // indicate that we are done transmitting - // twi_transmitting = 0; - return ret; - } - - void TwoWire::nbendTransmission(void (*function)(int)) { - twi_cbendTransmissionDone = function; - twi_writeTo(txAddress, txBuffer, txBufferLength, 1); - return; - } - - void TwoWire::send(uint8_t data) { - if (twi_transmitting) { - // in master transmitter mode - // don't bother if buffer is full - if (txBufferLength >= NBWIRE_BUFFER_LENGTH) { - return; - } - - // put byte in tx buffer - txBuffer[txBufferIndex] = data; - ++txBufferIndex; - - // update amount in buffer - txBufferLength = txBufferIndex; - } else { - // in slave send mode - // reply to master - //twi_transmit(&data, 1); - } - } - - uint8_t TwoWire::receive(void) { - // default to returning null char - // for people using with char strings - uint8_t value = 0; - - // get each successive byte on each call - if (rxBufferIndex < rxBufferLength) { - value = rxBuffer[rxBufferIndex]; - ++rxBufferIndex; - } - - return value; - } - - uint8_t TwoWire::requestFrom(uint8_t address, int quantity, uint16_t timeout) { - // clamp to buffer length - if (quantity > NBWIRE_BUFFER_LENGTH) { - quantity = NBWIRE_BUFFER_LENGTH; - } - - // perform blocking read into buffer - twi_cbreadFromDone = NULL; - twi_readFrom(address, rxBuffer, quantity); - uint8_t read = twii_WaitForDone(timeout); - - // set rx buffer iterator vars - rxBufferIndex = 0; - rxBufferLength = read; - - return read; - } - - void TwoWire::nbrequestFrom(uint8_t address, int quantity, void (*function)(int)) { - // clamp to buffer length - if (quantity > NBWIRE_BUFFER_LENGTH) { - quantity = NBWIRE_BUFFER_LENGTH; - } - - // perform blocking read into buffer - twi_cbreadFromDone = function; - twi_readFrom(address, rxBuffer, quantity); - //uint8_t read = twii_WaitForDone(); - - // set rx buffer iterator vars - //rxBufferIndex = 0; - //rxBufferLength = read; - - rxBufferIndex = 0; - rxBufferLength = quantity; // this is a hack - - return; //read; - } - - uint8_t TwoWire::available(void) { - return rxBufferLength - rxBufferIndex; - } - -#endif diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/I2Cdev.h b/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/I2Cdev.h deleted file mode 100644 index 0bff39ef..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/I2Cdev.h +++ /dev/null @@ -1,283 +0,0 @@ -// I2Cdev library collection - Main I2C device class header file -// Abstracts bit and byte I2C R/W functions into a convenient class -// 2013-06-05 by Jeff Rowberg -// -// Changelog: -// 2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1 -// 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications -// 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) -// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire -// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation -// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) -// 2011-10-03 - added automatic Arduino version detection for ease of use -// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications -// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) -// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default -// 2011-08-02 - added support for 16-bit registers -// - fixed incorrect Doxygen comments on some methods -// - added timeout value for read operations (thanks mem @ Arduino forums) -// 2011-07-30 - changed read/write function structures to return success or byte counts -// - made all methods static for multi-device memory savings -// 2011-07-28 - initial release - -/* ============================================ -I2Cdev device library code is placed under the MIT license -Copyright (c) 2013 Jeff Rowberg - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. -=============================================== -*/ - -#ifndef _I2CDEV_H_ -#define _I2CDEV_H_ - -// ----------------------------------------------------------------------------- -// I2C interface implementation setting -// ----------------------------------------------------------------------------- -#ifndef I2CDEV_IMPLEMENTATION -#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE -//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_SBWIRE -//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE -#endif // I2CDEV_IMPLEMENTATION - -// comment this out if you are using a non-optimal IDE/implementation setting -// but want the compiler to shut up about it -#define I2CDEV_IMPLEMENTATION_WARNINGS - -// ----------------------------------------------------------------------------- -// I2C interface implementation options -// ----------------------------------------------------------------------------- -#define I2CDEV_ARDUINO_WIRE 1 // Wire object from Arduino -#define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project - // ^^^ NBWire implementation is still buggy w/some interrupts! -#define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project -#define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library -#define I2CDEV_BUILTIN_SBWIRE 5 // I2C object from Shuning (Steve) Bian's SBWire Library at https://github.com/freespace/SBWire - -// ----------------------------------------------------------------------------- -// Arduino-style "Serial.print" debug constant (uncomment to enable) -// ----------------------------------------------------------------------------- -//#define I2CDEV_SERIAL_DEBUG - -#ifdef ARDUINO - #if ARDUINO < 100 - #include "WProgram.h" - #else - #include "Arduino.h" - #endif - #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE - #include - #endif - #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY - #include - #endif - #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE - #include "SBWire.h" - #endif -#endif - -#ifdef SPARK - #include - #define ARDUINO 101 -#endif - - -// 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];") -#define I2CDEV_DEFAULT_READ_TIMEOUT 1000 - -class I2Cdev { - public: - I2Cdev(); - - static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); - - static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data); - static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data); - static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); - static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data); - static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data); - static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data); - static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data); - static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data); - - static uint16_t readTimeout; -}; - -#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE - ////////////////////// - // FastWire 0.24 - // This is a library to help faster programs to read I2C devices. - // Copyright(C) 2012 - // Francesco Ferrara - ////////////////////// - - /* Master */ - #define TW_START 0x08 - #define TW_REP_START 0x10 - - /* Master Transmitter */ - #define TW_MT_SLA_ACK 0x18 - #define TW_MT_SLA_NACK 0x20 - #define TW_MT_DATA_ACK 0x28 - #define TW_MT_DATA_NACK 0x30 - #define TW_MT_ARB_LOST 0x38 - - /* Master Receiver */ - #define TW_MR_ARB_LOST 0x38 - #define TW_MR_SLA_ACK 0x40 - #define TW_MR_SLA_NACK 0x48 - #define TW_MR_DATA_ACK 0x50 - #define TW_MR_DATA_NACK 0x58 - - #define TW_OK 0 - #define TW_ERROR 1 - - class Fastwire { - private: - static boolean waitInt(); - - public: - static void setup(int khz, boolean pullup); - static byte beginTransmission(byte device); - static byte write(byte value); - static byte writeBuf(byte device, byte address, byte *data, byte num); - static byte readBuf(byte device, byte address, byte *data, byte num); - static void reset(); - static byte stop(); - }; -#endif - -#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE - // NBWire implementation based heavily on code by Gene Knight - // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html - // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html - - #define NBWIRE_BUFFER_LENGTH 32 - - class TwoWire { - private: - static uint8_t rxBuffer[]; - static uint8_t rxBufferIndex; - static uint8_t rxBufferLength; - - static uint8_t txAddress; - static uint8_t txBuffer[]; - static uint8_t txBufferIndex; - static uint8_t txBufferLength; - - // static uint8_t transmitting; - static void (*user_onRequest)(void); - static void (*user_onReceive)(int); - static void onRequestService(void); - static void onReceiveService(uint8_t*, int); - - public: - TwoWire(); - void begin(); - void begin(uint8_t); - void begin(int); - void beginTransmission(uint8_t); - //void beginTransmission(int); - uint8_t endTransmission(uint16_t timeout=0); - void nbendTransmission(void (*function)(int)) ; - uint8_t requestFrom(uint8_t, int, uint16_t timeout=0); - //uint8_t requestFrom(int, int); - void nbrequestFrom(uint8_t, int, void (*function)(int)); - void send(uint8_t); - void send(uint8_t*, uint8_t); - //void send(int); - void send(char*); - uint8_t available(void); - uint8_t receive(void); - void onReceive(void (*)(int)); - void onRequest(void (*)(void)); - }; - - #define TWI_READY 0 - #define TWI_MRX 1 - #define TWI_MTX 2 - #define TWI_SRX 3 - #define TWI_STX 4 - - #define TW_WRITE 0 - #define TW_READ 1 - - #define TW_MT_SLA_NACK 0x20 - #define TW_MT_DATA_NACK 0x30 - - #define CPU_FREQ 16000000L - #define TWI_FREQ 100000L - #define TWI_BUFFER_LENGTH 32 - - /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */ - - #define TW_STATUS_MASK (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3)) - #define TW_STATUS (TWSR & TW_STATUS_MASK) - #define TW_START 0x08 - #define TW_REP_START 0x10 - #define TW_MT_SLA_ACK 0x18 - #define TW_MT_SLA_NACK 0x20 - #define TW_MT_DATA_ACK 0x28 - #define TW_MT_DATA_NACK 0x30 - #define TW_MT_ARB_LOST 0x38 - #define TW_MR_ARB_LOST 0x38 - #define TW_MR_SLA_ACK 0x40 - #define TW_MR_SLA_NACK 0x48 - #define TW_MR_DATA_ACK 0x50 - #define TW_MR_DATA_NACK 0x58 - #define TW_ST_SLA_ACK 0xA8 - #define TW_ST_ARB_LOST_SLA_ACK 0xB0 - #define TW_ST_DATA_ACK 0xB8 - #define TW_ST_DATA_NACK 0xC0 - #define TW_ST_LAST_DATA 0xC8 - #define TW_SR_SLA_ACK 0x60 - #define TW_SR_ARB_LOST_SLA_ACK 0x68 - #define TW_SR_GCALL_ACK 0x70 - #define TW_SR_ARB_LOST_GCALL_ACK 0x78 - #define TW_SR_DATA_ACK 0x80 - #define TW_SR_DATA_NACK 0x88 - #define TW_SR_GCALL_DATA_ACK 0x90 - #define TW_SR_GCALL_DATA_NACK 0x98 - #define TW_SR_STOP 0xA0 - #define TW_NO_INFO 0xF8 - #define TW_BUS_ERROR 0x00 - - //#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr)) - //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr)) - - #ifndef sbi // set bit - #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) - #endif // sbi - - #ifndef cbi // clear bit - #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) - #endif // cbi - - extern TwoWire Wire; - -#endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE - -#endif /* _I2CDEV_H_ */ diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050.cpp b/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050.cpp deleted file mode 100644 index ae7a555e..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050.cpp +++ /dev/null @@ -1,3330 +0,0 @@ -// I2Cdev library collection - MPU6050 I2C device class -// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) -// 8/24/2011 by Jeff Rowberg -// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib -// -// Changelog: -// 2019-07-08 - Added Auto Calibration routine -// ... - ongoing debug release - -// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE -// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF -// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING. - -/* ============================================ -I2Cdev device library code is placed under the MIT license -Copyright (c) 2012 Jeff Rowberg - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. -=============================================== -*/ - -#include "MPU6050.h" - -/** Specific address constructor. - * @param address I2C address, uses default I2C address if none is specified - * @see MPU6050_DEFAULT_ADDRESS - * @see MPU6050_ADDRESS_AD0_LOW - * @see MPU6050_ADDRESS_AD0_HIGH - */ -MPU6050::MPU6050(uint8_t address):devAddr(address) { -} - -/** Power on and prepare for general usage. - * This will activate the device and take it out of sleep mode (which must be done - * after start-up). This function also sets both the accelerometer and the gyroscope - * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets - * the clock source to use the X Gyro for reference, which is slightly better than - * the default internal clock source. - */ -void MPU6050::initialize() { - setClockSource(MPU6050_CLOCK_PLL_XGYRO); - setFullScaleGyroRange(MPU6050_GYRO_FS_250); - setFullScaleAccelRange(MPU6050_ACCEL_FS_2); - setSleepEnabled(false); // thanks to Jack Elston for pointing this one out! -} - -/** Verify the I2C connection. - * Make sure the device is connected and responds as expected. - * @return True if connection is valid, false otherwise - */ -bool MPU6050::testConnection() { - return getDeviceID() == 0x34; -} - -// AUX_VDDIO register (InvenSense demo code calls this RA_*G_OFFS_TC) - -/** Get the auxiliary I2C supply voltage level. - * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to - * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to - * the MPU-6000, which does not have a VLOGIC pin. - * @return I2C supply voltage level (0=VLOGIC, 1=VDD) - */ -uint8_t MPU6050::getAuxVDDIOLevel() { - I2Cdev::readBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, buffer); - return buffer[0]; -} -/** Set the auxiliary I2C supply voltage level. - * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to - * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to - * the MPU-6000, which does not have a VLOGIC pin. - * @param level I2C supply voltage level (0=VLOGIC, 1=VDD) - */ -void MPU6050::setAuxVDDIOLevel(uint8_t level) { - I2Cdev::writeBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level); -} - -// SMPLRT_DIV register - -/** Get gyroscope output rate divider. - * The sensor register output, FIFO output, DMP sampling, Motion detection, Zero - * Motion detection, and Free Fall detection are all based on the Sample Rate. - * The Sample Rate is generated by dividing the gyroscope output rate by - * SMPLRT_DIV: - * - * Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) - * - * where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or - * 7), and 1kHz when the DLPF is enabled (see Register 26). - * - * Note: The accelerometer output rate is 1kHz. This means that for a Sample - * Rate greater than 1kHz, the same accelerometer sample may be output to the - * FIFO, DMP, and sensor registers more than once. - * - * For a diagram of the gyroscope and accelerometer signal paths, see Section 8 - * of the MPU-6000/MPU-6050 Product Specification document. - * - * @return Current sample rate - * @see MPU6050_RA_SMPLRT_DIV - */ -uint8_t MPU6050::getRate() { - I2Cdev::readByte(devAddr, MPU6050_RA_SMPLRT_DIV, buffer); - return buffer[0]; -} -/** Set gyroscope sample rate divider. - * @param rate New sample rate divider - * @see getRate() - * @see MPU6050_RA_SMPLRT_DIV - */ -void MPU6050::setRate(uint8_t rate) { - I2Cdev::writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, rate); -} - -// CONFIG register - -/** Get external FSYNC configuration. - * Configures the external Frame Synchronization (FSYNC) pin sampling. An - * external signal connected to the FSYNC pin can be sampled by configuring - * EXT_SYNC_SET. Signal changes to the FSYNC pin are latched so that short - * strobes may be captured. The latched FSYNC signal will be sampled at the - * Sampling Rate, as defined in register 25. After sampling, the latch will - * reset to the current FSYNC signal state. - * - * The sampled value will be reported in place of the least significant bit in - * a sensor data register determined by the value of EXT_SYNC_SET according to - * the following table. - * - * 
- * EXT_SYNC_SET | FSYNC Bit Location
- * -------------+-------------------
- * 0            | Input disabled
- * 1            | TEMP_OUT_L[0]
- * 2            | GYRO_XOUT_L[0]
- * 3            | GYRO_YOUT_L[0]
- * 4            | GYRO_ZOUT_L[0]
- * 5            | ACCEL_XOUT_L[0]
- * 6            | ACCEL_YOUT_L[0]
- * 7            | ACCEL_ZOUT_L[0]
- *
- * - * @return FSYNC configuration value - */ -uint8_t MPU6050::getExternalFrameSync() { - I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer); - return buffer[0]; -} -/** Set external FSYNC configuration. - * @see getExternalFrameSync() - * @see MPU6050_RA_CONFIG - * @param sync New FSYNC configuration value - */ -void MPU6050::setExternalFrameSync(uint8_t sync) { - I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync); -} -/** Get digital low-pass filter configuration. - * The DLPF_CFG parameter sets the digital low pass filter configuration. It - * also determines the internal sampling rate used by the device as shown in - * the table below. - * - * Note: The accelerometer output rate is 1kHz. This means that for a Sample - * Rate greater than 1kHz, the same accelerometer sample may be output to the - * FIFO, DMP, and sensor registers more than once. - * - * 
- *          |   ACCELEROMETER    |           GYROSCOPE
- * DLPF_CFG | Bandwidth | Delay  | Bandwidth | Delay  | Sample Rate
- * ---------+-----------+--------+-----------+--------+-------------
- * 0        | 260Hz     | 0ms    | 256Hz     | 0.98ms | 8kHz
- * 1        | 184Hz     | 2.0ms  | 188Hz     | 1.9ms  | 1kHz
- * 2        | 94Hz      | 3.0ms  | 98Hz      | 2.8ms  | 1kHz
- * 3        | 44Hz      | 4.9ms  | 42Hz      | 4.8ms  | 1kHz
- * 4        | 21Hz      | 8.5ms  | 20Hz      | 8.3ms  | 1kHz
- * 5        | 10Hz      | 13.8ms | 10Hz      | 13.4ms | 1kHz
- * 6        | 5Hz       | 19.0ms | 5Hz       | 18.6ms | 1kHz
- * 7        |   -- Reserved --   |   -- Reserved --   | Reserved
- *
- * - * @return DLFP configuration - * @see MPU6050_RA_CONFIG - * @see MPU6050_CFG_DLPF_CFG_BIT - * @see MPU6050_CFG_DLPF_CFG_LENGTH - */ -uint8_t MPU6050::getDLPFMode() { - I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, buffer); - return buffer[0]; -} -/** Set digital low-pass filter configuration. - * @param mode New DLFP configuration setting - * @see getDLPFBandwidth() - * @see MPU6050_DLPF_BW_256 - * @see MPU6050_RA_CONFIG - * @see MPU6050_CFG_DLPF_CFG_BIT - * @see MPU6050_CFG_DLPF_CFG_LENGTH - */ -void MPU6050::setDLPFMode(uint8_t mode) { - I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode); -} - -// GYRO_CONFIG register - -/** Get full-scale gyroscope range. - * The FS_SEL parameter allows setting the full-scale range of the gyro sensors, - * as described in the table below. - * - * 
- * 0 = +/- 250 degrees/sec
- * 1 = +/- 500 degrees/sec
- * 2 = +/- 1000 degrees/sec
- * 3 = +/- 2000 degrees/sec
- *
- * - * @return Current full-scale gyroscope range setting - * @see MPU6050_GYRO_FS_250 - * @see MPU6050_RA_GYRO_CONFIG - * @see MPU6050_GCONFIG_FS_SEL_BIT - * @see MPU6050_GCONFIG_FS_SEL_LENGTH - */ -uint8_t MPU6050::getFullScaleGyroRange() { - I2Cdev::readBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, buffer); - return buffer[0]; -} -/** Set full-scale gyroscope range. - * @param range New full-scale gyroscope range value - * @see getFullScaleRange() - * @see MPU6050_GYRO_FS_250 - * @see MPU6050_RA_GYRO_CONFIG - * @see MPU6050_GCONFIG_FS_SEL_BIT - * @see MPU6050_GCONFIG_FS_SEL_LENGTH - */ -void MPU6050::setFullScaleGyroRange(uint8_t range) { - I2Cdev::writeBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range); -} - -// SELF TEST FACTORY TRIM VALUES - -/** Get self-test factory trim value for accelerometer X axis. - * @return factory trim value - * @see MPU6050_RA_SELF_TEST_X - */ -uint8_t MPU6050::getAccelXSelfTestFactoryTrim() { - I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, &buffer[0]); - I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1]); - return (buffer[0]>>3) | ((buffer[1]>>4) & 0x03); -} - -/** Get self-test factory trim value for accelerometer Y axis. - * @return factory trim value - * @see MPU6050_RA_SELF_TEST_Y - */ -uint8_t MPU6050::getAccelYSelfTestFactoryTrim() { - I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, &buffer[0]); - I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1]); - return (buffer[0]>>3) | ((buffer[1]>>2) & 0x03); -} - -/** Get self-test factory trim value for accelerometer Z axis. - * @return factory trim value - * @see MPU6050_RA_SELF_TEST_Z - */ -uint8_t MPU6050::getAccelZSelfTestFactoryTrim() { - I2Cdev::readBytes(devAddr, MPU6050_RA_SELF_TEST_Z, 2, buffer); - return (buffer[0]>>3) | (buffer[1] & 0x03); -} - -/** Get self-test factory trim value for gyro X axis. - * @return factory trim value - * @see MPU6050_RA_SELF_TEST_X - */ -uint8_t MPU6050::getGyroXSelfTestFactoryTrim() { - I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, buffer); - return (buffer[0] & 0x1F); -} - -/** Get self-test factory trim value for gyro Y axis. - * @return factory trim value - * @see MPU6050_RA_SELF_TEST_Y - */ -uint8_t MPU6050::getGyroYSelfTestFactoryTrim() { - I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, buffer); - return (buffer[0] & 0x1F); -} - -/** Get self-test factory trim value for gyro Z axis. - * @return factory trim value - * @see MPU6050_RA_SELF_TEST_Z - */ -uint8_t MPU6050::getGyroZSelfTestFactoryTrim() { - I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Z, buffer); - return (buffer[0] & 0x1F); -} - -// ACCEL_CONFIG register - -/** Get self-test enabled setting for accelerometer X axis. - * @return Self-test enabled value - * @see MPU6050_RA_ACCEL_CONFIG - */ -bool MPU6050::getAccelXSelfTest() { - I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, buffer); - return buffer[0]; -} -/** Get self-test enabled setting for accelerometer X axis. - * @param enabled Self-test enabled value - * @see MPU6050_RA_ACCEL_CONFIG - */ -void MPU6050::setAccelXSelfTest(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, enabled); -} -/** Get self-test enabled value for accelerometer Y axis. - * @return Self-test enabled value - * @see MPU6050_RA_ACCEL_CONFIG - */ -bool MPU6050::getAccelYSelfTest() { - I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, buffer); - return buffer[0]; -} -/** Get self-test enabled value for accelerometer Y axis. - * @param enabled Self-test enabled value - * @see MPU6050_RA_ACCEL_CONFIG - */ -void MPU6050::setAccelYSelfTest(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, enabled); -} -/** Get self-test enabled value for accelerometer Z axis. - * @return Self-test enabled value - * @see MPU6050_RA_ACCEL_CONFIG - */ -bool MPU6050::getAccelZSelfTest() { - I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, buffer); - return buffer[0]; -} -/** Set self-test enabled value for accelerometer Z axis. - * @param enabled Self-test enabled value - * @see MPU6050_RA_ACCEL_CONFIG - */ -void MPU6050::setAccelZSelfTest(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, enabled); -} -/** Get full-scale accelerometer range. - * The FS_SEL parameter allows setting the full-scale range of the accelerometer - * sensors, as described in the table below. - * - * 
- * 0 = +/- 2g
- * 1 = +/- 4g
- * 2 = +/- 8g
- * 3 = +/- 16g
- *
- * - * @return Current full-scale accelerometer range setting - * @see MPU6050_ACCEL_FS_2 - * @see MPU6050_RA_ACCEL_CONFIG - * @see MPU6050_ACONFIG_AFS_SEL_BIT - * @see MPU6050_ACONFIG_AFS_SEL_LENGTH - */ -uint8_t MPU6050::getFullScaleAccelRange() { - I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, buffer); - return buffer[0]; -} -/** Set full-scale accelerometer range. - * @param range New full-scale accelerometer range setting - * @see getFullScaleAccelRange() - */ -void MPU6050::setFullScaleAccelRange(uint8_t range) { - I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range); -} -/** Get the high-pass filter configuration. - * The DHPF is a filter module in the path leading to motion detectors (Free - * Fall, Motion threshold, and Zero Motion). The high pass filter output is not - * available to the data registers (see Figure in Section 8 of the MPU-6000/ - * MPU-6050 Product Specification document). - * - * The high pass filter has three modes: - * - * 
- *    Reset: The filter output settles to zero within one sample. This
- *           effectively disables the high pass filter. This mode may be toggled
- *           to quickly settle the filter.
- *
- *    On:    The high pass filter will pass signals above the cut off frequency.
- *
- *    Hold:  When triggered, the filter holds the present sample. The filter
- *           output will be the difference between the input sample and the held
- *           sample.
- *
- * - * 
- * ACCEL_HPF | Filter Mode | Cut-off Frequency
- * ----------+-------------+------------------
- * 0         | Reset       | None
- * 1         | On          | 5Hz
- * 2         | On          | 2.5Hz
- * 3         | On          | 1.25Hz
- * 4         | On          | 0.63Hz
- * 7         | Hold        | None
- *
- * - * @return Current high-pass filter configuration - * @see MPU6050_DHPF_RESET - * @see MPU6050_RA_ACCEL_CONFIG - */ -uint8_t MPU6050::getDHPFMode() { - I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, buffer); - return buffer[0]; -} -/** Set the high-pass filter configuration. - * @param bandwidth New high-pass filter configuration - * @see setDHPFMode() - * @see MPU6050_DHPF_RESET - * @see MPU6050_RA_ACCEL_CONFIG - */ -void MPU6050::setDHPFMode(uint8_t bandwidth) { - I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, bandwidth); -} - -// FF_THR register - -/** Get free-fall event acceleration threshold. - * This register configures the detection threshold for Free Fall event - * detection. The unit of FF_THR is 1LSB = 2mg. Free Fall is detected when the - * absolute value of the accelerometer measurements for the three axes are each - * less than the detection threshold. This condition increments the Free Fall - * duration counter (Register 30). The Free Fall interrupt is triggered when the - * Free Fall duration counter reaches the time specified in FF_DUR. - * - * For more details on the Free Fall detection interrupt, see Section 8.2 of the - * MPU-6000/MPU-6050 Product Specification document as well as Registers 56 and - * 58 of this document. - * - * @return Current free-fall acceleration threshold value (LSB = 2mg) - * @see MPU6050_RA_FF_THR - */ -uint8_t MPU6050::getFreefallDetectionThreshold() { - I2Cdev::readByte(devAddr, MPU6050_RA_FF_THR, buffer); - return buffer[0]; -} -/** Get free-fall event acceleration threshold. - * @param threshold New free-fall acceleration threshold value (LSB = 2mg) - * @see getFreefallDetectionThreshold() - * @see MPU6050_RA_FF_THR - */ -void MPU6050::setFreefallDetectionThreshold(uint8_t threshold) { - I2Cdev::writeByte(devAddr, MPU6050_RA_FF_THR, threshold); -} - -// FF_DUR register - -/** Get free-fall event duration threshold. - * This register configures the duration counter threshold for Free Fall event - * detection. The duration counter ticks at 1kHz, therefore FF_DUR has a unit - * of 1 LSB = 1 ms. - * - * The Free Fall duration counter increments while the absolute value of the - * accelerometer measurements are each less than the detection threshold - * (Register 29). The Free Fall interrupt is triggered when the Free Fall - * duration counter reaches the time specified in this register. - * - * For more details on the Free Fall detection interrupt, see Section 8.2 of - * the MPU-6000/MPU-6050 Product Specification document as well as Registers 56 - * and 58 of this document. - * - * @return Current free-fall duration threshold value (LSB = 1ms) - * @see MPU6050_RA_FF_DUR - */ -uint8_t MPU6050::getFreefallDetectionDuration() { - I2Cdev::readByte(devAddr, MPU6050_RA_FF_DUR, buffer); - return buffer[0]; -} -/** Get free-fall event duration threshold. - * @param duration New free-fall duration threshold value (LSB = 1ms) - * @see getFreefallDetectionDuration() - * @see MPU6050_RA_FF_DUR - */ -void MPU6050::setFreefallDetectionDuration(uint8_t duration) { - I2Cdev::writeByte(devAddr, MPU6050_RA_FF_DUR, duration); -} - -// MOT_THR register - -/** Get motion detection event acceleration threshold. - * This register configures the detection threshold for Motion interrupt - * generation. The unit of MOT_THR is 1LSB = 2mg. Motion is detected when the - * absolute value of any of the accelerometer measurements exceeds this Motion - * detection threshold. This condition increments the Motion detection duration - * counter (Register 32). The Motion detection interrupt is triggered when the - * Motion Detection counter reaches the time count specified in MOT_DUR - * (Register 32). - * - * The Motion interrupt will indicate the axis and polarity of detected motion - * in MOT_DETECT_STATUS (Register 97). - * - * For more details on the Motion detection interrupt, see Section 8.3 of the - * MPU-6000/MPU-6050 Product Specification document as well as Registers 56 and - * 58 of this document. - * - * @return Current motion detection acceleration threshold value (LSB = 2mg) - * @see MPU6050_RA_MOT_THR - */ -uint8_t MPU6050::getMotionDetectionThreshold() { - I2Cdev::readByte(devAddr, MPU6050_RA_MOT_THR, buffer); - return buffer[0]; -} -/** Set motion detection event acceleration threshold. - * @param threshold New motion detection acceleration threshold value (LSB = 2mg) - * @see getMotionDetectionThreshold() - * @see MPU6050_RA_MOT_THR - */ -void MPU6050::setMotionDetectionThreshold(uint8_t threshold) { - I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_THR, threshold); -} - -// MOT_DUR register - -/** Get motion detection event duration threshold. - * This register configures the duration counter threshold for Motion interrupt - * generation. The duration counter ticks at 1 kHz, therefore MOT_DUR has a unit - * of 1LSB = 1ms. The Motion detection duration counter increments when the - * absolute value of any of the accelerometer measurements exceeds the Motion - * detection threshold (Register 31). The Motion detection interrupt is - * triggered when the Motion detection counter reaches the time count specified - * in this register. - * - * For more details on the Motion detection interrupt, see Section 8.3 of the - * MPU-6000/MPU-6050 Product Specification document. - * - * @return Current motion detection duration threshold value (LSB = 1ms) - * @see MPU6050_RA_MOT_DUR - */ -uint8_t MPU6050::getMotionDetectionDuration() { - I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DUR, buffer); - return buffer[0]; -} -/** Set motion detection event duration threshold. - * @param duration New motion detection duration threshold value (LSB = 1ms) - * @see getMotionDetectionDuration() - * @see MPU6050_RA_MOT_DUR - */ -void MPU6050::setMotionDetectionDuration(uint8_t duration) { - I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_DUR, duration); -} - -// ZRMOT_THR register - -/** Get zero motion detection event acceleration threshold. - * This register configures the detection threshold for Zero Motion interrupt - * generation. The unit of ZRMOT_THR is 1LSB = 2mg. Zero Motion is detected when - * the absolute value of the accelerometer measurements for the 3 axes are each - * less than the detection threshold. This condition increments the Zero Motion - * duration counter (Register 34). The Zero Motion interrupt is triggered when - * the Zero Motion duration counter reaches the time count specified in - * ZRMOT_DUR (Register 34). - * - * Unlike Free Fall or Motion detection, Zero Motion detection triggers an - * interrupt both when Zero Motion is first detected and when Zero Motion is no - * longer detected. - * - * When a zero motion event is detected, a Zero Motion Status will be indicated - * in the MOT_DETECT_STATUS register (Register 97). When a motion-to-zero-motion - * condition is detected, the status bit is set to 1. When a zero-motion-to- - * motion condition is detected, the status bit is set to 0. - * - * For more details on the Zero Motion detection interrupt, see Section 8.4 of - * the MPU-6000/MPU-6050 Product Specification document as well as Registers 56 - * and 58 of this document. - * - * @return Current zero motion detection acceleration threshold value (LSB = 2mg) - * @see MPU6050_RA_ZRMOT_THR - */ -uint8_t MPU6050::getZeroMotionDetectionThreshold() { - I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_THR, buffer); - return buffer[0]; -} -/** Set zero motion detection event acceleration threshold. - * @param threshold New zero motion detection acceleration threshold value (LSB = 2mg) - * @see getZeroMotionDetectionThreshold() - * @see MPU6050_RA_ZRMOT_THR - */ -void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold) { - I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_THR, threshold); -} - -// ZRMOT_DUR register - -/** Get zero motion detection event duration threshold. - * This register configures the duration counter threshold for Zero Motion - * interrupt generation. The duration counter ticks at 16 Hz, therefore - * ZRMOT_DUR has a unit of 1 LSB = 64 ms. The Zero Motion duration counter - * increments while the absolute value of the accelerometer measurements are - * each less than the detection threshold (Register 33). The Zero Motion - * interrupt is triggered when the Zero Motion duration counter reaches the time - * count specified in this register. - * - * For more details on the Zero Motion detection interrupt, see Section 8.4 of - * the MPU-6000/MPU-6050 Product Specification document, as well as Registers 56 - * and 58 of this document. - * - * @return Current zero motion detection duration threshold value (LSB = 64ms) - * @see MPU6050_RA_ZRMOT_DUR - */ -uint8_t MPU6050::getZeroMotionDetectionDuration() { - I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_DUR, buffer); - return buffer[0]; -} -/** Set zero motion detection event duration threshold. - * @param duration New zero motion detection duration threshold value (LSB = 1ms) - * @see getZeroMotionDetectionDuration() - * @see MPU6050_RA_ZRMOT_DUR - */ -void MPU6050::setZeroMotionDetectionDuration(uint8_t duration) { - I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_DUR, duration); -} - -// FIFO_EN register - -/** Get temperature FIFO enabled value. - * When set to 1, this bit enables TEMP_OUT_H and TEMP_OUT_L (Registers 65 and - * 66) to be written into the FIFO buffer. - * @return Current temperature FIFO enabled value - * @see MPU6050_RA_FIFO_EN - */ -bool MPU6050::getTempFIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, buffer); - return buffer[0]; -} -/** Set temperature FIFO enabled value. - * @param enabled New temperature FIFO enabled value - * @see getTempFIFOEnabled() - * @see MPU6050_RA_FIFO_EN - */ -void MPU6050::setTempFIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, enabled); -} -/** Get gyroscope X-axis FIFO enabled value. - * When set to 1, this bit enables GYRO_XOUT_H and GYRO_XOUT_L (Registers 67 and - * 68) to be written into the FIFO buffer. - * @return Current gyroscope X-axis FIFO enabled value - * @see MPU6050_RA_FIFO_EN - */ -bool MPU6050::getXGyroFIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, buffer); - return buffer[0]; -} -/** Set gyroscope X-axis FIFO enabled value. - * @param enabled New gyroscope X-axis FIFO enabled value - * @see getXGyroFIFOEnabled() - * @see MPU6050_RA_FIFO_EN - */ -void MPU6050::setXGyroFIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, enabled); -} -/** Get gyroscope Y-axis FIFO enabled value. - * When set to 1, this bit enables GYRO_YOUT_H and GYRO_YOUT_L (Registers 69 and - * 70) to be written into the FIFO buffer. - * @return Current gyroscope Y-axis FIFO enabled value - * @see MPU6050_RA_FIFO_EN - */ -bool MPU6050::getYGyroFIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, buffer); - return buffer[0]; -} -/** Set gyroscope Y-axis FIFO enabled value. - * @param enabled New gyroscope Y-axis FIFO enabled value - * @see getYGyroFIFOEnabled() - * @see MPU6050_RA_FIFO_EN - */ -void MPU6050::setYGyroFIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, enabled); -} -/** Get gyroscope Z-axis FIFO enabled value. - * When set to 1, this bit enables GYRO_ZOUT_H and GYRO_ZOUT_L (Registers 71 and - * 72) to be written into the FIFO buffer. - * @return Current gyroscope Z-axis FIFO enabled value - * @see MPU6050_RA_FIFO_EN - */ -bool MPU6050::getZGyroFIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, buffer); - return buffer[0]; -} -/** Set gyroscope Z-axis FIFO enabled value. - * @param enabled New gyroscope Z-axis FIFO enabled value - * @see getZGyroFIFOEnabled() - * @see MPU6050_RA_FIFO_EN - */ -void MPU6050::setZGyroFIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, enabled); -} -/** Get accelerometer FIFO enabled value. - * When set to 1, this bit enables ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H, - * ACCEL_YOUT_L, ACCEL_ZOUT_H, and ACCEL_ZOUT_L (Registers 59 to 64) to be - * written into the FIFO buffer. - * @return Current accelerometer FIFO enabled value - * @see MPU6050_RA_FIFO_EN - */ -bool MPU6050::getAccelFIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, buffer); - return buffer[0]; -} -/** Set accelerometer FIFO enabled value. - * @param enabled New accelerometer FIFO enabled value - * @see getAccelFIFOEnabled() - * @see MPU6050_RA_FIFO_EN - */ -void MPU6050::setAccelFIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, enabled); -} -/** Get Slave 2 FIFO enabled value. - * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) - * associated with Slave 2 to be written into the FIFO buffer. - * @return Current Slave 2 FIFO enabled value - * @see MPU6050_RA_FIFO_EN - */ -bool MPU6050::getSlave2FIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, buffer); - return buffer[0]; -} -/** Set Slave 2 FIFO enabled value. - * @param enabled New Slave 2 FIFO enabled value - * @see getSlave2FIFOEnabled() - * @see MPU6050_RA_FIFO_EN - */ -void MPU6050::setSlave2FIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, enabled); -} -/** Get Slave 1 FIFO enabled value. - * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) - * associated with Slave 1 to be written into the FIFO buffer. - * @return Current Slave 1 FIFO enabled value - * @see MPU6050_RA_FIFO_EN - */ -bool MPU6050::getSlave1FIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, buffer); - return buffer[0]; -} -/** Set Slave 1 FIFO enabled value. - * @param enabled New Slave 1 FIFO enabled value - * @see getSlave1FIFOEnabled() - * @see MPU6050_RA_FIFO_EN - */ -void MPU6050::setSlave1FIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, enabled); -} -/** Get Slave 0 FIFO enabled value. - * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) - * associated with Slave 0 to be written into the FIFO buffer. - * @return Current Slave 0 FIFO enabled value - * @see MPU6050_RA_FIFO_EN - */ -bool MPU6050::getSlave0FIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, buffer); - return buffer[0]; -} -/** Set Slave 0 FIFO enabled value. - * @param enabled New Slave 0 FIFO enabled value - * @see getSlave0FIFOEnabled() - * @see MPU6050_RA_FIFO_EN - */ -void MPU6050::setSlave0FIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, enabled); -} - -// I2C_MST_CTRL register - -/** Get multi-master enabled value. - * Multi-master capability allows multiple I2C masters to operate on the same - * bus. In circuits where multi-master capability is required, set MULT_MST_EN - * to 1. This will increase current drawn by approximately 30uA. - * - * In circuits where multi-master capability is required, the state of the I2C - * bus must always be monitored by each separate I2C Master. Before an I2C - * Master can assume arbitration of the bus, it must first confirm that no other - * I2C Master has arbitration of the bus. When MULT_MST_EN is set to 1, the - * MPU-60X0's bus arbitration detection logic is turned on, enabling it to - * detect when the bus is available. - * - * @return Current multi-master enabled value - * @see MPU6050_RA_I2C_MST_CTRL - */ -bool MPU6050::getMultiMasterEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, buffer); - return buffer[0]; -} -/** Set multi-master enabled value. - * @param enabled New multi-master enabled value - * @see getMultiMasterEnabled() - * @see MPU6050_RA_I2C_MST_CTRL - */ -void MPU6050::setMultiMasterEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, enabled); -} -/** Get wait-for-external-sensor-data enabled value. - * When the WAIT_FOR_ES bit is set to 1, the Data Ready interrupt will be - * delayed until External Sensor data from the Slave Devices are loaded into the - * EXT_SENS_DATA registers. This is used to ensure that both the internal sensor - * data (i.e. from gyro and accel) and external sensor data have been loaded to - * their respective data registers (i.e. the data is synced) when the Data Ready - * interrupt is triggered. - * - * @return Current wait-for-external-sensor-data enabled value - * @see MPU6050_RA_I2C_MST_CTRL - */ -bool MPU6050::getWaitForExternalSensorEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, buffer); - return buffer[0]; -} -/** Set wait-for-external-sensor-data enabled value. - * @param enabled New wait-for-external-sensor-data enabled value - * @see getWaitForExternalSensorEnabled() - * @see MPU6050_RA_I2C_MST_CTRL - */ -void MPU6050::setWaitForExternalSensorEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, enabled); -} -/** Get Slave 3 FIFO enabled value. - * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) - * associated with Slave 3 to be written into the FIFO buffer. - * @return Current Slave 3 FIFO enabled value - * @see MPU6050_RA_MST_CTRL - */ -bool MPU6050::getSlave3FIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, buffer); - return buffer[0]; -} -/** Set Slave 3 FIFO enabled value. - * @param enabled New Slave 3 FIFO enabled value - * @see getSlave3FIFOEnabled() - * @see MPU6050_RA_MST_CTRL - */ -void MPU6050::setSlave3FIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, enabled); -} -/** Get slave read/write transition enabled value. - * The I2C_MST_P_NSR bit configures the I2C Master's transition from one slave - * read to the next slave read. If the bit equals 0, there will be a restart - * between reads. If the bit equals 1, there will be a stop followed by a start - * of the following read. When a write transaction follows a read transaction, - * the stop followed by a start of the successive write will be always used. - * - * @return Current slave read/write transition enabled value - * @see MPU6050_RA_I2C_MST_CTRL - */ -bool MPU6050::getSlaveReadWriteTransitionEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, buffer); - return buffer[0]; -} -/** Set slave read/write transition enabled value. - * @param enabled New slave read/write transition enabled value - * @see getSlaveReadWriteTransitionEnabled() - * @see MPU6050_RA_I2C_MST_CTRL - */ -void MPU6050::setSlaveReadWriteTransitionEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, enabled); -} -/** Get I2C master clock speed. - * I2C_MST_CLK is a 4 bit unsigned value which configures a divider on the - * MPU-60X0 internal 8MHz clock. It sets the I2C master clock speed according to - * the following table: - * - * 
- * I2C_MST_CLK | I2C Master Clock Speed | 8MHz Clock Divider
- * ------------+------------------------+-------------------
- * 0           | 348kHz                 | 23
- * 1           | 333kHz                 | 24
- * 2           | 320kHz                 | 25
- * 3           | 308kHz                 | 26
- * 4           | 296kHz                 | 27
- * 5           | 286kHz                 | 28
- * 6           | 276kHz                 | 29
- * 7           | 267kHz                 | 30
- * 8           | 258kHz                 | 31
- * 9           | 500kHz                 | 16
- * 10          | 471kHz                 | 17
- * 11          | 444kHz                 | 18
- * 12          | 421kHz                 | 19
- * 13          | 400kHz                 | 20
- * 14          | 381kHz                 | 21
- * 15          | 364kHz                 | 22
- *
- * - * @return Current I2C master clock speed - * @see MPU6050_RA_I2C_MST_CTRL - */ -uint8_t MPU6050::getMasterClockSpeed() { - I2Cdev::readBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, buffer); - return buffer[0]; -} -/** Set I2C master clock speed. - * @reparam speed Current I2C master clock speed - * @see MPU6050_RA_I2C_MST_CTRL - */ -void MPU6050::setMasterClockSpeed(uint8_t speed) { - I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, speed); -} - -// I2C_SLV* registers (Slave 0-3) - -/** Get the I2C address of the specified slave (0-3). - * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read - * operation, and if it is cleared, then it's a write operation. The remaining - * bits (6-0) are the 7-bit device address of the slave device. - * - * In read mode, the result of the read is placed in the lowest available - * EXT_SENS_DATA register. For further information regarding the allocation of - * read results, please refer to the EXT_SENS_DATA register description - * (Registers 73 - 96). - * - * The MPU-6050 supports a total of five slaves, but Slave 4 has unique - * characteristics, and so it has its own functions (getSlave4* and setSlave4*). - * - * I2C data transactions are performed at the Sample Rate, as defined in - * Register 25. The user is responsible for ensuring that I2C data transactions - * to and from each enabled Slave can be completed within a single period of the - * Sample Rate. - * - * The I2C slave access rate can be reduced relative to the Sample Rate. This - * reduced access rate is determined by I2C_MST_DLY (Register 52). Whether a - * slave's access rate is reduced relative to the Sample Rate is determined by - * I2C_MST_DELAY_CTRL (Register 103). - * - * The processing order for the slaves is fixed. The sequence followed for - * processing the slaves is Slave 0, Slave 1, Slave 2, Slave 3 and Slave 4. If a - * particular Slave is disabled it will be skipped. - * - * Each slave can either be accessed at the sample rate or at a reduced sample - * rate. In a case where some slaves are accessed at the Sample Rate and some - * slaves are accessed at the reduced rate, the sequence of accessing the slaves - * (Slave 0 to Slave 4) is still followed. However, the reduced rate slaves will - * be skipped if their access rate dictates that they should not be accessed - * during that particular cycle. For further information regarding the reduced - * access rate, please refer to Register 52. Whether a slave is accessed at the - * Sample Rate or at the reduced rate is determined by the Delay Enable bits in - * Register 103. - * - * @param num Slave number (0-3) - * @return Current address for specified slave - * @see MPU6050_RA_I2C_SLV0_ADDR - */ -uint8_t MPU6050::getSlaveAddress(uint8_t num) { - if (num > 3) return 0; - I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, buffer); - return buffer[0]; -} -/** Set the I2C address of the specified slave (0-3). - * @param num Slave number (0-3) - * @param address New address for specified slave - * @see getSlaveAddress() - * @see MPU6050_RA_I2C_SLV0_ADDR - */ -void MPU6050::setSlaveAddress(uint8_t num, uint8_t address) { - if (num > 3) return; - I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, address); -} -/** Get the active internal register for the specified slave (0-3). - * Read/write operations for this slave will be done to whatever internal - * register address is stored in this MPU register. - * - * The MPU-6050 supports a total of five slaves, but Slave 4 has unique - * characteristics, and so it has its own functions. - * - * @param num Slave number (0-3) - * @return Current active register for specified slave - * @see MPU6050_RA_I2C_SLV0_REG - */ -uint8_t MPU6050::getSlaveRegister(uint8_t num) { - if (num > 3) return 0; - I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, buffer); - return buffer[0]; -} -/** Set the active internal register for the specified slave (0-3). - * @param num Slave number (0-3) - * @param reg New active register for specified slave - * @see getSlaveRegister() - * @see MPU6050_RA_I2C_SLV0_REG - */ -void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg) { - if (num > 3) return; - I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, reg); -} -/** Get the enabled value for the specified slave (0-3). - * When set to 1, this bit enables Slave 0 for data transfer operations. When - * cleared to 0, this bit disables Slave 0 from data transfer operations. - * @param num Slave number (0-3) - * @return Current enabled value for specified slave - * @see MPU6050_RA_I2C_SLV0_CTRL - */ -bool MPU6050::getSlaveEnabled(uint8_t num) { - if (num > 3) return 0; - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, buffer); - return buffer[0]; -} -/** Set the enabled value for the specified slave (0-3). - * @param num Slave number (0-3) - * @param enabled New enabled value for specified slave - * @see getSlaveEnabled() - * @see MPU6050_RA_I2C_SLV0_CTRL - */ -void MPU6050::setSlaveEnabled(uint8_t num, bool enabled) { - if (num > 3) return; - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, enabled); -} -/** Get word pair byte-swapping enabled for the specified slave (0-3). - * When set to 1, this bit enables byte swapping. When byte swapping is enabled, - * the high and low bytes of a word pair are swapped. Please refer to - * I2C_SLV0_GRP for the pairing convention of the word pairs. When cleared to 0, - * bytes transferred to and from Slave 0 will be written to EXT_SENS_DATA - * registers in the order they were transferred. - * - * @param num Slave number (0-3) - * @return Current word pair byte-swapping enabled value for specified slave - * @see MPU6050_RA_I2C_SLV0_CTRL - */ -bool MPU6050::getSlaveWordByteSwap(uint8_t num) { - if (num > 3) return 0; - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, buffer); - return buffer[0]; -} -/** Set word pair byte-swapping enabled for the specified slave (0-3). - * @param num Slave number (0-3) - * @param enabled New word pair byte-swapping enabled value for specified slave - * @see getSlaveWordByteSwap() - * @see MPU6050_RA_I2C_SLV0_CTRL - */ -void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled) { - if (num > 3) return; - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled); -} -/** Get write mode for the specified slave (0-3). - * When set to 1, the transaction will read or write data only. When cleared to - * 0, the transaction will write a register address prior to reading or writing - * data. This should equal 0 when specifying the register address within the - * Slave device to/from which the ensuing data transaction will take place. - * - * @param num Slave number (0-3) - * @return Current write mode for specified slave (0 = register address + data, 1 = data only) - * @see MPU6050_RA_I2C_SLV0_CTRL - */ -bool MPU6050::getSlaveWriteMode(uint8_t num) { - if (num > 3) return 0; - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, buffer); - return buffer[0]; -} -/** Set write mode for the specified slave (0-3). - * @param num Slave number (0-3) - * @param mode New write mode for specified slave (0 = register address + data, 1 = data only) - * @see getSlaveWriteMode() - * @see MPU6050_RA_I2C_SLV0_CTRL - */ -void MPU6050::setSlaveWriteMode(uint8_t num, bool mode) { - if (num > 3) return; - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, mode); -} -/** Get word pair grouping order offset for the specified slave (0-3). - * This sets specifies the grouping order of word pairs received from registers. - * When cleared to 0, bytes from register addresses 0 and 1, 2 and 3, etc (even, - * then odd register addresses) are paired to form a word. When set to 1, bytes - * from register addresses are paired 1 and 2, 3 and 4, etc. (odd, then even - * register addresses) are paired to form a word. - * - * @param num Slave number (0-3) - * @return Current word pair grouping order offset for specified slave - * @see MPU6050_RA_I2C_SLV0_CTRL - */ -bool MPU6050::getSlaveWordGroupOffset(uint8_t num) { - if (num > 3) return 0; - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, buffer); - return buffer[0]; -} -/** Set word pair grouping order offset for the specified slave (0-3). - * @param num Slave number (0-3) - * @param enabled New word pair grouping order offset for specified slave - * @see getSlaveWordGroupOffset() - * @see MPU6050_RA_I2C_SLV0_CTRL - */ -void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled) { - if (num > 3) return; - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, enabled); -} -/** Get number of bytes to read for the specified slave (0-3). - * Specifies the number of bytes transferred to and from Slave 0. Clearing this - * bit to 0 is equivalent to disabling the register by writing 0 to I2C_SLV0_EN. - * @param num Slave number (0-3) - * @return Number of bytes to read for specified slave - * @see MPU6050_RA_I2C_SLV0_CTRL - */ -uint8_t MPU6050::getSlaveDataLength(uint8_t num) { - if (num > 3) return 0; - I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, buffer); - return buffer[0]; -} -/** Set number of bytes to read for the specified slave (0-3). - * @param num Slave number (0-3) - * @param length Number of bytes to read for specified slave - * @see getSlaveDataLength() - * @see MPU6050_RA_I2C_SLV0_CTRL - */ -void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length) { - if (num > 3) return; - I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, length); -} - -// I2C_SLV* registers (Slave 4) - -/** Get the I2C address of Slave 4. - * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read - * operation, and if it is cleared, then it's a write operation. The remaining - * bits (6-0) are the 7-bit device address of the slave device. - * - * @return Current address for Slave 4 - * @see getSlaveAddress() - * @see MPU6050_RA_I2C_SLV4_ADDR - */ -uint8_t MPU6050::getSlave4Address() { - I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, buffer); - return buffer[0]; -} -/** Set the I2C address of Slave 4. - * @param address New address for Slave 4 - * @see getSlave4Address() - * @see MPU6050_RA_I2C_SLV4_ADDR - */ -void MPU6050::setSlave4Address(uint8_t address) { - I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, address); -} -/** Get the active internal register for the Slave 4. - * Read/write operations for this slave will be done to whatever internal - * register address is stored in this MPU register. - * - * @return Current active register for Slave 4 - * @see MPU6050_RA_I2C_SLV4_REG - */ -uint8_t MPU6050::getSlave4Register() { - I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_REG, buffer); - return buffer[0]; -} -/** Set the active internal register for Slave 4. - * @param reg New active register for Slave 4 - * @see getSlave4Register() - * @see MPU6050_RA_I2C_SLV4_REG - */ -void MPU6050::setSlave4Register(uint8_t reg) { - I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_REG, reg); -} -/** Set new byte to write to Slave 4. - * This register stores the data to be written into the Slave 4. If I2C_SLV4_RW - * is set 1 (set to read), this register has no effect. - * @param data New byte to write to Slave 4 - * @see MPU6050_RA_I2C_SLV4_DO - */ -void MPU6050::setSlave4OutputByte(uint8_t data) { - I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_DO, data); -} -/** Get the enabled value for the Slave 4. - * When set to 1, this bit enables Slave 4 for data transfer operations. When - * cleared to 0, this bit disables Slave 4 from data transfer operations. - * @return Current enabled value for Slave 4 - * @see MPU6050_RA_I2C_SLV4_CTRL - */ -bool MPU6050::getSlave4Enabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, buffer); - return buffer[0]; -} -/** Set the enabled value for Slave 4. - * @param enabled New enabled value for Slave 4 - * @see getSlave4Enabled() - * @see MPU6050_RA_I2C_SLV4_CTRL - */ -void MPU6050::setSlave4Enabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, enabled); -} -/** Get the enabled value for Slave 4 transaction interrupts. - * When set to 1, this bit enables the generation of an interrupt signal upon - * completion of a Slave 4 transaction. When cleared to 0, this bit disables the - * generation of an interrupt signal upon completion of a Slave 4 transaction. - * The interrupt status can be observed in Register 54. - * - * @return Current enabled value for Slave 4 transaction interrupts. - * @see MPU6050_RA_I2C_SLV4_CTRL - */ -bool MPU6050::getSlave4InterruptEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, buffer); - return buffer[0]; -} -/** Set the enabled value for Slave 4 transaction interrupts. - * @param enabled New enabled value for Slave 4 transaction interrupts. - * @see getSlave4InterruptEnabled() - * @see MPU6050_RA_I2C_SLV4_CTRL - */ -void MPU6050::setSlave4InterruptEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, enabled); -} -/** Get write mode for Slave 4. - * When set to 1, the transaction will read or write data only. When cleared to - * 0, the transaction will write a register address prior to reading or writing - * data. This should equal 0 when specifying the register address within the - * Slave device to/from which the ensuing data transaction will take place. - * - * @return Current write mode for Slave 4 (0 = register address + data, 1 = data only) - * @see MPU6050_RA_I2C_SLV4_CTRL - */ -bool MPU6050::getSlave4WriteMode() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, buffer); - return buffer[0]; -} -/** Set write mode for the Slave 4. - * @param mode New write mode for Slave 4 (0 = register address + data, 1 = data only) - * @see getSlave4WriteMode() - * @see MPU6050_RA_I2C_SLV4_CTRL - */ -void MPU6050::setSlave4WriteMode(bool mode) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mode); -} -/** Get Slave 4 master delay value. - * This configures the reduced access rate of I2C slaves relative to the Sample - * Rate. When a slave's access rate is decreased relative to the Sample Rate, - * the slave is accessed every: - * - * 1 / (1 + I2C_MST_DLY) samples - * - * This base Sample Rate in turn is determined by SMPLRT_DIV (register 25) and - * DLPF_CFG (register 26). Whether a slave's access rate is reduced relative to - * the Sample Rate is determined by I2C_MST_DELAY_CTRL (register 103). For - * further information regarding the Sample Rate, please refer to register 25. - * - * @return Current Slave 4 master delay value - * @see MPU6050_RA_I2C_SLV4_CTRL - */ -uint8_t MPU6050::getSlave4MasterDelay() { - I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, buffer); - return buffer[0]; -} -/** Set Slave 4 master delay value. - * @param delay New Slave 4 master delay value - * @see getSlave4MasterDelay() - * @see MPU6050_RA_I2C_SLV4_CTRL - */ -void MPU6050::setSlave4MasterDelay(uint8_t delay) { - I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay); -} -/** Get last available byte read from Slave 4. - * This register stores the data read from Slave 4. This field is populated - * after a read transaction. - * @return Last available byte read from to Slave 4 - * @see MPU6050_RA_I2C_SLV4_DI - */ -uint8_t MPU6050::getSlate4InputByte() { - I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_DI, buffer); - return buffer[0]; -} - -// I2C_MST_STATUS register - -/** Get FSYNC interrupt status. - * This bit reflects the status of the FSYNC interrupt from an external device - * into the MPU-60X0. This is used as a way to pass an external interrupt - * through the MPU-60X0 to the host application processor. When set to 1, this - * bit will cause an interrupt if FSYNC_INT_EN is asserted in INT_PIN_CFG - * (Register 55). - * @return FSYNC interrupt status - * @see MPU6050_RA_I2C_MST_STATUS - */ -bool MPU6050::getPassthroughStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, buffer); - return buffer[0]; -} -/** Get Slave 4 transaction done status. - * Automatically sets to 1 when a Slave 4 transaction has completed. This - * triggers an interrupt if the I2C_MST_INT_EN bit in the INT_ENABLE register - * (Register 56) is asserted and if the SLV_4_DONE_INT bit is asserted in the - * I2C_SLV4_CTRL register (Register 52). - * @return Slave 4 transaction done status - * @see MPU6050_RA_I2C_MST_STATUS - */ -bool MPU6050::getSlave4IsDone() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, buffer); - return buffer[0]; -} -/** Get master arbitration lost status. - * This bit automatically sets to 1 when the I2C Master has lost arbitration of - * the auxiliary I2C bus (an error condition). This triggers an interrupt if the - * I2C_MST_INT_EN bit in the INT_ENABLE register (Register 56) is asserted. - * @return Master arbitration lost status - * @see MPU6050_RA_I2C_MST_STATUS - */ -bool MPU6050::getLostArbitration() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, buffer); - return buffer[0]; -} -/** Get Slave 4 NACK status. - * This bit automatically sets to 1 when the I2C Master receives a NACK in a - * transaction with Slave 4. This triggers an interrupt if the I2C_MST_INT_EN - * bit in the INT_ENABLE register (Register 56) is asserted. - * @return Slave 4 NACK interrupt status - * @see MPU6050_RA_I2C_MST_STATUS - */ -bool MPU6050::getSlave4Nack() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, buffer); - return buffer[0]; -} -/** Get Slave 3 NACK status. - * This bit automatically sets to 1 when the I2C Master receives a NACK in a - * transaction with Slave 3. This triggers an interrupt if the I2C_MST_INT_EN - * bit in the INT_ENABLE register (Register 56) is asserted. - * @return Slave 3 NACK interrupt status - * @see MPU6050_RA_I2C_MST_STATUS - */ -bool MPU6050::getSlave3Nack() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, buffer); - return buffer[0]; -} -/** Get Slave 2 NACK status. - * This bit automatically sets to 1 when the I2C Master receives a NACK in a - * transaction with Slave 2. This triggers an interrupt if the I2C_MST_INT_EN - * bit in the INT_ENABLE register (Register 56) is asserted. - * @return Slave 2 NACK interrupt status - * @see MPU6050_RA_I2C_MST_STATUS - */ -bool MPU6050::getSlave2Nack() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, buffer); - return buffer[0]; -} -/** Get Slave 1 NACK status. - * This bit automatically sets to 1 when the I2C Master receives a NACK in a - * transaction with Slave 1. This triggers an interrupt if the I2C_MST_INT_EN - * bit in the INT_ENABLE register (Register 56) is asserted. - * @return Slave 1 NACK interrupt status - * @see MPU6050_RA_I2C_MST_STATUS - */ -bool MPU6050::getSlave1Nack() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, buffer); - return buffer[0]; -} -/** Get Slave 0 NACK status. - * This bit automatically sets to 1 when the I2C Master receives a NACK in a - * transaction with Slave 0. This triggers an interrupt if the I2C_MST_INT_EN - * bit in the INT_ENABLE register (Register 56) is asserted. - * @return Slave 0 NACK interrupt status - * @see MPU6050_RA_I2C_MST_STATUS - */ -bool MPU6050::getSlave0Nack() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, buffer); - return buffer[0]; -} - -// INT_PIN_CFG register - -/** Get interrupt logic level mode. - * Will be set 0 for active-high, 1 for active-low. - * @return Current interrupt mode (0=active-high, 1=active-low) - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_INT_LEVEL_BIT - */ -bool MPU6050::getInterruptMode() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, buffer); - return buffer[0]; -} -/** Set interrupt logic level mode. - * @param mode New interrupt mode (0=active-high, 1=active-low) - * @see getInterruptMode() - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_INT_LEVEL_BIT - */ -void MPU6050::setInterruptMode(bool mode) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mode); -} -/** Get interrupt drive mode. - * Will be set 0 for push-pull, 1 for open-drain. - * @return Current interrupt drive mode (0=push-pull, 1=open-drain) - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_INT_OPEN_BIT - */ -bool MPU6050::getInterruptDrive() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, buffer); - return buffer[0]; -} -/** Set interrupt drive mode. - * @param drive New interrupt drive mode (0=push-pull, 1=open-drain) - * @see getInterruptDrive() - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_INT_OPEN_BIT - */ -void MPU6050::setInterruptDrive(bool drive) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, drive); -} -/** Get interrupt latch mode. - * Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared. - * @return Current latch mode (0=50us-pulse, 1=latch-until-int-cleared) - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_LATCH_INT_EN_BIT - */ -bool MPU6050::getInterruptLatch() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, buffer); - return buffer[0]; -} -/** Set interrupt latch mode. - * @param latch New latch mode (0=50us-pulse, 1=latch-until-int-cleared) - * @see getInterruptLatch() - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_LATCH_INT_EN_BIT - */ -void MPU6050::setInterruptLatch(bool latch) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, latch); -} -/** Get interrupt latch clear mode. - * Will be set 0 for status-read-only, 1 for any-register-read. - * @return Current latch clear mode (0=status-read-only, 1=any-register-read) - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT - */ -bool MPU6050::getInterruptLatchClear() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, buffer); - return buffer[0]; -} -/** Set interrupt latch clear mode. - * @param clear New latch clear mode (0=status-read-only, 1=any-register-read) - * @see getInterruptLatchClear() - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT - */ -void MPU6050::setInterruptLatchClear(bool clear) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear); -} -/** Get FSYNC interrupt logic level mode. - * @return Current FSYNC interrupt mode (0=active-high, 1=active-low) - * @see getFSyncInterruptMode() - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT - */ -bool MPU6050::getFSyncInterruptLevel() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, buffer); - return buffer[0]; -} -/** Set FSYNC interrupt logic level mode. - * @param mode New FSYNC interrupt mode (0=active-high, 1=active-low) - * @see getFSyncInterruptMode() - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT - */ -void MPU6050::setFSyncInterruptLevel(bool level) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level); -} -/** Get FSYNC pin interrupt enabled setting. - * Will be set 0 for disabled, 1 for enabled. - * @return Current interrupt enabled setting - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT - */ -bool MPU6050::getFSyncInterruptEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, buffer); - return buffer[0]; -} -/** Set FSYNC pin interrupt enabled setting. - * @param enabled New FSYNC pin interrupt enabled setting - * @see getFSyncInterruptEnabled() - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT - */ -void MPU6050::setFSyncInterruptEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled); -} -/** Get I2C bypass enabled status. - * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to - * 0, the host application processor will be able to directly access the - * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host - * application processor will not be able to directly access the auxiliary I2C - * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106 - * bit[5]). - * @return Current I2C bypass enabled status - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT - */ -bool MPU6050::getI2CBypassEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, buffer); - return buffer[0]; -} -/** Set I2C bypass enabled status. - * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to - * 0, the host application processor will be able to directly access the - * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host - * application processor will not be able to directly access the auxiliary I2C - * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106 - * bit[5]). - * @param enabled New I2C bypass enabled status - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT - */ -void MPU6050::setI2CBypassEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled); -} -/** Get reference clock output enabled status. - * When this bit is equal to 1, a reference clock output is provided at the - * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For - * further information regarding CLKOUT, please refer to the MPU-60X0 Product - * Specification document. - * @return Current reference clock output enabled status - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_CLKOUT_EN_BIT - */ -bool MPU6050::getClockOutputEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, buffer); - return buffer[0]; -} -/** Set reference clock output enabled status. - * When this bit is equal to 1, a reference clock output is provided at the - * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For - * further information regarding CLKOUT, please refer to the MPU-60X0 Product - * Specification document. - * @param enabled New reference clock output enabled status - * @see MPU6050_RA_INT_PIN_CFG - * @see MPU6050_INTCFG_CLKOUT_EN_BIT - */ -void MPU6050::setClockOutputEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled); -} - -// INT_ENABLE register - -/** Get full interrupt enabled status. - * Full register byte for all interrupts, for quick reading. Each bit will be - * set 0 for disabled, 1 for enabled. - * @return Current interrupt enabled status - * @see MPU6050_RA_INT_ENABLE - * @see MPU6050_INTERRUPT_FF_BIT - **/ -uint8_t MPU6050::getIntEnabled() { - I2Cdev::readByte(devAddr, MPU6050_RA_INT_ENABLE, buffer); - return buffer[0]; -} -/** Set full interrupt enabled status. - * Full register byte for all interrupts, for quick reading. Each bit should be - * set 0 for disabled, 1 for enabled. - * @param enabled New interrupt enabled status - * @see getIntFreefallEnabled() - * @see MPU6050_RA_INT_ENABLE - * @see MPU6050_INTERRUPT_FF_BIT - **/ -void MPU6050::setIntEnabled(uint8_t enabled) { - I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, enabled); -} -/** Get Free Fall interrupt enabled status. - * Will be set 0 for disabled, 1 for enabled. - * @return Current interrupt enabled status - * @see MPU6050_RA_INT_ENABLE - * @see MPU6050_INTERRUPT_FF_BIT - **/ -bool MPU6050::getIntFreefallEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, buffer); - return buffer[0]; -} -/** Set Free Fall interrupt enabled status. - * @param enabled New interrupt enabled status - * @see getIntFreefallEnabled() - * @see MPU6050_RA_INT_ENABLE - * @see MPU6050_INTERRUPT_FF_BIT - **/ -void MPU6050::setIntFreefallEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, enabled); -} -/** Get Motion Detection interrupt enabled status. - * Will be set 0 for disabled, 1 for enabled. - * @return Current interrupt enabled status - * @see MPU6050_RA_INT_ENABLE - * @see MPU6050_INTERRUPT_MOT_BIT - **/ -bool MPU6050::getIntMotionEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, buffer); - return buffer[0]; -} -/** Set Motion Detection interrupt enabled status. - * @param enabled New interrupt enabled status - * @see getIntMotionEnabled() - * @see MPU6050_RA_INT_ENABLE - * @see MPU6050_INTERRUPT_MOT_BIT - **/ -void MPU6050::setIntMotionEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, enabled); -} -/** Get Zero Motion Detection interrupt enabled status. - * Will be set 0 for disabled, 1 for enabled. - * @return Current interrupt enabled status - * @see MPU6050_RA_INT_ENABLE - * @see MPU6050_INTERRUPT_ZMOT_BIT - **/ -bool MPU6050::getIntZeroMotionEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, buffer); - return buffer[0]; -} -/** Set Zero Motion Detection interrupt enabled status. - * @param enabled New interrupt enabled status - * @see getIntZeroMotionEnabled() - * @see MPU6050_RA_INT_ENABLE - * @see MPU6050_INTERRUPT_ZMOT_BIT - **/ -void MPU6050::setIntZeroMotionEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, enabled); -} -/** Get FIFO Buffer Overflow interrupt enabled status. - * Will be set 0 for disabled, 1 for enabled. - * @return Current interrupt enabled status - * @see MPU6050_RA_INT_ENABLE - * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT - **/ -bool MPU6050::getIntFIFOBufferOverflowEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer); - return buffer[0]; -} -/** Set FIFO Buffer Overflow interrupt enabled status. - * @param enabled New interrupt enabled status - * @see getIntFIFOBufferOverflowEnabled() - * @see MPU6050_RA_INT_ENABLE - * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT - **/ -void MPU6050::setIntFIFOBufferOverflowEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled); -} -/** Get I2C Master interrupt enabled status. - * This enables any of the I2C Master interrupt sources to generate an - * interrupt. Will be set 0 for disabled, 1 for enabled. - * @return Current interrupt enabled status - * @see MPU6050_RA_INT_ENABLE - * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT - **/ -bool MPU6050::getIntI2CMasterEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer); - return buffer[0]; -} -/** Set I2C Master interrupt enabled status. - * @param enabled New interrupt enabled status - * @see getIntI2CMasterEnabled() - * @see MPU6050_RA_INT_ENABLE - * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT - **/ -void MPU6050::setIntI2CMasterEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, enabled); -} -/** Get Data Ready interrupt enabled setting. - * This event occurs each time a write operation to all of the sensor registers - * has been completed. Will be set 0 for disabled, 1 for enabled. - * @return Current interrupt enabled status - * @see MPU6050_RA_INT_ENABLE - * @see MPU6050_INTERRUPT_DATA_RDY_BIT - */ -bool MPU6050::getIntDataReadyEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer); - return buffer[0]; -} -/** Set Data Ready interrupt enabled status. - * @param enabled New interrupt enabled status - * @see getIntDataReadyEnabled() - * @see MPU6050_RA_INT_CFG - * @see MPU6050_INTERRUPT_DATA_RDY_BIT - */ -void MPU6050::setIntDataReadyEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, enabled); -} - -// INT_STATUS register - -/** Get full set of interrupt status bits. - * These bits clear to 0 after the register has been read. Very useful - * for getting multiple INT statuses, since each single bit read clears - * all of them because it has to read the whole byte. - * @return Current interrupt status - * @see MPU6050_RA_INT_STATUS - */ -uint8_t MPU6050::getIntStatus() { - I2Cdev::readByte(devAddr, MPU6050_RA_INT_STATUS, buffer); - return buffer[0]; -} -/** Get Free Fall interrupt status. - * This bit automatically sets to 1 when a Free Fall interrupt has been - * generated. The bit clears to 0 after the register has been read. - * @return Current interrupt status - * @see MPU6050_RA_INT_STATUS - * @see MPU6050_INTERRUPT_FF_BIT - */ -bool MPU6050::getIntFreefallStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, buffer); - return buffer[0]; -} -/** Get Motion Detection interrupt status. - * This bit automatically sets to 1 when a Motion Detection interrupt has been - * generated. The bit clears to 0 after the register has been read. - * @return Current interrupt status - * @see MPU6050_RA_INT_STATUS - * @see MPU6050_INTERRUPT_MOT_BIT - */ -bool MPU6050::getIntMotionStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, buffer); - return buffer[0]; -} -/** Get Zero Motion Detection interrupt status. - * This bit automatically sets to 1 when a Zero Motion Detection interrupt has - * been generated. The bit clears to 0 after the register has been read. - * @return Current interrupt status - * @see MPU6050_RA_INT_STATUS - * @see MPU6050_INTERRUPT_ZMOT_BIT - */ -bool MPU6050::getIntZeroMotionStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, buffer); - return buffer[0]; -} -/** Get FIFO Buffer Overflow interrupt status. - * This bit automatically sets to 1 when a Free Fall interrupt has been - * generated. The bit clears to 0 after the register has been read. - * @return Current interrupt status - * @see MPU6050_RA_INT_STATUS - * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT - */ -bool MPU6050::getIntFIFOBufferOverflowStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer); - return buffer[0]; -} -/** Get I2C Master interrupt status. - * This bit automatically sets to 1 when an I2C Master interrupt has been - * generated. For a list of I2C Master interrupts, please refer to Register 54. - * The bit clears to 0 after the register has been read. - * @return Current interrupt status - * @see MPU6050_RA_INT_STATUS - * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT - */ -bool MPU6050::getIntI2CMasterStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer); - return buffer[0]; -} -/** Get Data Ready interrupt status. - * This bit automatically sets to 1 when a Data Ready interrupt has been - * generated. The bit clears to 0 after the register has been read. - * @return Current interrupt status - * @see MPU6050_RA_INT_STATUS - * @see MPU6050_INTERRUPT_DATA_RDY_BIT - */ -bool MPU6050::getIntDataReadyStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer); - return buffer[0]; -} - -// ACCEL_*OUT_* registers - -/** Get raw 9-axis motion sensor readings (accel/gyro/compass). - * FUNCTION NOT FULLY IMPLEMENTED YET. - * @param ax 16-bit signed integer container for accelerometer X-axis value - * @param ay 16-bit signed integer container for accelerometer Y-axis value - * @param az 16-bit signed integer container for accelerometer Z-axis value - * @param gx 16-bit signed integer container for gyroscope X-axis value - * @param gy 16-bit signed integer container for gyroscope Y-axis value - * @param gz 16-bit signed integer container for gyroscope Z-axis value - * @param mx 16-bit signed integer container for magnetometer X-axis value - * @param my 16-bit signed integer container for magnetometer Y-axis value - * @param mz 16-bit signed integer container for magnetometer Z-axis value - * @see getMotion6() - * @see getAcceleration() - * @see getRotation() - * @see MPU6050_RA_ACCEL_XOUT_H - */ -void MPU6050::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) { - getMotion6(ax, ay, az, gx, gy, gz); - // TODO: magnetometer integration -} -/** Get raw 6-axis motion sensor readings (accel/gyro). - * Retrieves all currently available motion sensor values. - * @param ax 16-bit signed integer container for accelerometer X-axis value - * @param ay 16-bit signed integer container for accelerometer Y-axis value - * @param az 16-bit signed integer container for accelerometer Z-axis value - * @param gx 16-bit signed integer container for gyroscope X-axis value - * @param gy 16-bit signed integer container for gyroscope Y-axis value - * @param gz 16-bit signed integer container for gyroscope Z-axis value - * @see getAcceleration() - * @see getRotation() - * @see MPU6050_RA_ACCEL_XOUT_H - */ -void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { - I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer); - *ax = (((int16_t)buffer[0]) << 8) | buffer[1]; - *ay = (((int16_t)buffer[2]) << 8) | buffer[3]; - *az = (((int16_t)buffer[4]) << 8) | buffer[5]; - *gx = (((int16_t)buffer[8]) << 8) | buffer[9]; - *gy = (((int16_t)buffer[10]) << 8) | buffer[11]; - *gz = (((int16_t)buffer[12]) << 8) | buffer[13]; -} -/** Get 3-axis accelerometer readings. - * These registers store the most recent accelerometer measurements. - * Accelerometer measurements are written to these registers at the Sample Rate - * as defined in Register 25. - * - * The accelerometer measurement registers, along with the temperature - * measurement registers, gyroscope measurement registers, and external sensor - * data registers, are composed of two sets of registers: an internal register - * set and a user-facing read register set. - * - * The data within the accelerometer sensors' internal register set is always - * updated at the Sample Rate. Meanwhile, the user-facing read register set - * duplicates the internal register set's data values whenever the serial - * interface is idle. This guarantees that a burst read of sensor registers will - * read measurements from the same sampling instant. Note that if burst reads - * are not used, the user is responsible for ensuring a set of single byte reads - * correspond to a single sampling instant by checking the Data Ready interrupt. - * - * Each 16-bit accelerometer measurement has a full scale defined in ACCEL_FS - * (Register 28). For each full scale setting, the accelerometers' sensitivity - * per LSB in ACCEL_xOUT is shown in the table below: - * - * 
- * AFS_SEL | Full Scale Range | LSB Sensitivity
- * --------+------------------+----------------
- * 0       | +/- 2g           | 8192 LSB/mg
- * 1       | +/- 4g           | 4096 LSB/mg
- * 2       | +/- 8g           | 2048 LSB/mg
- * 3       | +/- 16g          | 1024 LSB/mg
- *
- * - * @param x 16-bit signed integer container for X-axis acceleration - * @param y 16-bit signed integer container for Y-axis acceleration - * @param z 16-bit signed integer container for Z-axis acceleration - * @see MPU6050_RA_GYRO_XOUT_H - */ -void MPU6050::getAcceleration(int16_t* x, int16_t* y, int16_t* z) { - I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buffer); - *x = (((int16_t)buffer[0]) << 8) | buffer[1]; - *y = (((int16_t)buffer[2]) << 8) | buffer[3]; - *z = (((int16_t)buffer[4]) << 8) | buffer[5]; -} -/** Get X-axis accelerometer reading. - * @return X-axis acceleration measurement in 16-bit 2's complement format - * @see getMotion6() - * @see MPU6050_RA_ACCEL_XOUT_H - */ -int16_t MPU6050::getAccelerationX() { - I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; -} -/** Get Y-axis accelerometer reading. - * @return Y-axis acceleration measurement in 16-bit 2's complement format - * @see getMotion6() - * @see MPU6050_RA_ACCEL_YOUT_H - */ -int16_t MPU6050::getAccelerationY() { - I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; -} -/** Get Z-axis accelerometer reading. - * @return Z-axis acceleration measurement in 16-bit 2's complement format - * @see getMotion6() - * @see MPU6050_RA_ACCEL_ZOUT_H - */ -int16_t MPU6050::getAccelerationZ() { - I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; -} - -// TEMP_OUT_* registers - -/** Get current internal temperature. - * @return Temperature reading in 16-bit 2's complement format - * @see MPU6050_RA_TEMP_OUT_H - */ -int16_t MPU6050::getTemperature() { - I2Cdev::readBytes(devAddr, MPU6050_RA_TEMP_OUT_H, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; -} - -// GYRO_*OUT_* registers - -/** Get 3-axis gyroscope readings. - * These gyroscope measurement registers, along with the accelerometer - * measurement registers, temperature measurement registers, and external sensor - * data registers, are composed of two sets of registers: an internal register - * set and a user-facing read register set. - * The data within the gyroscope sensors' internal register set is always - * updated at the Sample Rate. Meanwhile, the user-facing read register set - * duplicates the internal register set's data values whenever the serial - * interface is idle. This guarantees that a burst read of sensor registers will - * read measurements from the same sampling instant. Note that if burst reads - * are not used, the user is responsible for ensuring a set of single byte reads - * correspond to a single sampling instant by checking the Data Ready interrupt. - * - * Each 16-bit gyroscope measurement has a full scale defined in FS_SEL - * (Register 27). For each full scale setting, the gyroscopes' sensitivity per - * LSB in GYRO_xOUT is shown in the table below: - * - * 
- * FS_SEL | Full Scale Range   | LSB Sensitivity
- * -------+--------------------+----------------
- * 0      | +/- 250 degrees/s  | 131 LSB/deg/s
- * 1      | +/- 500 degrees/s  | 65.5 LSB/deg/s
- * 2      | +/- 1000 degrees/s | 32.8 LSB/deg/s
- * 3      | +/- 2000 degrees/s | 16.4 LSB/deg/s
- *
- * - * @param x 16-bit signed integer container for X-axis rotation - * @param y 16-bit signed integer container for Y-axis rotation - * @param z 16-bit signed integer container for Z-axis rotation - * @see getMotion6() - * @see MPU6050_RA_GYRO_XOUT_H - */ -void MPU6050::getRotation(int16_t* x, int16_t* y, int16_t* z) { - I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer); - *x = (((int16_t)buffer[0]) << 8) | buffer[1]; - *y = (((int16_t)buffer[2]) << 8) | buffer[3]; - *z = (((int16_t)buffer[4]) << 8) | buffer[5]; -} -/** Get X-axis gyroscope reading. - * @return X-axis rotation measurement in 16-bit 2's complement format - * @see getMotion6() - * @see MPU6050_RA_GYRO_XOUT_H - */ -int16_t MPU6050::getRotationX() { - I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; -} -/** Get Y-axis gyroscope reading. - * @return Y-axis rotation measurement in 16-bit 2's complement format - * @see getMotion6() - * @see MPU6050_RA_GYRO_YOUT_H - */ -int16_t MPU6050::getRotationY() { - I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_YOUT_H, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; -} -/** Get Z-axis gyroscope reading. - * @return Z-axis rotation measurement in 16-bit 2's complement format - * @see getMotion6() - * @see MPU6050_RA_GYRO_ZOUT_H - */ -int16_t MPU6050::getRotationZ() { - I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; -} - -// EXT_SENS_DATA_* registers - -/** Read single byte from external sensor data register. - * These registers store data read from external sensors by the Slave 0, 1, 2, - * and 3 on the auxiliary I2C interface. Data read by Slave 4 is stored in - * I2C_SLV4_DI (Register 53). - * - * External sensor data is written to these registers at the Sample Rate as - * defined in Register 25. This access rate can be reduced by using the Slave - * Delay Enable registers (Register 103). - * - * External sensor data registers, along with the gyroscope measurement - * registers, accelerometer measurement registers, and temperature measurement - * registers, are composed of two sets of registers: an internal register set - * and a user-facing read register set. - * - * The data within the external sensors' internal register set is always updated - * at the Sample Rate (or the reduced access rate) whenever the serial interface - * is idle. This guarantees that a burst read of sensor registers will read - * measurements from the same sampling instant. Note that if burst reads are not - * used, the user is responsible for ensuring a set of single byte reads - * correspond to a single sampling instant by checking the Data Ready interrupt. - * - * Data is placed in these external sensor data registers according to - * I2C_SLV0_CTRL, I2C_SLV1_CTRL, I2C_SLV2_CTRL, and I2C_SLV3_CTRL (Registers 39, - * 42, 45, and 48). When more than zero bytes are read (I2C_SLVx_LEN > 0) from - * an enabled slave (I2C_SLVx_EN = 1), the slave is read at the Sample Rate (as - * defined in Register 25) or delayed rate (if specified in Register 52 and - * 103). During each Sample cycle, slave reads are performed in order of Slave - * number. If all slaves are enabled with more than zero bytes to be read, the - * order will be Slave 0, followed by Slave 1, Slave 2, and Slave 3. - * - * Each enabled slave will have EXT_SENS_DATA registers associated with it by - * number of bytes read (I2C_SLVx_LEN) in order of slave number, starting from - * EXT_SENS_DATA_00. Note that this means enabling or disabling a slave may - * change the higher numbered slaves' associated registers. Furthermore, if - * fewer total bytes are being read from the external sensors as a result of - * such a change, then the data remaining in the registers which no longer have - * an associated slave device (i.e. high numbered registers) will remain in - * these previously allocated registers unless reset. - * - * If the sum of the read lengths of all SLVx transactions exceed the number of - * available EXT_SENS_DATA registers, the excess bytes will be dropped. There - * are 24 EXT_SENS_DATA registers and hence the total read lengths between all - * the slaves cannot be greater than 24 or some bytes will be lost. - * - * Note: Slave 4's behavior is distinct from that of Slaves 0-3. For further - * information regarding the characteristics of Slave 4, please refer to - * Registers 49 to 53. - * - * EXAMPLE: - * Suppose that Slave 0 is enabled with 4 bytes to be read (I2C_SLV0_EN = 1 and - * I2C_SLV0_LEN = 4) while Slave 1 is enabled with 2 bytes to be read so that - * I2C_SLV1_EN = 1 and I2C_SLV1_LEN = 2. In such a situation, EXT_SENS_DATA _00 - * through _03 will be associated with Slave 0, while EXT_SENS_DATA _04 and 05 - * will be associated with Slave 1. If Slave 2 is enabled as well, registers - * starting from EXT_SENS_DATA_06 will be allocated to Slave 2. - * - * If Slave 2 is disabled while Slave 3 is enabled in this same situation, then - * registers starting from EXT_SENS_DATA_06 will be allocated to Slave 3 - * instead. - * - * REGISTER ALLOCATION FOR DYNAMIC DISABLE VS. NORMAL DISABLE: - * If a slave is disabled at any time, the space initially allocated to the - * slave in the EXT_SENS_DATA register, will remain associated with that slave. - * This is to avoid dynamic adjustment of the register allocation. - * - * The allocation of the EXT_SENS_DATA registers is recomputed only when (1) all - * slaves are disabled, or (2) the I2C_MST_RST bit is set (Register 106). - * - * This above is also true if one of the slaves gets NACKed and stops - * functioning. - * - * @param position Starting position (0-23) - * @return Byte read from register - */ -uint8_t MPU6050::getExternalSensorByte(int position) { - I2Cdev::readByte(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, buffer); - return buffer[0]; -} -/** Read word (2 bytes) from external sensor data registers. - * @param position Starting position (0-21) - * @return Word read from register - * @see getExternalSensorByte() - */ -uint16_t MPU6050::getExternalSensorWord(int position) { - I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, buffer); - return (((uint16_t)buffer[0]) << 8) | buffer[1]; -} -/** Read double word (4 bytes) from external sensor data registers. - * @param position Starting position (0-20) - * @return Double word read from registers - * @see getExternalSensorByte() - */ -uint32_t MPU6050::getExternalSensorDWord(int position) { - I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, buffer); - return (((uint32_t)buffer[0]) << 24) | (((uint32_t)buffer[1]) << 16) | (((uint16_t)buffer[2]) << 8) | buffer[3]; -} - -// MOT_DETECT_STATUS register - -/** Get full motion detection status register content (all bits). - * @return Motion detection status byte - * @see MPU6050_RA_MOT_DETECT_STATUS - */ -uint8_t MPU6050::getMotionStatus() { - I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DETECT_STATUS, buffer); - return buffer[0]; -} -/** Get X-axis negative motion detection interrupt status. - * @return Motion detection status - * @see MPU6050_RA_MOT_DETECT_STATUS - * @see MPU6050_MOTION_MOT_XNEG_BIT - */ -bool MPU6050::getXNegMotionDetected() { - I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, buffer); - return buffer[0]; -} -/** Get X-axis positive motion detection interrupt status. - * @return Motion detection status - * @see MPU6050_RA_MOT_DETECT_STATUS - * @see MPU6050_MOTION_MOT_XPOS_BIT - */ -bool MPU6050::getXPosMotionDetected() { - I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, buffer); - return buffer[0]; -} -/** Get Y-axis negative motion detection interrupt status. - * @return Motion detection status - * @see MPU6050_RA_MOT_DETECT_STATUS - * @see MPU6050_MOTION_MOT_YNEG_BIT - */ -bool MPU6050::getYNegMotionDetected() { - I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, buffer); - return buffer[0]; -} -/** Get Y-axis positive motion detection interrupt status. - * @return Motion detection status - * @see MPU6050_RA_MOT_DETECT_STATUS - * @see MPU6050_MOTION_MOT_YPOS_BIT - */ -bool MPU6050::getYPosMotionDetected() { - I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, buffer); - return buffer[0]; -} -/** Get Z-axis negative motion detection interrupt status. - * @return Motion detection status - * @see MPU6050_RA_MOT_DETECT_STATUS - * @see MPU6050_MOTION_MOT_ZNEG_BIT - */ -bool MPU6050::getZNegMotionDetected() { - I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, buffer); - return buffer[0]; -} -/** Get Z-axis positive motion detection interrupt status. - * @return Motion detection status - * @see MPU6050_RA_MOT_DETECT_STATUS - * @see MPU6050_MOTION_MOT_ZPOS_BIT - */ -bool MPU6050::getZPosMotionDetected() { - I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, buffer); - return buffer[0]; -} -/** Get zero motion detection interrupt status. - * @return Motion detection status - * @see MPU6050_RA_MOT_DETECT_STATUS - * @see MPU6050_MOTION_MOT_ZRMOT_BIT - */ -bool MPU6050::getZeroMotionDetected() { - I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, buffer); - return buffer[0]; -} - -// I2C_SLV*_DO register - -/** Write byte to Data Output container for specified slave. - * This register holds the output data written into Slave when Slave is set to - * write mode. For further information regarding Slave control, please - * refer to Registers 37 to 39 and immediately following. - * @param num Slave number (0-3) - * @param data Byte to write - * @see MPU6050_RA_I2C_SLV0_DO - */ -void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data) { - if (num > 3) return; - I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + num, data); -} - -// I2C_MST_DELAY_CTRL register - -/** Get external data shadow delay enabled status. - * This register is used to specify the timing of external sensor data - * shadowing. When DELAY_ES_SHADOW is set to 1, shadowing of external - * sensor data is delayed until all data has been received. - * @return Current external data shadow delay enabled status. - * @see MPU6050_RA_I2C_MST_DELAY_CTRL - * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT - */ -bool MPU6050::getExternalShadowDelayEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, buffer); - return buffer[0]; -} -/** Set external data shadow delay enabled status. - * @param enabled New external data shadow delay enabled status. - * @see getExternalShadowDelayEnabled() - * @see MPU6050_RA_I2C_MST_DELAY_CTRL - * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT - */ -void MPU6050::setExternalShadowDelayEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled); -} -/** Get slave delay enabled status. - * When a particular slave delay is enabled, the rate of access for the that - * slave device is reduced. When a slave's access rate is decreased relative to - * the Sample Rate, the slave is accessed every: - * - * 1 / (1 + I2C_MST_DLY) Samples - * - * This base Sample Rate in turn is determined by SMPLRT_DIV (register * 25) - * and DLPF_CFG (register 26). - * - * For further information regarding I2C_MST_DLY, please refer to register 52. - * For further information regarding the Sample Rate, please refer to register 25. - * - * @param num Slave number (0-4) - * @return Current slave delay enabled status. - * @see MPU6050_RA_I2C_MST_DELAY_CTRL - * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT - */ -bool MPU6050::getSlaveDelayEnabled(uint8_t num) { - // MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc. - if (num > 4) return 0; - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, buffer); - return buffer[0]; -} -/** Set slave delay enabled status. - * @param num Slave number (0-4) - * @param enabled New slave delay enabled status. - * @see MPU6050_RA_I2C_MST_DELAY_CTRL - * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT - */ -void MPU6050::setSlaveDelayEnabled(uint8_t num, bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled); -} - -// SIGNAL_PATH_RESET register - -/** Reset gyroscope signal path. - * The reset will revert the signal path analog to digital converters and - * filters to their power up configurations. - * @see MPU6050_RA_SIGNAL_PATH_RESET - * @see MPU6050_PATHRESET_GYRO_RESET_BIT - */ -void MPU6050::resetGyroscopePath() { - I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, true); -} -/** Reset accelerometer signal path. - * The reset will revert the signal path analog to digital converters and - * filters to their power up configurations. - * @see MPU6050_RA_SIGNAL_PATH_RESET - * @see MPU6050_PATHRESET_ACCEL_RESET_BIT - */ -void MPU6050::resetAccelerometerPath() { - I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, true); -} -/** Reset temperature sensor signal path. - * The reset will revert the signal path analog to digital converters and - * filters to their power up configurations. - * @see MPU6050_RA_SIGNAL_PATH_RESET - * @see MPU6050_PATHRESET_TEMP_RESET_BIT - */ -void MPU6050::resetTemperaturePath() { - I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, true); -} - -// MOT_DETECT_CTRL register - -/** Get accelerometer power-on delay. - * The accelerometer data path provides samples to the sensor registers, Motion - * detection, Zero Motion detection, and Free Fall detection modules. The - * signal path contains filters which must be flushed on wake-up with new - * samples before the detection modules begin operations. The default wake-up - * delay, of 4ms can be lengthened by up to 3ms. This additional delay is - * specified in ACCEL_ON_DELAY in units of 1 LSB = 1 ms. The user may select - * any value above zero unless instructed otherwise by InvenSense. Please refer - * to Section 8 of the MPU-6000/MPU-6050 Product Specification document for - * further information regarding the detection modules. - * @return Current accelerometer power-on delay - * @see MPU6050_RA_MOT_DETECT_CTRL - * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT - */ -uint8_t MPU6050::getAccelerometerPowerOnDelay() { - I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, buffer); - return buffer[0]; -} -/** Set accelerometer power-on delay. - * @param delay New accelerometer power-on delay (0-3) - * @see getAccelerometerPowerOnDelay() - * @see MPU6050_RA_MOT_DETECT_CTRL - * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT - */ -void MPU6050::setAccelerometerPowerOnDelay(uint8_t delay) { - I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay); -} -/** Get Free Fall detection counter decrement configuration. - * Detection is registered by the Free Fall detection module after accelerometer - * measurements meet their respective threshold conditions over a specified - * number of samples. When the threshold conditions are met, the corresponding - * detection counter increments by 1. The user may control the rate at which the - * detection counter decrements when the threshold condition is not met by - * configuring FF_COUNT. The decrement rate can be set according to the - * following table: - * - * 
- * FF_COUNT | Counter Decrement
- * ---------+------------------
- * 0        | Reset
- * 1        | 1
- * 2        | 2
- * 3        | 4
- *
- * - * When FF_COUNT is configured to 0 (reset), any non-qualifying sample will - * reset the counter to 0. For further information on Free Fall detection, - * please refer to Registers 29 to 32. - * - * @return Current decrement configuration - * @see MPU6050_RA_MOT_DETECT_CTRL - * @see MPU6050_DETECT_FF_COUNT_BIT - */ -uint8_t MPU6050::getFreefallDetectionCounterDecrement() { - I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, buffer); - return buffer[0]; -} -/** Set Free Fall detection counter decrement configuration. - * @param decrement New decrement configuration value - * @see getFreefallDetectionCounterDecrement() - * @see MPU6050_RA_MOT_DETECT_CTRL - * @see MPU6050_DETECT_FF_COUNT_BIT - */ -void MPU6050::setFreefallDetectionCounterDecrement(uint8_t decrement) { - I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, decrement); -} -/** Get Motion detection counter decrement configuration. - * Detection is registered by the Motion detection module after accelerometer - * measurements meet their respective threshold conditions over a specified - * number of samples. When the threshold conditions are met, the corresponding - * detection counter increments by 1. The user may control the rate at which the - * detection counter decrements when the threshold condition is not met by - * configuring MOT_COUNT. The decrement rate can be set according to the - * following table: - * - * 
- * MOT_COUNT | Counter Decrement
- * ----------+------------------
- * 0         | Reset
- * 1         | 1
- * 2         | 2
- * 3         | 4
- *
- * - * When MOT_COUNT is configured to 0 (reset), any non-qualifying sample will - * reset the counter to 0. For further information on Motion detection, - * please refer to Registers 29 to 32. - * - */ -uint8_t MPU6050::getMotionDetectionCounterDecrement() { - I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, buffer); - return buffer[0]; -} -/** Set Motion detection counter decrement configuration. - * @param decrement New decrement configuration value - * @see getMotionDetectionCounterDecrement() - * @see MPU6050_RA_MOT_DETECT_CTRL - * @see MPU6050_DETECT_MOT_COUNT_BIT - */ -void MPU6050::setMotionDetectionCounterDecrement(uint8_t decrement) { - I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, decrement); -} - -// USER_CTRL register - -/** Get FIFO enabled status. - * When this bit is set to 0, the FIFO buffer is disabled. The FIFO buffer - * cannot be written to or read from while disabled. The FIFO buffer's state - * does not change unless the MPU-60X0 is power cycled. - * @return Current FIFO enabled status - * @see MPU6050_RA_USER_CTRL - * @see MPU6050_USERCTRL_FIFO_EN_BIT - */ -bool MPU6050::getFIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, buffer); - return buffer[0]; -} -/** Set FIFO enabled status. - * @param enabled New FIFO enabled status - * @see getFIFOEnabled() - * @see MPU6050_RA_USER_CTRL - * @see MPU6050_USERCTRL_FIFO_EN_BIT - */ -void MPU6050::setFIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled); -} -/** Get I2C Master Mode enabled status. - * When this mode is enabled, the MPU-60X0 acts as the I2C Master to the - * external sensor slave devices on the auxiliary I2C bus. When this bit is - * cleared to 0, the auxiliary I2C bus lines (AUX_DA and AUX_CL) are logically - * driven by the primary I2C bus (SDA and SCL). This is a precondition to - * enabling Bypass Mode. For further information regarding Bypass Mode, please - * refer to Register 55. - * @return Current I2C Master Mode enabled status - * @see MPU6050_RA_USER_CTRL - * @see MPU6050_USERCTRL_I2C_MST_EN_BIT - */ -bool MPU6050::getI2CMasterModeEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, buffer); - return buffer[0]; -} -/** Set I2C Master Mode enabled status. - * @param enabled New I2C Master Mode enabled status - * @see getI2CMasterModeEnabled() - * @see MPU6050_RA_USER_CTRL - * @see MPU6050_USERCTRL_I2C_MST_EN_BIT - */ -void MPU6050::setI2CMasterModeEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled); -} -/** Switch from I2C to SPI mode (MPU-6000 only) - * If this is set, the primary SPI interface will be enabled in place of the - * disabled primary I2C interface. - */ -void MPU6050::switchSPIEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled); -} -/** Reset the FIFO. - * This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This - * bit automatically clears to 0 after the reset has been triggered. - * @see MPU6050_RA_USER_CTRL - * @see MPU6050_USERCTRL_FIFO_RESET_BIT - */ -void MPU6050::resetFIFO() { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, true); -} -/** Reset the I2C Master. - * This bit resets the I2C Master when set to 1 while I2C_MST_EN equals 0. - * This bit automatically clears to 0 after the reset has been triggered. - * @see MPU6050_RA_USER_CTRL - * @see MPU6050_USERCTRL_I2C_MST_RESET_BIT - */ -void MPU6050::resetI2CMaster() { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, true); -} -/** Reset all sensor registers and signal paths. - * When set to 1, this bit resets the signal paths for all sensors (gyroscopes, - * accelerometers, and temperature sensor). This operation will also clear the - * sensor registers. This bit automatically clears to 0 after the reset has been - * triggered. - * - * When resetting only the signal path (and not the sensor registers), please - * use Register 104, SIGNAL_PATH_RESET. - * - * @see MPU6050_RA_USER_CTRL - * @see MPU6050_USERCTRL_SIG_COND_RESET_BIT - */ -void MPU6050::resetSensors() { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, true); -} - -// PWR_MGMT_1 register - -/** Trigger a full device reset. - * A small delay of ~50ms may be desirable after triggering a reset. - * @see MPU6050_RA_PWR_MGMT_1 - * @see MPU6050_PWR1_DEVICE_RESET_BIT - */ -void MPU6050::reset() { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true); -} -/** Get sleep mode status. - * Setting the SLEEP bit in the register puts the device into very low power - * sleep mode. In this mode, only the serial interface and internal registers - * remain active, allowing for a very low standby current. Clearing this bit - * puts the device back into normal mode. To save power, the individual standby - * selections for each of the gyros should be used if any gyro axis is not used - * by the application. - * @return Current sleep mode enabled status - * @see MPU6050_RA_PWR_MGMT_1 - * @see MPU6050_PWR1_SLEEP_BIT - */ -bool MPU6050::getSleepEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, buffer); - return buffer[0]; -} -/** Set sleep mode status. - * @param enabled New sleep mode enabled status - * @see getSleepEnabled() - * @see MPU6050_RA_PWR_MGMT_1 - * @see MPU6050_PWR1_SLEEP_BIT - */ -void MPU6050::setSleepEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled); -} -/** Get wake cycle enabled status. - * When this bit is set to 1 and SLEEP is disabled, the MPU-60X0 will cycle - * between sleep mode and waking up to take a single sample of data from active - * sensors at a rate determined by LP_WAKE_CTRL (register 108). - * @return Current sleep mode enabled status - * @see MPU6050_RA_PWR_MGMT_1 - * @see MPU6050_PWR1_CYCLE_BIT - */ -bool MPU6050::getWakeCycleEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, buffer); - return buffer[0]; -} -/** Set wake cycle enabled status. - * @param enabled New sleep mode enabled status - * @see getWakeCycleEnabled() - * @see MPU6050_RA_PWR_MGMT_1 - * @see MPU6050_PWR1_CYCLE_BIT - */ -void MPU6050::setWakeCycleEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled); -} -/** Get temperature sensor enabled status. - * Control the usage of the internal temperature sensor. - * - * Note: this register stores the *disabled* value, but for consistency with the - * rest of the code, the function is named and used with standard true/false - * values to indicate whether the sensor is enabled or disabled, respectively. - * - * @return Current temperature sensor enabled status - * @see MPU6050_RA_PWR_MGMT_1 - * @see MPU6050_PWR1_TEMP_DIS_BIT - */ -bool MPU6050::getTempSensorEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, buffer); - return buffer[0] == 0; // 1 is actually disabled here -} -/** Set temperature sensor enabled status. - * Note: this register stores the *disabled* value, but for consistency with the - * rest of the code, the function is named and used with standard true/false - * values to indicate whether the sensor is enabled or disabled, respectively. - * - * @param enabled New temperature sensor enabled status - * @see getTempSensorEnabled() - * @see MPU6050_RA_PWR_MGMT_1 - * @see MPU6050_PWR1_TEMP_DIS_BIT - */ -void MPU6050::setTempSensorEnabled(bool enabled) { - // 1 is actually disabled here - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled); -} -/** Get clock source setting. - * @return Current clock source setting - * @see MPU6050_RA_PWR_MGMT_1 - * @see MPU6050_PWR1_CLKSEL_BIT - * @see MPU6050_PWR1_CLKSEL_LENGTH - */ -uint8_t MPU6050::getClockSource() { - I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, buffer); - return buffer[0]; -} -/** Set clock source setting. - * An internal 8MHz oscillator, gyroscope based clock, or external sources can - * be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillator - * or an external source is chosen as the clock source, the MPU-60X0 can operate - * in low power modes with the gyroscopes disabled. - * - * Upon power up, the MPU-60X0 clock source defaults to the internal oscillator. - * However, it is highly recommended that the device be configured to use one of - * the gyroscopes (or an external clock source) as the clock reference for - * improved stability. The clock source can be selected according to the following table: - * - * 
- * CLK_SEL | Clock Source
- * --------+--------------------------------------
- * 0       | Internal oscillator
- * 1       | PLL with X Gyro reference
- * 2       | PLL with Y Gyro reference
- * 3       | PLL with Z Gyro reference
- * 4       | PLL with external 32.768kHz reference
- * 5       | PLL with external 19.2MHz reference
- * 6       | Reserved
- * 7       | Stops the clock and keeps the timing generator in reset
- *
- * - * @param source New clock source setting - * @see getClockSource() - * @see MPU6050_RA_PWR_MGMT_1 - * @see MPU6050_PWR1_CLKSEL_BIT - * @see MPU6050_PWR1_CLKSEL_LENGTH - */ -void MPU6050::setClockSource(uint8_t source) { - I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source); -} - -// PWR_MGMT_2 register - -/** Get wake frequency in Accel-Only Low Power Mode. - * The MPU-60X0 can be put into Accerlerometer Only Low Power Mode by setting - * PWRSEL to 1 in the Power Management 1 register (Register 107). In this mode, - * the device will power off all devices except for the primary I2C interface, - * waking only the accelerometer at fixed intervals to take a single - * measurement. The frequency of wake-ups can be configured with LP_WAKE_CTRL - * as shown below: - * - * 
- * LP_WAKE_CTRL | Wake-up Frequency
- * -------------+------------------
- * 0            | 1.25 Hz
- * 1            | 2.5 Hz
- * 2            | 5 Hz
- * 3            | 10 Hz
- *
- * - * For further information regarding the MPU-60X0's power modes, please refer to - * Register 107. - * - * @return Current wake frequency - * @see MPU6050_RA_PWR_MGMT_2 - */ -uint8_t MPU6050::getWakeFrequency() { - I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, buffer); - return buffer[0]; -} -/** Set wake frequency in Accel-Only Low Power Mode. - * @param frequency New wake frequency - * @see MPU6050_RA_PWR_MGMT_2 - */ -void MPU6050::setWakeFrequency(uint8_t frequency) { - I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency); -} - -/** Get X-axis accelerometer standby enabled status. - * If enabled, the X-axis will not gather or report data (or use power). - * @return Current X-axis standby enabled status - * @see MPU6050_RA_PWR_MGMT_2 - * @see MPU6050_PWR2_STBY_XA_BIT - */ -bool MPU6050::getStandbyXAccelEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, buffer); - return buffer[0]; -} -/** Set X-axis accelerometer standby enabled status. - * @param New X-axis standby enabled status - * @see getStandbyXAccelEnabled() - * @see MPU6050_RA_PWR_MGMT_2 - * @see MPU6050_PWR2_STBY_XA_BIT - */ -void MPU6050::setStandbyXAccelEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, enabled); -} -/** Get Y-axis accelerometer standby enabled status. - * If enabled, the Y-axis will not gather or report data (or use power). - * @return Current Y-axis standby enabled status - * @see MPU6050_RA_PWR_MGMT_2 - * @see MPU6050_PWR2_STBY_YA_BIT - */ -bool MPU6050::getStandbyYAccelEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, buffer); - return buffer[0]; -} -/** Set Y-axis accelerometer standby enabled status. - * @param New Y-axis standby enabled status - * @see getStandbyYAccelEnabled() - * @see MPU6050_RA_PWR_MGMT_2 - * @see MPU6050_PWR2_STBY_YA_BIT - */ -void MPU6050::setStandbyYAccelEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, enabled); -} -/** Get Z-axis accelerometer standby enabled status. - * If enabled, the Z-axis will not gather or report data (or use power). - * @return Current Z-axis standby enabled status - * @see MPU6050_RA_PWR_MGMT_2 - * @see MPU6050_PWR2_STBY_ZA_BIT - */ -bool MPU6050::getStandbyZAccelEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, buffer); - return buffer[0]; -} -/** Set Z-axis accelerometer standby enabled status. - * @param New Z-axis standby enabled status - * @see getStandbyZAccelEnabled() - * @see MPU6050_RA_PWR_MGMT_2 - * @see MPU6050_PWR2_STBY_ZA_BIT - */ -void MPU6050::setStandbyZAccelEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, enabled); -} -/** Get X-axis gyroscope standby enabled status. - * If enabled, the X-axis will not gather or report data (or use power). - * @return Current X-axis standby enabled status - * @see MPU6050_RA_PWR_MGMT_2 - * @see MPU6050_PWR2_STBY_XG_BIT - */ -bool MPU6050::getStandbyXGyroEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, buffer); - return buffer[0]; -} -/** Set X-axis gyroscope standby enabled status. - * @param New X-axis standby enabled status - * @see getStandbyXGyroEnabled() - * @see MPU6050_RA_PWR_MGMT_2 - * @see MPU6050_PWR2_STBY_XG_BIT - */ -void MPU6050::setStandbyXGyroEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, enabled); -} -/** Get Y-axis gyroscope standby enabled status. - * If enabled, the Y-axis will not gather or report data (or use power). - * @return Current Y-axis standby enabled status - * @see MPU6050_RA_PWR_MGMT_2 - * @see MPU6050_PWR2_STBY_YG_BIT - */ -bool MPU6050::getStandbyYGyroEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, buffer); - return buffer[0]; -} -/** Set Y-axis gyroscope standby enabled status. - * @param New Y-axis standby enabled status - * @see getStandbyYGyroEnabled() - * @see MPU6050_RA_PWR_MGMT_2 - * @see MPU6050_PWR2_STBY_YG_BIT - */ -void MPU6050::setStandbyYGyroEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, enabled); -} -/** Get Z-axis gyroscope standby enabled status. - * If enabled, the Z-axis will not gather or report data (or use power). - * @return Current Z-axis standby enabled status - * @see MPU6050_RA_PWR_MGMT_2 - * @see MPU6050_PWR2_STBY_ZG_BIT - */ -bool MPU6050::getStandbyZGyroEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, buffer); - return buffer[0]; -} -/** Set Z-axis gyroscope standby enabled status. - * @param New Z-axis standby enabled status - * @see getStandbyZGyroEnabled() - * @see MPU6050_RA_PWR_MGMT_2 - * @see MPU6050_PWR2_STBY_ZG_BIT - */ -void MPU6050::setStandbyZGyroEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled); -} - -// FIFO_COUNT* registers - -/** Get current FIFO buffer size. - * This value indicates the number of bytes stored in the FIFO buffer. This - * number is in turn the number of bytes that can be read from the FIFO buffer - * and it is directly proportional to the number of samples available given the - * set of sensor data bound to be stored in the FIFO (register 35 and 36). - * @return Current FIFO buffer size - */ -uint16_t MPU6050::getFIFOCount() { - I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer); - return (((uint16_t)buffer[0]) << 8) | buffer[1]; -} - -// FIFO_R_W register - -/** Get byte from FIFO buffer. - * This register is used to read and write data from the FIFO buffer. Data is - * written to the FIFO in order of register number (from lowest to highest). If - * all the FIFO enable flags (see below) are enabled and all External Sensor - * Data registers (Registers 73 to 96) are associated with a Slave device, the - * contents of registers 59 through 96 will be written in order at the Sample - * Rate. - * - * The contents of the sensor data registers (Registers 59 to 96) are written - * into the FIFO buffer when their corresponding FIFO enable flags are set to 1 - * in FIFO_EN (Register 35). An additional flag for the sensor data registers - * associated with I2C Slave 3 can be found in I2C_MST_CTRL (Register 36). - * - * If the FIFO buffer has overflowed, the status bit FIFO_OFLOW_INT is - * automatically set to 1. This bit is located in INT_STATUS (Register 58). - * When the FIFO buffer has overflowed, the oldest data will be lost and new - * data will be written to the FIFO. - * - * If the FIFO buffer is empty, reading this register will return the last byte - * that was previously read from the FIFO until new data is available. The user - * should check FIFO_COUNT to ensure that the FIFO buffer is not read when - * empty. - * - * @return Byte from FIFO buffer - */ -uint8_t MPU6050::getFIFOByte() { - I2Cdev::readByte(devAddr, MPU6050_RA_FIFO_R_W, buffer); - return buffer[0]; -} -void MPU6050::getFIFOBytes(uint8_t *data, uint8_t length) { - if(length > 0){ - I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data); - } else { - *data = 0; - } -} -/** Write byte to FIFO buffer. - * @see getFIFOByte() - * @see MPU6050_RA_FIFO_R_W - */ -void MPU6050::setFIFOByte(uint8_t data) { - I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data); -} - -// WHO_AM_I register - -/** Get Device ID. - * This register is used to verify the identity of the device (0b110100, 0x34). - * @return Device ID (6 bits only! should be 0x34) - * @see MPU6050_RA_WHO_AM_I - * @see MPU6050_WHO_AM_I_BIT - * @see MPU6050_WHO_AM_I_LENGTH - */ -uint8_t MPU6050::getDeviceID() { - I2Cdev::readBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer); - return buffer[0]; -} -/** Set Device ID. - * Write a new ID into the WHO_AM_I register (no idea why this should ever be - * necessary though). - * @param id New device ID to set. - * @see getDeviceID() - * @see MPU6050_RA_WHO_AM_I - * @see MPU6050_WHO_AM_I_BIT - * @see MPU6050_WHO_AM_I_LENGTH - */ -void MPU6050::setDeviceID(uint8_t id) { - I2Cdev::writeBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, id); -} - -// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== - -// XG_OFFS_TC register - -uint8_t MPU6050::getOTPBankValid() { - I2Cdev::readBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer); - return buffer[0]; -} -void MPU6050::setOTPBankValid(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled); -} -int8_t MPU6050::getXGyroOffsetTC() { - I2Cdev::readBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer); - return buffer[0]; -} -void MPU6050::setXGyroOffsetTC(int8_t offset) { - I2Cdev::writeBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); -} - -// YG_OFFS_TC register - -int8_t MPU6050::getYGyroOffsetTC() { - I2Cdev::readBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer); - return buffer[0]; -} -void MPU6050::setYGyroOffsetTC(int8_t offset) { - I2Cdev::writeBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); -} - -// ZG_OFFS_TC register - -int8_t MPU6050::getZGyroOffsetTC() { - I2Cdev::readBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer); - return buffer[0]; -} -void MPU6050::setZGyroOffsetTC(int8_t offset) { - I2Cdev::writeBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); -} - -// X_FINE_GAIN register - -int8_t MPU6050::getXFineGain() { - I2Cdev::readByte(devAddr, MPU6050_RA_X_FINE_GAIN, buffer); - return buffer[0]; -} -void MPU6050::setXFineGain(int8_t gain) { - I2Cdev::writeByte(devAddr, MPU6050_RA_X_FINE_GAIN, gain); -} - -// Y_FINE_GAIN register - -int8_t MPU6050::getYFineGain() { - I2Cdev::readByte(devAddr, MPU6050_RA_Y_FINE_GAIN, buffer); - return buffer[0]; -} -void MPU6050::setYFineGain(int8_t gain) { - I2Cdev::writeByte(devAddr, MPU6050_RA_Y_FINE_GAIN, gain); -} - -// Z_FINE_GAIN register - -int8_t MPU6050::getZFineGain() { - I2Cdev::readByte(devAddr, MPU6050_RA_Z_FINE_GAIN, buffer); - return buffer[0]; -} -void MPU6050::setZFineGain(int8_t gain) { - I2Cdev::writeByte(devAddr, MPU6050_RA_Z_FINE_GAIN, gain); -} - -// XA_OFFS_* registers - -int16_t MPU6050::getXAccelOffset() { - uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250 - I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; -} -void MPU6050::setXAccelOffset(int16_t offset) { - uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250 - I2Cdev::writeWord(devAddr, SaveAddress, offset); -} - -// YA_OFFS_* register - -int16_t MPU6050::getYAccelOffset() { - uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_YA_OFFS_H:0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250 - I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; -} -void MPU6050::setYAccelOffset(int16_t offset) { - uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_YA_OFFS_H:0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250 - I2Cdev::writeWord(devAddr, SaveAddress, offset); -} - -// ZA_OFFS_* register - -int16_t MPU6050::getZAccelOffset() { - uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_ZA_OFFS_H:0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250 - I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; -} -void MPU6050::setZAccelOffset(int16_t offset) { - uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_ZA_OFFS_H:0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250 - I2Cdev::writeWord(devAddr, SaveAddress, offset); -} - -// XG_OFFS_USR* registers - -int16_t MPU6050::getXGyroOffset() { - I2Cdev::readBytes(devAddr, MPU6050_RA_XG_OFFS_USRH, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; -} -void MPU6050::setXGyroOffset(int16_t offset) { - I2Cdev::writeWord(devAddr, MPU6050_RA_XG_OFFS_USRH, offset); -} - -// YG_OFFS_USR* register - -int16_t MPU6050::getYGyroOffset() { - I2Cdev::readBytes(devAddr, MPU6050_RA_YG_OFFS_USRH, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; -} -void MPU6050::setYGyroOffset(int16_t offset) { - I2Cdev::writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset); -} - -// ZG_OFFS_USR* register - -int16_t MPU6050::getZGyroOffset() { - I2Cdev::readBytes(devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; -} -void MPU6050::setZGyroOffset(int16_t offset) { - I2Cdev::writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset); -} - -// INT_ENABLE register (DMP functions) - -bool MPU6050::getIntPLLReadyEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer); - return buffer[0]; -} -void MPU6050::setIntPLLReadyEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled); -} -bool MPU6050::getIntDMPEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, buffer); - return buffer[0]; -} -void MPU6050::setIntDMPEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled); -} - -// DMP_INT_STATUS - -bool MPU6050::getDMPInt5Status() { - I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer); - return buffer[0]; -} -bool MPU6050::getDMPInt4Status() { - I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer); - return buffer[0]; -} -bool MPU6050::getDMPInt3Status() { - I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, buffer); - return buffer[0]; -} -bool MPU6050::getDMPInt2Status() { - I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer); - return buffer[0]; -} -bool MPU6050::getDMPInt1Status() { - I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, buffer); - return buffer[0]; -} -bool MPU6050::getDMPInt0Status() { - I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, buffer); - return buffer[0]; -} - -// INT_STATUS register (DMP functions) - -bool MPU6050::getIntPLLReadyStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer); - return buffer[0]; -} -bool MPU6050::getIntDMPStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, buffer); - return buffer[0]; -} - -// USER_CTRL register (DMP functions) - -bool MPU6050::getDMPEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer); - return buffer[0]; -} -void MPU6050::setDMPEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled); -} -void MPU6050::resetDMP() { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true); -} - -// BANK_SEL register - -void MPU6050::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) { - bank &= 0x1F; - if (userBank) bank |= 0x20; - if (prefetchEnabled) bank |= 0x40; - I2Cdev::writeByte(devAddr, MPU6050_RA_BANK_SEL, bank); -} - -// MEM_START_ADDR register - -void MPU6050::setMemoryStartAddress(uint8_t address) { - I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address); -} - -// MEM_R_W register - -uint8_t MPU6050::readMemoryByte() { - I2Cdev::readByte(devAddr, MPU6050_RA_MEM_R_W, buffer); - return buffer[0]; -} -void MPU6050::writeMemoryByte(uint8_t data) { - I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_R_W, data); -} -void MPU6050::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) { - setMemoryBank(bank); - setMemoryStartAddress(address); - uint8_t chunkSize; - for (uint16_t i = 0; i < dataSize;) { - // determine correct chunk size according to bank position and data size - chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE; - - // make sure we don't go past the data size - if (i + chunkSize > dataSize) chunkSize = dataSize - i; - - // make sure this chunk doesn't go past the bank boundary (256 bytes) - if (chunkSize > 256 - address) chunkSize = 256 - address; - - // read the chunk of data as specified - I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i); - - // increase byte index by [chunkSize] - i += chunkSize; - - // uint8_t automatically wraps to 0 at 256 - address += chunkSize; - - // if we aren't done, update bank (if necessary) and address - if (i < dataSize) { - if (address == 0) bank++; - setMemoryBank(bank); - setMemoryStartAddress(address); - } - } -} -bool MPU6050::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) { - setMemoryBank(bank); - setMemoryStartAddress(address); - uint8_t chunkSize; - uint8_t *verifyBuffer=0; - uint8_t *progBuffer=0; - uint16_t i; - uint8_t j; - if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); - if (useProgMem) progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); - for (i = 0; i < dataSize;) { - // determine correct chunk size according to bank position and data size - chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE; - - // make sure we don't go past the data size - if (i + chunkSize > dataSize) chunkSize = dataSize - i; - - // make sure this chunk doesn't go past the bank boundary (256 bytes) - if (chunkSize > 256 - address) chunkSize = 256 - address; - - if (useProgMem) { - // write the chunk of data as specified - for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j); - } else { - // write the chunk of data as specified - progBuffer = (uint8_t *)data + i; - } - - I2Cdev::writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer); - - // verify data if needed - if (verify && verifyBuffer) { - setMemoryBank(bank); - setMemoryStartAddress(address); - I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer); - if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) { - /*Serial.print("Block write verification error, bank "); - Serial.print(bank, DEC); - Serial.print(", address "); - Serial.print(address, DEC); - Serial.print("!\nExpected:"); - for (j = 0; j < chunkSize; j++) { - Serial.print(" 0x"); - if (progBuffer[j] < 16) Serial.print("0"); - Serial.print(progBuffer[j], HEX); - } - Serial.print("\nReceived:"); - for (uint8_t j = 0; j < chunkSize; j++) { - Serial.print(" 0x"); - if (verifyBuffer[i + j] < 16) Serial.print("0"); - Serial.print(verifyBuffer[i + j], HEX); - } - Serial.print("\n");*/ - free(verifyBuffer); - if (useProgMem) free(progBuffer); - return false; // uh oh. - } - } - - // increase byte index by [chunkSize] - i += chunkSize; - - // uint8_t automatically wraps to 0 at 256 - address += chunkSize; - - // if we aren't done, update bank (if necessary) and address - if (i < dataSize) { - if (address == 0) bank++; - setMemoryBank(bank); - setMemoryStartAddress(address); - } - } - if (verify) free(verifyBuffer); - if (useProgMem) free(progBuffer); - return true; -} -bool MPU6050::writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) { - return writeMemoryBlock(data, dataSize, bank, address, verify, true); -} -bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) { - uint8_t *progBuffer = 0; - uint8_t success, special; - uint16_t i, j; - if (useProgMem) { - progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary - } - - // config set data is a long string of blocks with the following structure: - // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]] - uint8_t bank, offset, length; - for (i = 0; i < dataSize;) { - if (useProgMem) { - bank = pgm_read_byte(data + i++); - offset = pgm_read_byte(data + i++); - length = pgm_read_byte(data + i++); - } else { - bank = data[i++]; - offset = data[i++]; - length = data[i++]; - } - - // write data or perform special action - if (length > 0) { - // regular block of data to write - /*Serial.print("Writing config block to bank "); - Serial.print(bank); - Serial.print(", offset "); - Serial.print(offset); - Serial.print(", length="); - Serial.println(length);*/ - if (useProgMem) { - if (sizeof(progBuffer) < length) progBuffer = (uint8_t *)realloc(progBuffer, length); - for (j = 0; j < length; j++) progBuffer[j] = pgm_read_byte(data + i + j); - } else { - progBuffer = (uint8_t *)data + i; - } - success = writeMemoryBlock(progBuffer, length, bank, offset, true); - i += length; - } else { - // special instruction - // NOTE: this kind of behavior (what and when to do certain things) - // is totally undocumented. This code is in here based on observed - // behavior only, and exactly why (or even whether) it has to be here - // is anybody's guess for now. - if (useProgMem) { - special = pgm_read_byte(data + i++); - } else { - special = data[i++]; - } - /*Serial.print("Special command code "); - Serial.print(special, HEX); - Serial.println(" found...");*/ - if (special == 0x01) { - // enable DMP-related interrupts - - //setIntZeroMotionEnabled(true); - //setIntFIFOBufferOverflowEnabled(true); - //setIntDMPEnabled(true); - I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, 0x32); // single operation - - success = true; - } else { - // unknown special command - success = false; - } - } - - if (!success) { - if (useProgMem) free(progBuffer); - return false; // uh oh - } - } - if (useProgMem) free(progBuffer); - return true; -} -bool MPU6050::writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) { - return writeDMPConfigurationSet(data, dataSize, true); -} - -// DMP_CFG_1 register - -uint8_t MPU6050::getDMPConfig1() { - I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_1, buffer); - return buffer[0]; -} -void MPU6050::setDMPConfig1(uint8_t config) { - I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config); -} - -// DMP_CFG_2 register - -uint8_t MPU6050::getDMPConfig2() { - I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_2, buffer); - return buffer[0]; -} -void MPU6050::setDMPConfig2(uint8_t config) { - I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config); -} - - -//*************************************************************************************** -//********************** Calibration Routines ********************** -//*************************************************************************************** -/** - @brief Fully calibrate Gyro from ZERO in about 6-7 Loops 600-700 readings -*/ -void MPU6050::CalibrateGyro(uint8_t Loops ) { - double kP = 0.3; - double kI = 90; - float x; - x = (100 - map(Loops, 1, 5, 20, 0)) * .01; - kP *= x; - kI *= x; - - PID( 0x43, kP, kI, Loops); -} - -/** - @brief Fully calibrate Accel from ZERO in about 6-7 Loops 600-700 readings -*/ -void MPU6050::CalibrateAccel(uint8_t Loops ) { - - float kP = 0.3; - float kI = 20; - float x; - x = (100 - map(Loops, 1, 5, 20, 0)) * .01; - kP *= x; - kI *= x; - PID( 0x3B, kP, kI, Loops); -} - -void MPU6050::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){ - uint8_t SaveAddress = (ReadAddress == 0x3B)?((getDeviceID() < 0x38 )? 0x06:0x77):0x13; - - int16_t Data; - float Reading; - int16_t BitZero[3]; - uint8_t shift =(SaveAddress == 0x77)?3:2; - float Error, PTerm, ITerm[3]; - int16_t eSample; - uint32_t eSum ; - Serial.write('>'); - for (int i = 0; i < 3; i++) { - I2Cdev::readWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word) - Reading = Data; - if(SaveAddress != 0x13){ - BitZero[i] = Data & 1; // Capture Bit Zero to properly handle Accelerometer calibration - ITerm[i] = ((float)Reading) * 8; - } else { - ITerm[i] = Reading * 4; - } - } - for (int L = 0; L < Loops; L++) { - eSample = 0; - for (int c = 0; c < 100; c++) {// 100 PI Calculations - eSum = 0; - for (int i = 0; i < 3; i++) { - I2Cdev::readWords(devAddr, ReadAddress + (i * 2), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word) - Reading = Data; - if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384; //remove Gravity - Error = -Reading; - eSum += abs(Reading); - PTerm = kP * Error; - ITerm[i] += (Error * 0.001) * kI; // Integral term 1000 Calculations a second = 0.001 - if(SaveAddress != 0x13){ - Data = round((PTerm + ITerm[i] ) / 8); //Compute PID Output - Data = ((Data)&0xFFFE) |BitZero[i]; // Insert Bit0 Saved at beginning - } else Data = round((PTerm + ITerm[i] ) / 4); //Compute PID Output - I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data); - } - if((c == 99) && eSum > 1000){ // Error is still to great to continue - c = 0; - Serial.write('*'); - } - if((eSum * ((ReadAddress == 0x3B)?.05: 1)) < 5) eSample++; // Successfully found offsets prepare to advance - if((eSum < 100) && (c > 10) && (eSample >= 10)) break; // Advance to next Loop - delay(1); - } - Serial.write('.'); - kP *= .75; - kI *= .75; - for (int i = 0; i < 3; i++){ - if(SaveAddress != 0x13) { - Data = round((ITerm[i] ) / 8); //Compute PID Output - Data = ((Data)&0xFFFE) |BitZero[i]; // Insert Bit0 Saved at beginning - } else Data = round((ITerm[i]) / 4); - I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data); - } - } - resetFIFO(); - resetDMP(); -} - -#define printfloatx(Name,Variable,Spaces,Precision,EndTxt) { Serial.print(F(Name)); {char S[(Spaces + Precision + 3)];Serial.print(F(" ")); Serial.print(dtostrf((float)Variable,Spaces,Precision ,S));}Serial.print(F(EndTxt)); }//Name,Variable,Spaces,Precision,EndTxt -void MPU6050::PrintActiveOffsets() { - uint8_t AOffsetRegister = (getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77; - int16_t Data[3]; - //Serial.print(F("Offset Register 0x")); - //Serial.print(AOffsetRegister>>4,HEX);Serial.print(AOffsetRegister&0x0F,HEX); - Serial.print(F("\n// X Accel Y Accel Z Accel X Gyro Y Gyro Z Gyro\n//OFFSETS ")); - if(AOffsetRegister == 0x06) I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)Data); - else { - I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)Data); - I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)Data+1); - I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)Data+2); - } - // A_OFFSET_H_READ_A_OFFS(Data); - printfloatx("", Data[0], 5, 0, ", "); - printfloatx("", Data[1], 5, 0, ", "); - printfloatx("", Data[2], 5, 0, ", "); - I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)Data); - // XG_OFFSET_H_READ_OFFS_USR(Data); - printfloatx("", Data[0], 5, 0, ", "); - printfloatx("", Data[1], 5, 0, ", "); - printfloatx("", Data[2], 5, 0, "\n"); -} diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050.h b/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050.h deleted file mode 100644 index 871f423a..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050.h +++ /dev/null @@ -1,1041 +0,0 @@ -// I2Cdev library collection - MPU6050 I2C device class -// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) -// 10/3/2011 by Jeff Rowberg -// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib -// -// Changelog: -// ... - ongoing debug release - -// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE -// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF -// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING. - -/* ============================================ -I2Cdev device library code is placed under the MIT license -Copyright (c) 2012 Jeff Rowberg - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. -=============================================== -*/ - -#ifndef _MPU6050_H_ -#define _MPU6050_H_ - -#include "I2Cdev.h" - -// supporting link: http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517 -// also: http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s - -#ifdef __AVR__ -#include -#elif defined(ARDUINO_ARCH_SAMD) -#include -#else -//#define PROGMEM /* empty */ -//#define pgm_read_byte(x) (*(x)) -//#define pgm_read_word(x) (*(x)) -//#define pgm_read_float(x) (*(x)) -//#define PSTR(STR) STR -#endif - - -#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board -#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) -#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW - -#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD -#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD -#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD -#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN -#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN -#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN -#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS -#define MPU6050_RA_XA_OFFS_L_TC 0x07 -#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS -#define MPU6050_RA_YA_OFFS_L_TC 0x09 -#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS -#define MPU6050_RA_ZA_OFFS_L_TC 0x0B -#define MPU6050_RA_SELF_TEST_X 0x0D //[7:5] XA_TEST[4-2], [4:0] XG_TEST[4-0] -#define MPU6050_RA_SELF_TEST_Y 0x0E //[7:5] YA_TEST[4-2], [4:0] YG_TEST[4-0] -#define MPU6050_RA_SELF_TEST_Z 0x0F //[7:5] ZA_TEST[4-2], [4:0] ZG_TEST[4-0] -#define MPU6050_RA_SELF_TEST_A 0x10 //[5:4] XA_TEST[1-0], [3:2] YA_TEST[1-0], [1:0] ZA_TEST[1-0] -#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR -#define MPU6050_RA_XG_OFFS_USRL 0x14 -#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR -#define MPU6050_RA_YG_OFFS_USRL 0x16 -#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR -#define MPU6050_RA_ZG_OFFS_USRL 0x18 -#define MPU6050_RA_SMPLRT_DIV 0x19 -#define MPU6050_RA_CONFIG 0x1A -#define MPU6050_RA_GYRO_CONFIG 0x1B -#define MPU6050_RA_ACCEL_CONFIG 0x1C -#define MPU6050_RA_FF_THR 0x1D -#define MPU6050_RA_FF_DUR 0x1E -#define MPU6050_RA_MOT_THR 0x1F -#define MPU6050_RA_MOT_DUR 0x20 -#define MPU6050_RA_ZRMOT_THR 0x21 -#define MPU6050_RA_ZRMOT_DUR 0x22 -#define MPU6050_RA_FIFO_EN 0x23 -#define MPU6050_RA_I2C_MST_CTRL 0x24 -#define MPU6050_RA_I2C_SLV0_ADDR 0x25 -#define MPU6050_RA_I2C_SLV0_REG 0x26 -#define MPU6050_RA_I2C_SLV0_CTRL 0x27 -#define MPU6050_RA_I2C_SLV1_ADDR 0x28 -#define MPU6050_RA_I2C_SLV1_REG 0x29 -#define MPU6050_RA_I2C_SLV1_CTRL 0x2A -#define MPU6050_RA_I2C_SLV2_ADDR 0x2B -#define MPU6050_RA_I2C_SLV2_REG 0x2C -#define MPU6050_RA_I2C_SLV2_CTRL 0x2D -#define MPU6050_RA_I2C_SLV3_ADDR 0x2E -#define MPU6050_RA_I2C_SLV3_REG 0x2F -#define MPU6050_RA_I2C_SLV3_CTRL 0x30 -#define MPU6050_RA_I2C_SLV4_ADDR 0x31 -#define MPU6050_RA_I2C_SLV4_REG 0x32 -#define MPU6050_RA_I2C_SLV4_DO 0x33 -#define MPU6050_RA_I2C_SLV4_CTRL 0x34 -#define MPU6050_RA_I2C_SLV4_DI 0x35 -#define MPU6050_RA_I2C_MST_STATUS 0x36 -#define MPU6050_RA_INT_PIN_CFG 0x37 -#define MPU6050_RA_INT_ENABLE 0x38 -#define MPU6050_RA_DMP_INT_STATUS 0x39 -#define MPU6050_RA_INT_STATUS 0x3A -#define MPU6050_RA_ACCEL_XOUT_H 0x3B -#define MPU6050_RA_ACCEL_XOUT_L 0x3C -#define MPU6050_RA_ACCEL_YOUT_H 0x3D -#define MPU6050_RA_ACCEL_YOUT_L 0x3E -#define MPU6050_RA_ACCEL_ZOUT_H 0x3F -#define MPU6050_RA_ACCEL_ZOUT_L 0x40 -#define MPU6050_RA_TEMP_OUT_H 0x41 -#define MPU6050_RA_TEMP_OUT_L 0x42 -#define MPU6050_RA_GYRO_XOUT_H 0x43 -#define MPU6050_RA_GYRO_XOUT_L 0x44 -#define MPU6050_RA_GYRO_YOUT_H 0x45 -#define MPU6050_RA_GYRO_YOUT_L 0x46 -#define MPU6050_RA_GYRO_ZOUT_H 0x47 -#define MPU6050_RA_GYRO_ZOUT_L 0x48 -#define MPU6050_RA_EXT_SENS_DATA_00 0x49 -#define MPU6050_RA_EXT_SENS_DATA_01 0x4A -#define MPU6050_RA_EXT_SENS_DATA_02 0x4B -#define MPU6050_RA_EXT_SENS_DATA_03 0x4C -#define MPU6050_RA_EXT_SENS_DATA_04 0x4D -#define MPU6050_RA_EXT_SENS_DATA_05 0x4E -#define MPU6050_RA_EXT_SENS_DATA_06 0x4F -#define MPU6050_RA_EXT_SENS_DATA_07 0x50 -#define MPU6050_RA_EXT_SENS_DATA_08 0x51 -#define MPU6050_RA_EXT_SENS_DATA_09 0x52 -#define MPU6050_RA_EXT_SENS_DATA_10 0x53 -#define MPU6050_RA_EXT_SENS_DATA_11 0x54 -#define MPU6050_RA_EXT_SENS_DATA_12 0x55 -#define MPU6050_RA_EXT_SENS_DATA_13 0x56 -#define MPU6050_RA_EXT_SENS_DATA_14 0x57 -#define MPU6050_RA_EXT_SENS_DATA_15 0x58 -#define MPU6050_RA_EXT_SENS_DATA_16 0x59 -#define MPU6050_RA_EXT_SENS_DATA_17 0x5A -#define MPU6050_RA_EXT_SENS_DATA_18 0x5B -#define MPU6050_RA_EXT_SENS_DATA_19 0x5C -#define MPU6050_RA_EXT_SENS_DATA_20 0x5D -#define MPU6050_RA_EXT_SENS_DATA_21 0x5E -#define MPU6050_RA_EXT_SENS_DATA_22 0x5F -#define MPU6050_RA_EXT_SENS_DATA_23 0x60 -#define MPU6050_RA_MOT_DETECT_STATUS 0x61 -#define MPU6050_RA_I2C_SLV0_DO 0x63 -#define MPU6050_RA_I2C_SLV1_DO 0x64 -#define MPU6050_RA_I2C_SLV2_DO 0x65 -#define MPU6050_RA_I2C_SLV3_DO 0x66 -#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 -#define MPU6050_RA_SIGNAL_PATH_RESET 0x68 -#define MPU6050_RA_MOT_DETECT_CTRL 0x69 -#define MPU6050_RA_USER_CTRL 0x6A -#define MPU6050_RA_PWR_MGMT_1 0x6B -#define MPU6050_RA_PWR_MGMT_2 0x6C -#define MPU6050_RA_BANK_SEL 0x6D -#define MPU6050_RA_MEM_START_ADDR 0x6E -#define MPU6050_RA_MEM_R_W 0x6F -#define MPU6050_RA_DMP_CFG_1 0x70 -#define MPU6050_RA_DMP_CFG_2 0x71 -#define MPU6050_RA_FIFO_COUNTH 0x72 -#define MPU6050_RA_FIFO_COUNTL 0x73 -#define MPU6050_RA_FIFO_R_W 0x74 -#define MPU6050_RA_WHO_AM_I 0x75 - -#define MPU6050_SELF_TEST_XA_1_BIT 0x07 -#define MPU6050_SELF_TEST_XA_1_LENGTH 0x03 -#define MPU6050_SELF_TEST_XA_2_BIT 0x05 -#define MPU6050_SELF_TEST_XA_2_LENGTH 0x02 -#define MPU6050_SELF_TEST_YA_1_BIT 0x07 -#define MPU6050_SELF_TEST_YA_1_LENGTH 0x03 -#define MPU6050_SELF_TEST_YA_2_BIT 0x03 -#define MPU6050_SELF_TEST_YA_2_LENGTH 0x02 -#define MPU6050_SELF_TEST_ZA_1_BIT 0x07 -#define MPU6050_SELF_TEST_ZA_1_LENGTH 0x03 -#define MPU6050_SELF_TEST_ZA_2_BIT 0x01 -#define MPU6050_SELF_TEST_ZA_2_LENGTH 0x02 - -#define MPU6050_SELF_TEST_XG_1_BIT 0x04 -#define MPU6050_SELF_TEST_XG_1_LENGTH 0x05 -#define MPU6050_SELF_TEST_YG_1_BIT 0x04 -#define MPU6050_SELF_TEST_YG_1_LENGTH 0x05 -#define MPU6050_SELF_TEST_ZG_1_BIT 0x04 -#define MPU6050_SELF_TEST_ZG_1_LENGTH 0x05 - -#define MPU6050_TC_PWR_MODE_BIT 7 -#define MPU6050_TC_OFFSET_BIT 6 -#define MPU6050_TC_OFFSET_LENGTH 6 -#define MPU6050_TC_OTP_BNK_VLD_BIT 0 - -#define MPU6050_VDDIO_LEVEL_VLOGIC 0 -#define MPU6050_VDDIO_LEVEL_VDD 1 - -#define MPU6050_CFG_EXT_SYNC_SET_BIT 5 -#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 -#define MPU6050_CFG_DLPF_CFG_BIT 2 -#define MPU6050_CFG_DLPF_CFG_LENGTH 3 - -#define MPU6050_EXT_SYNC_DISABLED 0x0 -#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 -#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 -#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 -#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 -#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 -#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 -#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 - -#define MPU6050_DLPF_BW_256 0x00 -#define MPU6050_DLPF_BW_188 0x01 -#define MPU6050_DLPF_BW_98 0x02 -#define MPU6050_DLPF_BW_42 0x03 -#define MPU6050_DLPF_BW_20 0x04 -#define MPU6050_DLPF_BW_10 0x05 -#define MPU6050_DLPF_BW_5 0x06 - -#define MPU6050_GCONFIG_FS_SEL_BIT 4 -#define MPU6050_GCONFIG_FS_SEL_LENGTH 2 - -#define MPU6050_GYRO_FS_250 0x00 -#define MPU6050_GYRO_FS_500 0x01 -#define MPU6050_GYRO_FS_1000 0x02 -#define MPU6050_GYRO_FS_2000 0x03 - -#define MPU6050_ACONFIG_XA_ST_BIT 7 -#define MPU6050_ACONFIG_YA_ST_BIT 6 -#define MPU6050_ACONFIG_ZA_ST_BIT 5 -#define MPU6050_ACONFIG_AFS_SEL_BIT 4 -#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 -#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 -#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 - -#define MPU6050_ACCEL_FS_2 0x00 -#define MPU6050_ACCEL_FS_4 0x01 -#define MPU6050_ACCEL_FS_8 0x02 -#define MPU6050_ACCEL_FS_16 0x03 - -#define MPU6050_DHPF_RESET 0x00 -#define MPU6050_DHPF_5 0x01 -#define MPU6050_DHPF_2P5 0x02 -#define MPU6050_DHPF_1P25 0x03 -#define MPU6050_DHPF_0P63 0x04 -#define MPU6050_DHPF_HOLD 0x07 - -#define MPU6050_TEMP_FIFO_EN_BIT 7 -#define MPU6050_XG_FIFO_EN_BIT 6 -#define MPU6050_YG_FIFO_EN_BIT 5 -#define MPU6050_ZG_FIFO_EN_BIT 4 -#define MPU6050_ACCEL_FIFO_EN_BIT 3 -#define MPU6050_SLV2_FIFO_EN_BIT 2 -#define MPU6050_SLV1_FIFO_EN_BIT 1 -#define MPU6050_SLV0_FIFO_EN_BIT 0 - -#define MPU6050_MULT_MST_EN_BIT 7 -#define MPU6050_WAIT_FOR_ES_BIT 6 -#define MPU6050_SLV_3_FIFO_EN_BIT 5 -#define MPU6050_I2C_MST_P_NSR_BIT 4 -#define MPU6050_I2C_MST_CLK_BIT 3 -#define MPU6050_I2C_MST_CLK_LENGTH 4 - -#define MPU6050_CLOCK_DIV_348 0x0 -#define MPU6050_CLOCK_DIV_333 0x1 -#define MPU6050_CLOCK_DIV_320 0x2 -#define MPU6050_CLOCK_DIV_308 0x3 -#define MPU6050_CLOCK_DIV_296 0x4 -#define MPU6050_CLOCK_DIV_286 0x5 -#define MPU6050_CLOCK_DIV_276 0x6 -#define MPU6050_CLOCK_DIV_267 0x7 -#define MPU6050_CLOCK_DIV_258 0x8 -#define MPU6050_CLOCK_DIV_500 0x9 -#define MPU6050_CLOCK_DIV_471 0xA -#define MPU6050_CLOCK_DIV_444 0xB -#define MPU6050_CLOCK_DIV_421 0xC -#define MPU6050_CLOCK_DIV_400 0xD -#define MPU6050_CLOCK_DIV_381 0xE -#define MPU6050_CLOCK_DIV_364 0xF - -#define MPU6050_I2C_SLV_RW_BIT 7 -#define MPU6050_I2C_SLV_ADDR_BIT 6 -#define MPU6050_I2C_SLV_ADDR_LENGTH 7 -#define MPU6050_I2C_SLV_EN_BIT 7 -#define MPU6050_I2C_SLV_BYTE_SW_BIT 6 -#define MPU6050_I2C_SLV_REG_DIS_BIT 5 -#define MPU6050_I2C_SLV_GRP_BIT 4 -#define MPU6050_I2C_SLV_LEN_BIT 3 -#define MPU6050_I2C_SLV_LEN_LENGTH 4 - -#define MPU6050_I2C_SLV4_RW_BIT 7 -#define MPU6050_I2C_SLV4_ADDR_BIT 6 -#define MPU6050_I2C_SLV4_ADDR_LENGTH 7 -#define MPU6050_I2C_SLV4_EN_BIT 7 -#define MPU6050_I2C_SLV4_INT_EN_BIT 6 -#define MPU6050_I2C_SLV4_REG_DIS_BIT 5 -#define MPU6050_I2C_SLV4_MST_DLY_BIT 4 -#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 - -#define MPU6050_MST_PASS_THROUGH_BIT 7 -#define MPU6050_MST_I2C_SLV4_DONE_BIT 6 -#define MPU6050_MST_I2C_LOST_ARB_BIT 5 -#define MPU6050_MST_I2C_SLV4_NACK_BIT 4 -#define MPU6050_MST_I2C_SLV3_NACK_BIT 3 -#define MPU6050_MST_I2C_SLV2_NACK_BIT 2 -#define MPU6050_MST_I2C_SLV1_NACK_BIT 1 -#define MPU6050_MST_I2C_SLV0_NACK_BIT 0 - -#define MPU6050_INTCFG_INT_LEVEL_BIT 7 -#define MPU6050_INTCFG_INT_OPEN_BIT 6 -#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 -#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 -#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 -#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 -#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 -#define MPU6050_INTCFG_CLKOUT_EN_BIT 0 - -#define MPU6050_INTMODE_ACTIVEHIGH 0x00 -#define MPU6050_INTMODE_ACTIVELOW 0x01 - -#define MPU6050_INTDRV_PUSHPULL 0x00 -#define MPU6050_INTDRV_OPENDRAIN 0x01 - -#define MPU6050_INTLATCH_50USPULSE 0x00 -#define MPU6050_INTLATCH_WAITCLEAR 0x01 - -#define MPU6050_INTCLEAR_STATUSREAD 0x00 -#define MPU6050_INTCLEAR_ANYREAD 0x01 - -#define MPU6050_INTERRUPT_FF_BIT 7 -#define MPU6050_INTERRUPT_MOT_BIT 6 -#define MPU6050_INTERRUPT_ZMOT_BIT 5 -#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 -#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 -#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 -#define MPU6050_INTERRUPT_DMP_INT_BIT 1 -#define MPU6050_INTERRUPT_DATA_RDY_BIT 0 - -// TODO: figure out what these actually do -// UMPL source code is not very obivous -#define MPU6050_DMPINT_5_BIT 5 -#define MPU6050_DMPINT_4_BIT 4 -#define MPU6050_DMPINT_3_BIT 3 -#define MPU6050_DMPINT_2_BIT 2 -#define MPU6050_DMPINT_1_BIT 1 -#define MPU6050_DMPINT_0_BIT 0 - -#define MPU6050_MOTION_MOT_XNEG_BIT 7 -#define MPU6050_MOTION_MOT_XPOS_BIT 6 -#define MPU6050_MOTION_MOT_YNEG_BIT 5 -#define MPU6050_MOTION_MOT_YPOS_BIT 4 -#define MPU6050_MOTION_MOT_ZNEG_BIT 3 -#define MPU6050_MOTION_MOT_ZPOS_BIT 2 -#define MPU6050_MOTION_MOT_ZRMOT_BIT 0 - -#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 -#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 -#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 -#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 -#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 -#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 - -#define MPU6050_PATHRESET_GYRO_RESET_BIT 2 -#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 -#define MPU6050_PATHRESET_TEMP_RESET_BIT 0 - -#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 -#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 -#define MPU6050_DETECT_FF_COUNT_BIT 3 -#define MPU6050_DETECT_FF_COUNT_LENGTH 2 -#define MPU6050_DETECT_MOT_COUNT_BIT 1 -#define MPU6050_DETECT_MOT_COUNT_LENGTH 2 - -#define MPU6050_DETECT_DECREMENT_RESET 0x0 -#define MPU6050_DETECT_DECREMENT_1 0x1 -#define MPU6050_DETECT_DECREMENT_2 0x2 -#define MPU6050_DETECT_DECREMENT_4 0x3 - -#define MPU6050_USERCTRL_DMP_EN_BIT 7 -#define MPU6050_USERCTRL_FIFO_EN_BIT 6 -#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 -#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 -#define MPU6050_USERCTRL_DMP_RESET_BIT 3 -#define MPU6050_USERCTRL_FIFO_RESET_BIT 2 -#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 -#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 - -#define MPU6050_PWR1_DEVICE_RESET_BIT 7 -#define MPU6050_PWR1_SLEEP_BIT 6 -#define MPU6050_PWR1_CYCLE_BIT 5 -#define MPU6050_PWR1_TEMP_DIS_BIT 3 -#define MPU6050_PWR1_CLKSEL_BIT 2 -#define MPU6050_PWR1_CLKSEL_LENGTH 3 - -#define MPU6050_CLOCK_INTERNAL 0x00 -#define MPU6050_CLOCK_PLL_XGYRO 0x01 -#define MPU6050_CLOCK_PLL_YGYRO 0x02 -#define MPU6050_CLOCK_PLL_ZGYRO 0x03 -#define MPU6050_CLOCK_PLL_EXT32K 0x04 -#define MPU6050_CLOCK_PLL_EXT19M 0x05 -#define MPU6050_CLOCK_KEEP_RESET 0x07 - -#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 -#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 -#define MPU6050_PWR2_STBY_XA_BIT 5 -#define MPU6050_PWR2_STBY_YA_BIT 4 -#define MPU6050_PWR2_STBY_ZA_BIT 3 -#define MPU6050_PWR2_STBY_XG_BIT 2 -#define MPU6050_PWR2_STBY_YG_BIT 1 -#define MPU6050_PWR2_STBY_ZG_BIT 0 - -#define MPU6050_WAKE_FREQ_1P25 0x0 -#define MPU6050_WAKE_FREQ_2P5 0x1 -#define MPU6050_WAKE_FREQ_5 0x2 -#define MPU6050_WAKE_FREQ_10 0x3 - -#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 -#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 -#define MPU6050_BANKSEL_MEM_SEL_BIT 4 -#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 - -#define MPU6050_WHO_AM_I_BIT 6 -#define MPU6050_WHO_AM_I_LENGTH 6 - -#define MPU6050_DMP_MEMORY_BANKS 8 -#define MPU6050_DMP_MEMORY_BANK_SIZE 256 -#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 - -// note: DMP code memory blocks defined at end of header file - -class MPU6050 { - public: - MPU6050(uint8_t address=MPU6050_DEFAULT_ADDRESS); - - void initialize(); - bool testConnection(); - - // AUX_VDDIO register - uint8_t getAuxVDDIOLevel(); - void setAuxVDDIOLevel(uint8_t level); - - // SMPLRT_DIV register - uint8_t getRate(); - void setRate(uint8_t rate); - - // CONFIG register - uint8_t getExternalFrameSync(); - void setExternalFrameSync(uint8_t sync); - uint8_t getDLPFMode(); - void setDLPFMode(uint8_t bandwidth); - - // GYRO_CONFIG register - uint8_t getFullScaleGyroRange(); - void setFullScaleGyroRange(uint8_t range); - - // SELF_TEST registers - uint8_t getAccelXSelfTestFactoryTrim(); - uint8_t getAccelYSelfTestFactoryTrim(); - uint8_t getAccelZSelfTestFactoryTrim(); - - uint8_t getGyroXSelfTestFactoryTrim(); - uint8_t getGyroYSelfTestFactoryTrim(); - uint8_t getGyroZSelfTestFactoryTrim(); - - // ACCEL_CONFIG register - bool getAccelXSelfTest(); - void setAccelXSelfTest(bool enabled); - bool getAccelYSelfTest(); - void setAccelYSelfTest(bool enabled); - bool getAccelZSelfTest(); - void setAccelZSelfTest(bool enabled); - uint8_t getFullScaleAccelRange(); - void setFullScaleAccelRange(uint8_t range); - uint8_t getDHPFMode(); - void setDHPFMode(uint8_t mode); - - // FF_THR register - uint8_t getFreefallDetectionThreshold(); - void setFreefallDetectionThreshold(uint8_t threshold); - - // FF_DUR register - uint8_t getFreefallDetectionDuration(); - void setFreefallDetectionDuration(uint8_t duration); - - // MOT_THR register - uint8_t getMotionDetectionThreshold(); - void setMotionDetectionThreshold(uint8_t threshold); - - // MOT_DUR register - uint8_t getMotionDetectionDuration(); - void setMotionDetectionDuration(uint8_t duration); - - // ZRMOT_THR register - uint8_t getZeroMotionDetectionThreshold(); - void setZeroMotionDetectionThreshold(uint8_t threshold); - - // ZRMOT_DUR register - uint8_t getZeroMotionDetectionDuration(); - void setZeroMotionDetectionDuration(uint8_t duration); - - // FIFO_EN register - bool getTempFIFOEnabled(); - void setTempFIFOEnabled(bool enabled); - bool getXGyroFIFOEnabled(); - void setXGyroFIFOEnabled(bool enabled); - bool getYGyroFIFOEnabled(); - void setYGyroFIFOEnabled(bool enabled); - bool getZGyroFIFOEnabled(); - void setZGyroFIFOEnabled(bool enabled); - bool getAccelFIFOEnabled(); - void setAccelFIFOEnabled(bool enabled); - bool getSlave2FIFOEnabled(); - void setSlave2FIFOEnabled(bool enabled); - bool getSlave1FIFOEnabled(); - void setSlave1FIFOEnabled(bool enabled); - bool getSlave0FIFOEnabled(); - void setSlave0FIFOEnabled(bool enabled); - - // I2C_MST_CTRL register - bool getMultiMasterEnabled(); - void setMultiMasterEnabled(bool enabled); - bool getWaitForExternalSensorEnabled(); - void setWaitForExternalSensorEnabled(bool enabled); - bool getSlave3FIFOEnabled(); - void setSlave3FIFOEnabled(bool enabled); - bool getSlaveReadWriteTransitionEnabled(); - void setSlaveReadWriteTransitionEnabled(bool enabled); - uint8_t getMasterClockSpeed(); - void setMasterClockSpeed(uint8_t speed); - - // I2C_SLV* registers (Slave 0-3) - uint8_t getSlaveAddress(uint8_t num); - void setSlaveAddress(uint8_t num, uint8_t address); - uint8_t getSlaveRegister(uint8_t num); - void setSlaveRegister(uint8_t num, uint8_t reg); - bool getSlaveEnabled(uint8_t num); - void setSlaveEnabled(uint8_t num, bool enabled); - bool getSlaveWordByteSwap(uint8_t num); - void setSlaveWordByteSwap(uint8_t num, bool enabled); - bool getSlaveWriteMode(uint8_t num); - void setSlaveWriteMode(uint8_t num, bool mode); - bool getSlaveWordGroupOffset(uint8_t num); - void setSlaveWordGroupOffset(uint8_t num, bool enabled); - uint8_t getSlaveDataLength(uint8_t num); - void setSlaveDataLength(uint8_t num, uint8_t length); - - // I2C_SLV* registers (Slave 4) - uint8_t getSlave4Address(); - void setSlave4Address(uint8_t address); - uint8_t getSlave4Register(); - void setSlave4Register(uint8_t reg); - void setSlave4OutputByte(uint8_t data); - bool getSlave4Enabled(); - void setSlave4Enabled(bool enabled); - bool getSlave4InterruptEnabled(); - void setSlave4InterruptEnabled(bool enabled); - bool getSlave4WriteMode(); - void setSlave4WriteMode(bool mode); - uint8_t getSlave4MasterDelay(); - void setSlave4MasterDelay(uint8_t delay); - uint8_t getSlate4InputByte(); - - // I2C_MST_STATUS register - bool getPassthroughStatus(); - bool getSlave4IsDone(); - bool getLostArbitration(); - bool getSlave4Nack(); - bool getSlave3Nack(); - bool getSlave2Nack(); - bool getSlave1Nack(); - bool getSlave0Nack(); - - // INT_PIN_CFG register - bool getInterruptMode(); - void setInterruptMode(bool mode); - bool getInterruptDrive(); - void setInterruptDrive(bool drive); - bool getInterruptLatch(); - void setInterruptLatch(bool latch); - bool getInterruptLatchClear(); - void setInterruptLatchClear(bool clear); - bool getFSyncInterruptLevel(); - void setFSyncInterruptLevel(bool level); - bool getFSyncInterruptEnabled(); - void setFSyncInterruptEnabled(bool enabled); - bool getI2CBypassEnabled(); - void setI2CBypassEnabled(bool enabled); - bool getClockOutputEnabled(); - void setClockOutputEnabled(bool enabled); - - // INT_ENABLE register - uint8_t getIntEnabled(); - void setIntEnabled(uint8_t enabled); - bool getIntFreefallEnabled(); - void setIntFreefallEnabled(bool enabled); - bool getIntMotionEnabled(); - void setIntMotionEnabled(bool enabled); - bool getIntZeroMotionEnabled(); - void setIntZeroMotionEnabled(bool enabled); - bool getIntFIFOBufferOverflowEnabled(); - void setIntFIFOBufferOverflowEnabled(bool enabled); - bool getIntI2CMasterEnabled(); - void setIntI2CMasterEnabled(bool enabled); - bool getIntDataReadyEnabled(); - void setIntDataReadyEnabled(bool enabled); - - // INT_STATUS register - uint8_t getIntStatus(); - bool getIntFreefallStatus(); - bool getIntMotionStatus(); - bool getIntZeroMotionStatus(); - bool getIntFIFOBufferOverflowStatus(); - bool getIntI2CMasterStatus(); - bool getIntDataReadyStatus(); - - // ACCEL_*OUT_* registers - void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz); - void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); - void getAcceleration(int16_t* x, int16_t* y, int16_t* z); - int16_t getAccelerationX(); - int16_t getAccelerationY(); - int16_t getAccelerationZ(); - - // TEMP_OUT_* registers - int16_t getTemperature(); - - // GYRO_*OUT_* registers - void getRotation(int16_t* x, int16_t* y, int16_t* z); - int16_t getRotationX(); - int16_t getRotationY(); - int16_t getRotationZ(); - - // EXT_SENS_DATA_* registers - uint8_t getExternalSensorByte(int position); - uint16_t getExternalSensorWord(int position); - uint32_t getExternalSensorDWord(int position); - - // MOT_DETECT_STATUS register - uint8_t getMotionStatus(); - bool getXNegMotionDetected(); - bool getXPosMotionDetected(); - bool getYNegMotionDetected(); - bool getYPosMotionDetected(); - bool getZNegMotionDetected(); - bool getZPosMotionDetected(); - bool getZeroMotionDetected(); - - // I2C_SLV*_DO register - void setSlaveOutputByte(uint8_t num, uint8_t data); - - // I2C_MST_DELAY_CTRL register - bool getExternalShadowDelayEnabled(); - void setExternalShadowDelayEnabled(bool enabled); - bool getSlaveDelayEnabled(uint8_t num); - void setSlaveDelayEnabled(uint8_t num, bool enabled); - - // SIGNAL_PATH_RESET register - void resetGyroscopePath(); - void resetAccelerometerPath(); - void resetTemperaturePath(); - - // MOT_DETECT_CTRL register - uint8_t getAccelerometerPowerOnDelay(); - void setAccelerometerPowerOnDelay(uint8_t delay); - uint8_t getFreefallDetectionCounterDecrement(); - void setFreefallDetectionCounterDecrement(uint8_t decrement); - uint8_t getMotionDetectionCounterDecrement(); - void setMotionDetectionCounterDecrement(uint8_t decrement); - - // USER_CTRL register - bool getFIFOEnabled(); - void setFIFOEnabled(bool enabled); - bool getI2CMasterModeEnabled(); - void setI2CMasterModeEnabled(bool enabled); - void switchSPIEnabled(bool enabled); - void resetFIFO(); - void resetI2CMaster(); - void resetSensors(); - - // PWR_MGMT_1 register - void reset(); - bool getSleepEnabled(); - void setSleepEnabled(bool enabled); - bool getWakeCycleEnabled(); - void setWakeCycleEnabled(bool enabled); - bool getTempSensorEnabled(); - void setTempSensorEnabled(bool enabled); - uint8_t getClockSource(); - void setClockSource(uint8_t source); - - // PWR_MGMT_2 register - uint8_t getWakeFrequency(); - void setWakeFrequency(uint8_t frequency); - bool getStandbyXAccelEnabled(); - void setStandbyXAccelEnabled(bool enabled); - bool getStandbyYAccelEnabled(); - void setStandbyYAccelEnabled(bool enabled); - bool getStandbyZAccelEnabled(); - void setStandbyZAccelEnabled(bool enabled); - bool getStandbyXGyroEnabled(); - void setStandbyXGyroEnabled(bool enabled); - bool getStandbyYGyroEnabled(); - void setStandbyYGyroEnabled(bool enabled); - bool getStandbyZGyroEnabled(); - void setStandbyZGyroEnabled(bool enabled); - - // FIFO_COUNT_* registers - uint16_t getFIFOCount(); - - // FIFO_R_W register - uint8_t getFIFOByte(); - void setFIFOByte(uint8_t data); - void getFIFOBytes(uint8_t *data, uint8_t length); - - // WHO_AM_I register - uint8_t getDeviceID(); - void setDeviceID(uint8_t id); - - // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== - - // XG_OFFS_TC register - uint8_t getOTPBankValid(); - void setOTPBankValid(bool enabled); - int8_t getXGyroOffsetTC(); - void setXGyroOffsetTC(int8_t offset); - - // YG_OFFS_TC register - int8_t getYGyroOffsetTC(); - void setYGyroOffsetTC(int8_t offset); - - // ZG_OFFS_TC register - int8_t getZGyroOffsetTC(); - void setZGyroOffsetTC(int8_t offset); - - // X_FINE_GAIN register - int8_t getXFineGain(); - void setXFineGain(int8_t gain); - - // Y_FINE_GAIN register - int8_t getYFineGain(); - void setYFineGain(int8_t gain); - - // Z_FINE_GAIN register - int8_t getZFineGain(); - void setZFineGain(int8_t gain); - - // XA_OFFS_* registers - int16_t getXAccelOffset(); - void setXAccelOffset(int16_t offset); - - // YA_OFFS_* register - int16_t getYAccelOffset(); - void setYAccelOffset(int16_t offset); - - // ZA_OFFS_* register - int16_t getZAccelOffset(); - void setZAccelOffset(int16_t offset); - - // XG_OFFS_USR* registers - int16_t getXGyroOffset(); - void setXGyroOffset(int16_t offset); - - // YG_OFFS_USR* register - int16_t getYGyroOffset(); - void setYGyroOffset(int16_t offset); - - // ZG_OFFS_USR* register - int16_t getZGyroOffset(); - void setZGyroOffset(int16_t offset); - - // INT_ENABLE register (DMP functions) - bool getIntPLLReadyEnabled(); - void setIntPLLReadyEnabled(bool enabled); - bool getIntDMPEnabled(); - void setIntDMPEnabled(bool enabled); - - // DMP_INT_STATUS - bool getDMPInt5Status(); - bool getDMPInt4Status(); - bool getDMPInt3Status(); - bool getDMPInt2Status(); - bool getDMPInt1Status(); - bool getDMPInt0Status(); - - // INT_STATUS register (DMP functions) - bool getIntPLLReadyStatus(); - bool getIntDMPStatus(); - - // USER_CTRL register (DMP functions) - bool getDMPEnabled(); - void setDMPEnabled(bool enabled); - void resetDMP(); - - // BANK_SEL register - void setMemoryBank(uint8_t bank, bool prefetchEnabled=false, bool userBank=false); - - // MEM_START_ADDR register - void setMemoryStartAddress(uint8_t address); - - // MEM_R_W register - uint8_t readMemoryByte(); - void writeMemoryByte(uint8_t data); - void readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0); - bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true, bool useProgMem=false); - bool writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true); - - bool writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem=false); - bool writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize); - - // DMP_CFG_1 register - uint8_t getDMPConfig1(); - void setDMPConfig1(uint8_t config); - - // DMP_CFG_2 register - uint8_t getDMPConfig2(); - void setDMPConfig2(uint8_t config); - - // Calibration Routines - void CalibrateGyro(uint8_t Loops = 15); // Fine tune after setting offsets with less Loops. - void CalibrateAccel(uint8_t Loops = 15);// Fine tune after setting offsets with less Loops. - void PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops); // Does the math - void PrintActiveOffsets(); // See the results of the Calibration - - - - // special methods for MotionApps 2.0 implementation - #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20 - - uint8_t dmpInitialize(); - bool dmpPacketAvailable(); - - uint8_t dmpSetFIFORate(uint8_t fifoRate); - uint8_t dmpGetFIFORate(); - uint8_t dmpGetSampleStepSizeMS(); - uint8_t dmpGetSampleFrequency(); - int32_t dmpDecodeTemperature(int8_t tempReg); - - // Register callbacks after a packet of FIFO data is processed - //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); - //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); - uint8_t dmpRunFIFORateProcesses(); - - // Setup FIFO for various output - uint8_t dmpSendQuaternion(uint_fast16_t accuracy); - uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); - uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); - - // Get Fixed Point data from FIFO - uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); - uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); - uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); - uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpSetLinearAccelFilterCoefficient(float coef); - uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); - uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); - uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); - uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); - uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); - uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); - - uint8_t dmpGetEuler(float *data, Quaternion *q); - uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); - - // Get Floating Point data from FIFO - uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); - uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); - - uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); - uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); - - uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); - - uint8_t dmpInitFIFOParam(); - uint8_t dmpCloseFIFO(); - uint8_t dmpSetGyroDataSource(uint8_t source); - uint8_t dmpDecodeQuantizedAccel(); - uint32_t dmpGetGyroSumOfSquare(); - uint32_t dmpGetAccelSumOfSquare(); - void dmpOverrideQuaternion(long *q); - uint16_t dmpGetFIFOPacketSize(); - #endif - - // special methods for MotionApps 4.1 implementation - #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41 - - uint8_t dmpInitialize(); - bool dmpPacketAvailable(); - - uint8_t dmpSetFIFORate(uint8_t fifoRate); - uint8_t dmpGetFIFORate(); - uint8_t dmpGetSampleStepSizeMS(); - uint8_t dmpGetSampleFrequency(); - int32_t dmpDecodeTemperature(int8_t tempReg); - - // Register callbacks after a packet of FIFO data is processed - //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); - //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); - uint8_t dmpRunFIFORateProcesses(); - - // Setup FIFO for various output - uint8_t dmpSendQuaternion(uint_fast16_t accuracy); - uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); - uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); - - // Get Fixed Point data from FIFO - uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); - uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); - uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); - uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0); - uint8_t dmpSetLinearAccelFilterCoefficient(float coef); - uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); - uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); - uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); - uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); - uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); - uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); - uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); - uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); - - uint8_t dmpGetEuler(float *data, Quaternion *q); - uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); - - // Get Floating Point data from FIFO - uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); - uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); - - uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); - uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); - - uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); - - uint8_t dmpInitFIFOParam(); - uint8_t dmpCloseFIFO(); - uint8_t dmpSetGyroDataSource(uint8_t source); - uint8_t dmpDecodeQuantizedAccel(); - uint32_t dmpGetGyroSumOfSquare(); - uint32_t dmpGetAccelSumOfSquare(); - void dmpOverrideQuaternion(long *q); - uint16_t dmpGetFIFOPacketSize(); - #endif - - private: - uint8_t devAddr; - uint8_t buffer[14]; - #if defined(MPU6050_INCLUDE_DMP_MOTIONAPPS20) or defined(MPU6050_INCLUDE_DMP_MOTIONAPPS41) - uint8_t *dmpPacketBuffer; - uint16_t dmpPacketSize; - #endif -}; - -#endif /* _MPU6050_H_ */ diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050_6Axis_MotionApps20.h b/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050_6Axis_MotionApps20.h deleted file mode 100644 index 97dfdcbd..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050_6Axis_MotionApps20.h +++ /dev/null @@ -1,617 +0,0 @@ -// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation -// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) -// 5/20/2013 by Jeff Rowberg -// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib -// -// Changelog: -// 2019/07/08 - merged all DMP Firmware configuration items into the dmpMemory array -// - Simplified dmpInitialize() to accomidate the dmpmemory array alterations -// ... - ongoing debug release - -/* ============================================ -I2Cdev device library code is placed under the MIT license -Copyright (c) 2012 Jeff Rowberg - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. -=============================================== -*/ - -#ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ -#define _MPU6050_6AXIS_MOTIONAPPS20_H_ - -#include "I2Cdev.h" -#include "helper_3dmath.h" - -// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board -#define MPU6050_INCLUDE_DMP_MOTIONAPPS20 - -#include "MPU6050.h" - -// Tom Carpenter's conditional PROGMEM code -// http://forum.arduino.cc/index.php?topic=129407.0 -#ifdef __AVR__ - #include -#elif defined(ESP8266) || defined(ESP32) - #include -#else - // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen - #ifndef __PGMSPACE_H_ - #define __PGMSPACE_H_ 1 - #include - - #define PROGMEM - #define PGM_P const char * - #define PSTR(str) (str) - #define F(x) x - - typedef void prog_void; - typedef char prog_char; - typedef unsigned char prog_uchar; - typedef int8_t prog_int8_t; - typedef uint8_t prog_uint8_t; - typedef int16_t prog_int16_t; - typedef uint16_t prog_uint16_t; - typedef int32_t prog_int32_t; - typedef uint32_t prog_uint32_t; - - #define strcpy_P(dest, src) strcpy((dest), (src)) - #define strcat_P(dest, src) strcat((dest), (src)) - #define strcmp_P(a, b) strcmp((a), (b)) - - #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) - #define pgm_read_word(addr) (*(const unsigned short *)(addr)) - #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) - #define pgm_read_float(addr) (*(const float *)(addr)) - - #define pgm_read_byte_near(addr) pgm_read_byte(addr) - #define pgm_read_word_near(addr) pgm_read_word(addr) - #define pgm_read_dword_near(addr) pgm_read_dword(addr) - #define pgm_read_float_near(addr) pgm_read_float(addr) - #define pgm_read_byte_far(addr) pgm_read_byte(addr) - #define pgm_read_word_far(addr) pgm_read_word(addr) - #define pgm_read_dword_far(addr) pgm_read_dword(addr) - #define pgm_read_float_far(addr) pgm_read_float(addr) - #endif -#endif - -/* Source is from the InvenSense MotionApps v2 demo code. Original source is - * unavailable, unless you happen to be amazing as decompiling binary by - * hand (in which case, please contact me, and I'm totally serious). - * - * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the - * DMP reverse-engineering he did to help make this bit of wizardry - * possible. - */ - -// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. -// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) -// after moving string constants to flash memory storage using the F() -// compiler macro (Arduino IDE 1.0+ required). - -//#define DEBUG -#ifdef DEBUG - #define DEBUG_PRINT(x) Serial.print(x) - #define DEBUG_PRINTF(x, y) Serial.print(x, y) - #define DEBUG_PRINTLN(x) Serial.println(x) - #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) -#else - #define DEBUG_PRINT(x) - #define DEBUG_PRINTF(x, y) - #define DEBUG_PRINTLN(x) - #define DEBUG_PRINTLNF(x, y) -#endif - -#define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] -#define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] -#define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] - -/* ================================================================================================ * - | Default MotionApps v2.0 42-byte FIFO packet structure: | - | | - | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | - | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | - | | - | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | - | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | - * ================================================================================================ */ - -// this block of memory gets written to the MPU on start-up, and it seems -// to be volatile memory, so it has to be done each time (it only takes ~1 -// second though) - -// I Only Changed this by applying all the configuration data and capturing it before startup: -// *** this is a capture of the DMP Firmware after all the messy changes were made so we can just load it -const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { - /* bank # 0 */ - 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, - 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, - 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x40, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCB, 0x47, 0xA2, 0x20, 0x00, 0x00, 0x00, - 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, - 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, - 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, - 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, - 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, - /* bank # 1 */ - 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, - 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, - 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x09, 0x23, 0xA1, 0x35, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, - 0x80, 0x00, 0xFF, 0xFF, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, - 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, - /* bank # 2 */ - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0x8B, 0xC1, 0x00, 0x00, 0x01, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - /* bank # 3 */ - 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, - 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, - 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, - 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, - 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, - 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, - 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, - 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C, - 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, - 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, - 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, - 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, - 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, - 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, - 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, - 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, - /* bank # 4 */ - 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, - 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, - 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, - 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, - 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, - 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, - 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, - 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, - 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, - 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, - 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, - 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, - 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, - 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, - 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, - 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, - /* bank # 5 */ - 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, - 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, - 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, - 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, - 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, - 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, - 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, - 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, - 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, - 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, - 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, - 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, - 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, - 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, - 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, - 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, - /* bank # 6 */ - 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, - 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, - 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, - 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, - 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, - 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, - 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, - 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, - 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, - 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, - 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, - 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, - 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, - 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, - 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, - 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, - /* bank # 7 */ - 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, - 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, - 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, - 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, - 0xDD, 0xF1, 0x20, 0x28, 0x30, 0x38, 0x9A, 0xF1, 0x28, 0x30, 0x38, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, - 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, - 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0x28, 0x30, 0x38, - 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0x30, 0xDC, - 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xFE, 0xD8, 0xFF, - -}; - -#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR -#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default -#endif - -// I Simplified this: -uint8_t MPU6050::dmpInitialize() { - // reset device - DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); - reset(); - delay(30); // wait after reset - - // enable sleep mode and wake cycle - /*Serial.println(F("Enabling sleep mode...")); - setSleepEnabled(true); - Serial.println(F("Enabling wake cycle...")); - setWakeCycleEnabled(true);*/ - - // disable sleep mode - setSleepEnabled(false); - - // get MPU hardware revision - setMemoryBank(0x10, true, true); - setMemoryStartAddress(0x06); - Serial.println(F("Checking hardware revision...")); - Serial.print(F("Revision @ user[16][6] = ")); - Serial.println(readMemoryByte(), HEX); - Serial.println(F("Resetting memory bank selection to 0...")); - setMemoryBank(0, false, false); - - // check OTP bank valid - DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); - DEBUG_PRINT(F("OTP bank is ")); - DEBUG_PRINTLN(getOTPBankValid() ? F("valid!") : F("invalid!")); - - // setup weird slave stuff (?) - DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); - setSlaveAddress(0, 0x7F); - DEBUG_PRINTLN(F("Disabling I2C Master mode...")); - setI2CMasterModeEnabled(false); - DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); - setSlaveAddress(0, 0x68); - DEBUG_PRINTLN(F("Resetting I2C Master control...")); - resetI2CMaster(); - delay(20); - DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); - setClockSource(MPU6050_CLOCK_PLL_ZGYRO); - - DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); - setIntEnabled(1<= dmpGetFIFOPacketSize(); -} - -// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); -// uint8_t MPU6050::dmpGetFIFORate(); -// uint8_t MPU6050::dmpGetSampleStepSizeMS(); -// uint8_t MPU6050::dmpGetSampleFrequency(); -// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); - -//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); -//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); -//uint8_t MPU6050::dmpRunFIFORateProcesses(); - -// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); - -uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]); - data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]); - data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]); - return 0; -} -uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[28] << 8) | packet[29]; - data[1] = (packet[32] << 8) | packet[33]; - data[2] = (packet[36] << 8) | packet[37]; - return 0; -} -uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[28] << 8) | packet[29]; - v -> y = (packet[32] << 8) | packet[33]; - v -> z = (packet[36] << 8) | packet[37]; - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); - data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); - data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); - data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[0] << 8) | packet[1]); - data[1] = ((packet[4] << 8) | packet[5]); - data[2] = ((packet[8] << 8) | packet[9]); - data[3] = ((packet[12] << 8) | packet[13]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - int16_t qI[4]; - uint8_t status = dmpGetQuaternion(qI, packet); - if (status == 0) { - q -> w = (float)qI[0] / 16384.0f; - q -> x = (float)qI[1] / 16384.0f; - q -> y = (float)qI[2] / 16384.0f; - q -> z = (float)qI[3] / 16384.0f; - return 0; - } - return status; // int16 return value, indicates error if this line is reached -} -// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); - data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); - data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); - return 0; -} -uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[16] << 8) | packet[17]; - data[1] = (packet[20] << 8) | packet[21]; - data[2] = (packet[24] << 8) | packet[25]; - return 0; -} -uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[16] << 8) | packet[17]; - v -> y = (packet[20] << 8) | packet[21]; - v -> z = (packet[24] << 8) | packet[25]; - return 0; -} -// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); -// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { - // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) - v -> x = vRaw -> x - gravity -> x*8192; - v -> y = vRaw -> y - gravity -> y*8192; - v -> z = vRaw -> z - gravity -> z*8192; - return 0; -} -// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { - // rotate measured 3D acceleration vector into original state - // frame of reference based on orientation quaternion - memcpy(v, vReal, sizeof(VectorInt16)); - v -> rotate(q); - return 0; -} -// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { - /* +1g corresponds to +8192, sensitivity is 2g. */ - int16_t qI[4]; - uint8_t status = dmpGetQuaternion(qI, packet); - data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; - data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; - data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] - - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); - return status; -} - -uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { - v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); - v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); - v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; - return 0; -} -// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); -// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); - -uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi - data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta - data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi - return 0; -} - -#ifdef USE_OLD_DMPGETYAWPITCHROLL -uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { - // yaw: (about Z axis) - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); - // pitch: (nose up/down, about Y axis) - data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); - // roll: (tilt left/right, about X axis) - data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); - return 0; -} -#else -uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { - // yaw: (about Z axis) - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); - // pitch: (nose up/down, about Y axis) - data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); - // roll: (tilt left/right, about X axis) - data[2] = atan2(gravity -> y , gravity -> z); - if (gravity -> z < 0) { - if(data[1] > 0) { - data[1] = PI - data[1]; - } else { - data[1] = -PI - data[1]; - } - } - return 0; -} -#endif - -// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); - -uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { - /*for (uint8_t k = 0; k < dmpPacketSize; k++) { - if (dmpData[k] < 0x10) Serial.print("0"); - Serial.print(dmpData[k], HEX); - Serial.print(" "); - } - Serial.print("\n");*/ - //Serial.println((uint16_t)dmpPacketBuffer); - return 0; -} -uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { - uint8_t status; - uint8_t buf[dmpPacketSize]; - for (uint8_t i = 0; i < numPackets; i++) { - // read packet from FIFO - getFIFOBytes(buf, dmpPacketSize); - - // process packet - if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; - - // increment external process count variable, if supplied - if (processed != 0) (*processed)++; - } - return 0; -} - -// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); - -// uint8_t MPU6050::dmpInitFIFOParam(); -// uint8_t MPU6050::dmpCloseFIFO(); -// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); -// uint8_t MPU6050::dmpDecodeQuantizedAccel(); -// uint32_t MPU6050::dmpGetGyroSumOfSquare(); -// uint32_t MPU6050::dmpGetAccelSumOfSquare(); -// void MPU6050::dmpOverrideQuaternion(long *q); -uint16_t MPU6050::dmpGetFIFOPacketSize() { - return dmpPacketSize; -} - -#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h b/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h deleted file mode 100644 index 34a4ccc5..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h +++ /dev/null @@ -1,617 +0,0 @@ -// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 6.12 implementation -// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) -// 5/20/2013 by Jeff Rowberg -// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib -// -// Changelog: -// 2019/7/10 - I incorporated DMP Firmware Version 6.12 Latest as of today with many features and bug fixes. -// - MPU6050 Registers have not changed just the DMP Image so that full backwards compatibility is present -// - Run-time calibration routine is enabled which calibrates after no motion state is detected -// - once no motion state is detected Calibration completes within 0.5 seconds -// - The Drawback is that the firmware image is larger. -// ... - ongoing debug release - -/* ============================================ -I2Cdev device library code is placed under the MIT license -Copyright (c) 2012 Jeff Rowberg - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. -=============================================== -*/ - -#ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ -#define _MPU6050_6AXIS_MOTIONAPPS20_H_ - -#include "I2Cdev.h" -#include "helper_3dmath.h" - -// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board -#define MPU6050_INCLUDE_DMP_MOTIONAPPS20 // same definitions Should work with V6.12 - -#include "MPU6050.h" - -// Tom Carpenter's conditional PROGMEM code -// http://forum.arduino.cc/index.php?topic=129407.0 -#ifdef __AVR__ - #include -#elif defined(ESP8266) || defined(ESP32) - #include -#else - // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen - #ifndef __PGMSPACE_H_ - #define __PGMSPACE_H_ 1 - #include - - #define PROGMEM - #define PGM_P const char * - #define PSTR(str) (str) - #define F(x) x - - typedef void prog_void; - typedef char prog_char; - typedef unsigned char prog_uchar; - typedef int8_t prog_int8_t; - typedef uint8_t prog_uint8_t; - typedef int16_t prog_int16_t; - typedef uint16_t prog_uint16_t; - typedef int32_t prog_int32_t; - typedef uint32_t prog_uint32_t; - - #define strcpy_P(dest, src) strcpy((dest), (src)) - #define strcat_P(dest, src) strcat((dest), (src)) - #define strcmp_P(a, b) strcmp((a), (b)) - - #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) - #define pgm_read_word(addr) (*(const unsigned short *)(addr)) - #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) - #define pgm_read_float(addr) (*(const float *)(addr)) - - #define pgm_read_byte_near(addr) pgm_read_byte(addr) - #define pgm_read_word_near(addr) pgm_read_word(addr) - #define pgm_read_dword_near(addr) pgm_read_dword(addr) - #define pgm_read_float_near(addr) pgm_read_float(addr) - #define pgm_read_byte_far(addr) pgm_read_byte(addr) - #define pgm_read_word_far(addr) pgm_read_word(addr) - #define pgm_read_dword_far(addr) pgm_read_dword(addr) - #define pgm_read_float_far(addr) pgm_read_float(addr) - #endif -#endif - -/* Source is from the InvenSense MotionApps v2 demo code. Original source is - * unavailable, unless you happen to be amazing as decompiling binary by - * hand (in which case, please contact me, and I'm totally serious). - * - * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the - * DMP reverse-engineering he did to help make this bit of wizardry - * possible. - */ - -// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. -// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) -// after moving string constants to flash memory storage using the F() -// compiler macro (Arduino IDE 1.0+ required). - -//#define DEBUG -#ifdef DEBUG - #define DEBUG_PRINT(x) Serial.print(x) - #define DEBUG_PRINTF(x, y) Serial.print(x, y) - #define DEBUG_PRINTLN(x) Serial.println(x) - #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) -#else - #define DEBUG_PRINT(x) - #define DEBUG_PRINTF(x, y) - #define DEBUG_PRINTLN(x) - #define DEBUG_PRINTLNF(x, y) -#endif - -#define MPU6050_DMP_CODE_SIZE 3062 // dmpMemory[] - -/* ================================================================ * - | Default MotionApps v6.12 28-byte FIFO packet structure: | - | | - | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ] | - | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 | - | | - | [GYRO X][GYRO Y][GYRO Z][ACC X ][ACC Y ][ACC Z ] | - | 16 17 18 19 20 21 22 23 24 25 26 27 | - * ================================================================ */ - -// this block of memory gets written to the MPU on start-up, and it seems -// to be volatile memory, so it has to be done each time (it only takes ~1 -// second though) - -// *** this is a capture of the DMP Firmware V6.1.2 after all the messy changes were made so we can just load it -const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { -/* bank # 0 */ -0x00, 0xF8, 0xF6, 0x2A, 0x3F, 0x68, 0xF5, 0x7A, 0x00, 0x06, 0xFF, 0xFE, 0x00, 0x03, 0x00, 0x00, -0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, -0x03, 0x0C, 0x30, 0xC3, 0x0A, 0x74, 0x56, 0x2D, 0x0D, 0x62, 0xDB, 0xC7, 0x16, 0xF4, 0xBA, 0x02, -0x38, 0x83, 0xF8, 0x83, 0x30, 0x00, 0xF8, 0x83, 0x25, 0x8E, 0xF8, 0x83, 0x30, 0x00, 0xF8, 0x83, -0xFF, 0xFF, 0xFF, 0xFF, 0x0C, 0xBD, 0xD8, 0x11, 0x24, 0x00, 0x04, 0x00, 0x1A, 0x82, 0x79, 0xA1, -0x00, 0x36, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6F, 0xA2, -0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, -0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, -0x1F, 0xA4, 0xE8, 0xE4, 0xFF, 0xF5, 0xDC, 0xB9, 0x00, 0x5B, 0x79, 0xCF, 0x1F, 0x3F, 0x78, 0x76, -0x00, 0x86, 0x7C, 0x5A, 0x00, 0x86, 0x23, 0x47, 0xFA, 0xB9, 0x86, 0x31, 0x00, 0x74, 0x87, 0x8A, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x43, 0x05, 0xFF, 0xFF, 0xE9, 0xA8, 0x00, 0x00, 0x21, 0x82, -0xFA, 0xB8, 0x4D, 0x46, 0xFF, 0xFA, 0xDF, 0x3D, 0xFF, 0xFF, 0xB2, 0xB3, 0x00, 0x00, 0x00, 0x00, -0x3F, 0xFF, 0xBA, 0x98, 0x00, 0x5D, 0xAC, 0x08, 0x00, 0x0A, 0x63, 0x78, 0x00, 0x01, 0x46, 0x21, -0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x42, 0xB5, 0x00, 0x06, 0x00, 0x64, 0x00, 0x64, 0x00, 0x06, -0x14, 0x06, 0x02, 0x9F, 0x0F, 0x47, 0x91, 0x32, 0xD9, 0x0E, 0x9F, 0xC9, 0x1D, 0xCF, 0x4C, 0x34, -0x3B, 0xB6, 0x7A, 0xE8, 0x00, 0x64, 0x00, 0x06, 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFE, -/* bank # 1 */ -0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -0x00, 0x07, 0x00, 0x00, 0xFF, 0xF1, 0x00, 0x00, 0xFA, 0x46, 0x00, 0x00, 0xA2, 0xB8, 0x00, 0x00, -0x10, 0x00, 0x00, 0x00, 0x04, 0xD6, 0x00, 0x00, 0x04, 0xCC, 0x00, 0x00, 0x04, 0xCC, 0x00, 0x00, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, -0x00, 0x00, 0x00, 0x32, 0xF8, 0x98, 0x00, 0x00, 0xFF, 0x65, 0x00, 0x00, 0x83, 0x0F, 0x00, 0x00, -0x00, 0x06, 0x00, 0x00, 0xFF, 0xF1, 0x00, 0x00, 0xFA, 0x46, 0x00, 0x00, 0xA2, 0xB8, 0x00, 0x00, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, -0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0D, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x02, 0x00, 0x00, -0x00, 0x01, 0xFB, 0x83, 0x00, 0x7C, 0x00, 0x00, 0xFB, 0x15, 0xFC, 0x00, 0x1F, 0xB4, 0xFF, 0x83, -0x00, 0x00, 0x00, 0x01, 0x00, 0x65, 0x00, 0x07, 0x00, 0x64, 0x03, 0xE8, 0x00, 0x64, 0x00, 0x28, -0x00, 0x00, 0xFF, 0xFD, 0x00, 0x00, 0x00, 0x00, 0x16, 0xA0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -0x00, 0x00, 0x10, 0x00, 0x00, 0x2F, 0x00, 0x00, 0x00, 0x00, 0x01, 0xF4, 0x00, 0x00, 0x10, 0x00, -/* bank # 2 */ -0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, -0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0xBA, 0xC6, 0x00, 0x47, 0x78, 0xA2, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, -0x00, 0x00, 0x23, 0xBB, 0x00, 0x2E, 0xA2, 0x5B, 0x00, 0x00, 0x05, 0x68, 0x00, 0x0B, 0xCF, 0x49, -0x00, 0x04, 0xFF, 0xFD, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, -0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -0x00, 0x64, 0x00, 0x07, 0x00, 0x08, 0x00, 0x06, 0x00, 0x06, 0xFF, 0xFE, 0x00, 0x00, 0x00, 0x00, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -0x00, 0x2E, 0xA2, 0x5B, 0x00, 0x00, 0x05, 0x68, 0x00, 0x0B, 0xCF, 0x49, 0x00, 0x00, 0x00, 0x00, -0x00, 0xF8, 0xF6, 0x2A, 0x3F, 0x68, 0xF5, 0x7A, 0x00, 0x04, 0xFF, 0xFD, 0x00, 0x02, 0x00, 0x00, -0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x0E, -0xFF, 0xFF, 0xFF, 0xCF, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xFF, 0xFF, 0xFF, 0x9C, -0x00, 0x00, 0x43, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, -0xFF, 0xE5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -/* bank # 3 */ -0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xD3, -0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3C, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1F, 0x9E, 0x65, 0x5D, -0x0C, 0x0A, 0x4E, 0x68, 0xCD, 0xCF, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xC6, 0x19, 0xCE, 0x82, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x71, 0x1C, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xD7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, -0x00, 0x11, 0xDC, 0x47, 0x03, 0x00, 0x00, 0x00, 0xC7, 0x93, 0x8F, 0x9D, 0x1E, 0x1B, 0x1C, 0x19, -0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0E, 0xDF, 0xA4, 0x38, 0x1F, 0x9E, 0x65, 0x5D, -0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x71, 0x1C, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -0x3F, 0xFF, 0xFF, 0xFD, 0xFF, 0xFF, 0xF4, 0xC9, 0xFF, 0xFF, 0xBC, 0xF0, 0x00, 0x01, 0x0C, 0x0F, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -0xF5, 0xB7, 0xBA, 0xB3, 0x67, 0x7D, 0xDF, 0x7E, 0x72, 0x90, 0x2E, 0x55, 0x4C, 0xF6, 0xE6, 0x88, -0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, -/* bank # 4 */ -0xD8, 0xDC, 0xB4, 0xB8, 0xB0, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xB3, 0xB7, 0xBB, 0x8E, 0x9E, -0xAE, 0xF1, 0x32, 0xF5, 0x1B, 0xF1, 0xB4, 0xB8, 0xB0, 0x80, 0x97, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, -0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0xF1, 0xA9, -0x89, 0x26, 0x46, 0x66, 0xB2, 0x89, 0x99, 0xA9, 0x2D, 0x55, 0x7D, 0xB0, 0xB0, 0x8A, 0xA8, 0x96, -0x36, 0x56, 0x76, 0xF1, 0xBA, 0xA3, 0xB4, 0xB2, 0x80, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB2, 0x83, -0x98, 0xBA, 0xA3, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xB2, 0xB9, 0xB4, 0x98, 0x83, 0xF1, -0xA3, 0x29, 0x55, 0x7D, 0xBA, 0xB5, 0xB1, 0xA3, 0x83, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xB2, -0xB6, 0xAA, 0x83, 0x93, 0x28, 0x54, 0x7C, 0xF1, 0xB9, 0xA3, 0x82, 0x93, 0x61, 0xBA, 0xA2, 0xDA, -0xDE, 0xDF, 0xDB, 0x81, 0x9A, 0xB9, 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xF1, 0xDA, 0xBA, 0xA2, 0xDF, -0xD9, 0xBA, 0xA2, 0xFA, 0xB9, 0xA3, 0x82, 0x92, 0xDB, 0x31, 0xBA, 0xA2, 0xD9, 0xBA, 0xA2, 0xF8, -0xDF, 0x85, 0xA4, 0xD0, 0xC1, 0xBB, 0xAD, 0x83, 0xC2, 0xC5, 0xC7, 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, -0xBA, 0xA0, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xAA, 0xB3, 0x8D, 0xB4, 0x98, 0x0D, 0x35, -0x5D, 0xB2, 0xB6, 0xBA, 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, -0xB8, 0xAA, 0x87, 0x2C, 0x54, 0x7C, 0xBA, 0xA4, 0xB0, 0x8A, 0xB6, 0x91, 0x32, 0x56, 0x76, 0xB2, -0x84, 0x94, 0xA4, 0xC8, 0x08, 0xCD, 0xD8, 0xB8, 0xB4, 0xB0, 0xF1, 0x99, 0x82, 0xA8, 0x2D, 0x55, -0x7D, 0x98, 0xA8, 0x0E, 0x16, 0x1E, 0xA2, 0x2C, 0x54, 0x7C, 0x92, 0xA4, 0xF0, 0x2C, 0x50, 0x78, -/* bank # 5 */ -0xF1, 0x84, 0xA8, 0x98, 0xC4, 0xCD, 0xFC, 0xD8, 0x0D, 0xDB, 0xA8, 0xFC, 0x2D, 0xF3, 0xD9, 0xBA, -0xA6, 0xF8, 0xDA, 0xBA, 0xA6, 0xDE, 0xD8, 0xBA, 0xB2, 0xB6, 0x86, 0x96, 0xA6, 0xD0, 0xF3, 0xC8, -0x41, 0xDA, 0xA6, 0xC8, 0xF8, 0xD8, 0xB0, 0xB4, 0xB8, 0x82, 0xA8, 0x92, 0xF5, 0x2C, 0x54, 0x88, -0x98, 0xF1, 0x35, 0xD9, 0xF4, 0x18, 0xD8, 0xF1, 0xA2, 0xD0, 0xF8, 0xF9, 0xA8, 0x84, 0xD9, 0xC7, -0xDF, 0xF8, 0xF8, 0x83, 0xC5, 0xDA, 0xDF, 0x69, 0xDF, 0x83, 0xC1, 0xD8, 0xF4, 0x01, 0x14, 0xF1, -0xA8, 0x82, 0x4E, 0xA8, 0x84, 0xF3, 0x11, 0xD1, 0x82, 0xF5, 0xD9, 0x92, 0x28, 0x97, 0x88, 0xF1, -0x09, 0xF4, 0x1C, 0x1C, 0xD8, 0x84, 0xA8, 0xF3, 0xC0, 0xF9, 0xD1, 0xD9, 0x97, 0x82, 0xF1, 0x29, -0xF4, 0x0D, 0xD8, 0xF3, 0xF9, 0xF9, 0xD1, 0xD9, 0x82, 0xF4, 0xC2, 0x03, 0xD8, 0xDE, 0xDF, 0x1A, -0xD8, 0xF1, 0xA2, 0xFA, 0xF9, 0xA8, 0x84, 0x98, 0xD9, 0xC7, 0xDF, 0xF8, 0xF8, 0xF8, 0x83, 0xC7, -0xDA, 0xDF, 0x69, 0xDF, 0xF8, 0x83, 0xC3, 0xD8, 0xF4, 0x01, 0x14, 0xF1, 0x98, 0xA8, 0x82, 0x2E, -0xA8, 0x84, 0xF3, 0x11, 0xD1, 0x82, 0xF5, 0xD9, 0x92, 0x50, 0x97, 0x88, 0xF1, 0x09, 0xF4, 0x1C, -0xD8, 0x84, 0xA8, 0xF3, 0xC0, 0xF8, 0xF9, 0xD1, 0xD9, 0x97, 0x82, 0xF1, 0x49, 0xF4, 0x0D, 0xD8, -0xF3, 0xF9, 0xF9, 0xD1, 0xD9, 0x82, 0xF4, 0xC4, 0x03, 0xD8, 0xDE, 0xDF, 0xD8, 0xF1, 0xAD, 0x88, -0x98, 0xCC, 0xA8, 0x09, 0xF9, 0xD9, 0x82, 0x92, 0xA8, 0xF5, 0x7C, 0xF1, 0x88, 0x3A, 0xCF, 0x94, -0x4A, 0x6E, 0x98, 0xDB, 0x69, 0x31, 0xDA, 0xAD, 0xF2, 0xDE, 0xF9, 0xD8, 0x87, 0x95, 0xA8, 0xF2, -0x21, 0xD1, 0xDA, 0xA5, 0xF9, 0xF4, 0x17, 0xD9, 0xF1, 0xAE, 0x8E, 0xD0, 0xC0, 0xC3, 0xAE, 0x82, -/* bank # 6 */ -0xC6, 0x84, 0xC3, 0xA8, 0x85, 0x95, 0xC8, 0xA5, 0x88, 0xF2, 0xC0, 0xF1, 0xF4, 0x01, 0x0E, 0xF1, -0x8E, 0x9E, 0xA8, 0xC6, 0x3E, 0x56, 0xF5, 0x54, 0xF1, 0x88, 0x72, 0xF4, 0x01, 0x15, 0xF1, 0x98, -0x45, 0x85, 0x6E, 0xF5, 0x8E, 0x9E, 0x04, 0x88, 0xF1, 0x42, 0x98, 0x5A, 0x8E, 0x9E, 0x06, 0x88, -0x69, 0xF4, 0x01, 0x1C, 0xF1, 0x98, 0x1E, 0x11, 0x08, 0xD0, 0xF5, 0x04, 0xF1, 0x1E, 0x97, 0x02, -0x02, 0x98, 0x36, 0x25, 0xDB, 0xF9, 0xD9, 0x85, 0xA5, 0xF3, 0xC1, 0xDA, 0x85, 0xA5, 0xF3, 0xDF, -0xD8, 0x85, 0x95, 0xA8, 0xF3, 0x09, 0xDA, 0xA5, 0xFA, 0xD8, 0x82, 0x92, 0xA8, 0xF5, 0x78, 0xF1, -0x88, 0x1A, 0x84, 0x9F, 0x26, 0x88, 0x98, 0x21, 0xDA, 0xF4, 0x1D, 0xF3, 0xD8, 0x87, 0x9F, 0x39, -0xD1, 0xAF, 0xD9, 0xDF, 0xDF, 0xFB, 0xF9, 0xF4, 0x0C, 0xF3, 0xD8, 0xFA, 0xD0, 0xF8, 0xDA, 0xF9, -0xF9, 0xD0, 0xDF, 0xD9, 0xF9, 0xD8, 0xF4, 0x0B, 0xD8, 0xF3, 0x87, 0x9F, 0x39, 0xD1, 0xAF, 0xD9, -0xDF, 0xDF, 0xF4, 0x1D, 0xF3, 0xD8, 0xFA, 0xFC, 0xA8, 0x69, 0xF9, 0xF9, 0xAF, 0xD0, 0xDA, 0xDE, -0xFA, 0xD9, 0xF8, 0x8F, 0x9F, 0xA8, 0xF1, 0xCC, 0xF3, 0x98, 0xDB, 0x45, 0xD9, 0xAF, 0xDF, 0xD0, -0xF8, 0xD8, 0xF1, 0x8F, 0x9F, 0xA8, 0xCA, 0xF3, 0x88, 0x09, 0xDA, 0xAF, 0x8F, 0xCB, 0xF8, 0xD8, -0xF2, 0xAD, 0x97, 0x8D, 0x0C, 0xD9, 0xA5, 0xDF, 0xF9, 0xBA, 0xA6, 0xF3, 0xFA, 0xF4, 0x12, 0xF2, -0xD8, 0x95, 0x0D, 0xD1, 0xD9, 0xBA, 0xA6, 0xF3, 0xFA, 0xDA, 0xA5, 0xF2, 0xC1, 0xBA, 0xA6, 0xF3, -0xDF, 0xD8, 0xF1, 0xBA, 0xB2, 0xB6, 0x86, 0x96, 0xA6, 0xD0, 0xCA, 0xF3, 0x49, 0xDA, 0xA6, 0xCB, -0xF8, 0xD8, 0xB0, 0xB4, 0xB8, 0xD8, 0xAD, 0x84, 0xF2, 0xC0, 0xDF, 0xF1, 0x8F, 0xCB, 0xC3, 0xA8, -/* bank # 7 */ -0xB2, 0xB6, 0x86, 0x96, 0xC8, 0xC1, 0xCB, 0xC3, 0xF3, 0xB0, 0xB4, 0x88, 0x98, 0xA8, 0x21, 0xDB, -0x71, 0x8D, 0x9D, 0x71, 0x85, 0x95, 0x21, 0xD9, 0xAD, 0xF2, 0xFA, 0xD8, 0x85, 0x97, 0xA8, 0x28, -0xD9, 0xF4, 0x08, 0xD8, 0xF2, 0x8D, 0x29, 0xDA, 0xF4, 0x05, 0xD9, 0xF2, 0x85, 0xA4, 0xC2, 0xF2, -0xD8, 0xA8, 0x8D, 0x94, 0x01, 0xD1, 0xD9, 0xF4, 0x11, 0xF2, 0xD8, 0x87, 0x21, 0xD8, 0xF4, 0x0A, -0xD8, 0xF2, 0x84, 0x98, 0xA8, 0xC8, 0x01, 0xD1, 0xD9, 0xF4, 0x11, 0xD8, 0xF3, 0xA4, 0xC8, 0xBB, -0xAF, 0xD0, 0xF2, 0xDE, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xD8, 0xF1, 0xB8, 0xF6, -0xB5, 0xB9, 0xB0, 0x8A, 0x95, 0xA3, 0xDE, 0x3C, 0xA3, 0xD9, 0xF8, 0xD8, 0x5C, 0xA3, 0xD9, 0xF8, -0xD8, 0x7C, 0xA3, 0xD9, 0xF8, 0xD8, 0xF8, 0xF9, 0xD1, 0xA5, 0xD9, 0xDF, 0xDA, 0xFA, 0xD8, 0xB1, -0x85, 0x30, 0xF7, 0xD9, 0xDE, 0xD8, 0xF8, 0x30, 0xAD, 0xDA, 0xDE, 0xD8, 0xF2, 0xB4, 0x8C, 0x99, -0xA3, 0x2D, 0x55, 0x7D, 0xA0, 0x83, 0xDF, 0xDF, 0xDF, 0xB5, 0x91, 0xA0, 0xF6, 0x29, 0xD9, 0xFB, -0xD8, 0xA0, 0xFC, 0x29, 0xD9, 0xFA, 0xD8, 0xA0, 0xD0, 0x51, 0xD9, 0xF8, 0xD8, 0xFC, 0x51, 0xD9, -0xF9, 0xD8, 0x79, 0xD9, 0xFB, 0xD8, 0xA0, 0xD0, 0xFC, 0x79, 0xD9, 0xFA, 0xD8, 0xA1, 0xF9, 0xF9, -0xF9, 0xF9, 0xF9, 0xA0, 0xDA, 0xDF, 0xDF, 0xDF, 0xD8, 0xA1, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xAC, -0xDE, 0xF8, 0xAD, 0xDE, 0x83, 0x93, 0xAC, 0x2C, 0x54, 0x7C, 0xF1, 0xA8, 0xDF, 0xDF, 0xDF, 0xF6, -0x9D, 0x2C, 0xDA, 0xA0, 0xDF, 0xD9, 0xFA, 0xDB, 0x2D, 0xF8, 0xD8, 0xA8, 0x50, 0xDA, 0xA0, 0xD0, -0xDE, 0xD9, 0xD0, 0xF8, 0xF8, 0xF8, 0xDB, 0x55, 0xF8, 0xD8, 0xA8, 0x78, 0xDA, 0xA0, 0xD0, 0xDF, -/* bank # 8 */ -0xD9, 0xD0, 0xFA, 0xF8, 0xF8, 0xF8, 0xF8, 0xDB, 0x7D, 0xF8, 0xD8, 0x9C, 0xA8, 0x8C, 0xF5, 0x30, -0xDB, 0x38, 0xD9, 0xD0, 0xDE, 0xDF, 0xA0, 0xD0, 0xDE, 0xDF, 0xD8, 0xA8, 0x48, 0xDB, 0x58, 0xD9, -0xDF, 0xD0, 0xDE, 0xA0, 0xDF, 0xD0, 0xDE, 0xD8, 0xA8, 0x68, 0xDB, 0x70, 0xD9, 0xDF, 0xDF, 0xA0, -0xDF, 0xDF, 0xD8, 0xF1, 0xA8, 0x88, 0x90, 0x2C, 0x54, 0x7C, 0x98, 0xA8, 0xD0, 0x5C, 0x38, 0xD1, -0xDA, 0xF2, 0xAE, 0x8C, 0xDF, 0xF9, 0xD8, 0xB0, 0x87, 0xA8, 0xC1, 0xC1, 0xB1, 0x88, 0xA8, 0xC6, -0xF9, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, -0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xF7, 0x8D, 0x9D, 0xAD, 0xF8, 0x18, 0xDA, -0xF2, 0xAE, 0xDF, 0xD8, 0xF7, 0xAD, 0xFA, 0x30, 0xD9, 0xA4, 0xDE, 0xF9, 0xD8, 0xF2, 0xAE, 0xDE, -0xFA, 0xF9, 0x83, 0xA7, 0xD9, 0xC3, 0xC5, 0xC7, 0xF1, 0x88, 0x9B, 0xA7, 0x7A, 0xAD, 0xF7, 0xDE, -0xDF, 0xA4, 0xF8, 0x84, 0x94, 0x08, 0xA7, 0x97, 0xF3, 0x00, 0xAE, 0xF2, 0x98, 0x19, 0xA4, 0x88, -0xC6, 0xA3, 0x94, 0x88, 0xF6, 0x32, 0xDF, 0xF2, 0x83, 0x93, 0xDB, 0x09, 0xD9, 0xF2, 0xAA, 0xDF, -0xD8, 0xD8, 0xAE, 0xF8, 0xF9, 0xD1, 0xDA, 0xF3, 0xA4, 0xDE, 0xA7, 0xF1, 0x88, 0x9B, 0x7A, 0xD8, -0xF3, 0x84, 0x94, 0xAE, 0x19, 0xF9, 0xDA, 0xAA, 0xF1, 0xDF, 0xD8, 0xA8, 0x81, 0xC0, 0xC3, 0xC5, -0xC7, 0xA3, 0x92, 0x83, 0xF6, 0x28, 0xAD, 0xDE, 0xD9, 0xF8, 0xD8, 0xA3, 0x50, 0xAD, 0xD9, 0xF8, -0xD8, 0xA3, 0x78, 0xAD, 0xD9, 0xF8, 0xD8, 0xF8, 0xF9, 0xD1, 0xA1, 0xDA, 0xDE, 0xC3, 0xC5, 0xC7, -0xD8, 0xA1, 0x81, 0x94, 0xF8, 0x18, 0xF2, 0xB0, 0x89, 0xAC, 0xC3, 0xC5, 0xC7, 0xF1, 0xD8, 0xB8, -/* bank # 9 */ -0xB4, 0xB0, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, -0x0C, 0x20, 0x14, 0x40, 0xB0, 0xB4, 0xB8, 0xF0, 0xA8, 0x8A, 0x9A, 0x28, 0x50, 0x78, 0xB7, 0x9B, -0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xF1, 0xBB, 0xAB, -0x88, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0xB3, 0x8B, 0xB8, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0xB0, -0x88, 0xB4, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xBB, 0xAB, 0xB3, 0x8B, 0x02, 0x26, 0x46, 0x66, 0xB0, -0xB8, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 0x8A, 0x24, 0x70, 0x59, -0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 0x8A, 0x64, 0x48, 0x31, -0x8B, 0x30, 0x49, 0x60, 0x88, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 0x28, -0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 0xF0, -0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xA9, -0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 0x8C, -0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 0x7E, -0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB5, 0xB9, 0xA3, 0xDF, 0xDF, 0xDF, 0xAE, 0xD0, -0xDF, 0xAA, 0xD0, 0xDE, 0xF2, 0xAB, 0xF8, 0xF9, 0xD9, 0xB0, 0x87, 0xC4, 0xAA, 0xF1, 0xDF, 0xDF, -0xBB, 0xAF, 0xDF, 0xDF, 0xB9, 0xD8, 0xB1, 0xF1, 0xA3, 0x97, 0x8E, 0x60, 0xDF, 0xB0, 0x84, 0xF2, -0xC8, 0xF8, 0xF9, 0xD9, 0xDE, 0xD8, 0x93, 0x85, 0xF1, 0x4A, 0xB1, 0x83, 0xA3, 0x08, 0xB5, 0x83, -/* bank # 10 */ -0x9A, 0x08, 0x10, 0xB7, 0x9F, 0x10, 0xD8, 0xF1, 0xB0, 0xBA, 0xAE, 0xB0, 0x8A, 0xC2, 0xB2, 0xB6, -0x8E, 0x9E, 0xF1, 0xFB, 0xD9, 0xF4, 0x1D, 0xD8, 0xF9, 0xD9, 0x0C, 0xF1, 0xD8, 0xF8, 0xF8, 0xAD, -0x61, 0xD9, 0xAE, 0xFB, 0xD8, 0xF4, 0x0C, 0xF1, 0xD8, 0xF8, 0xF8, 0xAD, 0x19, 0xD9, 0xAE, 0xFB, -0xDF, 0xD8, 0xF4, 0x16, 0xF1, 0xD8, 0xF8, 0xAD, 0x8D, 0x61, 0xD9, 0xF4, 0xF4, 0xAC, 0xF5, 0x9C, -0x9C, 0x8D, 0xDF, 0x2B, 0xBA, 0xB6, 0xAE, 0xFA, 0xF8, 0xF4, 0x0B, 0xD8, 0xF1, 0xAE, 0xD0, 0xF8, -0xAD, 0x51, 0xDA, 0xAE, 0xFA, 0xF8, 0xF1, 0xD8, 0xB9, 0xB1, 0xB6, 0xA3, 0x83, 0x9C, 0x08, 0xB9, -0xB1, 0x83, 0x9A, 0xB5, 0xAA, 0xC0, 0xFD, 0x30, 0x83, 0xB7, 0x9F, 0x10, 0xB5, 0x8B, 0x93, 0xF2, -0x02, 0x02, 0xD1, 0xAB, 0xDA, 0xDE, 0xD8, 0xF1, 0xB0, 0x80, 0xBA, 0xAB, 0xC0, 0xC3, 0xB2, 0x84, -0xC1, 0xC3, 0xD8, 0xB1, 0xB9, 0xF3, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, -0x87, 0x9C, 0xB9, 0xA3, 0xDD, 0xF1, 0xB3, 0x8B, 0x8B, 0x8B, 0x8B, 0x8B, 0xB0, 0x87, 0x20, 0x28, -0x30, 0x38, 0xB2, 0x8B, 0xB6, 0x9B, 0xF2, 0xA3, 0xC0, 0xC8, 0xC2, 0xC4, 0xCC, 0xC6, 0xA3, 0xA3, -0xA3, 0xF1, 0xB0, 0x87, 0xB5, 0x9A, 0xD8, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 0xBA, 0xAC, 0xDF, 0xB9, -0xA3, 0xFE, 0xF2, 0xAB, 0xC4, 0xAA, 0xF1, 0xDF, 0xDF, 0xBB, 0xAF, 0xDF, 0xDF, 0xA3, 0xA3, 0xA3, -0xD8, 0xD8, 0xD8, 0xBB, 0xB3, 0xB7, 0xF1, 0xAA, 0xF9, 0xDA, 0xFF, 0xD9, 0x80, 0x9A, 0xAA, 0x28, -0xB4, 0x80, 0x98, 0xA7, 0x20, 0xB7, 0x97, 0x87, 0xA8, 0x66, 0x88, 0xF0, 0x79, 0x51, 0xF1, 0x90, -0x2C, 0x87, 0x0C, 0xA7, 0x81, 0x97, 0x62, 0x93, 0xF0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, -/* bank # 11 */ -0x51, 0x79, 0x90, 0xA5, 0xF1, 0x28, 0x4C, 0x6C, 0x87, 0x0C, 0x95, 0x18, 0x85, 0x78, 0xA3, 0x83, -0x90, 0x28, 0x4C, 0x6C, 0x88, 0x6C, 0xD8, 0xF3, 0xA2, 0x82, 0x00, 0xF2, 0x10, 0xA8, 0x92, 0x19, -0x80, 0xA2, 0xF2, 0xD9, 0x26, 0xD8, 0xF1, 0x88, 0xA8, 0x4D, 0xD9, 0x48, 0xD8, 0x96, 0xA8, 0x39, -0x80, 0xD9, 0x3C, 0xD8, 0x95, 0x80, 0xA8, 0x39, 0xA6, 0x86, 0x98, 0xD9, 0x2C, 0xDA, 0x87, 0xA7, -0x2C, 0xD8, 0xA8, 0x89, 0x95, 0x19, 0xA9, 0x80, 0xD9, 0x38, 0xD8, 0xA8, 0x89, 0x39, 0xA9, 0x80, -0xDA, 0x3C, 0xD8, 0xA8, 0x2E, 0xA8, 0x39, 0x90, 0xD9, 0x0C, 0xD8, 0xA8, 0x95, 0x31, 0x98, 0xD9, -0x0C, 0xD8, 0xA8, 0x09, 0xD9, 0xFF, 0xD8, 0x01, 0xDA, 0xFF, 0xD8, 0x95, 0x39, 0xA9, 0xDA, 0x26, -0xFF, 0xD8, 0x90, 0xA8, 0x0D, 0x89, 0x99, 0xA8, 0x10, 0x80, 0x98, 0x21, 0xDA, 0x2E, 0xD8, 0x89, -0x99, 0xA8, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, 0x86, 0x96, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, -0x87, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, 0x82, 0x92, 0xF3, 0x41, 0x80, 0xF1, 0xD9, 0x2E, 0xD8, -0xA8, 0x82, 0xF3, 0x19, 0x80, 0xF1, 0xD9, 0x2E, 0xD8, 0x82, 0xAC, 0xF3, 0xC0, 0xA2, 0x80, 0x22, -0xF1, 0xA6, 0x2E, 0xA7, 0x2E, 0xA9, 0x22, 0x98, 0xA8, 0x29, 0xDA, 0xAC, 0xDE, 0xFF, 0xD8, 0xA2, -0xF2, 0x2A, 0xF1, 0xA9, 0x2E, 0x82, 0x92, 0xA8, 0xF2, 0x31, 0x80, 0xA6, 0x96, 0xF1, 0xD9, 0x00, -0xAC, 0x8C, 0x9C, 0x0C, 0x30, 0xAC, 0xDE, 0xD0, 0xDE, 0xFF, 0xD8, 0x8C, 0x9C, 0xAC, 0xD0, 0x10, -0xAC, 0xDE, 0x80, 0x92, 0xA2, 0xF2, 0x4C, 0x82, 0xA8, 0xF1, 0xCA, 0xF2, 0x35, 0xF1, 0x96, 0x88, -0xA6, 0xD9, 0x00, 0xD8, 0xF1, 0xFF, -}; - -// this divisor is pre configured into the above image and can't be modified at this time. -#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR -#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default -#endif - -// this is the most basic initialization I can create. with the intent that we access the register bytes as few times as needed to get the job done. -// for detailed descriptins of all registers and there purpose google "MPU-6000/MPU-6050 Register Map and Descriptions" -uint8_t MPU6050::dmpInitialize() { // Lets get it over with fast Write everything once and set it up necely - uint8_t val; - uint16_t ival; - // Reset procedure per instructions in the "MPU-6000/MPU-6050 Register Map and Descriptions" page 41 - I2Cdev::writeBit(devAddr,0x6B, 7, (val = 1)); //PWR_MGMT_1: reset with 100ms delay - delay(100); - I2Cdev::writeBits(devAddr,0x6A, 2, 3, (val = 0b111)); // full SIGNAL_PATH_RESET: with another 100ms delay - delay(100); - I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 1000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro - I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x00)); // 0000 0000 INT_ENABLE: no Interrupt - I2Cdev::writeBytes(devAddr,0x23, 1, &(val = 0x00)); // 0000 0000 MPU FIFO_EN: (all off) Using DMP's FIFO instead - I2Cdev::writeBytes(devAddr,0x1C, 1, &(val = 0x00)); // 0000 0000 ACCEL_CONFIG: 0 = Accel Full Scale Select: 2g - I2Cdev::writeBytes(devAddr,0x37, 1, &(val = 0x80)); // 1001 0000 INT_PIN_CFG: ACTL The logic level for int pin is active low. and interrupt status bits are cleared on any read - I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 0000 0001 PWR_MGMT_1: Clock Source Select PLL_X_gyro - I2Cdev::writeBytes(devAddr,0x19, 1, &(val = 0x04)); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)) - I2Cdev::writeBytes(devAddr,0x1A, 1, &(val = 0x01)); // 0000 0001 CONFIG: Digital Low Pass Filter (DLPF) Configuration 188HZ //Im betting this will be the beat - if (!writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) return 1; // Loads the DMP image into the MPU6050 Memory // Should Never Fail - I2Cdev::writeWords(devAddr, 0x70, 1, &(ival = 0x0400)); // DMP Program Start Address - I2Cdev::writeBytes(devAddr,0x1B, 1, &(val = 0x18)); // 0001 1000 GYRO_CONFIG: 3 = +2000 Deg/sec - I2Cdev::writeBytes(devAddr,0x6A, 1, &(val = 0xC0)); // 1100 1100 USER_CTRL: Enable Fifo and Reset Fifo - I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x02)); // 0000 0010 INT_ENABLE: RAW_DMP_INT_EN on - I2Cdev::writeBit(devAddr,0x6A, 2, 1); // Reset FIFO one last time just for kicks. (MPUi2cWrite reads 0x6A first and only alters 1 bit and then saves the byte) - - setDMPEnabled(false); // disable DMP for compatibility with the MPU6050 library -/* - dmpPacketSize += 16;//DMP_FEATURE_6X_LP_QUAT - dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_ACCEL - dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_GYRO -*/ - dmpPacketSize = 28; - return 0; -} - -bool MPU6050::dmpPacketAvailable() { - return getFIFOCount() >= dmpGetFIFOPacketSize(); -} - -// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); -// uint8_t MPU6050::dmpGetFIFORate(); -// uint8_t MPU6050::dmpGetSampleStepSizeMS(); -// uint8_t MPU6050::dmpGetSampleFrequency(); -// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); - -//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); -//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); -//uint8_t MPU6050::dmpRunFIFORateProcesses(); - -// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); - -uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[16] << 8) | packet[17]); - data[1] = (((uint32_t)packet[18] << 8) | packet[19]); - data[2] = (((uint32_t)packet[20] << 8) | packet[21]); - return 0; -} -uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[16] << 8) | packet[17]; - data[1] = (packet[18] << 8) | packet[19]; - data[2] = (packet[20] << 8) | packet[21]; - return 0; -} -uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[16] << 8) | packet[17]; - v -> y = (packet[18] << 8) | packet[19]; - v -> z = (packet[20] << 8) | packet[21]; - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); - data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); - data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); - data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[0] << 8) | packet[1]); - data[1] = ((packet[4] << 8) | packet[5]); - data[2] = ((packet[8] << 8) | packet[9]); - data[3] = ((packet[12] << 8) | packet[13]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - int16_t qI[4]; - uint8_t status = dmpGetQuaternion(qI, packet); - if (status == 0) { - q -> w = (float)qI[0] / 16384.0f; - q -> x = (float)qI[1] / 16384.0f; - q -> y = (float)qI[2] / 16384.0f; - q -> z = (float)qI[3] / 16384.0f; - return 0; - } - return status; // int16 return value, indicates error if this line is reached -} -// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[22] << 8) | packet[23]); - data[1] = (((uint32_t)packet[24] << 8) | packet[25]); - data[2] = (((uint32_t)packet[26] << 8) | packet[27]); - return 0; -} -uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[22] << 8) | packet[23]; - data[1] = (packet[24] << 8) | packet[25]; - data[2] = (packet[26] << 8) | packet[27]; - return 0; -} -uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[22] << 8) | packet[23]; - v -> y = (packet[24] << 8) | packet[25]; - v -> z = (packet[26] << 8) | packet[27]; - return 0; -} -// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); -// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { - // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) - v -> x = vRaw -> x - gravity -> x*8192; - v -> y = vRaw -> y - gravity -> y*8192; - v -> z = vRaw -> z - gravity -> z*8192; - return 0; -} -// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { - // rotate measured 3D acceleration vector into original state - // frame of reference based on orientation quaternion - memcpy(v, vReal, sizeof(VectorInt16)); - v -> rotate(q); - return 0; -} -// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { - /* +1g corresponds to +8192, sensitivity is 2g. */ - int16_t qI[4]; - uint8_t status = dmpGetQuaternion(qI, packet); - data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; - data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; - data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] - - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); - return status; -} - -uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { - v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); - v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); - v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; - return 0; -} -// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); -// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); - -uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi - data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta - data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi - return 0; -} - -#ifdef USE_OLD_DMPGETYAWPITCHROLL -uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { - // yaw: (about Z axis) - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); - // pitch: (nose up/down, about Y axis) - data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); - // roll: (tilt left/right, about X axis) - data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); - return 0; -} -#else -uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { - // yaw: (about Z axis) - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); - // pitch: (nose up/down, about Y axis) - data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); - // roll: (tilt left/right, about X axis) - data[2] = atan2(gravity -> y , gravity -> z); - if (gravity -> z < 0) { - if(data[1] > 0) { - data[1] = PI - data[1]; - } else { - data[1] = -PI - data[1]; - } - } - return 0; -} -#endif - -// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); - -uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { - /*for (uint8_t k = 0; k < dmpPacketSize; k++) { - if (dmpData[k] < 0x10) Serial.print("0"); - Serial.print(dmpData[k], HEX); - Serial.print(" "); - } - Serial.print("\n");*/ - //Serial.println((uint16_t)dmpPacketBuffer); - return 0; -} -uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { - uint8_t status; - uint8_t buf[dmpPacketSize]; - for (uint8_t i = 0; i < numPackets; i++) { - // read packet from FIFO - getFIFOBytes(buf, dmpPacketSize); - - // process packet - if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; - - // increment external process count variable, if supplied - if (processed != 0) (*processed)++; - } - return 0; -} - -// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); - -// uint8_t MPU6050::dmpInitFIFOParam(); -// uint8_t MPU6050::dmpCloseFIFO(); -// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); -// uint8_t MPU6050::dmpDecodeQuantizedAccel(); -// uint32_t MPU6050::dmpGetGyroSumOfSquare(); -// uint32_t MPU6050::dmpGetAccelSumOfSquare(); -// void MPU6050::dmpOverrideQuaternion(long *q); -uint16_t MPU6050::dmpGetFIFOPacketSize() { - return dmpPacketSize; -} - -#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050_9Axis_MotionApps41.h b/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050_9Axis_MotionApps41.h deleted file mode 100644 index 763a49b9..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/MPU6050_9Axis_MotionApps41.h +++ /dev/null @@ -1,887 +0,0 @@ -// I2Cdev library collection - MPU6050 I2C device class, 9-axis MotionApps 4.1 implementation -// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) -// 6/18/2012 by Jeff Rowberg -// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib -// -// Changelog: -// ... - ongoing debug release - -/* ============================================ -I2Cdev device library code is placed under the MIT license -Copyright (c) 2012 Jeff Rowberg - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. -=============================================== -*/ - -#ifndef _MPU6050_9AXIS_MOTIONAPPS41_H_ -#define _MPU6050_9AXIS_MOTIONAPPS41_H_ - -#include "I2Cdev.h" -#include "helper_3dmath.h" - -// MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board -#define MPU6050_INCLUDE_DMP_MOTIONAPPS41 - -#include "MPU6050.h" - -// Tom Carpenter's conditional PROGMEM code -// http://forum.arduino.cc/index.php?topic=129407.0 -#ifdef __AVR__ - #include -#else - // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen - #ifndef __PGMSPACE_H_ - #define __PGMSPACE_H_ 1 - #include - - #define PROGMEM - #define PGM_P const char * - #define PSTR(str) (str) - #define F(x) x - - typedef void prog_void; - typedef char prog_char; - //typedef unsigned char prog_uchar; - typedef int8_t prog_int8_t; - typedef uint8_t prog_uint8_t; - typedef int16_t prog_int16_t; - typedef uint16_t prog_uint16_t; - typedef int32_t prog_int32_t; - typedef uint32_t prog_uint32_t; - - #define strcpy_P(dest, src) strcpy((dest), (src)) - #define strcat_P(dest, src) strcat((dest), (src)) - #define strcmp_P(a, b) strcmp((a), (b)) - - #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) - #define pgm_read_word(addr) (*(const unsigned short *)(addr)) - #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) - #define pgm_read_float(addr) (*(const float *)(addr)) - - #define pgm_read_byte_near(addr) pgm_read_byte(addr) - #define pgm_read_word_near(addr) pgm_read_word(addr) - #define pgm_read_dword_near(addr) pgm_read_dword(addr) - #define pgm_read_float_near(addr) pgm_read_float(addr) - #define pgm_read_byte_far(addr) pgm_read_byte(addr) - #define pgm_read_word_far(addr) pgm_read_word(addr) - #define pgm_read_dword_far(addr) pgm_read_dword(addr) - #define pgm_read_float_far(addr) pgm_read_float(addr) - #endif -#endif - -// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. -// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) -// after moving string constants to flash memory storage using the F() -// compiler macro (Arduino IDE 1.0+ required). - -//#define DEBUG -#ifdef DEBUG - #define DEBUG_PRINT(x) Serial.print(x) - #define DEBUG_PRINTF(x, y) Serial.print(x, y) - #define DEBUG_PRINTLN(x) Serial.println(x) - #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) -#else - #define DEBUG_PRINT(x) - #define DEBUG_PRINTF(x, y) - #define DEBUG_PRINTLN(x) - #define DEBUG_PRINTLNF(x, y) -#endif - -#define MPU6050_DMP_CODE_SIZE 1962 // dmpMemory[] -#define MPU6050_DMP_CONFIG_SIZE 232 // dmpConfig[] -#define MPU6050_DMP_UPDATES_SIZE 140 // dmpUpdates[] - -/* ================================================================================================ * - | Default MotionApps v4.1 48-byte FIFO packet structure: | - | | - | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | - | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | - | | - | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | - | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | - * ================================================================================================ */ - -// this block of memory gets written to the MPU on start-up, and it seems -// to be volatile memory, so it has to be done each time (it only takes ~1 -// second though) -const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { - // bank 0, 256 bytes - 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, - 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, - 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, - 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, - 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, - 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, - 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, - 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, - - // bank 1, 256 bytes - 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, - 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, - 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, - 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, - - // bank 2, 256 bytes - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - - // bank 3, 256 bytes - 0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4, - 0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, - 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, - 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, - 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, - 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, - 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, - 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, - 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, - 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, - 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, - 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, - 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, - 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54, - 0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D, - 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86, - - // bank 4, 256 bytes - 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, - 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, - 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, - 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, - 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, - 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, - 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, - 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, - 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, - 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, - 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, - 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, - 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, - 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, - 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, - 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, - - // bank 5, 256 bytes - 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, - 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, - 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, - 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, - 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, - 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, - 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, - 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, - 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, - 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, - 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86, - 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, - 0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9, - 0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30, - 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0, - 0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24, - - // bank 6, 256 bytes - 0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40, - 0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, - 0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, - 0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, - 0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, - 0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, - 0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, - 0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, - 0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, - 0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, - 0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, - 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, - 0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, - 0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, - 0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, - 0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, - - // bank 7, 170 bytes (remainder) - 0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, - 0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, - 0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, - 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, - 0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, - 0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9, - 0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, - 0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, - 0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, - 0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, - 0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF -}; - -#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR -#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x03 -#endif - -const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { -// BANK OFFSET LENGTH [DATA] - 0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ? - 0x03, 0x82, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration - 0x03, 0xB2, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration - 0x00, 0x68, 0x04, 0x02, 0xCA, 0xE3, 0x09, // D_0_104 inv_set_gyro_calibration - 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration - 0x03, 0x86, 0x03, 0x0C, 0xC9, 0x2C, // FCFG_2 inv_set_accel_calibration - 0x03, 0x90, 0x03, 0x26, 0x46, 0x66, // (continued)...FCFG_2 inv_set_accel_calibration - 0x00, 0x6C, 0x02, 0x40, 0x00, // D_0_108 inv_set_accel_calibration - - 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration - 0x02, 0x44, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_01 - 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 - 0x02, 0x4C, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_10 - 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 - 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 - 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 - 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 - 0x02, 0xBC, 0x04, 0xC0, 0x00, 0x00, 0x00, // CPASS_MTX_22 - - 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel - 0x03, 0x86, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors - 0x04, 0x22, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion - 0x00, 0xA3, 0x01, 0x00, // ? - 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update - 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion - 0x07, 0x9F, 0x01, 0x30, // CFG_16 inv_set_footer - 0x07, 0x67, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro - 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo - 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? - 0x02, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // ? - 0x07, 0x83, 0x06, 0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ? - // SPECIAL 0x01 = enable interrupts - 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION - 0x07, 0xA7, 0x01, 0xFE, // ? - 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? - 0x07, 0x67, 0x01, 0x9A, // ? - 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo - 0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo - 0x02, 0x16, 0x02, 0x00, MPU6050_DMP_FIFO_RATE_DIVISOR // D_0_22 inv_set_fifo_rate - - // This very last 0x03 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, - // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. - // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) - - // It is important to make sure the host processor can keep up with reading and processing - // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. -}; - -const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { - 0x01, 0xB2, 0x02, 0xFF, 0xF5, - 0x01, 0x90, 0x04, 0x0A, 0x0D, 0x97, 0xC0, - 0x00, 0xA3, 0x01, 0x00, - 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, - 0x01, 0x6A, 0x02, 0x06, 0x00, - 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, - 0x02, 0x60, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x01, 0x08, 0x02, 0x01, 0x20, - 0x01, 0x0A, 0x02, 0x00, 0x4E, - 0x01, 0x02, 0x02, 0xFE, 0xB3, - 0x02, 0x6C, 0x04, 0x00, 0x00, 0x00, 0x00, // READ - 0x02, 0x6C, 0x04, 0xFA, 0xFE, 0x00, 0x00, - 0x02, 0x60, 0x0C, 0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C, - 0x02, 0xF4, 0x04, 0x00, 0x00, 0x00, 0x00, - 0x02, 0xF8, 0x04, 0x00, 0x00, 0x00, 0x00, - 0x02, 0xFC, 0x04, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 -}; - -uint8_t MPU6050::dmpInitialize() { - // reset device - DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); - reset(); - delay(30); // wait after reset - - // disable sleep mode - DEBUG_PRINTLN(F("Disabling sleep mode...")); - setSleepEnabled(false); - - // get MPU product ID - DEBUG_PRINTLN(F("Getting product ID...")); - //uint8_t productID = 0; //getProductID(); - DEBUG_PRINT(F("Product ID = ")); - DEBUG_PRINT(productID); - - // get MPU hardware revision - DEBUG_PRINTLN(F("Selecting user bank 16...")); - setMemoryBank(0x10, true, true); - DEBUG_PRINTLN(F("Selecting memory byte 6...")); - setMemoryStartAddress(0x06); - DEBUG_PRINTLN(F("Checking hardware revision...")); - uint8_t hwRevision = readMemoryByte(); - DEBUG_PRINT(F("Revision @ user[16][6] = ")); - DEBUG_PRINTLNF(hwRevision, HEX); - DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); - setMemoryBank(0, false, false); - - // check OTP bank valid - DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); - uint8_t otpValid = getOTPBankValid(); - DEBUG_PRINT(F("OTP bank is ")); - DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); - - // get X/Y/Z gyro offsets - DEBUG_PRINTLN(F("Reading gyro offset values...")); - int8_t xgOffset = getXGyroOffset(); - int8_t ygOffset = getYGyroOffset(); - int8_t zgOffset = getZGyroOffset(); - DEBUG_PRINT(F("X gyro offset = ")); - DEBUG_PRINTLN(xgOffset); - DEBUG_PRINT(F("Y gyro offset = ")); - DEBUG_PRINTLN(ygOffset); - DEBUG_PRINT(F("Z gyro offset = ")); - DEBUG_PRINTLN(zgOffset); - - I2Cdev::readByte(devAddr, MPU6050_RA_USER_CTRL, buffer); // ? - - DEBUG_PRINTLN(F("Enabling interrupt latch, clear on any read, AUX bypass enabled")); - I2Cdev::writeByte(devAddr, MPU6050_RA_INT_PIN_CFG, 0x32); - - // enable MPU AUX I2C bypass mode - //DEBUG_PRINTLN(F("Enabling AUX I2C bypass mode...")); - //setI2CBypassEnabled(true); - - DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); - //mag -> setMode(0); - I2Cdev::writeByte(0x0E, 0x0A, 0x00); - - DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access...")); - //mag -> setMode(0x0F); - I2Cdev::writeByte(0x0E, 0x0A, 0x0F); - - DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration...")); - int8_t asax, asay, asaz; - //mag -> getAdjustment(&asax, &asay, &asaz); - I2Cdev::readBytes(0x0E, 0x10, 3, buffer); - asax = (int8_t)buffer[0]; - asay = (int8_t)buffer[1]; - asaz = (int8_t)buffer[2]; - DEBUG_PRINT(F("Adjustment X/Y/Z = ")); - DEBUG_PRINT(asax); - DEBUG_PRINT(F(" / ")); - DEBUG_PRINT(asay); - DEBUG_PRINT(F(" / ")); - DEBUG_PRINTLN(asaz); - - DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); - //mag -> setMode(0); - I2Cdev::writeByte(0x0E, 0x0A, 0x00); - - // load DMP code into memory banks - DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); - DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); - DEBUG_PRINTLN(F(" bytes)")); - if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { - DEBUG_PRINTLN(F("Success! DMP code written and verified.")); - - DEBUG_PRINTLN(F("Configuring DMP and related settings...")); - - // write DMP configuration - DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); - DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); - DEBUG_PRINTLN(F(" bytes in config def)")); - if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { - DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); - - DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); - setIntEnabled(1< setMode(1); - I2Cdev::writeByte(0x0E, 0x0A, 0x01); - - // setup AK8975 (0x0E) as Slave 0 in read mode - DEBUG_PRINTLN(F("Setting up AK8975 read slave 0...")); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_ADDR, 0x8E); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_REG, 0x01); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_CTRL, 0xDA); - - // setup AK8975 (0x0E) as Slave 2 in write mode - DEBUG_PRINTLN(F("Setting up AK8975 write slave 2...")); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_ADDR, 0x0E); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_REG, 0x0A); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_CTRL, 0x81); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_DO, 0x01); - - // setup I2C timing/delay control - DEBUG_PRINTLN(F("Setting up slave access delay...")); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV4_CTRL, 0x18); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05); - - // enable interrupts - DEBUG_PRINTLN(F("Enabling default interrupt behavior/no bypass...")); - I2Cdev::writeByte(0x68, MPU6050_RA_INT_PIN_CFG, 0x00); - - // enable I2C master mode and reset DMP/FIFO - DEBUG_PRINTLN(F("Enabling I2C master mode...")); - I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20); - DEBUG_PRINTLN(F("Resetting FIFO...")); - I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x24); - DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know")); - I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20); - DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO...")); - I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0xE8); - - DEBUG_PRINTLN(F("Writing final memory update 5/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 6/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 7/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 8/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 9/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 10/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 11/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Reading final memory update 12/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - #ifdef DEBUG - DEBUG_PRINT(F("Read bytes: ")); - for (j = 0; j < 4; j++) { - DEBUG_PRINTF(dmpUpdate[3 + j], HEX); - DEBUG_PRINT(" "); - } - DEBUG_PRINTLN(""); - #endif - - DEBUG_PRINTLN(F("Writing final memory update 13/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 14/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 15/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 16/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 17/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Waiting for FIRO count >= 46...")); - while ((fifoCount = getFIFOCount()) < 46); - DEBUG_PRINTLN(F("Reading FIFO...")); - getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes - DEBUG_PRINTLN(F("Reading interrupt status...")); - getIntStatus(); - - DEBUG_PRINTLN(F("Writing final memory update 18/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); - while ((fifoCount = getFIFOCount()) < 48); - DEBUG_PRINTLN(F("Reading FIFO...")); - getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes - DEBUG_PRINTLN(F("Reading interrupt status...")); - getIntStatus(); - DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); - while ((fifoCount = getFIFOCount()) < 48); - DEBUG_PRINTLN(F("Reading FIFO...")); - getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes - DEBUG_PRINTLN(F("Reading interrupt status...")); - getIntStatus(); - - DEBUG_PRINTLN(F("Writing final memory update 19/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); - setDMPEnabled(false); - - DEBUG_PRINTLN(F("Setting up internal 48-byte (default) DMP packet buffer...")); - dmpPacketSize = 48; - /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { - return 3; // TODO: proper error code for no memory - }*/ - - DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); - resetFIFO(); - getIntStatus(); - } else { - DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); - return 2; // configuration block loading failed - } - } else { - DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); - return 1; // main binary block loading failed - } - return 0; // success -} - -bool MPU6050::dmpPacketAvailable() { - return getFIFOCount() >= dmpGetFIFOPacketSize(); -} - -// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); -// uint8_t MPU6050::dmpGetFIFORate(); -// uint8_t MPU6050::dmpGetSampleStepSizeMS(); -// uint8_t MPU6050::dmpGetSampleFrequency(); -// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); - -//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); -//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); -//uint8_t MPU6050::dmpRunFIFORateProcesses(); - -// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); - -uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[34] << 24) | ((uint32_t)packet[35] << 16) | ((uint32_t)packet[36] << 8) | packet[37]); - data[1] = (((uint32_t)packet[38] << 24) | ((uint32_t)packet[39] << 16) | ((uint32_t)packet[40] << 8) | packet[41]); - data[2] = (((uint32_t)packet[42] << 24) | ((uint32_t)packet[43] << 16) | ((uint32_t)packet[44] << 8) | packet[45]); - return 0; -} -uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[34] << 8) | packet[35]; - data[1] = (packet[38] << 8) | packet[39]; - data[2] = (packet[42] << 8) | packet[43]; - return 0; -} -uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[34] << 8) | packet[35]; - v -> y = (packet[38] << 8) | packet[39]; - v -> z = (packet[42] << 8) | packet[43]; - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); - data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); - data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); - data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[0] << 8) | packet[1]); - data[1] = ((packet[4] << 8) | packet[5]); - data[2] = ((packet[8] << 8) | packet[9]); - data[3] = ((packet[12] << 8) | packet[13]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - int16_t qI[4]; - uint8_t status = dmpGetQuaternion(qI, packet); - if (status == 0) { - q -> w = (float)qI[0] / 16384.0f; - q -> x = (float)qI[1] / 16384.0f; - q -> y = (float)qI[2] / 16384.0f; - q -> z = (float)qI[3] / 16384.0f; - return 0; - } - return status; // int16 return value, indicates error if this line is reached -} -// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); - data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); - data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); - return 0; -} -uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[16] << 8) | packet[17]; - data[1] = (packet[20] << 8) | packet[21]; - data[2] = (packet[24] << 8) | packet[25]; - return 0; -} -uint8_t MPU6050::dmpGetMag(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[28] << 8) | packet[29]; - data[1] = (packet[30] << 8) | packet[31]; - data[2] = (packet[32] << 8) | packet[33]; - return 0; -} -// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); -// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { - // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) - v -> x = vRaw -> x - gravity -> x*4096; - v -> y = vRaw -> y - gravity -> y*4096; - v -> z = vRaw -> z - gravity -> z*4096; - return 0; -} -// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { - // rotate measured 3D acceleration vector into original state - // frame of reference based on orientation quaternion - memcpy(v, vReal, sizeof(VectorInt16)); - v -> rotate(q); - return 0; -} -// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { - /* +1g corresponds to +8192, sensitivity is 2g. */ - int16_t qI[4]; - uint8_t status = dmpGetQuaternion(qI, packet); - data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; - data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; - data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] - - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); - return status; -} - -uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { - v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); - v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); - v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; - return 0; -} -// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); -// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); - -uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi - data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta - data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi - return 0; -} - -#ifdef USE_OLD_DMPGETYAWPITCHROLL -uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { - // yaw: (about Z axis) - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); - // pitch: (nose up/down, about Y axis) - data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); - // roll: (tilt left/right, about X axis) - data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); - return 0; -} -#else -uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { - // yaw: (about Z axis) - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); - // pitch: (nose up/down, about Y axis) - data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); - // roll: (tilt left/right, about X axis) - data[2] = atan2(gravity -> y , gravity -> z); - if(gravity->z<0) { - if(data[1]>0) { - data[1] = PI - data[1]; - } else { - data[1] = -PI - data[1]; - } - } - return 0; -} -#endif - -// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); - -uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { - /*for (uint8_t k = 0; k < dmpPacketSize; k++) { - if (dmpData[k] < 0x10) Serial.print("0"); - Serial.print(dmpData[k], HEX); - Serial.print(" "); - } - Serial.print("\n");*/ - //Serial.println((uint16_t)dmpPacketBuffer); - return 0; -} -uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { - uint8_t status; - uint8_t buf[dmpPacketSize]; - for (uint8_t i = 0; i < numPackets; i++) { - // read packet from FIFO - getFIFOBytes(buf, dmpPacketSize); - - // process packet - if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; - - // increment external process count variable, if supplied - if (processed != 0) *processed++; - } - return 0; -} - -// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); - -// uint8_t MPU6050::dmpInitFIFOParam(); -// uint8_t MPU6050::dmpCloseFIFO(); -// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); -// uint8_t MPU6050::dmpDecodeQuantizedAccel(); -// uint32_t MPU6050::dmpGetGyroSumOfSquare(); -// uint32_t MPU6050::dmpGetAccelSumOfSquare(); -// void MPU6050::dmpOverrideQuaternion(long *q); -uint16_t MPU6050::dmpGetFIFOPacketSize() { - return dmpPacketSize; -} - -#endif /* _MPU6050_9AXIS_MOTIONAPPS41_H_ */ diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/helper_3dmath.h b/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/helper_3dmath.h deleted file mode 100644 index 9ed260ec..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU6050/helper_3dmath.h +++ /dev/null @@ -1,216 +0,0 @@ -// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper -// 6/5/2012 by Jeff Rowberg -// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib -// -// Changelog: -// 2012-06-05 - add 3D math helper file to DMP6 example sketch - -/* ============================================ -I2Cdev device library code is placed under the MIT license -Copyright (c) 2012 Jeff Rowberg - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. -=============================================== -*/ - -#ifndef _HELPER_3DMATH_H_ -#define _HELPER_3DMATH_H_ - -class Quaternion { - public: - float w; - float x; - float y; - float z; - - Quaternion() { - w = 1.0f; - x = 0.0f; - y = 0.0f; - z = 0.0f; - } - - Quaternion(float nw, float nx, float ny, float nz) { - w = nw; - x = nx; - y = ny; - z = nz; - } - - Quaternion getProduct(Quaternion q) { - // Quaternion multiplication is defined by: - // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2) - // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2) - // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2) - // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2 - return Quaternion( - w*q.w - x*q.x - y*q.y - z*q.z, // new w - w*q.x + x*q.w + y*q.z - z*q.y, // new x - w*q.y - x*q.z + y*q.w + z*q.x, // new y - w*q.z + x*q.y - y*q.x + z*q.w); // new z - } - - Quaternion getConjugate() { - return Quaternion(w, -x, -y, -z); - } - - float getMagnitude() { - return sqrt(w*w + x*x + y*y + z*z); - } - - void normalize() { - float m = getMagnitude(); - w /= m; - x /= m; - y /= m; - z /= m; - } - - Quaternion getNormalized() { - Quaternion r(w, x, y, z); - r.normalize(); - return r; - } -}; - -class VectorInt16 { - public: - int16_t x; - int16_t y; - int16_t z; - - VectorInt16() { - x = 0; - y = 0; - z = 0; - } - - VectorInt16(int16_t nx, int16_t ny, int16_t nz) { - x = nx; - y = ny; - z = nz; - } - - float getMagnitude() { - return sqrt(x*x + y*y + z*z); - } - - void normalize() { - float m = getMagnitude(); - x /= m; - y /= m; - z /= m; - } - - VectorInt16 getNormalized() { - VectorInt16 r(x, y, z); - r.normalize(); - return r; - } - - void rotate(Quaternion *q) { - // http://www.cprogramming.com/tutorial/3d/quaternions.html - // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm - // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation - // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1 - - // P_out = q * P_in * conj(q) - // - P_out is the output vector - // - q is the orientation quaternion - // - P_in is the input vector (a*aReal) - // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z]) - Quaternion p(0, x, y, z); - - // quaternion multiplication: q * p, stored back in p - p = q -> getProduct(p); - - // quaternion multiplication: p * conj(q), stored back in p - p = p.getProduct(q -> getConjugate()); - - // p quaternion is now [0, x', y', z'] - x = p.x; - y = p.y; - z = p.z; - } - - VectorInt16 getRotated(Quaternion *q) { - VectorInt16 r(x, y, z); - r.rotate(q); - return r; - } -}; - -class VectorFloat { - public: - float x; - float y; - float z; - - VectorFloat() { - x = 0; - y = 0; - z = 0; - } - - VectorFloat(float nx, float ny, float nz) { - x = nx; - y = ny; - z = nz; - } - - float getMagnitude() { - return sqrt(x*x + y*y + z*z); - } - - void normalize() { - float m = getMagnitude(); - x /= m; - y /= m; - z /= m; - } - - VectorFloat getNormalized() { - VectorFloat r(x, y, z); - r.normalize(); - return r; - } - - void rotate(Quaternion *q) { - Quaternion p(0, x, y, z); - - // quaternion multiplication: q * p, stored back in p - p = q -> getProduct(p); - - // quaternion multiplication: p * conj(q), stored back in p - p = p.getProduct(q -> getConjugate()); - - // p quaternion is now [0, x', y', z'] - x = p.x; - y = p.y; - z = p.z; - } - - VectorFloat getRotated(Quaternion *q) { - VectorFloat r(x, y, z); - r.rotate(q); - return r; - } -}; - -#endif /* _HELPER_3DMATH_H_ */ \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU9250/MPU9250.cpp b/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU9250/MPU9250.cpp deleted file mode 100644 index 7d2722a8..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU9250/MPU9250.cpp +++ /dev/null @@ -1,1202 +0,0 @@ -/* -MPU9250.cpp -Brian R Taylor -brian.taylor@bolderflight.com -Copyright (c) 2017 Bolder Flight Systems -Permission is hereby granted, free of charge, to any person obtaining a copy of this software -and associated documentation files (the "Software"), to deal in the Software without restriction, -including without limitation the rights to use, copy, modify, merge, publish, distribute, -sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: -The above copyright notice and this permission notice shall be included in all copies or -substantial portions of the Software. -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING -BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND -NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, -DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. -*/ - -#include "Arduino.h" -#include "MPU9250.h" - -/* MPU9250 object, input the I2C bus and address */ -MPU9250::MPU9250(TwoWire &bus,uint8_t address){ - _i2c = &bus; // I2C bus - _address = address; // I2C address - _useSPI = false; // set to use I2C -} - -/* MPU9250 object, input the SPI bus and chip select pin */ -MPU9250::MPU9250(SPIClass &bus,uint8_t csPin){ - _spi = &bus; // SPI bus - _csPin = csPin; // chip select pin - _useSPI = true; // set to use SPI -} - -/* starts communication with the MPU-9250 */ -int MPU9250::begin(){ - if( _useSPI ) { // using SPI for communication - // use low speed SPI for register setting - _useSPIHS = false; - // setting CS pin to output - pinMode(_csPin,OUTPUT); - // setting CS pin high - digitalWrite(_csPin,HIGH); - // begin SPI communication - _spi->begin(); - } else { // using I2C for communication - // starting the I2C bus - _i2c->begin(); - // setting the I2C clock - _i2c->setClock(_i2cRate); - } - // select clock source to gyro - if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){ - return -1; - } - // enable I2C master mode - if(writeRegister(USER_CTRL,I2C_MST_EN) < 0){ - return -2; - } - // set the I2C bus speed to 400 kHz - if(writeRegister(I2C_MST_CTRL,I2C_MST_CLK) < 0){ - return -3; - } - // set AK8963 to Power Down - writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN); - // reset the MPU9250 - writeRegister(PWR_MGMNT_1,PWR_RESET); - // wait for MPU-9250 to come back up - delay(1); - // reset the AK8963 - writeAK8963Register(AK8963_CNTL2,AK8963_RESET); - // select clock source to gyro - if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){ - return -4; - } - // check the WHO AM I byte, expected value is 0x71 (decimal 113) or 0x73 (decimal 115) - if((whoAmI() != 113)&&(whoAmI() != 115)){ - return -5; - } - // enable accelerometer and gyro - if(writeRegister(PWR_MGMNT_2,SEN_ENABLE) < 0){ - return -6; - } - // setting accel range to 16G as default - if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_16G) < 0){ - return -7; - } - _accelScale = G * 16.0f/32767.5f; // setting the accel scale to 16G - _accelRange = ACCEL_RANGE_16G; - // setting the gyro range to 2000DPS as default - if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_2000DPS) < 0){ - return -8; - } - _gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS - _gyroRange = GYRO_RANGE_2000DPS; - // setting bandwidth to 184Hz as default - if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0){ - return -9; - } - if(writeRegister(CONFIG,GYRO_DLPF_184) < 0){ // setting gyro bandwidth to 184Hz - return -10; - } - _bandwidth = DLPF_BANDWIDTH_184HZ; - // setting the sample rate divider to 0 as default - if(writeRegister(SMPDIV,0x00) < 0){ - return -11; - } - _srd = 0; - // enable I2C master mode - if(writeRegister(USER_CTRL,I2C_MST_EN) < 0){ - return -12; - } - // set the I2C bus speed to 400 kHz - if( writeRegister(I2C_MST_CTRL,I2C_MST_CLK) < 0){ - return -13; - } - // check AK8963 WHO AM I register, expected value is 0x48 (decimal 72) - if( whoAmIAK8963() != 72 ){ - return -14; - } - /* get the magnetometer calibration */ - // set AK8963 to Power Down - if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ - return -15; - } - delay(100); // long wait between AK8963 mode changes - // set AK8963 to FUSE ROM access - if(writeAK8963Register(AK8963_CNTL1,AK8963_FUSE_ROM) < 0){ - return -16; - } - delay(100); // long wait between AK8963 mode changes - // read the AK8963 ASA registers and compute magnetometer scale factors - readAK8963Registers(AK8963_ASA,3,_buffer); - _magScaleX = ((((float)_buffer[0]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla - _magScaleY = ((((float)_buffer[1]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla - _magScaleZ = ((((float)_buffer[2]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla - // set AK8963 to Power Down - if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ - return -17; - } - delay(100); // long wait between AK8963 mode changes - // set AK8963 to 16 bit resolution, 100 Hz update rate - if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS2) < 0){ - return -18; - } - delay(100); // long wait between AK8963 mode changes - // select clock source to gyro - if(writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) < 0){ - return -19; - } - // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate - readAK8963Registers(AK8963_HXL,7,_buffer); - // estimate gyro bias - if (calibrateGyro() < 0) { - return -20; - } - // successful init, return 1 - return 1; -} - -/* sets the accelerometer full scale range to values other than default */ -int MPU9250::setAccelRange(AccelRange range) { - // use low speed SPI for register setting - _useSPIHS = false; - switch(range) { - case ACCEL_RANGE_2G: { - // setting the accel range to 2G - if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_2G) < 0){ - return -1; - } - _accelScale = G * 2.0f/32767.5f; // setting the accel scale to 2G - break; - } - case ACCEL_RANGE_4G: { - // setting the accel range to 4G - if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_4G) < 0){ - return -1; - } - _accelScale = G * 4.0f/32767.5f; // setting the accel scale to 4G - break; - } - case ACCEL_RANGE_8G: { - // setting the accel range to 8G - if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_8G) < 0){ - return -1; - } - _accelScale = G * 8.0f/32767.5f; // setting the accel scale to 8G - break; - } - case ACCEL_RANGE_16G: { - // setting the accel range to 16G - if(writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_16G) < 0){ - return -1; - } - _accelScale = G * 16.0f/32767.5f; // setting the accel scale to 16G - break; - } - } - _accelRange = range; - return 1; -} - -/* sets the gyro full scale range to values other than default */ -int MPU9250::setGyroRange(GyroRange range) { - // use low speed SPI for register setting - _useSPIHS = false; - switch(range) { - case GYRO_RANGE_250DPS: { - // setting the gyro range to 250DPS - if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_250DPS) < 0){ - return -1; - } - _gyroScale = 250.0f/32767.5f * _d2r; // setting the gyro scale to 250DPS - break; - } - case GYRO_RANGE_500DPS: { - // setting the gyro range to 500DPS - if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_500DPS) < 0){ - return -1; - } - _gyroScale = 500.0f/32767.5f * _d2r; // setting the gyro scale to 500DPS - break; - } - case GYRO_RANGE_1000DPS: { - // setting the gyro range to 1000DPS - if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_1000DPS) < 0){ - return -1; - } - _gyroScale = 1000.0f/32767.5f * _d2r; // setting the gyro scale to 1000DPS - break; - } - case GYRO_RANGE_2000DPS: { - // setting the gyro range to 2000DPS - if(writeRegister(GYRO_CONFIG,GYRO_FS_SEL_2000DPS) < 0){ - return -1; - } - _gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS - break; - } - } - _gyroRange = range; - return 1; -} - -/* sets the DLPF bandwidth to values other than default */ -int MPU9250::setDlpfBandwidth(DlpfBandwidth bandwidth) { - // use low speed SPI for register setting - _useSPIHS = false; - switch(bandwidth) { - case DLPF_BANDWIDTH_184HZ: { - if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0){ // setting accel bandwidth to 184Hz - return -1; - } - if(writeRegister(CONFIG,GYRO_DLPF_184) < 0){ // setting gyro bandwidth to 184Hz - return -2; - } - break; - } - case DLPF_BANDWIDTH_92HZ: { - if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_92) < 0){ // setting accel bandwidth to 92Hz - return -1; - } - if(writeRegister(CONFIG,GYRO_DLPF_92) < 0){ // setting gyro bandwidth to 92Hz - return -2; - } - break; - } - case DLPF_BANDWIDTH_41HZ: { - if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_41) < 0){ // setting accel bandwidth to 41Hz - return -1; - } - if(writeRegister(CONFIG,GYRO_DLPF_41) < 0){ // setting gyro bandwidth to 41Hz - return -2; - } - break; - } - case DLPF_BANDWIDTH_20HZ: { - if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_20) < 0){ // setting accel bandwidth to 20Hz - return -1; - } - if(writeRegister(CONFIG,GYRO_DLPF_20) < 0){ // setting gyro bandwidth to 20Hz - return -2; - } - break; - } - case DLPF_BANDWIDTH_10HZ: { - if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_10) < 0){ // setting accel bandwidth to 10Hz - return -1; - } - if(writeRegister(CONFIG,GYRO_DLPF_10) < 0){ // setting gyro bandwidth to 10Hz - return -2; - } - break; - } - case DLPF_BANDWIDTH_5HZ: { - if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_5) < 0){ // setting accel bandwidth to 5Hz - return -1; - } - if(writeRegister(CONFIG,GYRO_DLPF_5) < 0){ // setting gyro bandwidth to 5Hz - return -2; - } - break; - } - } - _bandwidth = bandwidth; - return 1; -} - -/* sets the sample rate divider to values other than default */ -int MPU9250::setSrd(uint8_t srd) { - // use low speed SPI for register setting - _useSPIHS = false; - /* setting the sample rate divider to 19 to facilitate setting up magnetometer */ - if(writeRegister(SMPDIV,19) < 0){ // setting the sample rate divider - return -1; - } - if(srd > 9){ - // set AK8963 to Power Down - if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ - return -2; - } - delay(100); // long wait between AK8963 mode changes - // set AK8963 to 16 bit resolution, 8 Hz update rate - if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS1) < 0){ - return -3; - } - delay(100); // long wait between AK8963 mode changes - // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate - readAK8963Registers(AK8963_HXL,7,_buffer); - } else { - // set AK8963 to Power Down - if(writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) < 0){ - return -2; - } - delay(100); // long wait between AK8963 mode changes - // set AK8963 to 16 bit resolution, 100 Hz update rate - if(writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS2) < 0){ - return -3; - } - delay(100); // long wait between AK8963 mode changes - // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate - readAK8963Registers(AK8963_HXL,7,_buffer); - } - /* setting the sample rate divider */ - if(writeRegister(SMPDIV,srd) < 0){ // setting the sample rate divider - return -4; - } - _srd = srd; - return 1; -} - -/* enables the data ready interrupt */ -int MPU9250::enableDataReadyInterrupt() { - // use low speed SPI for register setting - _useSPIHS = false; - /* setting the interrupt */ - if (writeRegister(INT_PIN_CFG,INT_PULSE_50US) < 0){ // setup interrupt, 50 us pulse - return -1; - } - if (writeRegister(INT_ENABLE,INT_RAW_RDY_EN) < 0){ // set to data ready - return -2; - } - return 1; -} - -/* disables the data ready interrupt */ -int MPU9250::disableDataReadyInterrupt() { - // use low speed SPI for register setting - _useSPIHS = false; - if(writeRegister(INT_ENABLE,INT_DISABLE) < 0){ // disable interrupt - return -1; - } - return 1; -} - -/* configures and enables wake on motion, low power mode */ -int MPU9250::enableWakeOnMotion(float womThresh_mg,LpAccelOdr odr) { - // use low speed SPI for register setting - _useSPIHS = false; - // set AK8963 to Power Down - writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN); - // reset the MPU9250 - writeRegister(PWR_MGMNT_1,PWR_RESET); - // wait for MPU-9250 to come back up - delay(1); - if(writeRegister(PWR_MGMNT_1,0x00) < 0){ // cycle 0, sleep 0, standby 0 - return -1; - } - if(writeRegister(PWR_MGMNT_2,DIS_GYRO) < 0){ // disable gyro measurements - return -2; - } - if(writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) < 0){ // setting accel bandwidth to 184Hz - return -3; - } - if(writeRegister(INT_ENABLE,INT_WOM_EN) < 0){ // enabling interrupt to wake on motion - return -4; - } - if(writeRegister(MOT_DETECT_CTRL,(ACCEL_INTEL_EN | ACCEL_INTEL_MODE)) < 0){ // enabling accel hardware intelligence - return -5; - } - _womThreshold = map(womThresh_mg, 0, 1020, 0, 255); - if(writeRegister(WOM_THR,_womThreshold) < 0){ // setting wake on motion threshold - return -6; - } - if(writeRegister(LP_ACCEL_ODR,(uint8_t)odr) < 0){ // set frequency of wakeup - return -7; - } - if(writeRegister(PWR_MGMNT_1,PWR_CYCLE) < 0){ // switch to accel low power mode - return -8; - } - return 1; -} - -/* configures and enables the FIFO buffer */ -int MPU9250FIFO::enableFifo(bool accel,bool gyro,bool mag,bool temp) { - // use low speed SPI for register setting - _useSPIHS = false; - if(writeRegister(USER_CTRL, (0x40 | I2C_MST_EN)) < 0){ - return -1; - } - if(writeRegister(FIFO_EN,(accel*FIFO_ACCEL)|(gyro*FIFO_GYRO)|(mag*FIFO_MAG)|(temp*FIFO_TEMP)) < 0){ - return -2; - } - _enFifoAccel = accel; - _enFifoGyro = gyro; - _enFifoMag = mag; - _enFifoTemp = temp; - _fifoFrameSize = accel*6 + gyro*6 + mag*7 + temp*2; - return 1; -} - -/* reads the most current data from MPU9250 and stores in buffer */ -int MPU9250::readSensor() { - _useSPIHS = true; // use the high speed SPI for data readout - // grab the data from the MPU9250 - if (readRegisters(ACCEL_OUT, 21, _buffer) < 0) { - return -1; - } - // combine into 16 bit values - _axcounts = (((int16_t)_buffer[0]) << 8) | _buffer[1]; - _aycounts = (((int16_t)_buffer[2]) << 8) | _buffer[3]; - _azcounts = (((int16_t)_buffer[4]) << 8) | _buffer[5]; - _tcounts = (((int16_t)_buffer[6]) << 8) | _buffer[7]; - _gxcounts = (((int16_t)_buffer[8]) << 8) | _buffer[9]; - _gycounts = (((int16_t)_buffer[10]) << 8) | _buffer[11]; - _gzcounts = (((int16_t)_buffer[12]) << 8) | _buffer[13]; - _hxcounts = (((int16_t)_buffer[15]) << 8) | _buffer[14]; - _hycounts = (((int16_t)_buffer[17]) << 8) | _buffer[16]; - _hzcounts = (((int16_t)_buffer[19]) << 8) | _buffer[18]; - // transform and convert to float values - _ax = (((float)(tX[0]*_axcounts + tX[1]*_aycounts + tX[2]*_azcounts) * _accelScale) - _axb)*_axs; - _ay = (((float)(tY[0]*_axcounts + tY[1]*_aycounts + tY[2]*_azcounts) * _accelScale) - _ayb)*_ays; - _az = (((float)(tZ[0]*_axcounts + tZ[1]*_aycounts + tZ[2]*_azcounts) * _accelScale) - _azb)*_azs; - _gx = ((float)(tX[0]*_gxcounts + tX[1]*_gycounts + tX[2]*_gzcounts) * _gyroScale) - _gxb; - _gy = ((float)(tY[0]*_gxcounts + tY[1]*_gycounts + tY[2]*_gzcounts) * _gyroScale) - _gyb; - _gz = ((float)(tZ[0]*_gxcounts + tZ[1]*_gycounts + tZ[2]*_gzcounts) * _gyroScale) - _gzb; - _hx = (((float)(_hxcounts) * _magScaleX) - _hxb)*_hxs; - _hy = (((float)(_hycounts) * _magScaleY) - _hyb)*_hys; - _hz = (((float)(_hzcounts) * _magScaleZ) - _hzb)*_hzs; - _t = ((((float) _tcounts) - _tempOffset)/_tempScale) + _tempOffset; - return 1; -} - -/* returns the accelerometer measurement in the x direction, m/s/s */ -float MPU9250::getAccelX_mss() { - return _ax; -} - -/* returns the accelerometer measurement in the y direction, m/s/s */ -float MPU9250::getAccelY_mss() { - return _ay; -} - -/* returns the accelerometer measurement in the z direction, m/s/s */ -float MPU9250::getAccelZ_mss() { - return _az; -} - -/* returns the gyroscope measurement in the x direction, rad/s */ -float MPU9250::getGyroX_rads() { - return _gx; -} - -/* returns the gyroscope measurement in the y direction, rad/s */ -float MPU9250::getGyroY_rads() { - return _gy; -} - -/* returns the gyroscope measurement in the z direction, rad/s */ -float MPU9250::getGyroZ_rads() { - return _gz; -} - -/* returns the magnetometer measurement in the x direction, uT */ -float MPU9250::getMagX_uT() { - return _hx; -} - -/* returns the magnetometer measurement in the y direction, uT */ -float MPU9250::getMagY_uT() { - return _hy; -} - -/* returns the magnetometer measurement in the z direction, uT */ -float MPU9250::getMagZ_uT() { - return _hz; -} - -/* returns the die temperature, C */ -float MPU9250::getTemperature_C() { - return _t; -} - -/* reads data from the MPU9250 FIFO and stores in buffer */ -int MPU9250FIFO::readFifo() { - _useSPIHS = true; // use the high speed SPI for data readout - // get the fifo size - readRegisters(FIFO_COUNT, 2, _buffer); - _fifoSize = (((uint16_t) (_buffer[0]&0x0F)) <<8) + (((uint16_t) _buffer[1])); - // read and parse the buffer - for (size_t i=0; i < _fifoSize/_fifoFrameSize; i++) { - // grab the data from the MPU9250 - if (readRegisters(FIFO_READ,_fifoFrameSize,_buffer) < 0) { - return -1; - } - if (_enFifoAccel) { - // combine into 16 bit values - _axcounts = (((int16_t)_buffer[0]) << 8) | _buffer[1]; - _aycounts = (((int16_t)_buffer[2]) << 8) | _buffer[3]; - _azcounts = (((int16_t)_buffer[4]) << 8) | _buffer[5]; - // transform and convert to float values - _axFifo[i] = (((float)(tX[0]*_axcounts + tX[1]*_aycounts + tX[2]*_azcounts) * _accelScale)-_axb)*_axs; - _ayFifo[i] = (((float)(tY[0]*_axcounts + tY[1]*_aycounts + tY[2]*_azcounts) * _accelScale)-_ayb)*_ays; - _azFifo[i] = (((float)(tZ[0]*_axcounts + tZ[1]*_aycounts + tZ[2]*_azcounts) * _accelScale)-_azb)*_azs; - _aSize = _fifoSize/_fifoFrameSize; - } - if (_enFifoTemp) { - // combine into 16 bit values - _tcounts = (((int16_t)_buffer[0 + _enFifoAccel*6]) << 8) | _buffer[1 + _enFifoAccel*6]; - // transform and convert to float values - _tFifo[i] = ((((float) _tcounts) - _tempOffset)/_tempScale) + _tempOffset; - _tSize = _fifoSize/_fifoFrameSize; - } - if (_enFifoGyro) { - // combine into 16 bit values - _gxcounts = (((int16_t)_buffer[0 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[1 + _enFifoAccel*6 + _enFifoTemp*2]; - _gycounts = (((int16_t)_buffer[2 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[3 + _enFifoAccel*6 + _enFifoTemp*2]; - _gzcounts = (((int16_t)_buffer[4 + _enFifoAccel*6 + _enFifoTemp*2]) << 8) | _buffer[5 + _enFifoAccel*6 + _enFifoTemp*2]; - // transform and convert to float values - _gxFifo[i] = ((float)(tX[0]*_gxcounts + tX[1]*_gycounts + tX[2]*_gzcounts) * _gyroScale) - _gxb; - _gyFifo[i] = ((float)(tY[0]*_gxcounts + tY[1]*_gycounts + tY[2]*_gzcounts) * _gyroScale) - _gyb; - _gzFifo[i] = ((float)(tZ[0]*_gxcounts + tZ[1]*_gycounts + tZ[2]*_gzcounts) * _gyroScale) - _gzb; - _gSize = _fifoSize/_fifoFrameSize; - } - if (_enFifoMag) { - // combine into 16 bit values - _hxcounts = (((int16_t)_buffer[1 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]) << 8) | _buffer[0 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]; - _hycounts = (((int16_t)_buffer[3 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]) << 8) | _buffer[2 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]; - _hzcounts = (((int16_t)_buffer[5 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]) << 8) | _buffer[4 + _enFifoAccel*6 + _enFifoTemp*2 + _enFifoGyro*6]; - // transform and convert to float values - _hxFifo[i] = (((float)(_hxcounts) * _magScaleX) - _hxb)*_hxs; - _hyFifo[i] = (((float)(_hycounts) * _magScaleY) - _hyb)*_hys; - _hzFifo[i] = (((float)(_hzcounts) * _magScaleZ) - _hzb)*_hzs; - _hSize = _fifoSize/_fifoFrameSize; - } - } - return 1; -} - -/* returns the accelerometer FIFO size and data in the x direction, m/s/s */ -void MPU9250FIFO::getFifoAccelX_mss(size_t *size,float* data) { - *size = _aSize; - memcpy(data,_axFifo,_aSize*sizeof(float)); -} - -/* returns the accelerometer FIFO size and data in the y direction, m/s/s */ -void MPU9250FIFO::getFifoAccelY_mss(size_t *size,float* data) { - *size = _aSize; - memcpy(data,_ayFifo,_aSize*sizeof(float)); -} - -/* returns the accelerometer FIFO size and data in the z direction, m/s/s */ -void MPU9250FIFO::getFifoAccelZ_mss(size_t *size,float* data) { - *size = _aSize; - memcpy(data,_azFifo,_aSize*sizeof(float)); -} - -/* returns the gyroscope FIFO size and data in the x direction, rad/s */ -void MPU9250FIFO::getFifoGyroX_rads(size_t *size,float* data) { - *size = _gSize; - memcpy(data,_gxFifo,_gSize*sizeof(float)); -} - -/* returns the gyroscope FIFO size and data in the y direction, rad/s */ -void MPU9250FIFO::getFifoGyroY_rads(size_t *size,float* data) { - *size = _gSize; - memcpy(data,_gyFifo,_gSize*sizeof(float)); -} - -/* returns the gyroscope FIFO size and data in the z direction, rad/s */ -void MPU9250FIFO::getFifoGyroZ_rads(size_t *size,float* data) { - *size = _gSize; - memcpy(data,_gzFifo,_gSize*sizeof(float)); -} - -/* returns the magnetometer FIFO size and data in the x direction, uT */ -void MPU9250FIFO::getFifoMagX_uT(size_t *size,float* data) { - *size = _hSize; - memcpy(data,_hxFifo,_hSize*sizeof(float)); -} - -/* returns the magnetometer FIFO size and data in the y direction, uT */ -void MPU9250FIFO::getFifoMagY_uT(size_t *size,float* data) { - *size = _hSize; - memcpy(data,_hyFifo,_hSize*sizeof(float)); -} - -/* returns the magnetometer FIFO size and data in the z direction, uT */ -void MPU9250FIFO::getFifoMagZ_uT(size_t *size,float* data) { - *size = _hSize; - memcpy(data,_hzFifo,_hSize*sizeof(float)); -} - -/* returns the die temperature FIFO size and data, C */ -void MPU9250FIFO::getFifoTemperature_C(size_t *size,float* data) { - *size = _tSize; - memcpy(data,_tFifo,_tSize*sizeof(float)); -} - -/* estimates the gyro biases */ -int MPU9250::calibrateGyro() { - // set the range, bandwidth, and srd - if (setGyroRange(GYRO_RANGE_250DPS) < 0) { - return -1; - } - if (setDlpfBandwidth(DLPF_BANDWIDTH_20HZ) < 0) { - return -2; - } - if (setSrd(19) < 0) { - return -3; - } - - // take samples and find bias - _gxbD = 0; - _gybD = 0; - _gzbD = 0; - for (size_t i=0; i < _numSamples; i++) { - readSensor(); - _gxbD += (getGyroX_rads() + _gxb)/((double)_numSamples); - _gybD += (getGyroY_rads() + _gyb)/((double)_numSamples); - _gzbD += (getGyroZ_rads() + _gzb)/((double)_numSamples); - delay(20); - } - _gxb = (float)_gxbD; - _gyb = (float)_gybD; - _gzb = (float)_gzbD; - - // set the range, bandwidth, and srd back to what they were - if (setGyroRange(_gyroRange) < 0) { - return -4; - } - if (setDlpfBandwidth(_bandwidth) < 0) { - return -5; - } - if (setSrd(_srd) < 0) { - return -6; - } - return 1; -} - -/* returns the gyro bias in the X direction, rad/s */ -float MPU9250::getGyroBiasX_rads() { - return _gxb; -} - -/* returns the gyro bias in the Y direction, rad/s */ -float MPU9250::getGyroBiasY_rads() { - return _gyb; -} - -/* returns the gyro bias in the Z direction, rad/s */ -float MPU9250::getGyroBiasZ_rads() { - return _gzb; -} - -/* sets the gyro bias in the X direction to bias, rad/s */ -void MPU9250::setGyroBiasX_rads(float bias) { - _gxb = bias; -} - -/* sets the gyro bias in the Y direction to bias, rad/s */ -void MPU9250::setGyroBiasY_rads(float bias) { - _gyb = bias; -} - -/* sets the gyro bias in the Z direction to bias, rad/s */ -void MPU9250::setGyroBiasZ_rads(float bias) { - _gzb = bias; -} - -/* finds bias and scale factor calibration for the accelerometer, -this should be run for each axis in each direction (6 total) to find -the min and max values along each */ -int MPU9250::calibrateAccel() { - // set the range, bandwidth, and srd - if (setAccelRange(ACCEL_RANGE_2G) < 0) { - return -1; - } - if (setDlpfBandwidth(DLPF_BANDWIDTH_20HZ) < 0) { - return -2; - } - if (setSrd(19) < 0) { - return -3; - } - - // take samples and find min / max - _axbD = 0; - _aybD = 0; - _azbD = 0; - for (size_t i=0; i < _numSamples; i++) { - readSensor(); - _axbD += (getAccelX_mss()/_axs + _axb)/((double)_numSamples); - _aybD += (getAccelY_mss()/_ays + _ayb)/((double)_numSamples); - _azbD += (getAccelZ_mss()/_azs + _azb)/((double)_numSamples); - delay(20); - } - if (_axbD > 9.0f) { - _axmax = (float)_axbD; - } - if (_aybD > 9.0f) { - _aymax = (float)_aybD; - } - if (_azbD > 9.0f) { - _azmax = (float)_azbD; - } - if (_axbD < -9.0f) { - _axmin = (float)_axbD; - } - if (_aybD < -9.0f) { - _aymin = (float)_aybD; - } - if (_azbD < -9.0f) { - _azmin = (float)_azbD; - } - - // find bias and scale factor - if ((abs(_axmin) > 9.0f) && (abs(_axmax) > 9.0f)) { - _axb = (_axmin + _axmax) / 2.0f; - _axs = G/((abs(_axmin) + abs(_axmax)) / 2.0f); - } - if ((abs(_aymin) > 9.0f) && (abs(_aymax) > 9.0f)) { - _ayb = (_aymin + _aymax) / 2.0f; - _ays = G/((abs(_aymin) + abs(_aymax)) / 2.0f); - } - if ((abs(_azmin) > 9.0f) && (abs(_azmax) > 9.0f)) { - _azb = (_azmin + _azmax) / 2.0f; - _azs = G/((abs(_azmin) + abs(_azmax)) / 2.0f); - } - - // set the range, bandwidth, and srd back to what they were - if (setAccelRange(_accelRange) < 0) { - return -4; - } - if (setDlpfBandwidth(_bandwidth) < 0) { - return -5; - } - if (setSrd(_srd) < 0) { - return -6; - } - return 1; -} - -/* returns the accelerometer bias in the X direction, m/s/s */ -float MPU9250::getAccelBiasX_mss() { - return _axb; -} - -/* returns the accelerometer scale factor in the X direction */ -float MPU9250::getAccelScaleFactorX() { - return _axs; -} - -/* returns the accelerometer bias in the Y direction, m/s/s */ -float MPU9250::getAccelBiasY_mss() { - return _ayb; -} - -/* returns the accelerometer scale factor in the Y direction */ -float MPU9250::getAccelScaleFactorY() { - return _ays; -} - -/* returns the accelerometer bias in the Z direction, m/s/s */ -float MPU9250::getAccelBiasZ_mss() { - return _azb; -} - -/* returns the accelerometer scale factor in the Z direction */ -float MPU9250::getAccelScaleFactorZ() { - return _azs; -} - -/* sets the accelerometer bias (m/s/s) and scale factor in the X direction */ -void MPU9250::setAccelCalX(float bias,float scaleFactor) { - _axb = bias; - _axs = scaleFactor; -} - -/* sets the accelerometer bias (m/s/s) and scale factor in the Y direction */ -void MPU9250::setAccelCalY(float bias,float scaleFactor) { - _ayb = bias; - _ays = scaleFactor; -} - -/* sets the accelerometer bias (m/s/s) and scale factor in the Z direction */ -void MPU9250::setAccelCalZ(float bias,float scaleFactor) { - _azb = bias; - _azs = scaleFactor; -} - -/* finds bias and scale factor calibration for the magnetometer, -the sensor should be rotated in a figure 8 motion until complete */ -int MPU9250::calibrateMag() { - // set the srd - if (setSrd(19) < 0) { - return -1; - } - - // get a starting set of data - readSensor(); - _hxmax = getMagX_uT(); - _hxmin = getMagX_uT(); - _hymax = getMagY_uT(); - _hymin = getMagY_uT(); - _hzmax = getMagZ_uT(); - _hzmin = getMagZ_uT(); - - // collect data to find max / min in each channel - _counter = 0; - while (_counter < _maxCounts) { - _delta = 0.0f; - _framedelta = 0.0f; - readSensor(); - _hxfilt = (_hxfilt*((float)_coeff-1)+(getMagX_uT()/_hxs+_hxb))/((float)_coeff); - _hyfilt = (_hyfilt*((float)_coeff-1)+(getMagY_uT()/_hys+_hyb))/((float)_coeff); - _hzfilt = (_hzfilt*((float)_coeff-1)+(getMagZ_uT()/_hzs+_hzb))/((float)_coeff); - if (_hxfilt > _hxmax) { - _delta = _hxfilt - _hxmax; - _hxmax = _hxfilt; - } - if (_delta > _framedelta) { - _framedelta = _delta; - } - if (_hyfilt > _hymax) { - _delta = _hyfilt - _hymax; - _hymax = _hyfilt; - } - if (_delta > _framedelta) { - _framedelta = _delta; - } - if (_hzfilt > _hzmax) { - _delta = _hzfilt - _hzmax; - _hzmax = _hzfilt; - } - if (_delta > _framedelta) { - _framedelta = _delta; - } - if (_hxfilt < _hxmin) { - _delta = abs(_hxfilt - _hxmin); - _hxmin = _hxfilt; - } - if (_delta > _framedelta) { - _framedelta = _delta; - } - if (_hyfilt < _hymin) { - _delta = abs(_hyfilt - _hymin); - _hymin = _hyfilt; - } - if (_delta > _framedelta) { - _framedelta = _delta; - } - if (_hzfilt < _hzmin) { - _delta = abs(_hzfilt - _hzmin); - _hzmin = _hzfilt; - } - if (_delta > _framedelta) { - _framedelta = _delta; - } - if (_framedelta > _deltaThresh) { - _counter = 0; - } else { - _counter++; - } - delay(20); - } - - // find the magnetometer bias - _hxb = (_hxmax + _hxmin) / 2.0f; - _hyb = (_hymax + _hymin) / 2.0f; - _hzb = (_hzmax + _hzmin) / 2.0f; - - // find the magnetometer scale factor - _hxs = (_hxmax - _hxmin) / 2.0f; - _hys = (_hymax - _hymin) / 2.0f; - _hzs = (_hzmax - _hzmin) / 2.0f; - _avgs = (_hxs + _hys + _hzs) / 3.0f; - _hxs = _avgs/_hxs; - _hys = _avgs/_hys; - _hzs = _avgs/_hzs; - - // set the srd back to what it was - if (setSrd(_srd) < 0) { - return -2; - } - return 1; -} - -/* returns the magnetometer bias in the X direction, uT */ -float MPU9250::getMagBiasX_uT() { - return _hxb; -} - -/* returns the magnetometer scale factor in the X direction */ -float MPU9250::getMagScaleFactorX() { - return _hxs; -} - -/* returns the magnetometer bias in the Y direction, uT */ -float MPU9250::getMagBiasY_uT() { - return _hyb; -} - -/* returns the magnetometer scale factor in the Y direction */ -float MPU9250::getMagScaleFactorY() { - return _hys; -} - -/* returns the magnetometer bias in the Z direction, uT */ -float MPU9250::getMagBiasZ_uT() { - return _hzb; -} - -/* returns the magnetometer scale factor in the Z direction */ -float MPU9250::getMagScaleFactorZ() { - return _hzs; -} - -/* sets the magnetometer bias (uT) and scale factor in the X direction */ -void MPU9250::setMagCalX(float bias,float scaleFactor) { - _hxb = bias; - _hxs = scaleFactor; -} - -/* sets the magnetometer bias (uT) and scale factor in the Y direction */ -void MPU9250::setMagCalY(float bias,float scaleFactor) { - _hyb = bias; - _hys = scaleFactor; -} - -/* sets the magnetometer bias (uT) and scale factor in the Z direction */ -void MPU9250::setMagCalZ(float bias,float scaleFactor) { - _hzb = bias; - _hzs = scaleFactor; -} - -#if defined(__IMXRT1062__) -/* writes a byte to MPU9250 register given a register address and data */ -int MPU9250::writeRegister(uint8_t subAddress, uint8_t data){ - /* write data to device */ - if( _useSPI ){ - _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction - digitalWriteFast(_csPin,LOW); // select the MPU9250 chip - delayNanoseconds(200); - _spi->transfer(subAddress); // write the register address - _spi->transfer(data); // write the data - digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip - delayNanoseconds(200); - _spi->endTransaction(); // end the transaction - } - else{ - _i2c->beginTransmission(_address); // open the device - _i2c->write(subAddress); // write the register address - _i2c->write(data); // write the data - _i2c->endTransmission(); - } - - delay(10); - - /* read back the register */ - readRegisters(subAddress,1,_buffer); - /* check the read back register against the written register */ - if(_buffer[0] == data) { - return 1; - } - else{ - return -1; - } -} - -/* reads registers from MPU9250 given a starting register address, number of bytes, and a pointer to store data */ -int MPU9250::readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest){ - if( _useSPI ){ - // begin the transaction - if(_useSPIHS){ - _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3)); - } - else{ - _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); - } - digitalWriteFast(_csPin,LOW); // select the MPU9250 chip - delayNanoseconds(200); - _spi->transfer(subAddress | SPI_READ); // specify the starting register address - for(uint8_t i = 0; i < count; i++){ - dest[i] = _spi->transfer(0x00); // read the data - } - digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip - delayNanoseconds(200); - _spi->endTransaction(); // end the transaction - return 1; - } - else{ - _i2c->beginTransmission(_address); // open the device - _i2c->write(subAddress); // specify the starting register address - _i2c->endTransmission(false); - _numBytes = _i2c->requestFrom(_address, count); // specify the number of bytes to receive - if (_numBytes == count) { - for(uint8_t i = 0; i < count; i++){ - dest[i] = _i2c->read(); - } - return 1; - } else { - return -1; - } - } -} -#else -/* writes a byte to MPU9250 register given a register address and data */ -int MPU9250::writeRegister(uint8_t subAddress, uint8_t data){ - /* write data to device */ - if( _useSPI ){ - _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction - digitalWrite(_csPin,LOW); // select the MPU9250 chip - _spi->transfer(subAddress); // write the register address - _spi->transfer(data); // write the data - digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip - _spi->endTransaction(); // end the transaction - } - else{ - _i2c->beginTransmission(_address); // open the device - _i2c->write(subAddress); // write the register address - _i2c->write(data); // write the data - _i2c->endTransmission(); - } - - delay(10); - - /* read back the register */ - readRegisters(subAddress,1,_buffer); - /* check the read back register against the written register */ - if(_buffer[0] == data) { - return 1; - } - else{ - return -1; - } -} - -/* reads registers from MPU9250 given a starting register address, number of bytes, and a pointer to store data */ -int MPU9250::readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest){ - if( _useSPI ){ - // begin the transaction - if(_useSPIHS){ - _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3)); - } - else{ - _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); - } - digitalWrite(_csPin,LOW); // select the MPU9250 chip - _spi->transfer(subAddress | SPI_READ); // specify the starting register address - for(uint8_t i = 0; i < count; i++){ - dest[i] = _spi->transfer(0x00); // read the data - } - digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip - _spi->endTransaction(); // end the transaction - return 1; - } - else{ - _i2c->beginTransmission(_address); // open the device - _i2c->write(subAddress); // specify the starting register address - _i2c->endTransmission(false); - _numBytes = _i2c->requestFrom(_address, count); // specify the number of bytes to receive - if (_numBytes == count) { - for(uint8_t i = 0; i < count; i++){ - dest[i] = _i2c->read(); - } - return 1; - } else { - return -1; - } - } -} -#endif - -/* writes a register to the AK8963 given a register address and data */ -int MPU9250::writeAK8963Register(uint8_t subAddress, uint8_t data){ - // set slave 0 to the AK8963 and set for write - if (writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR) < 0) { - return -1; - } - // set the register to the desired AK8963 sub address - if (writeRegister(I2C_SLV0_REG,subAddress) < 0) { - return -2; - } - // store the data for write - if (writeRegister(I2C_SLV0_DO,data) < 0) { - return -3; - } - // enable I2C and send 1 byte - if (writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1) < 0) { - return -4; - } - // read the register and confirm - if (readAK8963Registers(subAddress,1,_buffer) < 0) { - return -5; - } - if(_buffer[0] == data) { - return 1; - } else{ - return -6; - } -} - -/* reads registers from the AK8963 */ -int MPU9250::readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest){ - // set slave 0 to the AK8963 and set for read - if (writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) < 0) { - return -1; - } - // set the register to the desired AK8963 sub address - if (writeRegister(I2C_SLV0_REG,subAddress) < 0) { - return -2; - } - // enable I2C and request the bytes - if (writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | count) < 0) { - return -3; - } - delay(1); // takes some time for these registers to fill - // read the bytes off the MPU9250 EXT_SENS_DATA registers - _status = readRegisters(EXT_SENS_DATA_00,count,dest); - return _status; -} - -/* gets the MPU9250 WHO_AM_I register value, expected to be 0x71 */ -int MPU9250::whoAmI(){ - // read the WHO AM I register - if (readRegisters(WHO_AM_I,1,_buffer) < 0) { - return -1; - } - // return the register value - return _buffer[0]; -} - -/* gets the AK8963 WHO_AM_I register value, expected to be 0x48 */ -int MPU9250::whoAmIAK8963(){ - // read the WHO AM I register - if (readAK8963Registers(AK8963_WHO_AM_I,1,_buffer) < 0) { - return -1; - } - // return the register value - return _buffer[0]; -} - -// jihlein additions start - -void MPU9250::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { - uint8_t buffer[14]; - readRegisters(ACCEL_OUT, 14, buffer); - *ax = (((int16_t)buffer[0]) << 8) | buffer[1]; - *ay = (((int16_t)buffer[2]) << 8) | buffer[3]; - *az = (((int16_t)buffer[4]) << 8) | buffer[5]; - *gx = (((int16_t)buffer[8]) << 8) | buffer[9]; - *gy = (((int16_t)buffer[10]) << 8) | buffer[11]; - *gz = (((int16_t)buffer[12]) << 8) | buffer[13]; -} - -void MPU9250::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) { - uint8_t buffer[20]; - readRegisters(ACCEL_OUT, 20, buffer); - *ax = (((int16_t)buffer[0]) << 8) | buffer[1]; - *ay = (((int16_t)buffer[2]) << 8) | buffer[3]; - *az = (((int16_t)buffer[4]) << 8) | buffer[5]; - *gx = (((int16_t)buffer[8]) << 8) | buffer[9]; - *gy = (((int16_t)buffer[10]) << 8) | buffer[11]; - *gz = (((int16_t)buffer[12]) << 8) | buffer[13]; - *mx = (((int16_t)buffer[15]) << 8) | buffer[14]; - *my = (((int16_t)buffer[17]) << 8) | buffer[16]; - *mz = (((int16_t)buffer[19]) << 8) | buffer[18]; -} - -// jihlein additions end \ No newline at end of file diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU9250/MPU9250.h b/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU9250/MPU9250.h deleted file mode 100644 index bbbe2c5a..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/src/MPU9250/MPU9250.h +++ /dev/null @@ -1,313 +0,0 @@ -/* - MPU9250.h - Brian R Taylor - brian.taylor@bolderflight.com - - Copyright (c) 2017 Bolder Flight Systems - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -#ifndef MPU9250_h -#define MPU9250_h - -#include "Arduino.h" -#include "Wire.h" // I2C library -#include "SPI.h" // SPI library - -class MPU9250{ - public: - enum GyroRange - { - GYRO_RANGE_250DPS, - GYRO_RANGE_500DPS, - GYRO_RANGE_1000DPS, - GYRO_RANGE_2000DPS - }; - enum AccelRange - { - ACCEL_RANGE_2G, - ACCEL_RANGE_4G, - ACCEL_RANGE_8G, - ACCEL_RANGE_16G - }; - enum DlpfBandwidth - { - DLPF_BANDWIDTH_184HZ, - DLPF_BANDWIDTH_92HZ, - DLPF_BANDWIDTH_41HZ, - DLPF_BANDWIDTH_20HZ, - DLPF_BANDWIDTH_10HZ, - DLPF_BANDWIDTH_5HZ - }; - enum LpAccelOdr - { - LP_ACCEL_ODR_0_24HZ = 0, - LP_ACCEL_ODR_0_49HZ = 1, - LP_ACCEL_ODR_0_98HZ = 2, - LP_ACCEL_ODR_1_95HZ = 3, - LP_ACCEL_ODR_3_91HZ = 4, - LP_ACCEL_ODR_7_81HZ = 5, - LP_ACCEL_ODR_15_63HZ = 6, - LP_ACCEL_ODR_31_25HZ = 7, - LP_ACCEL_ODR_62_50HZ = 8, - LP_ACCEL_ODR_125HZ = 9, - LP_ACCEL_ODR_250HZ = 10, - LP_ACCEL_ODR_500HZ = 11 - }; - MPU9250(TwoWire &bus,uint8_t address); - MPU9250(SPIClass &bus,uint8_t csPin); - int begin(); - int setAccelRange(AccelRange range); - int setGyroRange(GyroRange range); - int setDlpfBandwidth(DlpfBandwidth bandwidth); - int setSrd(uint8_t srd); - int enableDataReadyInterrupt(); - int disableDataReadyInterrupt(); - int enableWakeOnMotion(float womThresh_mg,LpAccelOdr odr); - int readSensor(); - float getAccelX_mss(); - float getAccelY_mss(); - float getAccelZ_mss(); - float getGyroX_rads(); - float getGyroY_rads(); - float getGyroZ_rads(); - float getMagX_uT(); - float getMagY_uT(); - float getMagZ_uT(); - float getTemperature_C(); - - int calibrateGyro(); - float getGyroBiasX_rads(); - float getGyroBiasY_rads(); - float getGyroBiasZ_rads(); - void setGyroBiasX_rads(float bias); - void setGyroBiasY_rads(float bias); - void setGyroBiasZ_rads(float bias); - int calibrateAccel(); - float getAccelBiasX_mss(); - float getAccelScaleFactorX(); - float getAccelBiasY_mss(); - float getAccelScaleFactorY(); - float getAccelBiasZ_mss(); - float getAccelScaleFactorZ(); - void setAccelCalX(float bias,float scaleFactor); - void setAccelCalY(float bias,float scaleFactor); - void setAccelCalZ(float bias,float scaleFactor); - int calibrateMag(); - float getMagBiasX_uT(); - float getMagScaleFactorX(); - float getMagBiasY_uT(); - float getMagScaleFactorY(); - float getMagBiasZ_uT(); - float getMagScaleFactorZ(); - void setMagCalX(float bias,float scaleFactor); - void setMagCalY(float bias,float scaleFactor); - void setMagCalZ(float bias,float scaleFactor); - // jihlein additions start - void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz); - void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); - // jihlein additions end - protected: - // i2c - uint8_t _address; - TwoWire *_i2c; - const uint32_t _i2cRate = 400000; // 400 kHz - size_t _numBytes; // number of bytes received from I2C - // spi - SPIClass *_spi; - uint8_t _csPin; - bool _useSPI; - bool _useSPIHS; - const uint8_t SPI_READ = 0x80; - const uint32_t SPI_LS_CLOCK = 1000000; // 1 MHz - const uint32_t SPI_HS_CLOCK = 15000000; // 15 MHz - // track success of interacting with sensor - int _status; - // buffer for reading from sensor - uint8_t _buffer[21]; - // data counts - int16_t _axcounts,_aycounts,_azcounts; - int16_t _gxcounts,_gycounts,_gzcounts; - int16_t _hxcounts,_hycounts,_hzcounts; - int16_t _tcounts; - // data buffer - float _ax, _ay, _az; - float _gx, _gy, _gz; - float _hx, _hy, _hz; - float _t; - // wake on motion - uint8_t _womThreshold; - // scale factors - float _accelScale; - float _gyroScale; - float _magScaleX, _magScaleY, _magScaleZ; - const float _tempScale = 333.87f; - const float _tempOffset = 21.0f; - // configuration - AccelRange _accelRange; - GyroRange _gyroRange; - DlpfBandwidth _bandwidth; - uint8_t _srd; - // gyro bias estimation - size_t _numSamples = 100; - double _gxbD, _gybD, _gzbD; - float _gxb, _gyb, _gzb; - // accel bias and scale factor estimation - double _axbD, _aybD, _azbD; - float _axmax, _aymax, _azmax; - float _axmin, _aymin, _azmin; - float _axb, _ayb, _azb; - float _axs = 1.0f; - float _ays = 1.0f; - float _azs = 1.0f; - // magnetometer bias and scale factor estimation - uint16_t _maxCounts = 1000; - float _deltaThresh = 0.3f; - uint8_t _coeff = 8; - uint16_t _counter; - float _framedelta, _delta; - float _hxfilt, _hyfilt, _hzfilt; - float _hxmax, _hymax, _hzmax; - float _hxmin, _hymin, _hzmin; - float _hxb, _hyb, _hzb; - float _hxs = 1.0f; - float _hys = 1.0f; - float _hzs = 1.0f; - float _avgs; - // transformation matrix - /* transform the accel and gyro axes to match the magnetometer axes */ - const int16_t tX[3] = {0, 1, 0}; - const int16_t tY[3] = {1, 0, 0}; - const int16_t tZ[3] = {0, 0, -1}; - // constants - const float G = 9.807f; - const float _d2r = 3.14159265359f/180.0f; - // MPU9250 registers - const uint8_t ACCEL_OUT = 0x3B; - const uint8_t GYRO_OUT = 0x43; - const uint8_t TEMP_OUT = 0x41; - const uint8_t EXT_SENS_DATA_00 = 0x49; - const uint8_t ACCEL_CONFIG = 0x1C; - const uint8_t ACCEL_FS_SEL_2G = 0x00; - const uint8_t ACCEL_FS_SEL_4G = 0x08; - const uint8_t ACCEL_FS_SEL_8G = 0x10; - const uint8_t ACCEL_FS_SEL_16G = 0x18; - const uint8_t GYRO_CONFIG = 0x1B; - const uint8_t GYRO_FS_SEL_250DPS = 0x00; - const uint8_t GYRO_FS_SEL_500DPS = 0x08; - const uint8_t GYRO_FS_SEL_1000DPS = 0x10; - const uint8_t GYRO_FS_SEL_2000DPS = 0x18; - const uint8_t ACCEL_CONFIG2 = 0x1D; - const uint8_t ACCEL_DLPF_184 = 0x01; - const uint8_t ACCEL_DLPF_92 = 0x02; - const uint8_t ACCEL_DLPF_41 = 0x03; - const uint8_t ACCEL_DLPF_20 = 0x04; - const uint8_t ACCEL_DLPF_10 = 0x05; - const uint8_t ACCEL_DLPF_5 = 0x06; - const uint8_t CONFIG = 0x1A; - const uint8_t GYRO_DLPF_184 = 0x01; - const uint8_t GYRO_DLPF_92 = 0x02; - const uint8_t GYRO_DLPF_41 = 0x03; - const uint8_t GYRO_DLPF_20 = 0x04; - const uint8_t GYRO_DLPF_10 = 0x05; - const uint8_t GYRO_DLPF_5 = 0x06; - const uint8_t SMPDIV = 0x19; - const uint8_t INT_PIN_CFG = 0x37; - const uint8_t INT_ENABLE = 0x38; - const uint8_t INT_DISABLE = 0x00; - const uint8_t INT_PULSE_50US = 0x00; - const uint8_t INT_WOM_EN = 0x40; - const uint8_t INT_RAW_RDY_EN = 0x01; - const uint8_t PWR_MGMNT_1 = 0x6B; - const uint8_t PWR_CYCLE = 0x20; - const uint8_t PWR_RESET = 0x80; - const uint8_t CLOCK_SEL_PLL = 0x01; - const uint8_t PWR_MGMNT_2 = 0x6C; - const uint8_t SEN_ENABLE = 0x00; - const uint8_t DIS_GYRO = 0x07; - const uint8_t USER_CTRL = 0x6A; - const uint8_t I2C_MST_EN = 0x20; - const uint8_t I2C_MST_CLK = 0x0D; - const uint8_t I2C_MST_CTRL = 0x24; - const uint8_t I2C_SLV0_ADDR = 0x25; - const uint8_t I2C_SLV0_REG = 0x26; - const uint8_t I2C_SLV0_DO = 0x63; - const uint8_t I2C_SLV0_CTRL = 0x27; - const uint8_t I2C_SLV0_EN = 0x80; - const uint8_t I2C_READ_FLAG = 0x80; - const uint8_t MOT_DETECT_CTRL = 0x69; - const uint8_t ACCEL_INTEL_EN = 0x80; - const uint8_t ACCEL_INTEL_MODE = 0x40; - const uint8_t LP_ACCEL_ODR = 0x1E; - const uint8_t WOM_THR = 0x1F; - const uint8_t WHO_AM_I = 0x75; - const uint8_t FIFO_EN = 0x23; - const uint8_t FIFO_TEMP = 0x80; - const uint8_t FIFO_GYRO = 0x70; - const uint8_t FIFO_ACCEL = 0x08; - const uint8_t FIFO_MAG = 0x01; - const uint8_t FIFO_COUNT = 0x72; - const uint8_t FIFO_READ = 0x74; - // AK8963 registers - const uint8_t AK8963_I2C_ADDR = 0x0C; - const uint8_t AK8963_HXL = 0x03; - const uint8_t AK8963_CNTL1 = 0x0A; - const uint8_t AK8963_PWR_DOWN = 0x00; - const uint8_t AK8963_CNT_MEAS1 = 0x12; - const uint8_t AK8963_CNT_MEAS2 = 0x16; - const uint8_t AK8963_FUSE_ROM = 0x0F; - const uint8_t AK8963_CNTL2 = 0x0B; - const uint8_t AK8963_RESET = 0x01; - const uint8_t AK8963_ASA = 0x10; - const uint8_t AK8963_WHO_AM_I = 0x00; - // private functions - int writeRegister(uint8_t subAddress, uint8_t data); - int readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest); - int writeAK8963Register(uint8_t subAddress, uint8_t data); - int readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest); - int whoAmI(); - int whoAmIAK8963(); -}; - -class MPU9250FIFO: public MPU9250 { - public: - using MPU9250::MPU9250; - int enableFifo(bool accel,bool gyro,bool mag,bool temp); - int readFifo(); - void getFifoAccelX_mss(size_t *size,float* data); - void getFifoAccelY_mss(size_t *size,float* data); - void getFifoAccelZ_mss(size_t *size,float* data); - void getFifoGyroX_rads(size_t *size,float* data); - void getFifoGyroY_rads(size_t *size,float* data); - void getFifoGyroZ_rads(size_t *size,float* data); - void getFifoMagX_uT(size_t *size,float* data); - void getFifoMagY_uT(size_t *size,float* data); - void getFifoMagZ_uT(size_t *size,float* data); - void getFifoTemperature_C(size_t *size,float* data); - protected: - // fifo - bool _enFifoAccel,_enFifoGyro,_enFifoMag,_enFifoTemp; - size_t _fifoSize,_fifoFrameSize; - float _axFifo[85], _ayFifo[85], _azFifo[85]; - size_t _aSize; - float _gxFifo[85], _gyFifo[85], _gzFifo[85]; - size_t _gSize; - float _hxFifo[73], _hyFifo[73], _hzFifo[73]; - size_t _hSize; - float _tFifo[256]; - size_t _tSize; -}; - -#endif diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/src/SBUS/SBUS.cpp b/Versions/dRehmFlight_Teensy_BETA_1.2/src/SBUS/SBUS.cpp deleted file mode 100644 index 07085420..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/src/SBUS/SBUS.cpp +++ /dev/null @@ -1,376 +0,0 @@ -/* - SBUS.cpp - Brian R Taylor - brian.taylor@bolderflight.com - - Copyright (c) 2016 Bolder Flight Systems - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -#include "SBUS.h" - -// SEE: https://learn.adafruit.com/adafruit-feather-m0-basic-proto/adapting-sketches-to-m0 -#if defined(ARDUINO_SAMD_ZERO) && defined(SERIAL_PORT_USBVIRTUAL) - // Required for Serial on Zero based boards - #define Serial SERIAL_PORT_USBVIRTUAL -#endif - -#if defined(__MK20DX128__) || defined(__MK20DX256__) - // globals needed for emulating two stop bytes on Teensy 3.0 and 3.1/3.2 - IntervalTimer serialTimer; - HardwareSerial* SERIALPORT; - uint8_t PACKET[25]; - volatile int SENDINDEX; - void sendByte(); -#endif -/* SBUS object, input the serial bus */ -SBUS::SBUS(HardwareSerial& bus) -{ - _bus = &bus; -} - -/* starts the serial communication */ -void SBUS::begin() -{ - // initialize parsing state - _parserState = 0; - // initialize default scale factors and biases - for (uint8_t i = 0; i < _numChannels; i++) { - setEndPoints(i,_defaultMin,_defaultMax); - } - // begin the serial port for SBUS - #if defined(__MK20DX128__) || defined(__MK20DX256__) // Teensy 3.0 || Teensy 3.1/3.2 - _bus->begin(_sbusBaud,SERIAL_8E1_RXINV_TXINV); - SERIALPORT = _bus; - #elif defined(__IMXRT1062__) || defined(__IMXRT1052__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) || defined(__MKL26Z64__) // Teensy 4.0 || Teensy 4.0 Beta || Teensy 3.5 || Teensy 3.6 || Teensy LC - _bus->begin(_sbusBaud,SERIAL_8E2_RXINV_TXINV); - #elif defined(STM32L496xx) || defined(STM32L476xx) || defined(STM32L433xx) || defined(STM32L432xx) // STM32L4 - _bus->begin(_sbusBaud,SERIAL_SBUS); - #elif defined(_BOARD_MAPLE_MINI_H_) // Maple Mini - _bus->begin(_sbusBaud,SERIAL_8E2); - #elif defined(ESP32) // ESP32 - _bus->begin(_sbusBaud,SERIAL_8E2); - #elif defined(__AVR_ATmega2560__) || defined(__AVR_ATmega328P__) || defined(__AVR_ATmega32U4__) // Arduino Mega 2560, 328P or 32u4 - _bus->begin(_sbusBaud, SERIAL_8E2); - #elif defined(ARDUINO_SAMD_ZERO) // Adafruit Feather M0 - _bus->begin(_sbusBaud, SERIAL_8E2); - #else - #error unsupported device - #endif -} - -/* read the SBUS data */ -bool SBUS::read(uint16_t* channels, bool* failsafe, bool* lostFrame) -{ - // parse the SBUS packet - if (parse()) { - if (channels) { - // 16 channels of 11 bit data - channels[0] = (uint16_t) ((_payload[0] |_payload[1] <<8) & 0x07FF); - channels[1] = (uint16_t) ((_payload[1]>>3 |_payload[2] <<5) & 0x07FF); - channels[2] = (uint16_t) ((_payload[2]>>6 |_payload[3] <<2 |_payload[4]<<10) & 0x07FF); - channels[3] = (uint16_t) ((_payload[4]>>1 |_payload[5] <<7) & 0x07FF); - channels[4] = (uint16_t) ((_payload[5]>>4 |_payload[6] <<4) & 0x07FF); - channels[5] = (uint16_t) ((_payload[6]>>7 |_payload[7] <<1 |_payload[8]<<9) & 0x07FF); - channels[6] = (uint16_t) ((_payload[8]>>2 |_payload[9] <<6) & 0x07FF); - channels[7] = (uint16_t) ((_payload[9]>>5 |_payload[10]<<3) & 0x07FF); - channels[8] = (uint16_t) ((_payload[11] |_payload[12]<<8) & 0x07FF); - channels[9] = (uint16_t) ((_payload[12]>>3|_payload[13]<<5) & 0x07FF); - channels[10] = (uint16_t) ((_payload[13]>>6|_payload[14]<<2 |_payload[15]<<10) & 0x07FF); - channels[11] = (uint16_t) ((_payload[15]>>1|_payload[16]<<7) & 0x07FF); - channels[12] = (uint16_t) ((_payload[16]>>4|_payload[17]<<4) & 0x07FF); - channels[13] = (uint16_t) ((_payload[17]>>7|_payload[18]<<1 |_payload[19]<<9) & 0x07FF); - channels[14] = (uint16_t) ((_payload[19]>>2|_payload[20]<<6) & 0x07FF); - channels[15] = (uint16_t) ((_payload[20]>>5|_payload[21]<<3) & 0x07FF); - } - if (lostFrame) { - // count lost frames - if (_payload[22] & _sbusLostFrame) { - *lostFrame = true; - } else { - *lostFrame = false; - } - } - if (failsafe) { - // failsafe state - if (_payload[22] & _sbusFailSafe) { - *failsafe = true; - } - else{ - *failsafe = false; - } - } - // return true on receiving a full packet - return true; - } else { - // return false if a full packet is not received - return false; - } -} - -/* read the SBUS data and calibrate it to +/- 1 */ -bool SBUS::readCal(float* calChannels, bool* failsafe, bool* lostFrame) -{ - uint16_t channels[_numChannels]; - // read the SBUS data - if(read(&channels[0],failsafe,lostFrame)) { - // linear calibration - for (uint8_t i = 0; i < _numChannels; i++) { - calChannels[i] = channels[i] * _sbusScale[i] + _sbusBias[i]; - if (_useReadCoeff[i]) { - calChannels[i] = PolyVal(_readLen[i],_readCoeff[i],calChannels[i]); - } - } - // return true on receiving a full packet - return true; - } else { - // return false if a full packet is not received - return false; - } -} - -/* write SBUS packets */ -void SBUS::write(uint16_t* channels) -{ - static uint8_t packet[25]; - /* assemble the SBUS packet */ - // SBUS header - packet[0] = _sbusHeader; - // 16 channels of 11 bit data - if (channels) { - packet[1] = (uint8_t) ((channels[0] & 0x07FF)); - packet[2] = (uint8_t) ((channels[0] & 0x07FF)>>8 | (channels[1] & 0x07FF)<<3); - packet[3] = (uint8_t) ((channels[1] & 0x07FF)>>5 | (channels[2] & 0x07FF)<<6); - packet[4] = (uint8_t) ((channels[2] & 0x07FF)>>2); - packet[5] = (uint8_t) ((channels[2] & 0x07FF)>>10 | (channels[3] & 0x07FF)<<1); - packet[6] = (uint8_t) ((channels[3] & 0x07FF)>>7 | (channels[4] & 0x07FF)<<4); - packet[7] = (uint8_t) ((channels[4] & 0x07FF)>>4 | (channels[5] & 0x07FF)<<7); - packet[8] = (uint8_t) ((channels[5] & 0x07FF)>>1); - packet[9] = (uint8_t) ((channels[5] & 0x07FF)>>9 | (channels[6] & 0x07FF)<<2); - packet[10] = (uint8_t) ((channels[6] & 0x07FF)>>6 | (channels[7] & 0x07FF)<<5); - packet[11] = (uint8_t) ((channels[7] & 0x07FF)>>3); - packet[12] = (uint8_t) ((channels[8] & 0x07FF)); - packet[13] = (uint8_t) ((channels[8] & 0x07FF)>>8 | (channels[9] & 0x07FF)<<3); - packet[14] = (uint8_t) ((channels[9] & 0x07FF)>>5 | (channels[10] & 0x07FF)<<6); - packet[15] = (uint8_t) ((channels[10] & 0x07FF)>>2); - packet[16] = (uint8_t) ((channels[10] & 0x07FF)>>10 | (channels[11] & 0x07FF)<<1); - packet[17] = (uint8_t) ((channels[11] & 0x07FF)>>7 | (channels[12] & 0x07FF)<<4); - packet[18] = (uint8_t) ((channels[12] & 0x07FF)>>4 | (channels[13] & 0x07FF)<<7); - packet[19] = (uint8_t) ((channels[13] & 0x07FF)>>1); - packet[20] = (uint8_t) ((channels[13] & 0x07FF)>>9 | (channels[14] & 0x07FF)<<2); - packet[21] = (uint8_t) ((channels[14] & 0x07FF)>>6 | (channels[15] & 0x07FF)<<5); - packet[22] = (uint8_t) ((channels[15] & 0x07FF)>>3); - } - // flags - packet[23] = 0x00; - // footer - packet[24] = _sbusFooter; - #if defined(__MK20DX128__) || defined(__MK20DX256__) // Teensy 3.0 || Teensy 3.1/3.2 - // use ISR to send byte at a time, - // 130 us between bytes to emulate 2 stop bits - noInterrupts(); - memcpy(&PACKET,&packet,sizeof(packet)); - interrupts(); - serialTimer.priority(255); - serialTimer.begin(sendByte,130); - #else - // write packet - _bus->write(packet,25); - #endif -} - -/* write SBUS packets from calibrated inputs */ -void SBUS::writeCal(float* calChannels) -{ - uint16_t channels[_numChannels] = {0}; - // linear calibration - if (calChannels) { - for (uint8_t i = 0; i < _numChannels; i++) { - if (_useWriteCoeff[i]) { - calChannels[i] = PolyVal(_writeLen[i],_writeCoeff[i],calChannels[i]); - } - channels[i] = (calChannels[i] - _sbusBias[i]) / _sbusScale[i]; - } - } - write(channels); -} - -void SBUS::setEndPoints(uint8_t channel,uint16_t min,uint16_t max) -{ - _sbusMin[channel] = min; - _sbusMax[channel] = max; - scaleBias(channel); -} - -void SBUS::getEndPoints(uint8_t channel,uint16_t *min,uint16_t *max) -{ - if (min&&max) { - *min = _sbusMin[channel]; - *max = _sbusMax[channel]; - } -} - -void SBUS::setReadCal(uint8_t channel,float *coeff,uint8_t len) -{ - if (coeff) { - if (!_readCoeff) { - _readCoeff = (float**) malloc(sizeof(float*)*_numChannels); - } - if (!_readCoeff[channel]) { - _readCoeff[channel] = (float*) malloc(sizeof(float)*len); - } else { - free(_readCoeff[channel]); - _readCoeff[channel] = (float*) malloc(sizeof(float)*len); - } - for (uint8_t i = 0; i < len; i++) { - _readCoeff[channel][i] = coeff[i]; - } - _readLen[channel] = len; - _useReadCoeff[channel] = true; - } -} - -void SBUS::getReadCal(uint8_t channel,float *coeff,uint8_t len) -{ - if (coeff) { - for (uint8_t i = 0; (i < _readLen[channel]) && (i < len); i++) { - coeff[i] = _readCoeff[channel][i]; - } - } -} - -void SBUS::setWriteCal(uint8_t channel,float *coeff,uint8_t len) -{ - if (coeff) { - if (!_writeCoeff) { - _writeCoeff = (float**) malloc(sizeof(float*)*_numChannels); - } - if (!_writeCoeff[channel]) { - _writeCoeff[channel] = (float*) malloc(sizeof(float)*len); - } else { - free(_writeCoeff[channel]); - _writeCoeff[channel] = (float*) malloc(sizeof(float)*len); - } - for (uint8_t i = 0; i < len; i++) { - _writeCoeff[channel][i] = coeff[i]; - } - _writeLen[channel] = len; - _useWriteCoeff[channel] = true; - } -} - -void SBUS::getWriteCal(uint8_t channel,float *coeff,uint8_t len) -{ - if (coeff) { - for (uint8_t i = 0; (i < _writeLen[channel]) && (i < len); i++) { - coeff[i] = _writeCoeff[channel][i]; - } - } -} - -/* destructor, free dynamically allocated memory */ -SBUS::~SBUS() -{ - if (_readCoeff) { - for (uint8_t i = 0; i < _numChannels; i++) { - if (_readCoeff[i]) { - free(_readCoeff[i]); - } - } - free(_readCoeff); - } - if (_writeCoeff) { - for (uint8_t i = 0; i < _numChannels; i++) { - if (_writeCoeff[i]) { - free(_writeCoeff[i]); - } - } - free(_writeCoeff); - } -} - -/* parse the SBUS data */ -bool SBUS::parse() -{ - // reset the parser state if too much time has passed - static elapsedMicros _sbusTime = 0; - if (_sbusTime > SBUS_TIMEOUT_US) {_parserState = 0;} - // see if serial data is available - while (_bus->available() > 0) { - _sbusTime = 0; - _curByte = _bus->read(); - // find the header - if (_parserState == 0) { - if ((_curByte == _sbusHeader) && ((_prevByte == _sbusFooter) || ((_prevByte & _sbus2Mask) == _sbus2Footer))) { - _parserState++; - } else { - _parserState = 0; - } - } else { - // strip off the data - if ((_parserState-1) < _payloadSize) { - _payload[_parserState-1] = _curByte; - _parserState++; - } - // check the end byte - if ((_parserState-1) == _payloadSize) { - if ((_curByte == _sbusFooter) || ((_curByte & _sbus2Mask) == _sbus2Footer)) { - _parserState = 0; - return true; - } else { - _parserState = 0; - return false; - } - } - } - _prevByte = _curByte; - } - // return false if a partial packet - return false; -} - -/* compute scale factor and bias from end points */ -void SBUS::scaleBias(uint8_t channel) -{ - _sbusScale[channel] = 2.0f / ((float)_sbusMax[channel] - (float)_sbusMin[channel]); - _sbusBias[channel] = -1.0f*((float)_sbusMin[channel] + ((float)_sbusMax[channel] - (float)_sbusMin[channel]) / 2.0f) * _sbusScale[channel]; -} - -float SBUS::PolyVal(size_t PolySize, float *Coefficients, float X) { - if (Coefficients) { - float Y = Coefficients[0]; - for (uint8_t i = 1; i < PolySize; i++) { - Y = Y*X + Coefficients[i]; - } - return(Y); - } else { - return 0; - } -} - -// function to send byte at a time with -// ISR to emulate 2 stop bits on Teensy 3.0 and 3.1/3.2 -#if defined(__MK20DX128__) || defined(__MK20DX256__) // Teensy 3.0 || Teensy 3.1/3.2 - void sendByte() - { - if (SENDINDEX < 25) { - SERIALPORT->write(PACKET[SENDINDEX]); - SENDINDEX++; - } else { - serialTimer.end(); - SENDINDEX = 0; - } - } -#endif diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/src/SBUS/SBUS.h b/Versions/dRehmFlight_Teensy_BETA_1.2/src/SBUS/SBUS.h deleted file mode 100644 index 215cb9b7..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/src/SBUS/SBUS.h +++ /dev/null @@ -1,82 +0,0 @@ -/* - SBUS.h - Brian R Taylor - brian.taylor@bolderflight.com - - Copyright (c) 2016 Bolder Flight Systems - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -#ifndef SBUS_h -#define SBUS_h - -#include "Arduino.h" -#include "elapsedMillis.h" - -/* -* Hardware Serial Supported: -* Teensy 3.0 || Teensy 3.1/3.2 || Teensy 3.5 || Teensy 3.6 || Teensy LC || STM32L4 || Maple Mini || Arduino Mega 2560 || ESP32 -*/ -#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) \ - || defined(__MK66FX1M0__) || defined(__MKL26Z64__) || defined(__IMXRT1052__) \ - || defined(STM32L496xx) || defined(STM32L476xx) || defined(STM32L433xx) \ - || defined(STM32L432xx) || defined(_BOARD_MAPLE_MINI_H_) \ - || defined(__AVR_ATmega2560__) || defined(ESP32) -#endif - -class SBUS{ - public: - SBUS(HardwareSerial& bus); - void begin(); - bool read(uint16_t* channels, bool* failsafe, bool* lostFrame); - bool readCal(float* calChannels, bool* failsafe, bool* lostFrame); - void write(uint16_t* channels); - void writeCal(float *channels); - void setEndPoints(uint8_t channel,uint16_t min,uint16_t max); - void getEndPoints(uint8_t channel,uint16_t *min,uint16_t *max); - void setReadCal(uint8_t channel,float *coeff,uint8_t len); - void getReadCal(uint8_t channel,float *coeff,uint8_t len); - void setWriteCal(uint8_t channel,float *coeff,uint8_t len); - void getWriteCal(uint8_t channel,float *coeff,uint8_t len); - ~SBUS(); - private: - const uint32_t _sbusBaud = 100000; - static const uint8_t _numChannels = 16; - const uint8_t _sbusHeader = 0x0F; - const uint8_t _sbusFooter = 0x00; - const uint8_t _sbus2Footer = 0x04; - const uint8_t _sbus2Mask = 0x0F; - const uint32_t SBUS_TIMEOUT_US = 7000; - uint8_t _parserState, _prevByte = _sbusFooter, _curByte; - static const uint8_t _payloadSize = 24; - uint8_t _payload[_payloadSize]; - const uint8_t _sbusLostFrame = 0x04; - const uint8_t _sbusFailSafe = 0x08; - const uint16_t _defaultMin = 172; - const uint16_t _defaultMax = 1811; - uint16_t _sbusMin[_numChannels]; - uint16_t _sbusMax[_numChannels]; - float _sbusScale[_numChannels]; - float _sbusBias[_numChannels]; - float **_readCoeff, **_writeCoeff; - uint8_t _readLen[_numChannels],_writeLen[_numChannels]; - bool _useReadCoeff[_numChannels], _useWriteCoeff[_numChannels]; - HardwareSerial* _bus; - bool parse(); - void scaleBias(uint8_t channel); - float PolyVal(size_t PolySize, float *Coefficients, float X); -}; - -#endif diff --git a/Versions/dRehmFlight_Teensy_BETA_1.2/src/SBUS/elapsedMillis.h b/Versions/dRehmFlight_Teensy_BETA_1.2/src/SBUS/elapsedMillis.h deleted file mode 100644 index 48cff11d..00000000 --- a/Versions/dRehmFlight_Teensy_BETA_1.2/src/SBUS/elapsedMillis.h +++ /dev/null @@ -1,81 +0,0 @@ -/* Elapsed time types - for easy-to-use measurements of elapsed time - * http://www.pjrc.com/teensy/ - * Copyright (c) 2017 PJRC.COM, LLC - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#ifndef elapsedMillis_h -#define elapsedMillis_h -#ifdef __cplusplus - -#if ARDUINO >= 100 -#include "Arduino.h" -#else -#include "WProgram.h" -#endif - -class elapsedMillis -{ -private: - unsigned long ms; -public: - elapsedMillis(void) { ms = millis(); } - elapsedMillis(unsigned long val) { ms = millis() - val; } - elapsedMillis(const elapsedMillis &orig) { ms = orig.ms; } - operator unsigned long () const { return millis() - ms; } - elapsedMillis & operator = (const elapsedMillis &rhs) { ms = rhs.ms; return *this; } - elapsedMillis & operator = (unsigned long val) { ms = millis() - val; return *this; } - elapsedMillis & operator -= (unsigned long val) { ms += val ; return *this; } - elapsedMillis & operator += (unsigned long val) { ms -= val ; return *this; } - elapsedMillis operator - (int val) const { elapsedMillis r(*this); r.ms += val; return r; } - elapsedMillis operator - (unsigned int val) const { elapsedMillis r(*this); r.ms += val; return r; } - elapsedMillis operator - (long val) const { elapsedMillis r(*this); r.ms += val; return r; } - elapsedMillis operator - (unsigned long val) const { elapsedMillis r(*this); r.ms += val; return r; } - elapsedMillis operator + (int val) const { elapsedMillis r(*this); r.ms -= val; return r; } - elapsedMillis operator + (unsigned int val) const { elapsedMillis r(*this); r.ms -= val; return r; } - elapsedMillis operator + (long val) const { elapsedMillis r(*this); r.ms -= val; return r; } - elapsedMillis operator + (unsigned long val) const { elapsedMillis r(*this); r.ms -= val; return r; } -}; - -class elapsedMicros -{ -private: - unsigned long us; -public: - elapsedMicros(void) { us = micros(); } - elapsedMicros(unsigned long val) { us = micros() - val; } - elapsedMicros(const elapsedMicros &orig) { us = orig.us; } - operator unsigned long () const { return micros() - us; } - elapsedMicros & operator = (const elapsedMicros &rhs) { us = rhs.us; return *this; } - elapsedMicros & operator = (unsigned long val) { us = micros() - val; return *this; } - elapsedMicros & operator -= (unsigned long val) { us += val ; return *this; } - elapsedMicros & operator += (unsigned long val) { us -= val ; return *this; } - elapsedMicros operator - (int val) const { elapsedMicros r(*this); r.us += val; return r; } - elapsedMicros operator - (unsigned int val) const { elapsedMicros r(*this); r.us += val; return r; } - elapsedMicros operator - (long val) const { elapsedMicros r(*this); r.us += val; return r; } - elapsedMicros operator - (unsigned long val) const { elapsedMicros r(*this); r.us += val; return r; } - elapsedMicros operator + (int val) const { elapsedMicros r(*this); r.us -= val; return r; } - elapsedMicros operator + (unsigned int val) const { elapsedMicros r(*this); r.us -= val; return r; } - elapsedMicros operator + (long val) const { elapsedMicros r(*this); r.us -= val; return r; } - elapsedMicros operator + (unsigned long val) const { elapsedMicros r(*this); r.us -= val; return r; } -}; - -#endif // __cplusplus -#endif // elapsedMillis_h