diff --git a/GOST4401_81.cpp b/GOST4401_81.cpp deleted file mode 100755 index 58a0c99..0000000 --- a/GOST4401_81.cpp +++ /dev/null @@ -1,114 +0,0 @@ -#include -#include - -static float _GOST4401_geopotential2geometric(float altitude){ - return altitude * GOST4401_E / (GOST4401_E - altitude); -} - -static float _GOST4401_geometric2geopotential(float altitude){ - return altitude * GOST4401_E / (GOST4401_E + altitude); -} - - -/** - * Returns geometric altitude value for the given pressure. - * - * @param float pressurePa - pressure in pascals - * @retval float geometric altitude in meters - */ - -float GOST4401_getAltitude(float pressurePa){ - - if ((pressurePa <= GOST4401_MIN_PRESSURE) || (pressurePa > GOST4401_MAX_PRESSURE)) - return NAN; - - int idx = 0; - - for (idx = 0; idx < GOST4401_LUT_RECORDS - 1; idx++){ - if ((pressurePa <= ag_table[idx].press) && (pressurePa > ag_table[idx + 1].press)) - break; - } - - float Ps = ag_table[idx].press; - float Bm = ag_table[idx].t_grad; - float Tm = ag_table[idx].temp; - float Hb = ag_table[idx].alt; - float geopotH = 0; - - - if (Bm != 0.0) { - geopotH = ((Tm * pow(Ps / pressurePa, Bm * GOST4401_R / GOST4401_G) - Tm) / Bm); - } else { - geopotH = log10(Ps / pressurePa) * (GOST4401_R * Tm) / (GOST4401_G * 0.434294); - } - - return _GOST4401_geopotential2geometric(Hb + geopotH); -}; - -/** - * Returns pressure in pascals for the given geometric altitude - * - * @param float altitude - geometric altitude in meters - * @retval float - pressure in pascals - */ - -float GOST4401_getPressure(float altitude){ - float geopotH = _GOST4401_geometric2geopotential(altitude); - - if ((geopotH < GOST4401_MIN_GPALT) || (geopotH >= GOST4401_MAX_GPALT)) - return NAN; - - int idx = 0; - - for (idx = 1; idx < GOST4401_LUT_RECORDS - 1; idx++) { - if ((geopotH >= ag_table[idx].alt) && (geopotH < ag_table[idx].alt)) - break; - } - - float Ps = ag_table[idx].press; - float Bm = ag_table[idx].t_grad; - float Tm = ag_table[idx].temp; - float Hb = ag_table[idx].alt; - float lP = 0; - - if (Bm != 0.0) { - lP = log10(Ps) - (GOST4401_G / (Bm * GOST4401_R)) * log10((Tm + Bm * (geopotH - Hb)) / Tm); - } else { - lP = log10(Ps) - 0.434294 *(GOST4401_G * (geopotH - Hb)) / (GOST4401_R * Tm); - } - - return pow(10, lP); -} - -/** - * Returns temperature value in K for the given geometric altitude. - * - * @param float altitude - geometric altitude in meters - * @retval float - temperature in degrees K - */ - -float GOST4401_getTemperature(float altitude){ - float geopotH = _GOST4401_geometric2geopotential(altitude); - - if ((geopotH < GOST4401_MIN_GPALT) || (geopotH >= GOST4401_MAX_GPALT)) - return NAN; - - int idx = 0; - - for (idx = 1; idx < GOST4401_LUT_RECORDS - 1; idx++) { - if ((geopotH >= ag_table[idx].alt) && (geopotH < ag_table[idx].alt)) - break; - } - - float Bm = ag_table[idx].t_grad; - float Tm = ag_table[idx].temp; - float Hb = ag_table[idx].alt; - float temp = Tm; - - if (Bm != 0.0){ - temp += Bm * (geopotH - Hb); - } - - return temp; -} - diff --git a/GOST4401_81.h b/GOST4401_81.h deleted file mode 100755 index ca6274a..0000000 --- a/GOST4401_81.h +++ /dev/null @@ -1,56 +0,0 @@ -/** - * Partial implementation of standard atmospheric model as described in - * GOST 4401-81 useful for processing of data from meteorological balloon - * sensors. - * - * Supported modelling of temperature and pressure over the altitude span from - * 0 up to 51km. - * - * by Oleg Kochetov - */ - -#ifndef _ATMOSPHERE_GOST4401_H_ -#define _ATMOSPHERE_GOST4401_H_ - -#include - - -// Constants -#define GOST4401_G 9.80665 -#define GOST4401_R 287.05287 -#define GOST4401_E 6356766 - -#define GOST4401_MIN_PRESSURE 6.69384 -#define GOST4401_MAX_PRESSURE 101325.00 - -#define GOST4401_MIN_GPALT 0.00 -#define GOST4401_MAX_GPALT 51000.00 - - -// Export functions -float GOST4401_getAltitude(float pressurePa); -float GOST4401_getPressure(float altitude); -float GOST4401_getTemperature(float altitude); - - -// Structure for lookup table record -typedef struct __attribute__((packed)) _GOST4401_RECORD{ - float alt; // Geopotentional altitude - float temp; // degrees K - float t_grad; // degrees K per meter - float press; // pascals -} GOST4401_RECORD; - -// Lookup table with averaged empirical parameters for -// lower layers of atmosphere in accordance with GOST 4401-81 -#define GOST4401_LUT_RECORDS 6 -static const GOST4401_RECORD ag_table[] = { - { 0, 288.15, -0.0065, 101325.00 }, - { 11000, 216.65, 0.0, 22632.04 }, - { 20000, 216.65, 0.0010, 5474.87 }, - { 32000, 228.65, 0.0028, 868.0146 }, - { 47000, 270.65, 0.0, 110.9056 }, - { 51000, 270.65, -0.0028, 6.69384 } -}; - -#endif \ No newline at end of file diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..f288702 --- /dev/null +++ b/LICENSE @@ -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 +. diff --git a/LPS331.cpp b/LPS331.cpp deleted file mode 100755 index d311595..0000000 --- a/LPS331.cpp +++ /dev/null @@ -1,168 +0,0 @@ -#include -#include -#include - -// Constructors ////////////////////////////////////////////////////// - -LPS331::LPS331(uint8_t addr) -{ - address = addr; -} - -// Public Methods //////////////////////////////////////////////////// - -// sets or detects slave address; returns bool indicating success -void LPS331::begin() -{ - WIRE_IMU.begin(); - writeReg(LPS331_CTRL_REG1, 0b11100000); - delay(100); -} - -// writes register -void LPS331::writeReg(byte reg, byte value) -{ - WIRE_IMU.beginTransmission(address); - WIRE_IMU.write(reg); - WIRE_IMU.write(value); - WIRE_IMU.endTransmission(); -} - -// reads register -byte LPS331::readReg(byte reg) -{ - byte value; - - WIRE_IMU.beginTransmission(address); - WIRE_IMU.write(reg); - WIRE_IMU.endTransmission(false); // restart - WIRE_IMU.requestFrom(address, (byte)1); - value = WIRE_IMU.read(); - WIRE_IMU.endTransmission(); - - return value; -} - -// read pressure in pascals -float LPS331::readPressurePascals(void) -{ - return (float)readPressureRaw() / 40.96; -} - -// reads pressure in millibars (mbar)/hectopascals (hPa) -float LPS331::readPressureMillibars(void) -{ - return (float)readPressureRaw() / 4096; -} - -// reads pressure in inches of mercury (inHg) -float LPS331::readPressureInchesHg(void) -{ - return (float)readPressureRaw() / 138706.5; -} - -// reads pressure in millimeters of mercury (mmHg) -float LPS331::readPressureMillimetersHg(void) -{ - // 1 mbar * 0,75006168270417 = 1 mmHg - return (float)(readPressureRaw()) * 0.75006375541921 / 4096.0; -} - -// reads pressure and returns raw 24-bit sensor output -int32_t LPS331::readPressureRaw(void) -{ - WIRE_IMU.beginTransmission(address); - // assert MSB to enable register address auto-increment - WIRE_IMU.write(LPS331_PRESS_OUT_XL | (1 << 7)); - WIRE_IMU.endTransmission(); - WIRE_IMU.requestFrom(address, (byte)3); - - while (WIRE_IMU.available() < 3); - - uint8_t pxl = WIRE_IMU.read(); - uint8_t pl = WIRE_IMU.read(); - uint8_t ph = WIRE_IMU.read(); - - // combine bytes - return (int32_t)(int8_t)ph << 16 | (uint16_t)pl << 8 | pxl; -} - -// reads temperature in degrees K -float LPS331::readTemperatureK(){ - return readTemperatureC() + LPS331_CELSIUS_TO_KELVIN_OFFSET; -} - -// reads temperature in degrees C -float LPS331::readTemperatureC(void) -{ - return 42.5 + (float)readTemperatureRaw() / 480; -} - -// reads temperature in degrees F -float LPS331::readTemperatureF(void) -{ - return 108.5 + (float)readTemperatureRaw() / 480 * 1.8; -} - -// reads temperature and returns raw 16-bit sensor output -int16_t LPS331::readTemperatureRaw(void) -{ - WIRE_IMU.beginTransmission(address); - // assert MSB to enable register address auto-increment - WIRE_IMU.write(LPS331_TEMP_OUT_L | (1 << 7)); - WIRE_IMU.endTransmission(); - WIRE_IMU.requestFrom(address, (byte)2); - - while (WIRE_IMU.available() < 2); - - uint8_t tl = WIRE_IMU.read(); - uint8_t th = WIRE_IMU.read(); - - // combine bytes - return (int16_t)(th << 8 | tl); -} - -// Calculates altitude in meters above MSL using GOST4401-81 -// atmosphere model from the given pressure in pascals -// The model implemented for height up to 51km -// -float LPS331::GOST4401_altitude(float pressure_pascals){ - return GOST4401_getAltitude(pressure_pascals); -} - -// converts pressure in mbar to altitude in meters, using 1976 US -// Standard Atmosphere model (note that this formula only applies to a -// height of 11 km, or about 36000 ft) -// If altimeter setting (QNH, barometric pressure adjusted to sea -// level) is given, this function returns an indicated altitude -// compensated for actual regional pressure; otherwise, it returns -// the pressure altitude above the standard pressure level of 1013.25 -// mbar or 29.9213 inHg -float LPS331::pressureToAltitudeMeters(float pressure_mbar, float altimeter_setting_mbar) -{ - return (1 - pow(pressure_mbar / altimeter_setting_mbar, 0.190263)) * 44330.8; -} - -// converts pressure in inHg to altitude in feet; see notes above -float LPS331::pressureToAltitudeFeet(float pressure_inHg, float altimeter_setting_inHg) -{ - return (1 - pow(pressure_inHg / altimeter_setting_inHg, 0.190263)) * 145442; -} - -// Private Methods /////////////////////////////////////////////////// - -bool LPS331::autoDetectAddress(void) -{ - // try each possible address and stop if reading WHO_AM_I returns the expected response - address = LPS331AP_ADDRESS_SA0_LOW; - if (testWhoAmI()) return true; - address = LPS331AP_ADDRESS_SA0_HIGH; - if (testWhoAmI()) return true; - - return false; -} - -bool LPS331::testWhoAmI(void) -{ - return (readReg(LPS331_WHO_AM_I) == 0xBB); -} diff --git a/LPS331.h b/LPS331.h deleted file mode 100755 index 134ca27..0000000 --- a/LPS331.h +++ /dev/null @@ -1,90 +0,0 @@ -#ifndef LPS331_h -#define LPS331_h - -#include // for byte data type -#include "stmhw.h" - -// The Arduino two-WIRE_IMU interface uses a 7-bit number for the address, -// and sets the last bit correctly based on reads and writes -#define LPS331AP_ADDRESS_SA0_LOW 0b1011100 -#define LPS331AP_ADDRESS_SA0_HIGH 0b1011101 -#define LPS331AP_TWI_ADDRESS 0b1011100 -#define LPS331AP_TWI_ADDRESS_V2 0b1011101 - -// SA0 states - -#define LPS331_SA0_LOW 0 -#define LPS331_SA0_HIGH 1 -#define LPS331_SA0_AUTO 2 - -// register addresses -// Note: Some of the register names in the datasheet are inconsistent -// between Table 14 in section 6 and the register descriptions in -// section 7. Where they differ, the names from section 7 have been -// used here. - -#define LPS331_REF_P_XL 0x08 -#define LPS331_REF_P_L 0x09 -#define LPS331_REF_P_H 0x0A - -#define LPS331_WHO_AM_I 0x0F - -#define LPS331_RES_CONF 0x10 - -#define LPS331_CTRL_REG1 0x20 -#define LPS331_CTRL_REG2 0x21 -#define LPS331_CTRL_REG3 0x22 -#define LPS331_INTERRUPT_CFG 0x23 -#define LPS331_INT_SOURCE 0x24 -#define LPS331_THS_P_L 0x25 -#define LPS331_THS_P_H 0x26 -#define LPS331_STATUS_REG 0x27 - -#define LPS331_PRESS_OUT_XL 0x28 -#define LPS331_PRESS_OUT_L 0x29 -#define LPS331_PRESS_OUT_H 0x2A - -#define LPS331_TEMP_OUT_L 0x2B -#define LPS331_TEMP_OUT_H 0x2C - -#define LPS331_AMP_CTRL 0x30 - -#define LPS331_DELTA_PRESS_XL 0x3C -#define LPS331_DELTA_PRESS_L 0x3D -#define LPS331_DELTA_PRESS_H 0x3E - -// Some physical constants -#define LPS331_CELSIUS_TO_KELVIN_OFFSET 273.15 - -class LPS331 -{ - public: - LPS331(uint8_t addr = LPS331AP_TWI_ADDRESS); - - void begin(); - - void writeReg(byte reg, byte value); - byte readReg(byte reg); - - float readPressurePascals(void); - float readPressureMillibars(void); - float readPressureInchesHg(void); - float readPressureMillimetersHg(void); - int32_t readPressureRaw(void); - float readTemperatureK(void); - float readTemperatureC(void); - float readTemperatureF(void); - int16_t readTemperatureRaw(void); - - static float GOST4401_altitude(float pressure_pascals); - static float pressureToAltitudeMeters(float pressure_mbar, float altimeter_setting_mbar = 1013.25); - static float pressureToAltitudeFeet(float pressure_inHg, float altimeter_setting_inHg = 29.9213); - - private: - byte address; - - bool autoDetectAddress(void); - bool testWhoAmI(void); -}; - -#endif diff --git a/MadgwickAHRS.cpp b/MadgwickAHRS.cpp deleted file mode 100755 index 790aecc..0000000 --- a/MadgwickAHRS.cpp +++ /dev/null @@ -1,235 +0,0 @@ -#include "MadgwickAHRS.h" -#include -#include - -Madgwick::Madgwick() { - -} - -void Madgwick::setKoeff(float sampleFreq, float beta) { - _beta = beta; - _sampleFreq = sampleFreq; -} -void Madgwick::reset() { - _q0 = 1.0; - _q1 = 0; - _q2 = 0; - _q3 = 0; -} - -void Madgwick::readQuaternions(float *q0, float *q1, float *q2, float *q3) { - *q0 = _q0; - *q1 = _q1; - *q2 = _q2; - *q3 = _q3; -} - -//--------------------------------------------------------------------------------------------------- -// AHRS algorithm update - -void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { - 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 IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) - if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { - update(gx, gy, gz, ax, ay, az); - return; - } - - // 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 = sqrt(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 -= _beta * s0; - qDot2 -= _beta * s1; - qDot3 -= _beta * s2; - qDot4 -= _beta * s3; - } - - // Integrate rate of change of quaternion to yield quaternion - _q0 += qDot1 * (1.0f / _sampleFreq); - _q1 += qDot2 * (1.0f / _sampleFreq); - _q2 += qDot3 * (1.0f / _sampleFreq); - _q3 += qDot4 * (1.0f / _sampleFreq); - - // Normalise quaternion - recipNorm = invSqrt(_q0 * _q0 + _q1 * _q1 + _q2 * _q2 + _q3 * _q3); - _q0 *= recipNorm; - _q1 *= recipNorm; - _q2 *= recipNorm; - _q3 *= recipNorm; -} - -//--------------------------------------------------------------------------------------------------- -// IMU algorithm update - -void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az) { - 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; - - // 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 * (1.0f / _sampleFreq); - _q1 += qDot2 * (1.0f / _sampleFreq); - _q2 += qDot3 * (1.0f / _sampleFreq); - _q3 += qDot4 * (1.0f / _sampleFreq); - - // Normalise quaternion - recipNorm = invSqrt(_q0 * _q0 + _q1 * _q1 + _q2 * _q2 + _q3 * _q3); - _q0 *= recipNorm; - _q1 *= recipNorm; - _q2 *= recipNorm; - _q3 *= recipNorm; -} - -//--------------------------------------------------------------------------------------------------- -// Fast inverse square-root -// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root - -float Madgwick::invSqrt(float x) { - 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)); - return y; -} - -float Madgwick::getYawRad() { - return atan2(2 * _q1 * _q2 - 2 * _q0 * _q3, 2 * _q0 * _q0 + 2 * _q1 * _q1 - 1); -} - -float Madgwick::getPitchRad() { - return atan2(2 * _q2 * _q3 - 2 * _q0 * _q1, 2 * _q0 * _q0 + 2 * _q3 * _q3 - 1); -} - -float Madgwick::getRollRad() { - return -1 * atan2(2.0f * (_q0 * _q2 - _q1 * _q3), 1.0f - 2.0f * (_q2 * _q2 + _q1 *_q1 )); -} - -float Madgwick::getYawDeg() { - return getYawRad() * RAD_TO_DEG; -} - -float Madgwick::getPitchDeg() { - return getPitchRad() * RAD_TO_DEG; -} - -float Madgwick::getRollDeg() { - return getRollRad() * RAD_TO_DEG; -} \ No newline at end of file diff --git a/MadgwickAHRS.h b/MadgwickAHRS.h deleted file mode 100755 index a3487fa..0000000 --- a/MadgwickAHRS.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef MADGWICK_AHRS_H_ -#define MADGWICK_AHRS_H_ - -#include - -#define SAMPLE_FREQ 1000.0f // sample frequency in Hz -#define BETA_DEF 0.5f // 2 * proportional gain - -class Madgwick { - -public: - Madgwick(); - void readQuaternions(float *q0, float *q1, float *q2, float *q3); - void reset(); - void setKoeff(float sampleFreq, float beta); - void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); - void update(float gx, float gy, float gz, float ax, float ay, float az); - float getPitchRad(); - float getRollRad(); - float getYawRad(); - float getPitchDeg(); - float getRollDeg(); - float getYawDeg(); - -private: - float invSqrt(float x); - volatile float _beta = BETA_DEF; // algorithm gain - volatile float _sampleFreq = SAMPLE_FREQ; - volatile float _q0 = 1.0f; - volatile float _q1 = 0.0f; - volatile float _q2 = 0.0f; - volatile float _q3 = 0.0f; -}; -#endif diff --git a/README.md b/README.md index 445e196..54411fd 100755 --- a/README.md +++ b/README.md @@ -1,22 +1,3 @@ -TroykaIMU -========== +# TroykaIMU -Библиотека для Arduino, позволяющая управлять [IMU-сенсором на 10 степеней свободы (Troyka-модуль)](http://amperka.ru/product/troyka-imu-10-dof) -от [Амперки](http://amperka.ru/). - -Установка библиотеки -==================== - -В Arduino IDE выберите пункт меню «Скетч» → «Импортировать библиотеку» → -«Добавить библиотеку…». В появившемся окне выберите скачаный архив с -библиотекой. Установка завершена. - -Подробности и примеры работы для [IMU-сенсора на 10 степеней свободы (Troyka-модуль)](http://wiki.amperka.ru/продукты:troyka-imu-10-dof). - -Подробности и примеры работы для [акселерометра (Troyka-модуль)](http://wiki.amperka.ru/продукты:troyka-accelerometer). - -Подробности и примеры работы для [гироскопа (Troyka-модуль)](http://wiki.amperka.ru/продукты:troyka-gyro). - -Подробности и примеры работы для [компаса (Troyka-модуль)](http://wiki.amperka.ru/продукты:troyka-compass). - -Подробности и примеры работы для [барометра (Troyka-модуль)](http://wiki.amperka.ru/продукты:troyka-barometer). \ No newline at end of file +Arduino library to interface with the [Amperka IMU 10-DOF Sensor](https://amperka.ru/product/troyka-imu-10-dof). diff --git a/TroykaIMU.h b/TroykaIMU.h deleted file mode 100755 index 7b39cb3..0000000 --- a/TroykaIMU.h +++ /dev/null @@ -1,50 +0,0 @@ -#include -#include "lis331dlh.h" -#include "l3g4200d.h" -#include "lis3mdl.h" -#include "LPS331.h" -#include "MadgwickAHRS.h" - -// Accelerometer -#define ACCEL_ADDRESS_V1 LIS331DLH_TWI_ADDRESS -#define ACCEL_ADDRESS_V2 LIS331DLH_TWI_ADDRESS_V2 - -// Gyroscope -#define GYRO_ADDRESS_V1 L3G4200D_TWI_ADDRESS -#define GYRO_ADDRESS_V2 L3G4200D_TWI_ADDRESS_V2 - -// Compass -#define COMPASS_ADDRESS_V1 LIS3MDL_TWI_ADDRESS -#define COMPASS_ADDRESS_V2 LIS3MDL_TWI_ADDRESS_V2 - -// Barometer -#define BARO_ADDRESS_V1 LPS331AP_TWI_ADDRESS -#define BARO_ADDRESS_V2 LPS331AP_TWI_ADDRESS_V2 - -class Accelerometer : public LIS331DLH_TWI -{ - public: - Accelerometer(uint8_t addr = ACCEL_ADDRESS_V1) : - LIS331DLH_TWI(addr) {} -}; - -class Gyroscope : public L3G4200D_TWI -{ - public: - Gyroscope(uint8_t addr = GYRO_ADDRESS_V1) : - L3G4200D_TWI(addr) {} -}; - -class Compass : public LIS3MDL_TWI -{ - public: - Compass(uint8_t addr = COMPASS_ADDRESS_V1) : - LIS3MDL_TWI(addr) {} -}; - -class Barometer : public LPS331 -{ - public: - Barometer(uint8_t addr = BARO_ADDRESS_V1) : - LPS331(addr) {} -}; diff --git a/examples/Accelerometer/AccelerometerSimple/AccelerometerSimple.ino b/examples/Accelerometer/AccelerometerSimple/AccelerometerSimple.ino new file mode 100644 index 0000000..131a4e0 --- /dev/null +++ b/examples/Accelerometer/AccelerometerSimple/AccelerometerSimple.ino @@ -0,0 +1,30 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для работы с акселерометром +Accelerometer accelerometer; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Выводим сообщение о начале инициализации + Serial.println("Accelerometer begin"); + // Инициализируем акселерометр + accelerometer.begin(); + // Выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); +} + +void loop() { + // Выводим направления и величины ускорения в м/с² по оси X + Serial.print(accelerometer.readAccelerationAX()); + Serial.print("\t\t"); + // Выводим направления и величины ускорения в м/с² по оси Y + Serial.print(accelerometer.readAccelerationAY()); + Serial.print("\t\t"); + // Выводим направления и величины ускорения в м/с² по оси Z + Serial.print(accelerometer.readAccelerationAZ()); + Serial.print("\t\t"); + Serial.println(); + delay(100); +} diff --git a/examples/Barometer/BarometerSimple/BarometerSimple.ino b/examples/Barometer/BarometerSimple/BarometerSimple.ino new file mode 100644 index 0000000..f0a4b44 --- /dev/null +++ b/examples/Barometer/BarometerSimple/BarometerSimple.ino @@ -0,0 +1,41 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для работы с акселерометром +Barometer barometer; + +void setup() { + // Открываем Serial-порт + Serial.begin(9600); + // Выводим сообщение о начале инициализации + Serial.println("Barometer begin"); + // Инициализируем барометр + barometer.begin(); + // Выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); +} + +void loop() { + // Создаём переменную для значения атмосферного давления в Паскалях + float pressurePascals = barometer.readPressurePascals(); + // Создаём переменную для значения атмосферного давления в мм рт.ст. + float pressureMillimetersHg = barometer.readPressureMillimetersHg(); + // Создаём переменную для значения высоты над уровнем море + float altitude = barometer.readAltitude(); + // Создаём переменную для значения температуры окружающей среды + float temperature = barometer.readTemperatureC(); + + // Вывод данных в Serial-порт + Serial.print("Pressure: "); + Serial.print(pressurePascals); + Serial.print(" Pa\t"); + Serial.print(pressureMillimetersHg); + Serial.print(" mmHg\t"); + Serial.print("Height: "); + Serial.print(altitude); + Serial.print(" m \t"); + Serial.print("Temperature: "); + Serial.print(temperature); + Serial.println(" C"); + delay(100); +} diff --git a/examples/Compass/CompassAzimuth/CompassAzimuth.ino b/examples/Compass/CompassAzimuth/CompassAzimuth.ino new file mode 100644 index 0000000..55ae7fe --- /dev/null +++ b/examples/Compass/CompassAzimuth/CompassAzimuth.ino @@ -0,0 +1,35 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для работы с магнитометром/компасом +Compass compass; + +// Калибровочные данные для работы магнитометра в режиме компаса +// Подробности читайте в документации про калибровку модуля +// http://wiki.amperka.ru/articles:troyka-magnetometer-compass-calibrate +const float compassCalibrationBias[3] = { 567.893, -825.35, 1061.436 }; + +const float compassCalibrationMatrix[3][3] = { { 1.909, 0.082, 0.004 }, + { 0.049, 1.942, -0.235 }, + { -0.003, 0.008, 1.944} }; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Выводим сообщение о начале инициализации + Serial.println("Compass begin"); + // Инициализируем компас + compass.begin(); + // Устанавливаем калибровочные данные + compass.setCalibrateMatrix(compassCalibrationMatrix, + compassCalibrationBias); + // Выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); +} + +void loop() { + // Выводим азимут относительно оси Z + Serial.print(compass.readAzimut()); + Serial.println(" Degrees"); + delay(100); +} diff --git a/examples/Compass/CompassCalibration/CompassCalibration.ino b/examples/Compass/CompassCalibration/CompassCalibration.ino new file mode 100644 index 0000000..26fdb80 --- /dev/null +++ b/examples/Compass/CompassCalibration/CompassCalibration.ino @@ -0,0 +1,27 @@ +// Пример выводит сырые значения магнитометра, которые +// используются для последующей калибровки сенсора +// Подробности читайте в документации про калибровку магнитометра +// http://wiki.amperka.ru/articles:troyka-magnetometer-compass-calibrate + +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для работы с магнитометром/компасом +Compass compass; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Инициализируем компас + compass.begin(); +} + +void loop() { + // Выводим «сырые» значения компаса по трём осям + Serial.print(compass.readX()); + Serial.print(","); + Serial.print(compass.readY()); + Serial.print(","); + Serial.println(compass.readZ()); + delay(100); +} diff --git a/examples/Compass/CompassSimple/CompassSimple.ino b/examples/Compass/CompassSimple/CompassSimple.ino new file mode 100644 index 0000000..4b8880b --- /dev/null +++ b/examples/Compass/CompassSimple/CompassSimple.ino @@ -0,0 +1,29 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для работы с магнитометром/компасом +Compass compass; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Выводим сообщение о начале инициализации + Serial.println("Compass begin"); + // Инициализируем компас + compass.begin(); + // Выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); +} + +void loop() { + // Выводим напряженность магнитного поля в Гауссах по оси X + Serial.print(compass.readMagneticGaussX()); + Serial.print("\t\t"); + // Выводим напряженность магнитного поля в Гауссах по оси Y + Serial.print(compass.readMagneticGaussY()); + Serial.print("\t\t"); + // Выводим напряженность магнитного поля в Гауссах по оси Z + Serial.print(compass.readMagneticGaussZ()); + Serial.println(); + delay(100); +} diff --git a/examples/Gyroscope/GyroscopeSimple/GyroscopeSimple.ino b/examples/Gyroscope/GyroscopeSimple/GyroscopeSimple.ino new file mode 100644 index 0000000..e69af7d --- /dev/null +++ b/examples/Gyroscope/GyroscopeSimple/GyroscopeSimple.ino @@ -0,0 +1,30 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для работы с гироскопом +Gyroscope gyroscope; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Выводим сообщение о начале инициализации + Serial.println("Gyroscope begin"); + // Инициализируем гироскоп + gyroscope.begin(); + // Выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); +} + +void loop() { + // Выводим угловую скорость в градусах в секунду относительно оси X + Serial.print(gyroscope.readRotationDegX()); + Serial.print("\t\t"); + // Выводим угловую скорость в градусах в секунду относительно оси Y + Serial.print(gyroscope.readRotationDegY()); + Serial.print("\t\t"); + // Выводим угловую скорость в градусах в секунду относительно оси Z + Serial.print(gyroscope.readRotationDegZ()); + Serial.print("\t\t"); + Serial.println(); + delay(100); +} diff --git a/examples/IMU/IMUSimple/IMUSimple.ino b/examples/IMU/IMUSimple/IMUSimple.ino new file mode 100644 index 0000000..0792e36 --- /dev/null +++ b/examples/IMU/IMUSimple/IMUSimple.ino @@ -0,0 +1,63 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для работы с гироскопом +Gyroscope gyroscope; +// Создаём объект для работы с акселерометром +Accelerometer accelerometer; +// Создаём объект для работы с магнитометром/компасом +Compass compass; +// Создаём объект для работы с барометром +Barometer barometer; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Выводим сообщение о начале инициализации + Serial.println("IMU Begin"); + // Инициализируем гироскоп + gyroscope.begin(); + // Инициализируем акселерометр + accelerometer.begin(); + // Инициализируем компас + compass.begin(); + // Инициализируем барометр + barometer.begin(); + // Выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); + Serial.println("Gyroscope\t\t\tAccelerometer\t\t\tCompass\t\tBarometer"); +} + +void loop() { + // Выводим угловую скорость в градусах в секунду относительно оси X, Y и Z + Serial.print(gyroscope.readRotationDegX()); + Serial.print("\t"); + Serial.print(gyroscope.readRotationDegY()); + Serial.print("\t"); + Serial.print(gyroscope.readRotationDegZ()); + Serial.print("\t\t"); + // Выводим направления и величины ускорения в м/с² относительно оси X, Y и Z + Serial.print(accelerometer.readAccelerationAX()); + Serial.print("\t"); + Serial.print(accelerometer.readAccelerationAY()); + Serial.print("\t"); + Serial.print(accelerometer.readAccelerationAZ()); + Serial.print("\t\t"); + // Выводим напряженность магнитного поля в Гауссах относительно оси X, Y и Z + Serial.print(compass.readMagneticGaussX()); + Serial.print("\t"); + Serial.print(compass.readMagneticGaussY()); + Serial.print("\t"); + Serial.print(compass.readMagneticGaussZ()); + Serial.print("\t\t"); + // Выводим значения атмосферного давления в мм рт.ст. + Serial.print(barometer.readPressureMillimetersHg()); + Serial.print("\t"); + // Выводим значения высоты над уровнем море + Serial.print(barometer.readAltitude()); + Serial.print("\t"); + // Выводим значения температуры окружающей среды + Serial.print(barometer.readTemperatureC()); + Serial.println(); + delay(100); +} diff --git a/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino b/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino new file mode 100644 index 0000000..0ce273a --- /dev/null +++ b/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino @@ -0,0 +1,66 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для фильтра Madgwick +Madgwick filter; +// Создаём объект для работы с гироскопом +Gyroscope gyroscope; +// Создаём объект для работы с акселерометром +Accelerometer accelerometer; + +// Переменные для данных с гироскопа и акселерометра +float gx, gy, gz, ax, ay, az; + +// Переменные для хранения самолётных углов ориентации +float yaw, pitch, roll; + +// Переменная для хранения частоты выборок фильтра +float sampleRate = 100; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Выводим сообщение о начале инициализации + Serial.println("IMU Begin"); + // Инициализируем гироскоп + gyroscope.begin(); + // Инициализируем акселерометр + accelerometer.begin(); + // Инициализируем фильтр + filter.begin(); + // Выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); +} + +void loop() { + // Запоминаем текущее время + unsigned long startMillis = millis(); + // Считываем данные с акселерометра в единицах G + accelerometer.readAccelerationGXYZ(ax, ay, az); + // Считываем данные с гироскопа в радианах в секунду + gyroscope.readRotationRadXYZ(gx, gy, gz); + // Устанавливаем частоту фильтра + filter.setFrequency(sampleRate); + // Обновляем входные данные в фильтр + filter.update(gx, gy, gz, ax, ay, az); + + // Получаем из фильтра углы: yaw, pitch и roll + yaw = filter.getYawDeg(); + pitch = filter.getPitchDeg(); + roll = filter.getRollDeg(); + + // Выводим полученные углы Эйлера в Serial-порт + Serial.print("yaw: "); + Serial.print(yaw); + Serial.print("\t\t"); + Serial.print("pitch: "); + Serial.print(pitch); + Serial.print("\t\t"); + Serial.print("roll: "); + Serial.println(roll); + + // Вычисляем затраченное время на обработку данных + unsigned long deltaMillis = millis() - startMillis; + // Вычисляем частоту обработки фильтра + sampleRate = 1000 / deltaMillis; +} diff --git a/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino b/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino new file mode 100644 index 0000000..c3b59db --- /dev/null +++ b/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino @@ -0,0 +1,85 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для фильтра Madgwick +Madgwick filter; +// Создаём объект для работы с гироскопом +Gyroscope gyroscope; +// Создаём объект для работы с акселерометром +Accelerometer accelerometer; +// Создаём объект для работы с магнитометром/компасом +Compass compass; + +// Переменные для данных с гироскопа, акселерометра и компаса +float gx, gy, gz, ax, ay, az, mx, my, mz; + +// Переменные для хранения самолётных углов ориентации +float yaw, pitch, roll; + +// Переменная для хранения частоты выборок фильтра +float sampleRate = 100; + +// Калибровочные данные для работы магнитометра в режиме компаса +// Подробности читайте в документации про калибровку модуля +// http://wiki.amperka.ru/articles:troyka-magnetometer-compass-calibrate +const float compassCalibrationBias[3] = { 567.893, -825.35, 1061.436 }; + +const float compassCalibrationMatrix[3][3] = { { 1.909, 0.082, 0.004 }, + { 0.049, 1.942, -0.235 }, + { -0.003, 0.008, 1.944} }; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Выводим сообщение о начале инициализации + Serial.println("IMU Begin"); + // Инициализируем гироскоп + gyroscope.begin(); + // Инициализируем акселерометр + accelerometer.begin(); + // Инициализируем компас + compass.begin(); + // Инициализируем фильтр + filter.begin(); + // Устанавливаем калибровочные данные + compass.setCalibrateMatrix(compassCalibrationMatrix, + compassCalibrationBias); + // Выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); +} + +void loop() { + // Запоминаем текущее время + unsigned long startMillis = millis(); + + // Считываем данные с акселерометра в единицах G + accelerometer.readAccelerationGXYZ(ax, ay, az); + // Считываем данные с гироскопа в радианах в секунду + gyroscope.readRotationRadXYZ(gx, gy, gz); + // Считываем данные с компаса в Гауссах + compass.readCalibrateMagneticGaussXYZ(mx, my, mz); + // Устанавливаем частоту фильтра + filter.setFrequency(sampleRate); + // Обновляем входные данные в фильтр + filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); + + // Получаем из фильтра углы: yaw, pitch и roll + yaw = filter.getYawDeg(); + pitch = filter.getPitchDeg(); + roll = filter.getRollDeg(); + + // Выводим полученные углы Эйлера в serial-порт + Serial.print("yaw: "); + Serial.print(yaw); + Serial.print("\t\t"); + Serial.print("pitch: "); + Serial.print(pitch); + Serial.print("\t\t"); + Serial.print("roll: "); + Serial.println(roll); + + // Вычисляем затраченное время на обработку данных + unsigned long deltaMillis = millis() - startMillis; + // Вычисляем частоту обработки фильтра + sampleRate = 1000 / deltaMillis; +} diff --git a/examples/IMU/MadgwickVisualization/Arduino/Madgwick6DOFVisualization/Madgwick6DOFVisualization.ino b/examples/IMU/MadgwickVisualization/Arduino/Madgwick6DOFVisualization/Madgwick6DOFVisualization.ino new file mode 100644 index 0000000..603747a --- /dev/null +++ b/examples/IMU/MadgwickVisualization/Arduino/Madgwick6DOFVisualization/Madgwick6DOFVisualization.ino @@ -0,0 +1,65 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для фильтра Madgwick +Madgwick filter; +// Создаём объект для работы с гироскопом +Gyroscope gyroscope; +// Создаём объект для работы с акселерометром +Accelerometer accelerometer; + +// Переменные для данных с гироскопа и акселерометра и компаса +float gx, gy, gz, ax, ay, az; + +// Переменные для хранения самолётных углов ориентации +float yaw, pitch, roll; + +// Переменная для хранения частоты выборок фильтра +float sampleRate = 100; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Инициализируем гироскоп + gyroscope.begin(); + // Инициализируем акселерометр + accelerometer.begin(); + // Инициализируем фильтр + filter.begin(); +} + +void loop() { + // Запоминаем текущее время + unsigned long startMillis = millis(); + + // Считываем данные с акселерометра в единицах G + accelerometer.readAccelerationGXYZ(ax, ay, az); + // Считываем данные с гироскопа в радианах в секунду + gyroscope.readRotationRadXYZ(gx, gy, gz); + + // Устанавливаем частоту фильтра + filter.setFrequency(sampleRate); + // Обновляем входные данные в фильтр + filter.update(gx, gy, gz, ax, ay, az); + + if (Serial.available() > 0) { + int val = Serial.read(); + // Если пришёл символ 's' + if (val == 's') { + float q0, q1, q2, q3; + filter.readQuaternion(q0, q1, q2, q3); + // Выводим кватернион в serial-порт + Serial.print(q0); + Serial.print(","); + Serial.print(q1); + Serial.print(","); + Serial.print(q2); + Serial.print(","); + Serial.println(q3); + } + } + // Вычисляем затраченное время на обработку данных + unsigned long deltaMillis = millis() - startMillis; + // Вычисляем частоту обработки фильтра + sampleRate = 1000 / deltaMillis; +} diff --git a/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino b/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino new file mode 100644 index 0000000..aacd248 --- /dev/null +++ b/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino @@ -0,0 +1,82 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для фильтра Madgwick +Madgwick filter; +// Создаём объект для работы с гироскопом +Gyroscope gyroscope; +// Создаём объект для работы с акселерометром +Accelerometer accelerometer; +// Создаём объект для работы с магнитометром/компасом +Compass compass; + +// Переменные для данных с гироскопа, акселерометра и компаса +float gx, gy, gz, ax, ay, az, mx, my, mz; + +// Переменные для хранения самолётных углов ориентации +float yaw, pitch, roll; + +// Переменная для хранения частоты фильтра +float sampleRate = 100; + +// Калибровочные данные для работы магнитометра в режиме компаса +// Подробности читайте в документации про калибровку модуля +// http://wiki.amperka.ru/articles:troyka-magnetometer-compass-calibrate +const float compassCalibrationBias[3] = { 567.893, -825.35, 1061.436 }; + +const float compassCalibrationMatrix[3][3] = { { 1.909, 0.082, 0.004 }, + { 0.049, 1.942, -0.235 }, + { -0.003, 0.008, 1.944 } }; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Инициализируем гироскоп + gyroscope.begin(); + // Инициализируем акселерометр + accelerometer.begin(); + // Инициализируем компас + compass.begin(); + // Инициализируем фильтр + filter.begin(); + // Устанавливаем калибровочные данные + compass.setCalibrateMatrix(compassCalibrationMatrix, + compassCalibrationBias); +} + +void loop() { + // Запоминаем текущее время + unsigned long startMillis = millis(); + + // Считываем данные с акселерометра в единицах G + accelerometer.readAccelerationGXYZ(ax, ay, az); + // Считываем данные с гироскопа в радианах в секунду + gyroscope.readRotationRadXYZ(gx, gy, gz); + // Считываем данные с компаса в Гауссах + compass.readCalibrateMagneticGaussXYZ(mx, my, mz); + // Устанавливаем частоту фильтра + filter.setFrequency(sampleRate); + // Обновляем входные данные в фильтр + filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); + + if (Serial.available() > 0) { + int val = Serial.read(); + // Если пришёл символ 's' + if (val == 's') { + float q0, q1, q2, q3; + filter.readQuaternion(q0, q1, q2, q3); + // Выводим кватернион в serial-порт + Serial.print(q0); + Serial.print(","); + Serial.print(q1); + Serial.print(","); + Serial.print(q2); + Serial.print(","); + Serial.println(q3); + } + } + // Вычисляем затраченное время на обработку данных + unsigned long deltaMillis = millis() - startMillis; + // Вычисляем частоту обработки фильтра + sampleRate = 1000 / deltaMillis; +} diff --git a/examples/IMU/MadgwickVisualization/Processing/MadgwickVisualizationDraw/MadgwickVisualizationDraw.pde b/examples/IMU/MadgwickVisualization/Processing/MadgwickVisualizationDraw/MadgwickVisualizationDraw.pde new file mode 100644 index 0000000..b834f74 --- /dev/null +++ b/examples/IMU/MadgwickVisualization/Processing/MadgwickVisualizationDraw/MadgwickVisualizationDraw.pde @@ -0,0 +1,188 @@ +import processing.serial.*; +import toxi.geom.*; +import toxi.processing.*; + +// NOTE: requires ToxicLibs to be installed in order to run properly. +// 1. Download from http://toxiclibs.org/downloads +// 2. Extract into [userdir]/Processing/libraries +// (location may be different on Mac/Linux) +// 3. Run and bask in awesomeness + +// The serial port +Serial port; + +String message; + +float[] q = new float[4]; +Quaternion quat = new Quaternion(1, 0, 0, 0); +// New line character in ASCII +final char newLine = '\n'; +String [] massQ = new String [4]; +float[] ypr = new float[3]; + +void setup() { + // Size form 400x400 + size(400, 400, P3D); + // Open serial port + // Replace "COM7" with the COM port on which your arduino is connected + port = new Serial(this, "COM7", 9600); +} + +void draw() { + // Read and parse incoming serial message + serialEvent(); + // Set background to black + background(0); + printQuaternions(); + printYawPitchRoll(); + // Set position to centre + translate(width / 2, height / 2); + // Begin object + pushMatrix(); + float[] axis = quat.toAxisAngle(); + rotate(axis[0], axis[2], axis[3], axis[1]); + // Draw main body in red + drawBody(); + // Draw front-facing tip in blue + drawCylinder(); + // Draw Triangles + drawTriangles(); + // Draw Quads + drawQuads(); + // End of object + popMatrix(); + // Send character 's' to Arduino + port.write('s'); +} + +void serialEvent() { + // Read from port until new line (ASCII code 13) + message = port.readStringUntil(newLine); + if (message != null) { + // Split message by commas and store in String array + massQ = split(message, ","); + q[0] = float(massQ[0]); + q[1] = float(massQ[1]); + q[2] = float(massQ[2]); + q[3] = float(massQ[3]); + } + // Print values to console + print(q[0]); + print("\t"); + print(q[1]); + print("\t"); + print(q[2]); + print("\t"); + print(q[3]); + println("\t"); + // Set our toxilibs quaternion to new data + quat.set(q[0], q[1], q[2], q[3]); + +} + +void drawCylinder() { + float topRadius = 0; + float bottomRadius = 20; + float tall = 20; + int sides = 8; + // Begin object + pushMatrix(); + translate(0, 0, -120); + rotateX(PI/2); + fill(0, 0, 255, 200); + + float angle = 0; + float angleIncrement = TWO_PI / sides; + beginShape(QUAD_STRIP); + for (int i = 0; i < sides + 1; ++i) { + vertex(topRadius * cos(angle), 0, topRadius * sin(angle)); + vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle)); + angle += angleIncrement; + } + + endShape(); + + // if it is not a cone, draw the circular top cap + if (topRadius != 0) { + angle = 0; + beginShape(TRIANGLE_FAN); + // Center point + vertex(0, 0, 0); + for (int i = 0; i < sides + 1; i++) { + vertex(topRadius * cos(angle), 0, topRadius * sin(angle)); + angle += angleIncrement; + } + endShape(); + } + + // If it is not a cone, draw the circular bottom cap + if (bottomRadius != 0) { + angle = 0; + beginShape(TRIANGLE_FAN); + // Center point + vertex(0, tall, 0); + for (int i = 0; i < sides + 1; i++) { + vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle)); + angle += angleIncrement; + } + endShape(); + } + popMatrix(); +} + +void drawBody() { + fill(255, 0, 0, 200); + box(10, 10, 200); +} + +void drawTriangles() { + // Draw wings and tail fin in green + fill(0, 255, 0, 200); + beginShape(TRIANGLES); + // Wing top layer + vertex(-100, 2, 30); vertex(0, 2, -80); vertex(100, 2, 30); + // Wing bottom layer + vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30); + // Tail left layer + vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70); + // Tail right layer + vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70); + endShape(); +} + +void drawQuads() { + beginShape(QUADS); + vertex(-100, 2, 30); vertex(-100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); + vertex( 100, 2, 30); vertex( 100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); + vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2, 30); vertex(100, 2, 30); + vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, -30, 98); vertex(-2, -30, 98); + vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, 0, 70); vertex(-2, 0, 70); + vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2, 0, 70); vertex(-2, 0, 70); + endShape(); +} + +void printQuaternions() { + // Set text mode to shape + textMode(SHAPE); + textSize(13); + fill(255, 255, 255); + text("Quaternions:", 20, 20, 10); + text(q[0], 20, 40, 10); + text(q[1], 20, 60, 10); + text(q[2], 20, 80, 10); + text(q[3], 20, 100, 10); +} + +void printYawPitchRoll() { + // Calculate yaw/pitch/roll angles + ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1) * 57.2; + ypr[1] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1) * 57.2; + ypr[2] = -atan2(2.0f * (q[0] * q[2] - q[1] * q[3]), 1.0f - 2.0f * (q[2] * q[2] + q[1] *q[1] )) * 57.2; + + text("Yaw:", 150, 20, 10); + text(ypr[0], 150, 40, 10); + text("Pitch:", 220, 20, 10); + text(ypr[1], 220, 40, 10); + text("Roll:", 290, 20, 10); + text(ypr[2], 290, 40, 10); +} diff --git a/examples/Madgwick6DOF/Madgwick6DOF.ino b/examples/Madgwick6DOF/Madgwick6DOF.ino deleted file mode 100755 index 6bd3e4a..0000000 --- a/examples/Madgwick6DOF/Madgwick6DOF.ino +++ /dev/null @@ -1,74 +0,0 @@ -// библиотека для работы с модулями IMU -#include - -// множитель фильтра -#define BETA 0.22f - -// создаём объект для фильтра Madgwick -Madgwick filter; - -// создаём объект для работы с акселерометром -Accelerometer accel(ACCEL_ADDRESS_V1); -// создаём объект для работы с гироскопом -Gyroscope gyro(GYRO_ADDRESS_V1); - -// если напаяны перемычки, устройства доступны по новым адресам -// Accelerometer accel(ACCEL_ADDRESS_V2); -// Gyroscope gyro(GYRO_ADDRESS_V2); - -// переменные для данных с гироскопов, акселерометров -float gx, gy, gz, ax, ay, az; - -// получаемые углы ориентации -float yaw, pitch, roll; - -// переменная для хранения частоты выборок фильтра -float fps = 100; - -void setup() -{ - // открываем последовательный порт - Serial.begin(115200); - Serial.println("Begin init..."); - // инициализация акселерометра - accel.begin(); - // инициализация гироскопа - gyro.begin(); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); -} - -void loop() -{ - // запоминаем текущее время - unsigned long startMillis = millis(); - - // считываем данные с акселерометра в единицах G - accel.readGXYZ(&ax, &ay, &az); - // считываем данные с акселерометра в радианах в секунду - gyro.readRadPerSecXYZ(&gx, &gy, &gz); - // устанавливаем коэффициенты фильтра - filter.setKoeff(fps, BETA); - // обновляем входные данные в фильтр - filter.update(gx, gy, gz, ax, ay, az); - - // получение углов yaw, pitch и roll из фильтра - yaw = filter.getYawDeg(); - pitch = filter.getPitchDeg(); - roll = filter.getRollDeg(); - - // выводим полученные углы в serial-порт - Serial.print("yaw: "); - Serial.print(yaw); - Serial.print("\t\t"); - Serial.print("pitch: "); - Serial.print(pitch); - Serial.print("\t\t"); - Serial.print("roll: "); - Serial.println(roll); - - // вычисляем затраченное время на обработку данных - unsigned long deltaMillis = millis() - startMillis; - // вычисляем частоту обработки фильтра - fps = 1000 / deltaMillis; -} diff --git a/examples/Madgwick9DOF/Madgwick9DOF.ino b/examples/Madgwick9DOF/Madgwick9DOF.ino deleted file mode 100755 index bc28b01..0000000 --- a/examples/Madgwick9DOF/Madgwick9DOF.ino +++ /dev/null @@ -1,101 +0,0 @@ -// библиотека для работы с модулями IMU -#include - -// множитель фильтра -#define BETA 0.22 - -// создаём объект для фильтра Madgwick -Madgwick filter; - -// создаём объект для работы с акселерометром -Accelerometer accel(ACCEL_ADDRESS_V1); -// создаём объект для работы с гироскопом -Gyroscope gyro(GYRO_ADDRESS_V1); -// создаём объект для работы с компасом -Compass compass(COMPASS_ADDRESS_V1); - -// если напаяны перемычки, устройства доступны по новым адресам -// Accelerometer accel(ACCEL_ADDRESS_V2); -// Gyroscope gyro(GYRO_ADDRESS_V2); -// Compass compass(COMPASS_ADDRESS_V2); - - -// переменные для данных с гироскопа, акселерометра и компаса -float gx, gy, gz, ax, ay, az, mx, my, mz; - -// получаемые углы ориентации (Эйлера) -float yaw, pitch, roll; - -// переменная для хранения частоты выборок фильтра -float fps = 100; - -// калибровочные значения компаса -// полученные в калибровочной матрице из примера «compassCalibrateMatrixx» -const double compassCalibrationBias[3] = { - 524.21, - 3352.214, - -1402.236 -}; - -const double compassCalibrationMatrix[3][3] = { - {1.757, 0.04, -0.028}, - {0.008, 1.767, -0.016}, - {-0.018, 0.077, 1.782} -}; - -void setup() -{ - // открываем последовательный порт - Serial.begin(115200); - Serial.println("Begin init..."); - // инициализация акселерометра - accel.begin(); - // инициализация гироскопа - gyro.begin(); - // инициализация компаса - compass.begin(); - - // калибровка компаса - compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); -} - -void loop() -{ - // запоминаем текущее время - unsigned long startMillis = millis(); - - // считываем данные с акселерометра в единицах G - accel.readGXYZ(&ax, &ay, &az); - // считываем данные с гироскопа в радианах в секунду - gyro.readRadPerSecXYZ(&gx, &gy, &gz); - // считываем данные с компаса в Гауссах - compass.readCalibrateGaussXYZ(&mx, &my, &mz); - - // устанавливаем коэффициенты фильтра - filter.setKoeff(fps, BETA); - // обновляем входные данные в фильтр - filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); - - // получение углов yaw, pitch и roll из фильтра - yaw = filter.getYawDeg(); - pitch = filter.getPitchDeg(); - roll = filter.getRollDeg(); - - // выводим полученные углы в serial-порт - Serial.print("yaw: "); - Serial.print(yaw); - Serial.print("\t\t"); - Serial.print("pitch: "); - Serial.print(pitch); - Serial.print("\t\t"); - Serial.print("roll: "); - Serial.println(roll); - - // вычисляем затраченное время на обработку данных - unsigned long deltaMillis = millis() - startMillis; - // вычисляем частоту обработки фильтра - fps = 1000 / deltaMillis; -} - diff --git a/examples/MadgwickProcessing9DOF/MadgwickProcessing9DOF.ino b/examples/MadgwickProcessing9DOF/MadgwickProcessing9DOF.ino deleted file mode 100755 index 917ba60..0000000 --- a/examples/MadgwickProcessing9DOF/MadgwickProcessing9DOF.ino +++ /dev/null @@ -1,98 +0,0 @@ -// скетч для вывода кватернионов в serial-порт -// для дальнейшего графического просмотра ориентации объекта -// в среде processing «MadgwickProcessingDraw9DOF.pde» - -// библиотека для работы с модулями IMU -#include - -// множитель фильтра -#define BETA 0.22 - -// создаём объект для фильтра Madgwick -Madgwick filter; - -// создаём объект для работы с акселерометром -Accelerometer accel(ACCEL_ADDRESS_V1); -// создаём объект для работы с гироскопом -Gyroscope gyro(GYRO_ADDRESS_V1); -// создаём объект для работы с компасом -Compass compass(COMPASS_ADDRESS_V1); - -// если напаяны перемычки, устройства доступны по новым адресам -// Accelerometer accel(ACCEL_ADDRESS_V2); -// Gyroscope gyro(GYRO_ADDRESS_V2); -// Compass compass(COMPASS_ADDRESS_V2); - -// переменные для данных с гироскопа, акселерометра и компаса -float gx, gy, gz, ax, ay, az, mx, my, mz; - -// получаемые углы ориентации (Эйлера) -float yaw, pitch, roll; - -// переменная для хранения частоты выборок фильтра -float fps = 100; - -// калибровочные значения компаса -// полученные в калибровочной матрице из примера «compassCalibrateMatrixx» -const double compassCalibrationBias[3] = { - 524.21, - 3352.214, - -1402.236 -}; - -const double compassCalibrationMatrix[3][3] = { - {1.757, 0.04, -0.028}, - {0.008, 1.767, -0.016}, - {-0.018, 0.077, 1.782} -}; - -void setup() -{ - // открываем последовательный порт - Serial.begin(115200); - // инициализация акселерометра - accel.begin(); - // инициализация гироскопа - gyro.begin(); - // инициализация компаса - compass.begin(); - // калибровка компаса - compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); -} - -void loop() -{ - // запоминаем текущее время - unsigned long startMillis = millis(); - // считываем данные с акселерометра в единицах G - accel.readGXYZ(&ax, &ay, &az); - // считываем данные с гироскопа в радианах в секунду - gyro.readRadPerSecXYZ(&gx, &gy, &gz); - // считываем данные с компаса в Гауссах - compass.readCalibrateGaussXYZ(&mx, &my, &mz); - // устанавливаем коэффициенты фильтра - filter.setKoeff(fps, BETA); - // обновляем входные данные в фильтр - filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); - - if (Serial.available() > 0) { - int val = Serial.read(); - // если пришёл символ 's' - if (val == 's') { - float q0, q1, q2, q3; - filter.readQuaternions(&q0, &q1, &q2, &q3); - // выводим кватернионы в serial-порт - Serial.print(q0); - Serial.print(","); - Serial.print(q1); - Serial.print(","); - Serial.print(q2); - Serial.print(","); - Serial.println(q3); - } - } - // вычисляем затраченное время на обработку данных - unsigned long deltaMillis = millis() - startMillis; - // вычисляем частоту обработки фильтра - fps = 1000 / deltaMillis; -} diff --git a/examples/MadgwickProcessing9DOF/MadgwickProcessingDraw9DOF/MadgwickProcessingDraw9DOF.pde b/examples/MadgwickProcessing9DOF/MadgwickProcessingDraw9DOF/MadgwickProcessingDraw9DOF.pde deleted file mode 100755 index ada68a7..0000000 --- a/examples/MadgwickProcessing9DOF/MadgwickProcessingDraw9DOF/MadgwickProcessingDraw9DOF.pde +++ /dev/null @@ -1,190 +0,0 @@ -import processing.serial.*; -import toxi.geom.*; -import toxi.processing.*; - -// NOTE: requires ToxicLibs to be installed in order to run properly. -// 1. Download from http://toxiclibs.org/downloads -// 2. Extract into [userdir]/Processing/libraries -// (location may be different on Mac/Linux) -// 3. Run and bask in awesomeness - -// The serial port -Serial port; - -String message; - -float[] q = new float[4]; -Quaternion quat = new Quaternion(1, 0, 0, 0); -// new line character in ASCII -int newLine = 13; -String [] massQ = new String [4]; -float[] ypr = new float[3]; - -void setup() -{ - // size form 300x300 - size(400, 400, P3D); - // open serial port - // replace "COM29" with the COM port on which your arduino is connected - port = new Serial(this, "COM61", 115200); -} - -void draw() -{ - // read and parse incoming serial message - serialEvent(); - // set background to black - background(0); - printQuaternions(); - printYawPitchRoll(); - // set position to centre - translate(width / 2, height / 2); - // begin object - pushMatrix(); - float[] axis = quat.toAxisAngle(); - rotate(axis[0], axis[2], axis[3], axis[1]); - // draw main body in red - drawBody(); - // draw front-facing tip in blue - drawCylinder(); - // draw Triangles - drawTriangles(); - // draw Quards - drawQuards(); - // end of object - popMatrix(); - // send 's' to Arduino - port.write('s'); -} - -void serialEvent() { - // read from port until new line (ASCII code 13) - message = port.readStringUntil(newLine); - if (message != null) { - // split message by commas and store in String array - massQ = split(message, ","); - q[0] = float(massQ[0]); - q[1] = float(massQ[1]); - q[2] = float(massQ[2]); - q[3] = float(massQ[3]); - } - // print values to console - print(q[0]); - print("\t"); - print(q[1]); - print("\t"); - print(q[2]); - print("\t"); - print(q[3]); - println("\t"); - // set our toxilibs quaternion to new data - quat.set(q[0], q[1], q[2], q[3]); - -} - -void drawCylinder() { - float topRadius = 0; - float bottomRadius = 20; - float tall = 20; - int sides = 8; - // begin object - pushMatrix(); - translate(0, 0, -120); - rotateX(PI/2); - fill(0, 0, 255, 200); - - float angle = 0; - float angleIncrement = TWO_PI / sides; - beginShape(QUAD_STRIP); - for (int i = 0; i < sides + 1; ++i) { - vertex(topRadius*cos(angle), 0, topRadius*sin(angle)); - vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle)); - angle += angleIncrement; - } - endShape(); - - // if it is not a cone, draw the circular top cap - if (topRadius != 0) { - angle = 0; - beginShape(TRIANGLE_FAN); - - // center point - vertex(0, 0, 0); - for (int i = 0; i < sides + 1; i++) { - vertex(topRadius * cos(angle), 0, topRadius * sin(angle)); - angle += angleIncrement; - } - endShape(); - } - - // if it is not a cone, draw the circular bottom cap - if (bottomRadius != 0) { - angle = 0; - beginShape(TRIANGLE_FAN); - - // center point - vertex(0, tall, 0); - for (int i = 0; i < sides + 1; i++) { - vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle)); - angle += angleIncrement; - } - endShape(); - } - popMatrix(); -} - -void drawBody() { - fill(255, 0, 0, 200); - box(10, 10, 200); -} - -void drawTriangles() { - // draw wings and tail fin in green - fill(0, 255, 0, 200); - beginShape(TRIANGLES); - // wing top layer - vertex(-100, 2, 30); vertex(0, 2, -80); vertex(100, 2, 30); - // wing bottom layer - vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30); - // tail left layer - vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70); - // tail right layer - vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70); - endShape(); -} - -void drawQuards() { - beginShape(QUADS); - vertex(-100, 2, 30); vertex(-100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); - vertex( 100, 2, 30); vertex( 100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); - vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2, 30); vertex(100, 2, 30); - vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, -30, 98); vertex(-2, -30, 98); - vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, 0, 70); vertex(-2, 0, 70); - vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2, 0, 70); vertex(-2, 0, 70); - endShape(); -} - -void printQuaternions() { - textMode(SHAPE); // set text mode to shape - textSize(13); - fill(255, 255, 255); - text("Quaternions:", 20, 20, 10); - text(q[0], 20, 40, 10); - text(q[1], 20, 60, 10); - text(q[2], 20, 80, 10); - text(q[3], 20, 100, 10); -} - -void printYawPitchRoll() { - // calculate yaw/pitch/roll angles - ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1) * 57.2; - ypr[1] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1) * 57.2; - ypr[2] = -atan2(2.0f * (q[0] * q[2] - q[1] * q[3]), 1.0f - 2.0f * (q[2] * q[2] + q[1] *q[1] )) * 57.2; - - text("Yaw:", 150, 20, 10); - text(ypr[0], 150, 40, 10); - text("Pitch:", 220, 20, 10); - text(ypr[1], 220, 40, 10); - text("Roll:", 290, 20, 10); - text(ypr[2], 290, 40, 10); -} \ No newline at end of file diff --git a/examples/accel/accel.ino b/examples/accel/accel.ino deleted file mode 100755 index fad5523..0000000 --- a/examples/accel/accel.ino +++ /dev/null @@ -1,50 +0,0 @@ -// библиотека для работы с модулями IMU -#include - -// создаём объект для работы с акселерометром -Accelerometer accel(ACCEL_ADDRESS_V1); - -// если напаяна перемычка, устройство доступно по новому адресу -// Accelerometer accel(ACCEL_ADDRESS_V2); - -void setup() -{ - // открываем последовательный порт - Serial.begin(115200); - // выводим сообщение о начале инициализации - Serial.println("Accelerometer init..."); - // инициализация акселерометра - accel.begin(); - // устанавливаем чувствительность акселерометра - // 2g — по умолчанию, 4g, 8g - accel.setRange(RANGE_2G); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); -} - -void loop() -{ - // вывод направления и величины ускорения в м/с² по оси X - Serial.print(accel.readAX()); - Serial.print("\t\t"); - // вывод направления и величины ускорения в м/с² по оси Y - Serial.print(accel.readAY()); - Serial.print("\t\t"); - // вывод направления и величины ускорения в м/с² по оси Z - Serial.print(accel.readAZ()); - Serial.print("\t\t"); - Serial.println(""); - delay(100); - /* - // вывод направления и величины ускорения в м/с² по X, Y и Z - float x, y, z; - accel.readAXYZ(&x, &y, &z); - Serial.print(x); - Serial.print("\t\t"); - Serial.print(y); - Serial.print("\t\t"); - Serial.print(z); - Serial.println(""); - delay(100); - */ -} diff --git a/examples/barometer/barometer.ino b/examples/barometer/barometer.ino deleted file mode 100755 index c2424cc..0000000 --- a/examples/barometer/barometer.ino +++ /dev/null @@ -1,41 +0,0 @@ -// библиотека для работы с модулями IMU -#include -// создаём объект для работы с барометром -Barometer barometer(BARO_ADDRESS_V1); - -// если напаяны перемычки, устройство доступно по новому дресу -// Barometer barometer(BARO_ADDRESS_V2); - -void setup() -{ - // открываем последовательный порт - Serial.begin(115200); - // выводим сообщение о начале инициализации - Serial.println("Begin init..."); - // инициализация барометра - barometer.begin(); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); -} - -void loop() -{ - // создаём переменную и присваиваем ей значения абсолютного давления - float pressure = barometer.readPressureMillibars(); - // создаём переменную и присваиваем ей значения высоты над уровнем море - float altitude = barometer.pressureToAltitudeMeters(pressure); - // создаём переменную и присваиваем ей температуру окружающей среды - float temperature = barometer.readTemperatureC(); - - // Вывод данных в Serial порт - Serial.print("p: "); - Serial.print(pressure); - Serial.print(" mbar \t"); - Serial.print("h: "); - Serial.print(altitude); - Serial.print(" m \t"); - Serial.print("t: "); - Serial.print(temperature); - Serial.println(" C"); - delay(100); -} diff --git a/examples/compass/compass/compass.ino b/examples/compass/compass/compass.ino deleted file mode 100755 index d2c8226..0000000 --- a/examples/compass/compass/compass.ino +++ /dev/null @@ -1,48 +0,0 @@ -// библиотека для работы с модулями IMU -#include - -// создаём объект для работы с компасом -Compass compass(COMPASS_ADDRESS_V1); - -// если напаяна перемычка, устройство доступно по новому адресу -// Compass compass(COMPASS_ADDRESS_V2); - -// калибровочные значения, полученные в калибровочной матрице -// из примера compass_cal_matrix -const double compassCalibrationBias[3] = { - 524.21, - 3352.214, - -1402.236 -}; - -const double compassCalibrationMatrix[3][3] = { - {1.757, 0.04, -0.028}, - {0.008, 1.767, -0.016}, - {-0.018, 0.077, 1.782} -}; - -void setup() -{ - // открываем последовательный порт - Serial.begin(115200); - // пока не появились данные с USB - // выводим сообщение о начале инициализации - Serial.println("Begin init..."); - // инициализация компаса - compass.begin(); - // устанавливаем чувствительность компаса - // ±4 gauss — по умолчанию, ±8 gauss, ±12 gauss, ±16 gauss - compass.setRange(RANGE_4_GAUSS); - // калибровка компаса - compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); -} - -void loop() -{ - // выводим азимут относительно оси Z - Serial.print(compass.readAzimut()); - Serial.println(" Degrees"); - delay(100); -} diff --git a/examples/compass/compassCalibrateMatrix/compassCalibrateMatrix.ino b/examples/compass/compassCalibrateMatrix/compassCalibrateMatrix.ino deleted file mode 100755 index 9ed237d..0000000 --- a/examples/compass/compassCalibrateMatrix/compassCalibrateMatrix.ino +++ /dev/null @@ -1,31 +0,0 @@ - -// библиотека для работы с модулями IMU -#include - -// создаём объект для работы с компасом -Compass compass(COMPASS_ADDRESS_V1); - -// если напаяна перемычка, устройство доступно по новому адресу -// Compass compass(COMPASS_ADDRESS_V2); - -void setup() -{ - // открываем последовательный порт - Serial.begin(9600); - // инициализация компаса - compass.begin(); - // устанавливаем чувствительность компаса - // ±4 gauss — по умолчанию, ±8 gauss, ±12 gauss, ±16 gauss - compass.setRange(RANGE_4_GAUSS); -} - -void loop() -{ - // Выводим «сырые» значения компаса по трём осям - Serial.print(compass.readX()); - Serial.print(","); - Serial.print(compass.readY()); - Serial.print(","); - Serial.println(compass.readZ()); - delay(100); -} diff --git a/examples/gyro/gyro.ino b/examples/gyro/gyro.ino deleted file mode 100755 index 6eda753..0000000 --- a/examples/gyro/gyro.ino +++ /dev/null @@ -1,53 +0,0 @@ -// библиотека для работы I²C -#include -// библиотека для работы с модулями IMU -#include - -// создаём объект для работы с гироскопом -Gyroscope gyro(GYRO_ADDRESS_V1); - -// если напаяна перемычка, устройство доступно по новому адресу -// Gyroscope gyro(GYRO_ADDRESS_V2); - - -void setup() -{ - // открываем последовательный порт - Serial.begin(115200); - // выводим сообщение о начале инициализации - Serial.println("Gyroscope init..."); - // инициализация гироскопа - gyro.begin(); - // устанавливаем чувствительность гироскопа - // 250dps — по умолчанию, 500dps, 2000dps - gyro.setRange(RANGE_250DPS); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); -} - -void loop() -{ - // вывод угловой скорости в градусах в секунду относительно оси X - Serial.print(gyro.readDegPerSecX()); - Serial.print("\t\t"); - // вывод угловой скорости в градусах в секунду относительно оси Y - Serial.print(gyro.readDegPerSecY()); - Serial.print("\t\t"); - // вывод угловой скорости в градусах в секунду относительно оси Z - Serial.print(gyro.readDegPerSecZ()); - Serial.print("\t\t"); - Serial.println(""); - delay(100); -/* - // вывод угловой скорости в градусах в секунду относительно оси X, Y и Z - float x, y, z; - gyro.readDegPerSecXYZ(&x, &y, &z); - Serial.print(x); - Serial.print("\t\t"); - Serial.print(y); - Serial.print("\t\t"); - Serial.print(z); - Serial.println(""); - delay(100); -*/ -} diff --git a/examples/imu/imu.ino b/examples/imu/imu.ino deleted file mode 100755 index 999e3d8..0000000 --- a/examples/imu/imu.ino +++ /dev/null @@ -1,85 +0,0 @@ -// библиотека для работы с модулями IMU -#include - -// создаём объект для работы с гироскопом -Gyroscope gyro(GYRO_ADDRESS_V1); -// создаём объект для работы с акселерометром -Accelerometer accel(ACCEL_ADDRESS_V1); -// создаём объект для работы с компасом -Compass compass(COMPASS_ADDRESS_V1); -// создаём объект для работы с барометром -Barometer barometer(BARO_ADDRESS_V1); - -// если напаяны перемычки, устройства доступны по новому адресу -// Gyroscope gyro(GYRO_ADDRESS_V2); -// Accelerometer accel(ACCEL_ADDRESS_V2); -// Compass compass(COMPASS_ADDRESS_V2); -// Barometer barometer(BARO_ADDRESS_V2); - -// калибровочные значения компаса -// полученные в калибровочной матрице из примера «compassCalibrateMatrix» -const double compassCalibrationBias[3] = { - 524.21, - 3352.214, - -1402.236 -}; - -const double compassCalibrationMatrix[3][3] = { - {1.757, 0.04, -0.028}, - {0.008, 1.767, -0.016}, - {-0.018, 0.077, 1.782} -}; - -void setup() -{ - // открываем последовательный порт - Serial.begin(115200); - // выводим сообщение о начале инициализации - Serial.println("Begin init..."); - // инициализация гироскопа - gyro.begin(); - // инициализация акселерометра - accel.begin(); - // инициализация компаса - compass.begin(); - // инициализация барометра - barometer.begin(); - // калибровка компаса - compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); - Serial.println("Gyroscope\t\t\tAccelerometer\t\t\tCompass\t\tBarometer"); -} - -void loop() -{ - // вывод угловой скорости в градусах в секунду относительно оси X - Serial.print(gyro.readDegPerSecX()); - Serial.print("\t"); - // вывод угловой скорости в градусах в секунду относительно оси Y - Serial.print(gyro.readDegPerSecY()); - Serial.print("\t"); - // вывод угловой скорости в градусах в секунду относительно оси Z - Serial.print(gyro.readDegPerSecZ()); - Serial.print("\t\t"); - // вывод направления и величины ускорения в м/с² по оси X - Serial.print(accel.readAX()); - Serial.print("\t"); - // вывод направления и величины ускорения в м/с² по оси Y - Serial.print(accel.readAY()); - Serial.print("\t"); - // вывод направления и величины ускорения в м/с² по оси Z - Serial.print(accel.readAZ()); - Serial.print("\t\t"); - // выводим азимут относительно оси Z - Serial.print(compass.readAzimut()); - Serial.print(" Degrees\t"); - // вывод значения абсолютного давления - Serial.print(barometer.readPressureMillibars()); - Serial.print("\t"); - // вывод значения температуры окружающей среды - Serial.print(barometer.readTemperatureC()); - Serial.print("\t"); - Serial.println(""); - delay(100); -} diff --git a/l3g4200d.cpp b/l3g4200d.cpp deleted file mode 100755 index e10bad0..0000000 --- a/l3g4200d.cpp +++ /dev/null @@ -1,102 +0,0 @@ -#include -#include -#include "l3g4200d.h" - -#define ADR_FS_250 0x00 -#define ADR_FS_500 0x10 -#define ADR_FS_2000 0x20 - -#define SENS_FS_250 0.00875 -#define SENS_FS_500 0.0175 -#define SENS_FS_2000 0.07 - -L3G4200D_TWI::L3G4200D_TWI(uint8_t addr) : AxisHw(addr) { -} - -void L3G4200D_TWI::setRange(uint8_t range) { - switch (range) { - case RANGE_250DPS: { - _ctrlReg4 = ADR_FS_250; - _mult = SENS_FS_250; - break; - } - case RANGE_500DPS: { - _ctrlReg4 = ADR_FS_500; - _mult = SENS_FS_500; - break; - } - case RANGE_2000DPS: { - _ctrlReg4 = ADR_FS_2000; - _mult = SENS_FS_2000; - break; - } - default: { - _mult = SENS_FS_250; - } - break; - } - writeCtrlReg4(); -} - -void L3G4200D_TWI::begin() { - // подключаемся к шине I²C - WIRE_IMU.begin(); - // включаем координаты x, y, z - _ctrlReg1 |= (1 << 0); - _ctrlReg1 |= (1 << 1); - _ctrlReg1 |= (1 << 2); - // включаем гироскоп - _ctrlReg1 |= (1 << 3); - // устанавливаем чувствительность - setRange(RANGE_250DPS); - writeCtrlReg1(); -} - - -void L3G4200D_TWI::sleep(bool enable) { - if (enable) - _ctrlReg1 &= ~(1 << 3); - else - _ctrlReg1 |= (1 << 3); - - writeCtrlReg1(); -} - -float L3G4200D_TWI::readDegPerSecX() { - return readX() * _mult; -} - -float L3G4200D_TWI::readDegPerSecY() { - return readY() * _mult; -} - -float L3G4200D_TWI::readDegPerSecZ() { - return readZ() * _mult; -} - -float L3G4200D_TWI::readRadPerSecX() { - return readDegPerSecX() * DEG_TO_RAD; -} - -float L3G4200D_TWI::readRadPerSecY() { - return readDegPerSecY() * DEG_TO_RAD; -} - -float L3G4200D_TWI::readRadPerSecZ() { - return readDegPerSecZ() * DEG_TO_RAD; -} - -void L3G4200D_TWI::readDegPerSecXYZ(float *gx, float *gy, float *gz) { - int16_t x, y, z; - readXYZ(&x, &y, &z); - *gx = x * _mult; - *gy = y * _mult; - *gz = z * _mult; -} - -void L3G4200D_TWI::readRadPerSecXYZ(float *gx, float *gy, float *gz) { - readDegPerSecXYZ(gx, gy, gz); - (*gx) *= DEG_TO_RAD; - (*gy) *= DEG_TO_RAD; - (*gz) *= DEG_TO_RAD; -} diff --git a/l3g4200d.h b/l3g4200d.h deleted file mode 100755 index 0b53b82..0000000 --- a/l3g4200d.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef L3G4200D_H -#define L3G4200D_H - -#include "stmhw.h" - -#define L3G4200D_TWI_ADDRESS 0b01101000 -#define L3G4200D_TWI_ADDRESS_V2 0b01101001 - -#define RANGE_250DPS 0 -#define RANGE_500DPS 1 -#define RANGE_2000DPS 2 - -class L3G4200D_TWI : public AxisHw -{ -public: - L3G4200D_TWI(uint8_t addr = L3G4200D_TWI_ADDRESS); - void begin(); - void sleep(bool enable); - void setRange(uint8_t range); - float readDegPerSecX(); - float readDegPerSecY(); - float readDegPerSecZ(); - float readRadPerSecX(); - float readRadPerSecY(); - float readRadPerSecZ(); - void readDegPerSecXYZ(float *gx, float *gy, float *gz); - void readRadPerSecXYZ(float *gx, float *gy, float *gz); -}; - -#endif diff --git a/library.properties b/library.properties new file mode 100644 index 0000000..4bed445 --- /dev/null +++ b/library.properties @@ -0,0 +1,9 @@ +name=Troyka-IMU +version=2.0.0 +author=Igor Dementiev +maintainer=Amperka +sentence=Arduino library for Amperka IMU-sensor. +paragraph=Allows you to read the accelerometer, magnetometer, gyroscope and barometer values from the Amperka IMU 10-DOF Sensor. +category=Sensors +url=https://github.com/amperka/Troyka-IMU +architectures=* diff --git a/lis331dlh.cpp b/lis331dlh.cpp deleted file mode 100755 index d3e5139..0000000 --- a/lis331dlh.cpp +++ /dev/null @@ -1,98 +0,0 @@ -#include -#include "lis331dlh.h" - -#define ADR_FS_2 0x00 -#define ADR_FS_4 0x10 -#define ADR_FS_8 0x30 - -#define G 9.8 - -LIS331DLH_TWI::LIS331DLH_TWI(uint8_t addr) : AxisHw(addr) { -} - -void LIS331DLH_TWI::begin() { - // подключаемся к шине I²C - WIRE_IMU.begin(); - // включаем координаты x, y, z - _ctrlReg1 |= (1 << 0); - _ctrlReg1 |= (1 << 1); - _ctrlReg1 |= (1 << 2); - // включаем аксселерометр - _ctrlReg1 |= (1 << 5); - // устанавливаем максимальное измеряемое ускорение в G - setRange(RANGE_2G); - writeCtrlReg1(); -} - -void LIS331DLH_TWI::setRange(uint8_t range) { - switch (range) { - case RANGE_2G: { - _ctrlReg4 = ADR_FS_2; - _mult = RANGE_2G / 32767.0; - break; - } - case RANGE_4G: { - _ctrlReg4 = ADR_FS_4; - _mult = RANGE_4G / 32767.0; - break; - } - case RANGE_8G: { - _ctrlReg4 = ADR_FS_8; - _mult = RANGE_8G / 32767.0; - break; - } - default: { - _mult = RANGE_2G / 32767.0; - } - break; - } - writeCtrlReg4(); -} - -void LIS331DLH_TWI::sleep(bool enable) { - if (enable) - _ctrlReg1 &= ~(1 << 5); - else - _ctrlReg1 |= (1 << 5); - - writeCtrlReg1(); -} - -float LIS331DLH_TWI::readGX() { - return readX()*_mult; -} - -float LIS331DLH_TWI::readGY() { - return readY()*_mult; -} - -float LIS331DLH_TWI::readGZ() { - return readZ()*_mult; -} - -float LIS331DLH_TWI::readAX() { - return readGX() * G; -} - -float LIS331DLH_TWI::readAY() { - return readGY() * G; -} - -float LIS331DLH_TWI::readAZ() { - return readGZ() * G; -} - -void LIS331DLH_TWI::readGXYZ(float *gx, float *gy, float *gz) { - int16_t x, y, z; - readXYZ(&x, &y, &z); - *gx = x * _mult; - *gy = y * _mult; - *gz = z * _mult; -} - -void LIS331DLH_TWI::readAXYZ(float *ax, float *ay, float *az) { - readGXYZ(ax, ay, az); - (*ax) *= G; - (*ay) *= G; - (*az) *= G; -} diff --git a/lis331dlh.h b/lis331dlh.h deleted file mode 100755 index 22ddbbb..0000000 --- a/lis331dlh.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef LIS331DLH_H -#define LIS331DLH_H - -#include "stmhw.h" - -#define LIS331DLH_TWI_ADDRESS 0b0011000 -#define LIS331DLH_TWI_ADDRESS_V2 0b0011001 - -#define RANGE_2G 2 -#define RANGE_4G 4 -#define RANGE_8G 8 - -class LIS331DLH_TWI : public AxisHw -{ -public: - LIS331DLH_TWI(uint8_t addr = LIS331DLH_TWI_ADDRESS); - void begin(); - void sleep(bool enable); - void setRange(uint8_t range); - float readGX(); - float readGY(); - float readGZ(); - float readAX(); - float readAY(); - float readAZ(); - void readGXYZ(float *ax, float *ay, float *az); - void readAXYZ(float *gx, float *gy, float *gz); -}; - -#endif diff --git a/lis3mdl.cpp b/lis3mdl.cpp deleted file mode 100755 index 6fdb1c9..0000000 --- a/lis3mdl.cpp +++ /dev/null @@ -1,143 +0,0 @@ -#include -#include -#include "lis3mdl.h" - -#define ADR_FS_4 0x00 -#define ADR_FS_8 0x20 -#define ADR_FS_12 0x40 -#define ADR_FS_16 0x60 - -#define SENS_FS_4 6842 -#define SENS_FS_8 3421 -#define SENS_FS_12 2281 -#define SENS_FS_16 1711 - -LIS3MDL_TWI::LIS3MDL_TWI(uint8_t addr) : AxisHw(addr) { -} - -void LIS3MDL_TWI::begin() { - WIRE_IMU.begin(); - // устанавливаем чувствительность - setRange(RANGE_4_GAUSS); - writeCtrlReg3(); -} - -void LIS3MDL_TWI::setRange(uint8_t range) { - switch (range) { - case RANGE_4_GAUSS: { - _ctrlReg2 = ADR_FS_4; - _mult = SENS_FS_4; - break; - } - case RANGE_8_GAUSS: { - _ctrlReg2 = ADR_FS_8; - _mult = SENS_FS_8; - break; - } - case RANGE_12_GAUSS: { - _ctrlReg2 = ADR_FS_12; - _mult = SENS_FS_12; - break; - } - case RANGE_16_GAUSS: { - _ctrlReg2 = ADR_FS_16; - _mult = SENS_FS_16; - break; - } - default: { - _mult = SENS_FS_4; - } - break; - } - writeCtrlReg2(); -} - -void LIS3MDL_TWI::sleep(bool enable) { - if (enable) - _ctrlReg3 |= (3 << 0); - else - _ctrlReg3 &= ~(3 << 0); - - writeCtrlReg3(); -} - -float LIS3MDL_TWI::readGaussX() { - return readX() / _mult; -} - -float LIS3MDL_TWI::readGaussY() { - return readY() / _mult; -} - -float LIS3MDL_TWI::readGaussZ() { - return readZ() / _mult; -} - -float LIS3MDL_TWI::readCalibrateX() { - calibrate(); - return _xCalibrate; -} - -float LIS3MDL_TWI::readCalibrateY() { - calibrate(); - return _yCalibrate; -} - -float LIS3MDL_TWI::readCalibrateZ() { - calibrate(); - return _zCalibrate; -} - -float LIS3MDL_TWI::readCalibrateGaussX() { - return readCalibrateX()/_mult; -} - -float LIS3MDL_TWI::readCalibrateGaussY() { - return readCalibrateY()/_mult; -} - -float LIS3MDL_TWI::readCalibrateGaussZ() { - return readCalibrateZ()/_mult; -} - -void LIS3MDL_TWI::calibrate() { - float result[3] = {0, 0, 0}; - float uncalibratedValues[3]; - uncalibratedValues[0] = readX() - _bias[0]; - uncalibratedValues[1] = readY() - _bias[1]; - uncalibratedValues[2] = readZ() - _bias[2]; - - for (int i = 0; i < 3; ++i) { - for (int j = 0; j < 3; ++j) { - result[i] += _calibrationMatrix[i][j] * uncalibratedValues[j]; - } - } - - _xCalibrate = result[0]; - _yCalibrate = result[1]; - _zCalibrate = result[2]; -} - -void LIS3MDL_TWI::calibrateMatrix(const double calibrationMatrix[3][3], const double bias[3]) { - memcpy (_bias, bias, 3 * sizeof (double)); - memcpy (_calibrationMatrix, calibrationMatrix, 3 * 3 * sizeof (double)); -} - -void LIS3MDL_TWI::readCalibrateGaussXYZ(float *x, float *y, float *z) { - calibrate(); - *x = _xCalibrate / _mult; - *y = _yCalibrate / _mult; - *z = _zCalibrate / _mult; -} - -float LIS3MDL_TWI::readAzimut() { - calibrate(); - float heading = atan2(_yCalibrate, _xCalibrate); - - if(heading < 0) - heading += TWO_PI; - else if(heading > TWO_PI) - heading -= TWO_PI; - float headingDegrees = heading * RAD_TO_DEG; - return headingDegrees; -} diff --git a/lis3mdl.h b/lis3mdl.h deleted file mode 100755 index 80abeb3..0000000 --- a/lis3mdl.h +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef LIS3MDL_H -#define LIS3MDL_H - -#include "stmhw.h" - -#define LIS3MDL_TWI_ADDRESS 0b0011100 -#define LIS3MDL_TWI_ADDRESS_V2 0b0011110 - -#define RANGE_4_GAUSS 0 -#define RANGE_8_GAUSS 1 -#define RANGE_12_GAUSS 2 -#define RANGE_16_GAUSS 3 - -class LIS3MDL_TWI : public AxisHw -{ -public: - LIS3MDL_TWI(uint8_t addr = LIS3MDL_TWI_ADDRESS); - void begin(); - void sleep(bool enable); - void setRange(uint8_t range); - void calibrateMatrix(const double calibrationMatrix[3][3], const double bias[3]); - void calibrate(); - float readGaussX(); - float readGaussY(); - float readGaussZ(); - float readCalibrateX(); - float readCalibrateY(); - float readCalibrateZ(); - float readCalibrateGaussX(); - float readCalibrateGaussY(); - float readCalibrateGaussZ(); - float readAzimut(); - void readCalibrateGaussXYZ(float *x, float *y, float *z); - -private: - float _xCalibrate; - float _yCalibrate; - float _zCalibrate; - double _calibrationMatrix[3][3]; - double _bias[3]; -}; - -#endif diff --git a/src/Accelerometer.cpp b/src/Accelerometer.cpp new file mode 100644 index 0000000..8a70046 --- /dev/null +++ b/src/Accelerometer.cpp @@ -0,0 +1,84 @@ +#include "Accelerometer.h" + +Accelerometer::Accelerometer(uint8_t slaveAddress) + : BaseIMU(slaveAddress) { } + +void Accelerometer::begin(TwoWire& wire) { + _wire = &wire; + _wire->begin(); + _scalingFactor = 1; + uint8_t data = LIS331DLH_CTRL_REG1_X_EN | LIS331DLH_CTRL_REG1_Y_EN + | LIS331DLH_CTRL_REG1_Z_EN; + data |= LIS331DLH_CTRL_REG1_PM0; + _writeByte(BASE_IMU_CTRL_REG1, data); + setRange(AccelerometerRange::RANGE_2G); +} + +// Set range scale output data from datasheet +void Accelerometer::setRange(AccelerometerRange range) { + uint8_t data = _readByte(BASE_IMU_CTRL_REG4); + data &= ~(LIS331DLH_CTRL_REG4_FS0 | LIS331DLH_CTRL_REG4_FS1); + switch (range) { + case AccelerometerRange::RANGE_2G: { + _scalingFactor = SENS_2G; + break; + } + case AccelerometerRange::RANGE_4G: { + data |= LIS331DLH_CTRL_REG4_FS0; + _scalingFactor = SENS_4G; + break; + } + case AccelerometerRange::RANGE_8G: { + data |= LIS331DLH_CTRL_REG4_FS0 | LIS331DLH_CTRL_REG4_FS1; + _scalingFactor = SENS_8G; + break; + } + default: { + _scalingFactor = SENS_2G * 4 / pow(2, 16); + } break; + } + _writeByte(BASE_IMU_CTRL_REG4, data); +} + +void Accelerometer::sleep(bool state) { + uint8_t data = _readByte(BASE_IMU_CTRL_REG1); + if (state) + data &= ~LIS331DLH_CTRL_REG1_PM0; + else + data |= LIS331DLH_CTRL_REG1_PM0; + + _writeByte(BASE_IMU_CTRL_REG1, data); +} + +float Accelerometer::readAccelerationGX() { return readX() * _scalingFactor; } + +float Accelerometer::readAccelerationGY() { return readY() * _scalingFactor; } + +float Accelerometer::readAccelerationGZ() { return readZ() * _scalingFactor; } + +float Accelerometer::readAccelerationAX() { + return readAccelerationGX() * GRAVITY_EARTH; +} + +float Accelerometer::readAccelerationAY() { + return readAccelerationGY() * GRAVITY_EARTH; +} + +float Accelerometer::readAccelerationAZ() { + return readAccelerationGZ() * GRAVITY_EARTH; +} + +void Accelerometer::readAccelerationGXYZ(float& ax, float& ay, float& az) { + int16_t x, y, z; + readXYZ(x, y, z); + ax = x * _scalingFactor; + ay = y * _scalingFactor; + az = z * _scalingFactor; +} + +void Accelerometer::readAccelerationAXYZ(float& ax, float& ay, float& az) { + readAccelerationGXYZ(ax, ay, az); + ax *= GRAVITY_EARTH; + ay *= GRAVITY_EARTH; + az *= GRAVITY_EARTH; +} diff --git a/src/Accelerometer.h b/src/Accelerometer.h new file mode 100644 index 0000000..b08b1c9 --- /dev/null +++ b/src/Accelerometer.h @@ -0,0 +1,73 @@ +#ifndef __ACCELEROMETER_H__ +#define __ACCELEROMETER_H__ + +#include "BaseIMU.h" + +// I²C-address device +constexpr uint8_t LIS331DLH_SLAVE_ADDRESS = 0x18; +constexpr uint8_t LIS331DLH_SLAVE_ADDRESS_ALT = 0x19; + +// Registers addrress +constexpr uint8_t LIS331DLH_CTRL_REG4_FS0 = 0x10; +constexpr uint8_t LIS331DLH_CTRL_REG4_FS1 = 0x20; + +// Registers value bits +constexpr uint8_t LIS331DLH_CTRL_REG1_X_EN = 0x01; +constexpr uint8_t LIS331DLH_CTRL_REG1_Y_EN = 0x02; +constexpr uint8_t LIS331DLH_CTRL_REG1_Z_EN = 0x04; +constexpr uint8_t LIS331DLH_CTRL_REG1_PM0 = 0x20; +constexpr uint8_t LIS331DLH_CTRL_REG1_PM1 = 0x40; +constexpr uint8_t LIS331DLH_CTRL_REG1_PM2 = 0x80; + +// Gravity of Earth +constexpr float GRAVITY_EARTH = 9.8; + +// Sensor sensitivity depending on selectable full scales +// Use the datasheet for details +constexpr float SENS_2G = 1 * 4 / pow(2, 16); +constexpr float SENS_4G = 2 * 4 / pow(2, 16); +constexpr float SENS_8G = 3.9 * 4 / pow(2, 16); + +enum class AccelerometerRange { RANGE_2G = 1, RANGE_4G = 2, RANGE_8G = 3 }; + +class Accelerometer : public BaseIMU { +public: + Accelerometer(uint8_t slaveAddress = LIS331DLH_SLAVE_ADDRESS); + void begin(TwoWire& wire = Wire); + void sleep(bool state); + void setRange(AccelerometerRange range); + float readAccelerationGX(); + float readAccelerationGY(); + float readAccelerationGZ(); + float readAccelerationAX(); + float readAccelerationAY(); + float readAccelerationAZ(); + void readAccelerationGXYZ(float& ax, float& ay, float& az); + void readAccelerationAXYZ(float& ax, float& ay, float& az); + // DEPRECATED fuctions + // Use readAccelerationGX instead + float readGX() { return readAccelerationGX(); } + // Use readAccelerationGY instead + float readGY() { return readAccelerationGY(); } + // Use readAccelerationGZ instead + float readGZ() { return readAccelerationGZ(); } + // Use readAccelerationAX instead + float readAX() { return readAccelerationAX(); } + // Use readAccelerationAY instead + float readAY() { return readAccelerationAY(); } + // Use readAccelerationAZ instead + float readAZ() { return readAccelerationAZ(); } + // Use readAccelerationGXYZ instead + void readGXYZ(float* ax, float* ay, float* az) { + readAccelerationGXYZ(*ax, *ay, *az); + } + // Use readAccelerationAXYZ instead + void readAXYZ(float* ax, float* ay, float* az) { + readAccelerationAXYZ(*ax, *ay, *az); + } + +private: + float _scalingFactor; +}; + +#endif // __ACCELEROMETER_H__ diff --git a/src/Barometer.cpp b/src/Barometer.cpp new file mode 100644 index 0000000..1db5a43 --- /dev/null +++ b/src/Barometer.cpp @@ -0,0 +1,60 @@ +#include +#include + +Barometer::Barometer(uint8_t slaveAddress) + : BaseIMU(slaveAddress) { } + +void Barometer::begin(TwoWire& wire) { + _wire = &wire; + _wire->begin(); + _deviceID = readDeviceID(); + uint8_t data = 0; + if (_deviceID == LPS331_WHO_AM_I) { + data |= LPS_CTRL_REG1_ODR0 | LPS_CTRL_REG1_ODR1 | LPS_CTRL_REG1_ODR2; + } else if (_deviceID == LPS25HB_WHO_AM_I) { + data |= LPS_CTRL_REG1_ODR2; + } + data |= LPS_CTRL_REG1_PD; + _writeByte(BASE_IMU_CTRL_REG1, data); +} + +float Barometer::readPressureMillibars() { + return static_cast(_readPressureRaw()) / 4096; +} + +float Barometer::readPressurePascals() { + return readPressureMillibars() * MILLIBARS_TO_PASCALS; +} + +float Barometer::readPressureMillimetersHg() { + return readPressureMillibars() * MILLIBARS_TO_MILLIMETERSHG; +} + +// Temperature output data from datasheet +float Barometer::readTemperatureC() { + return 42.5 + static_cast(_readTemperatureRaw()) / 480; +} + +// Convert Celsius scale to Kelvin +float Barometer::readTemperatureK() { + return readTemperatureC() + CELSIUS_TO_KELVIN; +} + +// Convert Celsius scale to Fahrenheit +float Barometer::readTemperatureF() { return readTemperatureC() * 1.8 + 32; } + +uint32_t Barometer::_readPressureRaw() { + uint8_t data[3]; + _readBytes(0x80 | LPS_PRESS_OUT_XL, data, 3); + return static_cast(data[2]) << 16 | static_cast(data[1]) << 8 | data[0]; +} + +int16_t Barometer::_readTemperatureRaw() { + return (static_cast(_readByte(LPS_TEMP_OUT_H)) << 8) + | _readByte(LPS_TEMP_OUT_L); +} + +float Barometer::readAltitude() { + float pressurePascals = readPressurePascals(); + return GOST4401_getAltitude(pressurePascals); +} diff --git a/src/Barometer.h b/src/Barometer.h new file mode 100644 index 0000000..c755769 --- /dev/null +++ b/src/Barometer.h @@ -0,0 +1,51 @@ +#ifndef __BAROMETER_H__ +#define __BAROMETER_H__ + +#include "BaseIMU.h" + +// I²C-address devices LPS25HB and LPS331 +constexpr uint8_t LPS_SLAVE_ADDRESS = 0x5C; +constexpr uint8_t LPS_SLAVE_ADDRESS_ALT = 0x5D; + +// Registers address LPS25HB and LPS331 +constexpr uint8_t LPS_CTRL_REG1_ODR0 = 0x10; +constexpr uint8_t LPS_CTRL_REG1_ODR1 = 0x20; +constexpr uint8_t LPS_CTRL_REG1_ODR2 = 0x40; +constexpr uint8_t LPS_CTRL_REG1_PD = 0x80; + +constexpr uint8_t LPS_PRESS_OUT_XL = 0x28; +constexpr uint8_t LPS_PRESS_OUT_L = 0x29; +constexpr uint8_t LPS_PRESS_OUT_H = 0x2A; +constexpr uint8_t LPS_TEMP_OUT_L = 0x2B; +constexpr uint8_t LPS_TEMP_OUT_H = 0x2C; + +// Registers value byte +constexpr uint8_t LPS331_WHO_AM_I = 0xBB; +constexpr uint8_t LPS25HB_WHO_AM_I = 0xBD; + +// Conversion factors units of measurement +constexpr float CELSIUS_TO_KELVIN = 273.15; +constexpr float MILLIBARS_TO_PASCALS = 100; +constexpr float MILLIBARS_TO_MILLIMETERSHG = 0.75; + +class Barometer : public BaseIMU { +public: + Barometer(uint8_t slaveAddress = LPS_SLAVE_ADDRESS); + void begin(TwoWire& wire = Wire); + float readPressurePascals(); + float readPressureMillibars(); + float readPressureMillimetersHg(); + float readTemperatureC(); + float readTemperatureK(); + float readTemperatureF(); + float readAltitude(); + // DEPRECATED fuctions + // Use readAccelerationGX instead + +private: + uint32_t _readPressureRaw(); + int16_t _readTemperatureRaw(); + uint8_t _deviceID; +}; + +#endif // __BAROMETER_H__ diff --git a/src/BaseIMU.cpp b/src/BaseIMU.cpp new file mode 100644 index 0000000..dea5564 --- /dev/null +++ b/src/BaseIMU.cpp @@ -0,0 +1,52 @@ +#include "BaseIMU.h" + +uint8_t BaseIMU::readDeviceID(){ + return _readByte(BASE_IMU_WHO_AM_I); +} + +int16_t BaseIMU::readX() { + return (static_cast(_readByte(BASE_IMU_OUT_X_H)) << 8) | _readByte(BASE_IMU_OUT_X_L); +} + +int16_t BaseIMU::readY() { + return (static_cast(_readByte(BASE_IMU_OUT_Y_H)) << 8) | _readByte(BASE_IMU_OUT_Y_L); +} + +int16_t BaseIMU::readZ() { + return (static_cast(_readByte(BASE_IMU_OUT_Z_H)) << 8) | _readByte(BASE_IMU_OUT_Z_L); +} + +void BaseIMU::readXYZ(int16_t& x, int16_t& y, int16_t& z) { + uint8_t data[6]; + _readBytes(0x80 | BASE_IMU_OUT_X_L, data, 6); + x = (static_cast(data[1]) << 8) | data[0]; + y = (static_cast(data[3]) << 8) | data[2]; + z = (static_cast(data[5]) << 8) | data[4]; +} + +uint8_t BaseIMU::_readByte(uint8_t regAddress) { + uint8_t data; + _wire->beginTransmission(_slaveAddress); + _wire->write(regAddress); + _wire->endTransmission(); + _wire->requestFrom(_slaveAddress, 1u); + data = _wire->read(); + return data; +} + +void BaseIMU::_writeByte(uint8_t regAddress, uint8_t data) { + _wire->beginTransmission(_slaveAddress); + _wire->write(regAddress); + _wire->write(data); + _wire->endTransmission(); +} + +void BaseIMU::_readBytes(uint8_t regAddress, uint8_t* data, uint8_t length) { + _wire->beginTransmission(_slaveAddress); + _wire->write(regAddress); + _wire->endTransmission(); + _wire->requestFrom(_slaveAddress, length); + for (size_t i = 0; i < length; i++) { + *data++ = _wire->read(); + } +} diff --git a/src/BaseIMU.h b/src/BaseIMU.h new file mode 100644 index 0000000..ccd33f5 --- /dev/null +++ b/src/BaseIMU.h @@ -0,0 +1,43 @@ +#ifndef __BASE_IMU_H__ +#define __BASE_IMU_H__ + +#include +#include + +// Registers addrress +constexpr uint8_t BASE_IMU_WHO_AM_I = 0x0F; +constexpr uint8_t BASE_IMU_CTRL_REG1 = 0x20; +constexpr uint8_t BASE_IMU_CTRL_REG2 = 0x21; +constexpr uint8_t BASE_IMU_CTRL_REG3 = 0x22; +constexpr uint8_t BASE_IMU_CTRL_REG4 = 0x23; +constexpr uint8_t BASE_IMU_CTRL_REG5 = 0x24; + +constexpr uint8_t BASE_IMU_OUT_X_L = 0x28; +constexpr uint8_t BASE_IMU_OUT_X_H = 0x29; +constexpr uint8_t BASE_IMU_OUT_Y_L = 0x2A; +constexpr uint8_t BASE_IMU_OUT_Y_H = 0x2B; +constexpr uint8_t BASE_IMU_OUT_Z_L = 0x2C; +constexpr uint8_t BASE_IMU_OUT_Z_H = 0x2D; + +class BaseIMU { +public: + BaseIMU(uint8_t slaveAddress) + : _slaveAddress(slaveAddress) { } + void begin(TwoWire& wire = Wire); + uint8_t readDeviceID(); + int16_t readX(); + int16_t readY(); + int16_t readZ(); + void readXYZ(int16_t& x, int16_t& y, int16_t& z); + +protected: + uint8_t _readByte(uint8_t regAddress); + void _writeByte(uint8_t regAddress, uint8_t data); + void _readBytes(uint8_t regAddress, uint8_t* data, uint8_t length); + TwoWire* _wire; + +private: + uint8_t _slaveAddress; +}; + +#endif // __BASE_IMU_H__ diff --git a/src/Compass.cpp b/src/Compass.cpp new file mode 100644 index 0000000..56cafe2 --- /dev/null +++ b/src/Compass.cpp @@ -0,0 +1,116 @@ +#include "Compass.h" + +Compass::Compass(uint8_t slaveAddress) + : BaseIMU(slaveAddress) { } + +void Compass::begin(TwoWire& wire) { + _wire = &wire; + _wire->begin(); + _scalingFactor = 1; + uint8_t data = 0; + data &= ~(LIS3MDL_CTRL_REG3_MD0 | LIS3MDL_CTRL_REG3_MD1); + _writeByte(BASE_IMU_CTRL_REG3, data); + setRange(CompassRange::RANGE_4GAUSS); +} + +void Compass::setRange(CompassRange range) { + uint8_t data = _readByte(BASE_IMU_CTRL_REG2); + data &= ~(LIS3MDL_CTRL_REG2_FS0 | LIS3MDL_CTRL_REG2_FS1); + switch (range) { + case CompassRange::RANGE_4GAUSS: { + _scalingFactor = SENS_4GAUSS; + break; + } + case CompassRange::RANGE_8GAUSS: { + data |= LIS3MDL_CTRL_REG2_FS0; + _scalingFactor = SENS_8GAUSS; + break; + } + case CompassRange::RANGE_12GAUSS: { + data |= LIS3MDL_CTRL_REG2_FS1; + _scalingFactor = SENS_12GAUSS; + break; + } + case CompassRange::RANGE_16GAUSS: { + data |= LIS3MDL_CTRL_REG2_FS0 | LIS3MDL_CTRL_REG2_FS1; + _scalingFactor = SENS_16GAUSS; + break; + } + default: { + _scalingFactor = SENS_4GAUSS; + } break; + } + _writeByte(BASE_IMU_CTRL_REG2, data); +} + +void Compass::sleep(bool state) { + uint8_t data = _readByte(BASE_IMU_CTRL_REG3); + if (state) + data |= LIS3MDL_CTRL_REG3_MD0 | LIS3MDL_CTRL_REG3_MD1; + else + data &= ~(LIS3MDL_CTRL_REG3_MD0 | LIS3MDL_CTRL_REG3_MD1); + + _writeByte(BASE_IMU_CTRL_REG3, data); +} + +float Compass::readMagneticGaussX() { return readX() / _scalingFactor; } + +float Compass::readMagneticGaussY() { return readY() / _scalingFactor; } + +float Compass::readMagneticGaussZ() { return readZ() / _scalingFactor; } + +void Compass::readMagneticGaussXYZ(float& mx, float& my, float& mz) { + int16_t x, y, z; + readXYZ(x, y, z); + mx = x / _scalingFactor; + my = y / _scalingFactor; + mz = z / _scalingFactor; +} + +void Compass::readCalibrateMagneticGaussXYZ(float& mx, float& my, float& mz) { + int16_t x, y, z; + readXYZ(x, y, z); + mx = x, my = y, mz = z; + _calibrate(mx, my, mz); + mx = x / _scalingFactor; + my = y / _scalingFactor; + mz = z / _scalingFactor; +} + +void Compass::setCalibrateMatrix(const float calibrationMatrix[3][3], + const float calibrationBias[3]) { + memcpy(_calibrationBias, calibrationBias, 3 * sizeof(float)); + memcpy(_calibrationMatrix, calibrationMatrix, 3 * 3 * sizeof(float)); +} + +void Compass::_calibrate(float& x, float& y, float& z) { + float calibrationValues[3] = { 0, 0, 0 }; + float nonCalibrationValues[3] = { 0, 0, 0 }; + nonCalibrationValues[0] = x - _calibrationBias[0]; + nonCalibrationValues[1] = y - _calibrationBias[1]; + nonCalibrationValues[2] = z - _calibrationBias[2]; + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + calibrationValues[i] + += _calibrationMatrix[i][j] * nonCalibrationValues[j]; + } + } + + x = calibrationValues[0]; + y = calibrationValues[1]; + z = calibrationValues[2]; +} + +float Compass::readAzimut() { + int16_t x, y, z; + readXYZ(x, y, z); + float mx = x, my = y, mz = z; + _calibrate(mx, my, mz); + float heading = atan2(mx, my); + if (heading < 0) { + heading += TWO_PI; + } + float headingDegrees = heading * RAD_TO_DEG; + return headingDegrees; +} diff --git a/src/Compass.h b/src/Compass.h new file mode 100644 index 0000000..bf8cb8a --- /dev/null +++ b/src/Compass.h @@ -0,0 +1,70 @@ +#ifndef __COMPASS_H__ +#define __COMPASS_H__ + +#include "BaseIMU.h" + +// I²C-address device +constexpr uint8_t LIS3MDL_SLAVE_ADDRESS = 0x1C; +constexpr uint8_t LIS3MDL_SLAVE_ADDRESS_ALT = 0x1E; + +// Registers addrress +constexpr uint8_t LIS3MDL_CTRL_REG2_FS0 = 0x20; +constexpr uint8_t LIS3MDL_CTRL_REG2_FS1 = 0x40; + +// Registers value bits +constexpr uint8_t LIS3MDL_CTRL_REG3_MD0 = 0x01; +constexpr uint8_t LIS3MDL_CTRL_REG3_MD1 = 0x02; + +// Sensor sensitivity depending on selectable full scales +// Use the datasheet for details +constexpr float SENS_4GAUSS = 6842; +constexpr float SENS_8GAUSS = 3421; +constexpr float SENS_12GAUSS = 2281; +constexpr float SENS_16GAUSS = 1711; + +enum class CompassRange { + RANGE_4GAUSS = 1, + RANGE_8GAUSS = 2, + RANGE_12GAUSS = 3, + RANGE_16GAUSS = 4 +}; + +class Compass : public BaseIMU { +public: + Compass(uint8_t slaveAddress = LIS3MDL_SLAVE_ADDRESS); + void begin(TwoWire& wire = Wire); + void sleep(bool state); + void setRange(CompassRange range); + float readMagneticGaussX(); + float readMagneticGaussY(); + float readMagneticGaussZ(); + void readCalibrateMagneticGaussXYZ(float& mx, float& my, float& mz); + void readMagneticGaussXYZ(float& mx, float& my, float& mz); + void setCalibrateMatrix(const float calibrationMatrix[3][3], + const float bias[3]); + float readAzimut(); + // DEPRECATED fuctions + // Use readMagneticGaussX instead + float readGaussX() { return readMagneticGaussX(); } + // Use readMagneticGaussY instead + float readGaussY() { return readMagneticGaussY(); } + // Use readMagneticGaussZ instead + float readGaussZ() { return readMagneticGaussZ(); } + // Use readCalibrateMagneticGaussXYZ instead + void readCalibrateGaussXYZ(float* mx, float* my, float* mz) { + readCalibrateMagneticGaussXYZ(*mx, *my, *mz); + } + // Use setCalibrateMatrix instead + void calibrateMatrix(const float compassCalibrationMatrix[3][3], + const float compassCalibrationBias[3]) { + setCalibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); + } + +private: + void _calibrate(float& mx, float& my, float& mz); + float _calibrationMatrix[3][3]; + float _calibrationBias[3]; + float _scalingFactor; +}; + +#endif // __COMPASS_H__ diff --git a/src/GOST4401_81.cpp b/src/GOST4401_81.cpp new file mode 100644 index 0000000..6e63236 --- /dev/null +++ b/src/GOST4401_81.cpp @@ -0,0 +1,119 @@ +#include +#include + +static float _GOST4401_geopotential2geometric(float altitude) { + return altitude * GOST4401_E / (GOST4401_E - altitude); +} + +static float _GOST4401_geometric2geopotential(float altitude) { + return altitude * GOST4401_E / (GOST4401_E + altitude); +} + +/** + * Returns geometric altitude value for the given pressure. + * + * @param float pressurePa - pressure in pascals + * @retval float geometric altitude in meters + */ + +float GOST4401_getAltitude(float pressurePa) { + + if ((pressurePa <= GOST4401_MIN_PRESSURE) + || (pressurePa > GOST4401_MAX_PRESSURE)) + return NAN; + + int idx = 0; + + for (idx = 0; idx < GOST4401_LUT_RECORDS - 1; idx++) { + if ((pressurePa <= ag_table[idx].press) + && (pressurePa > ag_table[idx + 1].press)) + break; + } + + float Ps = ag_table[idx].press; + float Bm = ag_table[idx].t_grad; + float Tm = ag_table[idx].temp; + float Hb = ag_table[idx].alt; + float geopotH = 0; + + if (Bm != 0.0) { + geopotH + = ((Tm * pow(Ps / pressurePa, Bm * GOST4401_R / GOST4401_G) - Tm) + / Bm); + } else { + geopotH = log10(Ps / pressurePa) * (GOST4401_R * Tm) + / (GOST4401_G * 0.434294); + } + + return _GOST4401_geopotential2geometric(Hb + geopotH); +}; + +/** + * Returns pressure in pascals for the given geometric altitude + * + * @param float altitude - geometric altitude in meters + * @retval float - pressure in pascals + */ + +float GOST4401_getPressure(float altitude) { + float geopotH = _GOST4401_geometric2geopotential(altitude); + + if ((geopotH < GOST4401_MIN_GPALT) || (geopotH >= GOST4401_MAX_GPALT)) + return NAN; + + int idx = 0; + + for (idx = 1; idx < GOST4401_LUT_RECORDS - 1; idx++) { + if ((geopotH >= ag_table[idx].alt) && (geopotH < ag_table[idx].alt)) + break; + } + + float Ps = ag_table[idx].press; + float Bm = ag_table[idx].t_grad; + float Tm = ag_table[idx].temp; + float Hb = ag_table[idx].alt; + float lP = 0; + + if (Bm != 0.0) { + lP = log10(Ps) + - (GOST4401_G / (Bm * GOST4401_R)) + * log10((Tm + Bm * (geopotH - Hb)) / Tm); + } else { + lP = log10(Ps) + - 0.434294 * (GOST4401_G * (geopotH - Hb)) / (GOST4401_R * Tm); + } + + return pow(10, lP); +} + +/** + * Returns temperature value in K for the given geometric altitude. + * + * @param float altitude - geometric altitude in meters + * @retval float - temperature in degrees K + */ + +float GOST4401_getTemperature(float altitude) { + float geopotH = _GOST4401_geometric2geopotential(altitude); + + if ((geopotH < GOST4401_MIN_GPALT) || (geopotH >= GOST4401_MAX_GPALT)) + return NAN; + + int idx = 0; + + for (idx = 1; idx < GOST4401_LUT_RECORDS - 1; idx++) { + if ((geopotH >= ag_table[idx].alt) && (geopotH < ag_table[idx].alt)) + break; + } + + float Bm = ag_table[idx].t_grad; + float Tm = ag_table[idx].temp; + float Hb = ag_table[idx].alt; + float temp = Tm; + + if (Bm != 0.0) { + temp += Bm * (geopotH - Hb); + } + + return temp; +} diff --git a/src/GOST4401_81.h b/src/GOST4401_81.h new file mode 100644 index 0000000..da773ba --- /dev/null +++ b/src/GOST4401_81.h @@ -0,0 +1,50 @@ +/** + * Partial implementation of standard atmospheric model as described in + * GOST 4401-81 useful for processing of data from meteorological balloon + * sensors. + * + * Supported modelling of temperature and pressure over the altitude span from + * 0 up to 51km. + * + * by Oleg Kochetov + */ + +#ifndef __ATMOSPHERE_GOST4401_81_H__ +#define __ATMOSPHERE_GOST4401_81_H__ + +#include + +// Constants +#define GOST4401_G 9.80665 +#define GOST4401_R 287.05287 +#define GOST4401_E 6356766 + +#define GOST4401_MIN_PRESSURE 6.69384 +#define GOST4401_MAX_PRESSURE 101325.00 + +#define GOST4401_MIN_GPALT 0.00 +#define GOST4401_MAX_GPALT 51000.00 + +// Export functions +float GOST4401_getAltitude(float pressurePa); +float GOST4401_getPressure(float altitude); +float GOST4401_getTemperature(float altitude); + +// Structure for lookup table record +typedef struct __attribute__((packed)) _GOST4401_RECORD { + float alt; // Geopotentional altitude + float temp; // degrees K + float t_grad; // degrees K per meter + float press; // pascals +} GOST4401_RECORD; + +// Lookup table with averaged empirical parameters for +// lower layers of atmosphere in accordance with GOST 4401-81 +#define GOST4401_LUT_RECORDS 6 +static const GOST4401_RECORD ag_table[] = { + { 0, 288.15, -0.0065, 101325.00 }, { 11000, 216.65, 0.0, 22632.04 }, + { 20000, 216.65, 0.0010, 5474.87 }, { 32000, 228.65, 0.0028, 868.0146 }, + { 47000, 270.65, 0.0, 110.9056 }, { 51000, 270.65, -0.0028, 6.69384 } +}; + +#endif // __ATMOSPHERE_GOST4401_81_H__ diff --git a/src/Gyroscope.cpp b/src/Gyroscope.cpp new file mode 100644 index 0000000..ddfcde5 --- /dev/null +++ b/src/Gyroscope.cpp @@ -0,0 +1,78 @@ +#include "Gyroscope.h" + +Gyroscope::Gyroscope(uint8_t slaveAddress) + : BaseIMU(slaveAddress) { } + +void Gyroscope::begin(TwoWire& wire) { + _wire = &wire; + _wire->begin(); + _scalingFactor = 1; + uint8_t data = L3G4200D_CTRL_REG1_X_EN | L3G4200D_CTRL_REG1_Y_EN + | L3G4200D_CTRL_REG1_Z_EN; + data |= L3G4200D_CTRL_REG1_PD; + _writeByte(BASE_IMU_CTRL_REG1, data); + setRange(GyroscopeRange::RANGE_2000DPS); +} + +// Set range scale output data from datasheet +void Gyroscope::setRange(GyroscopeRange range) { + uint8_t data = _readByte(BASE_IMU_CTRL_REG4); + data &= ~(L3G4200D_CTRL_REG4_FS0 | L3G4200D_CTRL_REG4_FS1); + switch (range) { + case GyroscopeRange::RANGE_250DPS: { + _scalingFactor = SENS_250DPS; + break; + } + case GyroscopeRange::RANGE_500DPS: { + data |= L3G4200D_CTRL_REG4_FS0; + _scalingFactor = SENS_500DPS; + break; + } + case GyroscopeRange::RANGE_2000DPS: { + data |= L3G4200D_CTRL_REG4_FS0 | L3G4200D_CTRL_REG4_FS1; + _scalingFactor = SENS_2000DPS; + break; + } + default: { + _scalingFactor = SENS_250DPS; + } break; + } + _writeByte(BASE_IMU_CTRL_REG4, data); +} + +void Gyroscope::sleep(bool state) { + uint8_t data = _readByte(BASE_IMU_CTRL_REG1); + if (state) { + data &= ~L3G4200D_CTRL_REG1_PD; + } else { + data |= L3G4200D_CTRL_REG1_PD; + } + _writeByte(BASE_IMU_CTRL_REG1, data); +} + +float Gyroscope::readRotationDegX() { return readX() * _scalingFactor; } + +float Gyroscope::readRotationDegY() { return readY() * _scalingFactor; } + +float Gyroscope::readRotationDegZ() { return readZ() * _scalingFactor; } + +float Gyroscope::readRotationRadX() { return readRotationDegX() * DEG_TO_RAD; } + +float Gyroscope::readRotationRadY() { return readRotationDegY() * DEG_TO_RAD; } + +float Gyroscope::readRotationRadZ() { return readRotationDegZ() * DEG_TO_RAD; } + +void Gyroscope::readRotationDegXYZ(float& gx, float& gy, float& gz) { + int16_t x, y, z; + readXYZ(x, y, z); + gx = x * _scalingFactor; + gy = y * _scalingFactor; + gz = z * _scalingFactor; +} + +void Gyroscope::readRotationRadXYZ(float& gx, float& gy, float& gz) { + readRotationDegXYZ(gx, gy, gz); + gx *= DEG_TO_RAD; + gy *= DEG_TO_RAD; + gz *= DEG_TO_RAD; +} diff --git a/src/Gyroscope.h b/src/Gyroscope.h new file mode 100644 index 0000000..9830dcd --- /dev/null +++ b/src/Gyroscope.h @@ -0,0 +1,72 @@ +#ifndef __GYROSCOPE_H__ +#define __GYROSCOPE_H__ + +#include "BaseIMU.h" + +// I²C-address device +constexpr uint8_t L3G4200D_SLAVE_ADDRESS = 0x68; +constexpr uint8_t L3G4200D_SLAVE_ADDRESS_ALT = 0x69; + +// Registers address +constexpr uint8_t L3G4200D_CTRL_REG4_FS0 = 0x10; +constexpr uint8_t L3G4200D_CTRL_REG4_FS1 = 0x20; + +// Registers value bits +constexpr uint8_t L3G4200D_CTRL_REG1_X_EN = 0x01; +constexpr uint8_t L3G4200D_CTRL_REG1_Y_EN = 0x02; +constexpr uint8_t L3G4200D_CTRL_REG1_Z_EN = 0x04; +constexpr uint8_t L3G4200D_CTRL_REG1_PD = 0x08; + +// Sensor sensitivity depending on selectable full scales +// Use the datasheet for details +constexpr float SENS_250DPS = 0.00875; +constexpr float SENS_500DPS = 0.0175; +constexpr float SENS_2000DPS = 0.07; + +enum class GyroscopeRange { + RANGE_250DPS = 1, + RANGE_500DPS = 2, + RANGE_2000DPS = 3 +}; + +class Gyroscope : public BaseIMU { +public: + Gyroscope(uint8_t slaveAddress = L3G4200D_SLAVE_ADDRESS); + void begin(TwoWire& wire = Wire); + void sleep(bool state); + void setRange(GyroscopeRange range); + float readRotationDegX(); + float readRotationDegY(); + float readRotationDegZ(); + float readRotationRadX(); + float readRotationRadY(); + float readRotationRadZ(); + void readRotationDegXYZ(float& gx, float& gy, float& gz); + void readRotationRadXYZ(float& gx, float& gy, float& gz); + // DEPRECATED fuctions + // Use readRotationDegX instead + float readDegPerSecX() { return readRotationDegX(); } + // Use readRotationDegY instead + float readDegPerSecY() { return readRotationDegY(); } + // Use readRotationDegZ instead + float readDegPerSecZ() { return readRotationDegZ(); } + // Use readRotationRadX instead + float readRadPerSecX() { return readRotationRadX(); } + // Use readRotationRadY instead + float readRadPerSecY() { return readRotationRadY(); } + // Use readRotationRadZ instead + float readRadPerSecZ() { return readRotationRadZ(); } + // Use readRotationDegXYZ instead + void readDegPerSecXYZ(float* ax, float* ay, float* az) { + readRotationDegXYZ(*ax, *ay, *az); + } + // Use readRotationRadXYZ instead + void readRadPerSecXYZ(float* ax, float* ay, float* az) { + readRotationRadXYZ(*ax, *ay, *az); + } + +private: + float _scalingFactor; +}; + +#endif // __GYROSCOPE_H__ diff --git a/src/MadgwickAHRS.cpp b/src/MadgwickAHRS.cpp new file mode 100644 index 0000000..1fc0a23 --- /dev/null +++ b/src/MadgwickAHRS.cpp @@ -0,0 +1,280 @@ +#include "MadgwickAHRS.h" +#include +#include + +Madgwick::Madgwick() { } + +void Madgwick::begin() { + reset(); + setSettings(); +} + +void Madgwick::reset() { + _q0 = 1.0f; + _q1 = 0.0f; + _q2 = 0.0f; + _q3 = 0.0f; +} + +void Madgwick::setSettings(float beta, float zeta) { + _beta = beta; + _zeta = zeta; +} + +void Madgwick::setFrequency(float frequency) { + _frequency = frequency; +} + +void Madgwick::readQuaternion(float& q0, float& q1, float& q2, float& q3) { + q0 = _q0; + q1 = _q1; + q2 = _q2; + q3 = _q3; +} + +// IMU algorithm update with magnetometer +void Madgwick::update(float gx, float gy, float gz, float ax, float ay, + float az, float mx, float my, float mz) { + float recipNorm; + float s0, s1, s2, s3; + float qDot0, qDot1, qDot2, qDot3; + 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; + + // Rate of change of quaternion from gyroscope + qDot0 = 0.5f * (-_q1 * gx - _q2 * gy - _q3 * gz); + qDot1 = 0.5f * (_q0 * gx + _q2 * gz - _q3 * gy); + qDot2 = 0.5f * (_q0 * gy - _q1 * gz + _q3 * gx); + qDot3 = 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 = sqrtf(ax * ax + ay * ay + az * az); + recipNorm = 1 / recipNorm; + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Normalise magnetometer measurement + recipNorm = sqrtf(mx * mx + my * my + mz * mz); + recipNorm = 1 / recipNorm; + 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 = sqrt(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); + // Normalise step magnitude + recipNorm = sqrtf(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); + recipNorm = 1 / recipNorm; + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot0 -= _beta * s0; + qDot1 -= _beta * s1; + qDot2 -= _beta * s2; + qDot3 -= _beta * s3; + } + + // Integrate rate of change of quaternion to yield quaternion + _q0 += qDot0 * (1.0f / _frequency); + _q1 += qDot1 * (1.0f / _frequency); + _q2 += qDot2 * (1.0f / _frequency); + _q3 += qDot3 * (1.0f / _frequency); + + // Normalise quaternion + recipNorm = sqrtf(_q0 * _q0 + _q1 * _q1 + _q2 * _q2 + _q3 * _q3); + recipNorm = 1 / recipNorm; + _q0 *= recipNorm; + _q1 *= recipNorm; + _q2 *= recipNorm; + _q3 *= recipNorm; +} + +// IMU algorithm update without magnetometer +void Madgwick::update(float gx, float gy, float gz, float ax, float ay, + float az) { + float recipNorm; + float s0, s1, s2, s3; + float qDot0, qDot1, qDot2, qDot3; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, + q2q2, q3q3; + // Rate of change of quaternion from gyroscope + qDot0 = 0.5f * (-_q1 * gx - _q2 * gy - _q3 * gz); + qDot1 = 0.5f * (_q0 * gx + _q2 * gz - _q3 * gy); + qDot2 = 0.5f * (_q0 * gy - _q1 * gz + _q3 * gx); + qDot3 = 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 = sqrtf(ax * ax + ay * ay + az * az); + recipNorm = 1 / recipNorm; + 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; + + // Normalise step magnitude + recipNorm = sqrtf(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); + recipNorm = 1 / recipNorm; + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot0 -= _beta * s0; + qDot1 -= _beta * s1; + qDot2 -= _beta * s2; + qDot3 -= _beta * s3; + } + + // Integrate rate of change of quaternion to yield quaternion + _q0 += qDot0 * (1.0f / _frequency); + _q1 += qDot1 * (1.0f / _frequency); + _q2 += qDot2 * (1.0f / _frequency); + _q3 += qDot3 * (1.0f / _frequency); + + // Normalise quaternion + recipNorm = sqrtf(_q0 * _q0 + _q1 * _q1 + _q2 * _q2 + _q3 * _q3); + recipNorm = 1 / recipNorm; + _q0 *= recipNorm; + _q1 *= recipNorm; + _q2 *= recipNorm; + _q3 *= recipNorm; +} + +float Madgwick::getYawRad() { + return atan2(2 * _q1 * _q2 - 2 * _q0 * _q3, + 2 * _q0 * _q0 + 2 * _q1 * _q1 - 1); +} + +float Madgwick::getPitchRad() { + return atan2(2 * _q2 * _q3 - 2 * _q0 * _q1, + 2 * _q0 * _q0 + 2 * _q3 * _q3 - 1); +} + +float Madgwick::getRollRad() { + return -1 + * atan2(2.0f * (_q0 * _q2 - _q1 * _q3), + 1.0f - 2.0f * (_q2 * _q2 + _q1 * _q1)); +} + +float Madgwick::getYawDeg() { return getYawRad() * RAD_TO_DEG; } + +float Madgwick::getPitchDeg() { return getPitchRad() * RAD_TO_DEG; } + +float Madgwick::getRollDeg() { return getRollRad() * RAD_TO_DEG; } + +// DEPRECATED: use readQuaternion instead +void Madgwick::readQuaternions(float *q0, float *q1, float *q2, float *q3) { + *q0 = _q0; + *q1 = _q1; + *q2 = _q2; + *q3 = _q3; +} + +// DEPRECATED: use setSettings and setFrequency instead +void Madgwick::setKoeff(float frequency, float beta) { + _beta = beta; + _frequency = frequency; +} diff --git a/src/MadgwickAHRS.h b/src/MadgwickAHRS.h new file mode 100644 index 0000000..b9aea88 --- /dev/null +++ b/src/MadgwickAHRS.h @@ -0,0 +1,44 @@ +#ifndef __MADGWICK_AHRS_H__ +#define __MADGWICK_AHRS_H__ + +#include + +// Gyroscope measurement error in rads/s (start at 40 deg/s) +constexpr float GYRO_MEAS_ERROR = M_PI * (40.0f / 180.0f); +// Gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) +constexpr float GYRO_MEAS_DRIFT = M_PI * (0.0f / 180.0f); +// Compute beta +constexpr float BETA_DEFAULT = sqrt(3.0f / 4.0f) * GYRO_MEAS_ERROR; +// Compute zeta +constexpr float ZETA_DEFAULT = sqrt(3.0f / 4.0f) * GYRO_MEAS_DRIFT; + +class Madgwick { +public: + Madgwick(); + void begin(); + void reset(); + void setSettings(float beta = BETA_DEFAULT, float zeta = ZETA_DEFAULT); + void setFrequency(float frequency); + void readQuaternion(float& q0, float& q1, float& q2, float& q3); + void update(float gx, float gy, float gz, float ax, float ay, float az, + float mx, float my, float mz); + void update(float gx, float gy, float gz, float ax, float ay, float az); + float getPitchRad(); + float getRollRad(); + float getYawRad(); + float getPitchDeg(); + float getRollDeg(); + float getYawDeg(); + // DEPRECATED fuctions + // Use readQuaternion instead + void readQuaternions(float *q0, float *q1, float *q2, float *q3); + // Use setSettings and setFrequency instead + void setKoeff(float frequency, float beta); +private: + float _beta; + float _zeta; + float _frequency; + float _q0, _q1, _q2, _q3; +}; + +#endif // __MADGWICK_AHRS_H__ diff --git a/src/TroykaIMU.h b/src/TroykaIMU.h new file mode 100644 index 0000000..88c3957 --- /dev/null +++ b/src/TroykaIMU.h @@ -0,0 +1,10 @@ +#ifndef __TROYKA_IMU_H__ +#define __TROYKA_IMU_H__ + +#include "Accelerometer.h" +#include "Barometer.h" +#include "Compass.h" +#include "Gyroscope.h" +#include "MadgwickAHRS.h" + +#endif // __TROYKA_IMU_H__ diff --git a/stmhw.cpp b/stmhw.cpp deleted file mode 100755 index 332ffe3..0000000 --- a/stmhw.cpp +++ /dev/null @@ -1,97 +0,0 @@ -#include "stmhw.h" -#include - -#define CTRL_REG1 0x20 -#define CTRL_REG2 0x21 -#define CTRL_REG3 0x22 -#define CTRL_REG4 0x23 -#define CTRL_REG5 0x24 - -#define OUT_X 0x28 -#define OUT_Y 0x2A -#define OUT_Z 0x2C - -int16_t AxisHw::readX() { - return readAxis(OUT_X); -} - -int16_t AxisHw::readY() { - return readAxis(OUT_Y); -} - -int16_t AxisHw::readZ() { - return readAxis(OUT_Z); -} - -int16_t AxisHw::readAxis(uint8_t reg) { - return ((int16_t)readByte(reg + 1) << 8) | readByte(reg); -} - -inline void AxisHw::waitForData() { - while (WIRE_IMU.available() < 1) - continue; -} - -uint8_t AxisHw::readByte(uint8_t reg) { - uint8_t value; - WIRE_IMU.beginTransmission(_addr); - WIRE_IMU.write(reg); - WIRE_IMU.endTransmission(); - WIRE_IMU.requestFrom(_addr, 1u); - waitForData(); - value = WIRE_IMU.read(); - return value; -} - -void AxisHw::readXYZ(int16_t *x, int16_t *y, int16_t *z) { - WIRE_IMU.beginTransmission(_addr); - WIRE_IMU.write(OUT_X | (1 << 7)); // assert MSB to enable register address auto-increment - WIRE_IMU.endTransmission(); - - uint8_t burstSize = 6; - WIRE_IMU.requestFrom(_addr, burstSize); - uint8_t values[burstSize]; - for (uint8_t i = 0; i < burstSize; i++) { - waitForData(); - values[i] = WIRE_IMU.read(); - } - - *x = *((int16_t*)&values[0]); - *y = *((int16_t*)&values[2]); - *z = *((int16_t*)&values[4]); -} - -void AxisHw::writeCtrlReg1(){ - WIRE_IMU.beginTransmission(_addr); - WIRE_IMU.write(CTRL_REG1); - WIRE_IMU.write(_ctrlReg1); - WIRE_IMU.endTransmission(); -} - -void AxisHw::writeCtrlReg2(){ - WIRE_IMU.beginTransmission(_addr); - WIRE_IMU.write(CTRL_REG2); - WIRE_IMU.write(_ctrlReg2); - WIRE_IMU.endTransmission(); -} - -void AxisHw::writeCtrlReg3(){ - WIRE_IMU.beginTransmission(_addr); - WIRE_IMU.write(CTRL_REG3); - WIRE_IMU.write(_ctrlReg3); - WIRE_IMU.endTransmission(); -} - -void AxisHw::writeCtrlReg4(){ - WIRE_IMU.beginTransmission(_addr); - WIRE_IMU.write(CTRL_REG4); - WIRE_IMU.write(_ctrlReg4); - WIRE_IMU.endTransmission(); -} - -void AxisHw::writeCtrlReg5(){ - WIRE_IMU.beginTransmission(_addr); - WIRE_IMU.write(CTRL_REG5); - WIRE_IMU.write(_ctrlReg5); - WIRE_IMU.endTransmission(); -} diff --git a/stmhw.h b/stmhw.h deleted file mode 100755 index 2ae1e87..0000000 --- a/stmhw.h +++ /dev/null @@ -1,46 +0,0 @@ -#ifndef HW_H -#define HW_H - -#if defined(__AVR__) || defined(__SAMD21G18A__) || defined(ESP8266) -#define WIRE_IMU Wire - -#elif defined(__SAM3X8E__) || defined(__SAM3A8C__) || defined(__SAM3A4C__) -#define WIRE_IMU Wire1 -#endif - -#include -#include -class AxisHw -{ -private: - uint8_t _addr; - - static inline void waitForData(); - -public: - AxisHw (uint8_t addr) : _addr (addr) {} - - uint8_t readByte(uint8_t reg); - int16_t readX(); - int16_t readY(); - int16_t readZ(); - void readXYZ(int16_t *x, int16_t *y, int16_t *z); - -protected: - uint8_t _ctrlReg1; - uint8_t _ctrlReg2; - uint8_t _ctrlReg3; - uint8_t _ctrlReg4; - uint8_t _ctrlReg5; - - float _mult; - - void writeCtrlReg1(); - void writeCtrlReg2(); - void writeCtrlReg3(); - void writeCtrlReg4(); - void writeCtrlReg5(); - - int16_t readAxis(uint8_t reg); -}; -#endif //HW_H diff --git a/troyka-imu.h b/troyka-imu.h deleted file mode 100755 index a78e331..0000000 --- a/troyka-imu.h +++ /dev/null @@ -1,3 +0,0 @@ -#include "TroykaIMU.h" - -#warning "Deprecated: use TroykaIMU.h" \ No newline at end of file