added bmm150 in ahrs calculation, and smothing tries

include/oscmgr.h

@@ -11,10 +11,11 @@
 template <typename TYPE> void sendOSC(const char * adress, TYPE parameter);
 void sendOSC_3f(const char * adress, float parameter, float parameter2, float parameter3);
 
-
+extern bool calib_flag;
 
 void printTest(OSCMessage &msg) ;
-
+void OSCcb_calibGyro(OSCMessage &msg) ;
 void setup_OSC(IPAddress _UDP_Out_IP, int _UDP_Out_Port, int _UDP_In_Port);
 void handle_OSC();
 
+

lib/Smoothed/LICENSE

@@ -0,0 +1,674 @@
+                    GNU GENERAL PUBLIC LICENSE
+                       Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
+ 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.
+
+    <one line to give the program's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    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 <http://www.gnu.org/licenses/>.
+
+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:
+
+    <program>  Copyright (C) <year>  <name of author>
+    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
+<http://www.gnu.org/licenses/>.
+
+  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
+<http://www.gnu.org/philosophy/why-not-lgpl.html>.

lib/Smoothed/examples/Smoothed/Smoothed.ino

@@ -0,0 +1,77 @@
+/*
+ Sensor Input Smoothing
+ Demonstrates smoothing of a sensor input via various methods.
+
+
+ Created by Matthew Fryer
+
+ This example code is in the public domain.
+
+ */
+
+#include <Smoothed.h> 	// Include the library
+ 
+#define SENSOR_PIN A0    // The input pin for the sensor. In this example we are reading from an Arduino analogue pin. 
+// If you don't have a sensor you can still see the effect if the analogue pin is left floating its value will vary wildly.
+
+// Create two instances of the class to use. 
+Smoothed <float> mySensor; 
+Smoothed <float> mySensor2;
+
+/* We are using a template class that can be used with any numeric data type. 
+   You can therefore replace <float> above with any other data type to suit your sensor readings and desired accuracy. 
+   All of the below are valid examples:
+		Smoothed <byte> mySensor;
+		Smoothed <int> mySensor;
+		Smoothed <unsigned int> mySensor;
+		Smoothed <long> mySensor;
+		Smoothed <unsigned long> mySensor;
+		Smoothed <short> mySensor;	
+		Smoothed <float> mySensor;
+		Smoothed <double> mySensor;
+*/
+
+void setup() {
+	Serial.begin(9600);
+
+	// Initialise the first sensor value store. We want this to be the simple average of the last 10 values.
+	// Note: The more values you store, the more memory will be used.
+	mySensor.begin(SMOOTHED_AVERAGE, 10);	
+
+	// Initialise the second sensor value store. We want this one to be a simple linear recursive exponential filter. 
+	// We set the filter level to 10. Higher numbers will result in less filtering/smoothing. Lower number result in more filtering/smoothing
+	mySensor2.begin(SMOOTHED_EXPONENTIAL, 10);
+	
+	/* Both parameters are optional and so both of the following are valid:
+			mySensor.begin();
+			mySensor.begin(SMOOTHED_AVERAGE);
+			
+	   The default parameters are SMOOTHED_EXPONENTIAL and 10.
+	*/
+
+    // Although it is unnecessary here, the stored values can be cleared if needed.
+    mySensor.clear();
+}
+
+void loop() {
+    // Read the value from the sensor
+    float currentSensorValue = analogRead(SENSOR_PIN);
+    
+    // Add the new value to both sensor value stores
+    mySensor.add(currentSensorValue);
+    mySensor2.add(currentSensorValue);
+    
+    // Get the smoothed values
+    float smoothedSensorValueAvg = mySensor.get();
+    float smoothedSensorValueExp = mySensor2.get();
+    
+    // Output the smoothed values to the serial stream. Open the Arduino IDE Serial plotter to see the effects of the smoothing methods.
+    Serial.print(currentSensorValue); Serial.print("\t"); Serial.print(smoothedSensorValueAvg); Serial.print("\t"); Serial.println(smoothedSensorValueExp);
+	
+    
+    // If needed we can also return the last stored value which will be unsmoothed
+    float lastValueStoredAvg = mySensor.getLast();
+    float lastValueStoredExp = mySensor2.getLast();	
+
+    delay(100);
+}

lib/Smoothed/keywords.txt

@@ -0,0 +1,36 @@
+#######################################
+# SMOOTHED
+# Syntax Coloring Map
+#######################################
+
+#######################################
+# Datatypes (DATA_TYPE)
+#######################################
+# 3 tabs required to make it bold?
+
+#######################################
+# Classes & Namespaces (KEYWORD1)
+#######################################
+
+Smoothed	KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+begin	KEYWORD2
+add	KEYWORD2
+get	KEYWORD2
+getLast	KEYWORD2
+clear	KEYWORD2
+
+#######################################
+# Instances (KEYWORD3)
+#######################################
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+
+SMOOTHED_AVERAGE	LITERAL1
+SMOOTHED_EXPONENTIAL	LITERAL1

lib/Smoothed/library.properties

@@ -0,0 +1,10 @@
+name=Smoothed
+version=1.2
+author=Matthew Fryer <matthew_fryer@hotmail.com>
+maintainer=Matthew Fryer <matthew_fryer@hotmail.com>
+sentence=A simple library to store and calculate smoothed values from sensors.
+paragraph=Expecially useful for reading values from the inbuilt, or an external ADC. Unlike most other similar libraries, Smoothed uses a template class allowing it to work with any numerical data type. This makes it both lightweight and flexible.
+category=Data Processing
+url=https://github.com/MattFryer/Smoothed
+architectures=*
+includes=Smoothed.h

lib/Smoothed/src/Smoothed.cpp

@@ -0,0 +1,7 @@
+/*
+ *  Smoothed.cpp
+ *  Store and calculate smoothed values from sensors.
+ *  Created by Matt Fryer on 2017-11-17.
+ *  Licensed under LGPL (free to modify and use as you wish)
+ */
+

lib/Smoothed/src/Smoothed.h

@@ -0,0 +1,199 @@
+/*
+ *  Smoothed.h
+ *  Store and calculate smoothed values from sensors.
+ *  Created by Matt Fryer on 2017-11-17.
+ *  Licensed under LGPL (free to modify and use as you wish)
+ */
+
+#pragma once
+
+#define SMOOTHED_AVERAGE 1
+#define SMOOTHED_EXPONENTIAL 2
+
+// A class used to store and calculate the values to be smoothed.
+template <typename T>
+class Smoothed {
+  private:
+    byte smoothMode;
+    uint16_t smoothReadingsFactor = 10; // The smoothing factor. In average mode, this is the number of readings to average. 
+    uint16_t smoothReadingsPosition = 0; // Current position in the array
+    uint16_t smoothReadingsNum = 0; // Number of readings currently being averaged
+    T *smoothReading; // Array of readings
+  public:
+    Smoothed();
+    ~Smoothed(); // Destructor to clean up when class instance killed
+    bool begin (byte smoothMode, uint16_t smoothFactor = 10);
+    bool add (T newReading);
+    T get ();
+    T getLast ();
+    bool clear ();
+};
+
+// Constructor
+template <typename T>
+Smoothed<T>::Smoothed () { // Constructor
+  
+}
+
+// Destructor
+template <typename T>
+Smoothed<T>::~Smoothed () { // Destructor
+  delete[] smoothReading;
+}
+
+// Inintialise the array for storing sensor values
+template <typename T>
+bool Smoothed<T>::begin (byte mode, uint16_t smoothFactor) { 
+  smoothMode = mode;
+  smoothReadingsFactor = smoothFactor; 
+  
+  switch (smoothMode) {  
+    case SMOOTHED_AVERAGE : // SMOOTHED_AVERAGE
+      
+      smoothReading = new T[smoothReadingsFactor]; // Create the actual array of the required size
+    
+      // Initialise all the values in the array to zero
+      for (int thisReading = 0; thisReading < smoothReadingsNum; thisReading++) {
+        smoothReading[thisReading] = 0;
+      }
+
+      return true;
+      break;
+      
+    case SMOOTHED_EXPONENTIAL : // SMOOTHED_EXPONENTIAL
+
+      smoothReading = new T[2];
+      smoothReading[0] = 0;
+      smoothReading[1] = 0; // Second value in array used for storing last value added
+      
+      return true;  
+      break;
+
+    default : 
+      return false;
+      break;
+  }
+  
+}
+
+// Add a value to the array
+template <typename T>
+bool Smoothed<T>::add (T newReading) {
+  switch (smoothMode) {    
+    case SMOOTHED_AVERAGE : // SMOOTHED_AVERAGE
+    
+      if(smoothReadingsNum < smoothReadingsFactor) { smoothReadingsNum++; } // Keep record of the number of readings being averaged. This will count up to the arrany saize then stay at that number
+       
+      smoothReading[smoothReadingsPosition] = newReading; // Add the new value
+      
+      if (smoothReadingsPosition == (smoothReadingsFactor - 1)) { // If at the end of the array
+        smoothReadingsPosition = 0; // Increment to the beginning of the array
+      } else {
+        smoothReadingsPosition++; // Increment to next array position position
+      }
+
+      return true;
+      break;
+
+    case SMOOTHED_EXPONENTIAL : // SMOOTHED_EXPONENTIAL
+
+      if( smoothReadingsNum == 0 ) {
+        smoothReadingsNum++;
+        smoothReading[0] = newReading;
+      } else {
+        smoothReading[0] = (T)(((long double)smoothReadingsFactor/100) * newReading + (1 - ((long double)smoothReadingsFactor/100)) * smoothReading[0]);
+      }
+
+      smoothReading[1] = newReading; // Update the last value added
+      
+      return true;
+      break;
+      
+    default : 
+      return false;
+      break;
+  }    
+}
+
+// Get the smoothed result
+template <typename T>
+T Smoothed<T>::get () {
+  switch (smoothMode) {
+    case SMOOTHED_AVERAGE : { // SMOOTHED_AVERAGE
+      T runningTotal = 0;
+      // calculating a `SUM(smoothReadings) / smoothReadingsNum` can lead to overflows.
+      T tmpRes = 0;
+      T remainder = 0;
+      for (int x = 0; x < smoothReadingsNum; x++) {
+        tmpRes = smoothReading[x] / smoothReadingsNum;
+        remainder += smoothReading[x] - tmpRes * smoothReadingsNum;
+        runningTotal += tmpRes;
+        if (remainder > smoothReadingsNum) {
+          tmpRes = remainder / smoothReadingsNum;
+          remainder -= tmpRes * smoothReadingsNum;
+          runningTotal += tmpRes;
+        }
+      }
+      return runningTotal;
+    }
+      break;
+
+    case SMOOTHED_EXPONENTIAL : // SMOOTHED_EXPONENTIAL
+      return smoothReading[0]; 
+      break;
+
+    default : 
+      return false;
+      break;
+  }   
+}
+
+// Gets the last result stored
+template <typename T>
+T Smoothed<T>::getLast () {
+  switch (smoothMode) {  
+    case SMOOTHED_AVERAGE : // SMOOTHED_AVERAGE 
+      // Just return the last reading
+      if (smoothReadingsPosition == 0) {
+        return smoothReading[smoothReadingsFactor-1]; 
+      } else {
+        return smoothReading[smoothReadingsPosition-1];
+      }
+      break;
+
+    case SMOOTHED_EXPONENTIAL : // SMOOTHED_EXPONENTIAL
+      return smoothReading[1]; 
+      break;
+
+    default : 
+      return false;
+      break;
+  }         
+}
+
+// Clears all stored values
+template <typename T>
+bool Smoothed<T>::clear () {
+  switch (smoothMode) {    
+    case SMOOTHED_AVERAGE : // SMOOTHED_AVERAGE    
+      // Reset the counters
+      smoothReadingsPosition = 0; 
+      smoothReadingsNum = 0; 
+      
+      // Set all the values in the array to zero.  Not really needed
+      for (int thisReading = 0; thisReading < smoothReadingsNum; thisReading++) {
+        smoothReading[thisReading] = 0;
+      }
+      break;
+
+    case SMOOTHED_EXPONENTIAL : // SMOOTHED_EXPONENTIAL
+      smoothReadingsNum = 0;
+	  smoothReading[0] = 0;
+      smoothReading[1] = 0;
+      break;
+
+    default : 
+      return false;
+      break;
+  }         
+}

lib/bmm150/BMM150class.cpp

@@ -0,0 +1,212 @@
+#include "BMM150class.h"
+
+BMM150class::BMM150class()
+{
+}
+
+int8_t i2c_read(uint8_t dev_id, uint8_t reg_addr, uint8_t *read_data, uint16_t len)
+{
+    if (M5.I2C.readBytes(dev_id, reg_addr, len, read_data))
+    {
+        return BMM150_OK;
+    }
+    else
+    {
+        return BMM150_E_DEV_NOT_FOUND;
+    }
+}
+
+int8_t i2c_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *read_data, uint16_t len)
+{
+    if (M5.I2C.writeBytes(dev_id, reg_addr, read_data, len))
+    {
+        return BMM150_OK;
+    }
+    else
+    {
+        return BMM150_E_DEV_NOT_FOUND;
+    }
+}
+
+int8_t BMM150class::bmm150_initialization()
+{
+    int8_t rslt = BMM150_OK;
+
+    /* Sensor interface over SPI with native chip select line */
+    dev.dev_id = 0x10;
+    dev.intf = BMM150_I2C_INTF;
+    dev.read = i2c_read;
+    dev.write = i2c_write;
+    dev.delay_ms = delay;
+
+    /* make sure max < mag data first  */
+    mag_max.x = -2000;
+    mag_max.y = -2000;
+    mag_max.z = -2000;
+
+    /* make sure min > mag data first  */
+    mag_min.x = 2000;
+    mag_min.y = 2000;
+    mag_min.z = 2000;
+
+    rslt = bmm150_init(&dev);
+    dev.settings.pwr_mode = BMM150_NORMAL_MODE;
+    rslt |= bmm150_set_op_mode(&dev);
+    dev.settings.preset_mode = BMM150_PRESETMODE_ENHANCED;
+    rslt |= bmm150_set_presetmode(&dev);
+    return rslt;
+}
+
+void BMM150class::bmm150_offset_save()
+{
+    prefs.begin("bmm150", false);
+    prefs.putBytes("offset", (uint8_t *)&mag_offset, sizeof(bmm150_mag_data));
+    // added soft-iron scale
+    prefs.putBytes("scale", (uint8_t *)&mag_scale, sizeof(bmm150_mag_data));
+    prefs.end();
+}
+
+void BMM150class::bmm150_offset_load()
+{
+    if (prefs.begin("bmm150", true))
+    {
+        prefs.getBytes("offset", (uint8_t *)&mag_offset, sizeof(bmm150_mag_data));
+        // added soft iron distortion scaling
+        prefs.getBytes("scale", (uint8_t *)&mag_scale, sizeof(bmm150_mag_data));
+        prefs.end();
+        Serial.printf("bmm150 load offset finish.... \r\n");
+    }
+    else
+    {
+        Serial.printf("bmm150 load offset failed.... \r\n");
+    }
+}
+/*
+void BMM150class::setup()
+{
+    M5.begin(true, false, true, false);
+    Wire.begin(21, 22, 400000);
+
+    M5.Lcd.fillScreen(BLACK);
+    M5.Lcd.setTextColor(WHITE);
+
+    if (bmm150_initialization() != BMM150_OK)
+    {
+        // img.fillSprite(0);
+        // img.drawCentreString("BMM150 init failed", 160, 110, 4);
+        // img.pushSprite(0, 0);
+        M5.Lcd.setCursor(0, 10);
+        M5.Lcd.print("BMM150 init failed");
+        for (;;)
+        {
+            delay(100);
+        }
+    }
+
+    bmm150_offset_load();
+}
+*/
+void BMM150class::bmm150_calibrate(uint32_t calibrate_time)
+{
+    uint32_t calibrate_timeout = 0;
+    float avg_chord;
+
+    calibrate_timeout = millis() + calibrate_time;
+    Serial.printf("Go calibrate, use %d ms \r\n", calibrate_time);
+    Serial.printf("running ...");
+
+    while (calibrate_timeout > millis())
+    {
+        bmm150_read_mag_data(&dev);
+        if (dev.data.x)
+        {
+            mag_min.x = (dev.data.x < mag_min.x) ? dev.data.x : mag_min.x;
+            mag_max.x = (dev.data.x > mag_max.x) ? dev.data.x : mag_max.x;
+        }
+
+        if (dev.data.y)
+        {
+            mag_max.y = (dev.data.y > mag_max.y) ? dev.data.y : mag_max.y;
+            mag_min.y = (dev.data.y < mag_min.y) ? dev.data.y : mag_min.y;
+        }
+
+        if (dev.data.z)
+        {
+            mag_min.z = (dev.data.z < mag_min.z) ? dev.data.z : mag_min.z;
+            mag_max.z = (dev.data.z > mag_max.z) ? dev.data.z : mag_max.z;
+        }
+        delay(100);
+    }
+
+    mag_offset.x = (mag_max.x + mag_min.x) / 2;
+    mag_offset.y = (mag_max.y + mag_min.y) / 2;
+    mag_offset.z = (mag_max.z + mag_min.z) / 2;
+    // added soft iron calibration
+    mag_chord.x = (mag_max.x - mag_min.x) / 2;
+    mag_chord.y = (mag_max.y - mag_min.y) / 2;
+    mag_chord.z = (mag_max.z - mag_min.z) / 2;
+    avg_chord = (mag_chord.x + mag_chord.y + mag_chord.z)/3;
+    mag_scale.x = avg_chord / mag_chord.x;
+    mag_scale.y = avg_chord / mag_chord.y;
+    mag_scale.z = avg_chord / mag_chord.z;
+    bmm150_offset_save();
+
+    Serial.printf("\n calibrate finish ... \r\n");
+//    Serial.printf("mag_max.x: %.2f x_min: %.2f \t", mag_max.x, mag_min.x);
+//    Serial.printf("y_max: %.2f y_min: %.2f \t", mag_max.y, mag_min.y);
+//    Serial.printf("z_max: %.2f z_min: %.2f \r\n", mag_max.z, mag_min.z);
+}
+
+void BMM150class::Init(void)
+{
+    Wire.begin(21, 22, 400000);
+    
+    M5.Lcd.fillScreen(BLACK);
+    M5.Lcd.setTextColor(WHITE);
+
+    if (bmm150_initialization() != BMM150_OK)
+    {
+        M5.Lcd.setCursor(0, 10);
+        M5.Lcd.print("BMM150 init failed");
+        for (;;)
+        {
+            delay(100);
+        }
+    }
+    
+    bmm150_offset_load();
+
+    Serial.printf("MID X : %.2f \t MID Y : %.2f \t MID Z : %.2f \n", mag_offset.x, mag_offset.y, mag_offset.z);
+    Serial.printf("SCALE X : %.2f \t SCALE Y : %.2f \t SCALE Z : %.2f \n", mag_scale.x, mag_scale.y, mag_scale.z);
+    M5.Lcd.setCursor(0, 10);
+    M5.Lcd.printf("offset X, Y, Z: %5.4f %5.4f %5.4f \n", mag_offset.x, mag_offset.y, mag_offset.z);
+    M5.Lcd.setCursor(0, 20);
+    M5.Lcd.printf("scale  X, Y, Z: %5.4f %5.4f %5.4f \n", mag_scale.x, mag_scale.y, mag_scale.z);
+
+    delay(3000); // see preset data 3 sec.
+}
+
+void BMM150class::getMagnetData(float *mx, float *my, float *mz)
+{
+    bmm150_read_mag_data(&dev);
+//    float head_dir = atan2(dev.data.x - mag_offset.x, dev.data.y - mag_offset.y) * 180.0 / M_PI;
+    *mx = dev.data.x;
+    *my = dev.data.y;
+    *mz = dev.data.z;
+}
+
+void BMM150class::getMagnetOffset(float *mx, float *my, float *mz)
+{
+    bmm150_offset_load();
+    *mx = mag_offset.x;
+    *my = mag_offset.y;
+    *mz = mag_offset.z;
+}
+
+void BMM150class::getMagnetScale(float *mx, float *my, float *mz)
+{
+    bmm150_offset_load();
+    *mx = mag_scale.x;
+    *my = mag_scale.y;
+    *mz = mag_scale.z;
+}

lib/bmm150/BMM150class.h

@@ -0,0 +1,35 @@
+#include <Arduino.h>
+#include <Wire.h>
+#include "Preferences.h"
+#include "M5Stack.h"
+#include "math.h"
+#include "bmm150.h"
+#include "bmm150_defs.h"
+
+class BMM150class
+{
+public:
+    Preferences prefs;
+    struct bmm150_dev dev;
+    bmm150_mag_data mag_offset;
+    bmm150_mag_data mag_scale; // for Soft iron distortion
+    bmm150_mag_data mag_max;
+    bmm150_mag_data mag_min;
+    bmm150_mag_data mag_chord; // for Soft iron distortion
+
+private:
+    // int8_t i2c_read(uint8_t dev_id, uint8_t reg_addr, uint8_t *read_data, uint16_t len);
+    // int8_t i2c_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *read_data, uint16_t len);
+    int8_t bmm150_initialization();
+    void bmm150_offset_save();
+    void bmm150_offset_load();
+    // void setup();
+
+public:
+    BMM150class();
+    void Init(void);
+    void bmm150_calibrate(uint32_t calibrate_time);
+    void getMagnetData(float *mx, float *my, float *mz);
+    void getMagnetOffset(float *mx, float *my, float *mz); // hard iron
+    void getMagnetScale(float *mx, float *my, float *mz);  // soft iron
+};

lib/bmm150/bmm150.h

@@ -0,0 +1,329 @@
+/**
+ * Copyright (C) 2015 - 2016 Bosch Sensortec GmbH
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * Neither the name of the copyright holder nor the names of the
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER
+ * OR CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
+ * OR CONSEQUENTIAL DAMAGES(INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE
+ *
+ * The information provided is believed to be accurate and reliable.
+ * The copyright holder assumes no responsibility
+ * for the consequences of use
+ * of such information nor for any infringement of patents or
+ * other rights of third parties which may result from its use.
+ * No license is granted by implication or otherwise under any patent or
+ * patent rights of the copyright holder.
+ *
+ * @file	bmm150.h
+ * @date	12 Sep 2017
+ * @version	1.0.0
+ *
+ */
+
+/*! @file bmm150.h */
+/*!
+ * @defgroup BMM150 SENSOR API
+ * @{*/
+
+#ifndef BMM150_H__
+#define BMM150_H__
+
+/*! CPP guard */
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/********************************************************************/
+/* header files */
+
+#include "bmm150_defs.h"
+
+/********************************************************************/
+/* (extern) variable declarations */
+
+
+/********************************************************************/
+/* function prototype declarations */
+
+/*!
+ *  @brief This API is the entry point, Call this API before using other APIs.
+ *  This API reads the chip-id of the sensor which is the first step to
+ *  verify the sensor and also it configures the read mechanism of SPI and
+ *  I2C interface.
+ *
+ *  @param[in,out] dev : Structure instance of bmm150_dev
+ *  @note : Refer user guide for detailed info.
+ *
+ *  @return Result of API execution status
+ *  @retval zero -> Success / +ve value -> Warning / -ve value -> Error
+ */
+int8_t bmm150_init(struct bmm150_dev *dev);
+
+/*!
+ * @brief This API writes the given data to the register address
+ * of the sensor.
+ *
+ * @param[in] reg_addr : Register address from where the data to be written.
+ * @param[in] reg_data : Pointer to data buffer which is to be written
+ *                       in the reg_addr of sensor.
+ * @param[in] len      : No of bytes of data to write..
+ * @param[in] dev      : Structure instance of bmm150_dev.
+ *
+ * @return Result of API execution status
+ * @retval zero -> Success / +ve value -> Warning / -ve value -> Error
+ */
+int8_t bmm150_set_regs(uint8_t reg_addr, uint8_t *reg_data, uint8_t len, const struct bmm150_dev *dev);
+
+/*!
+ * @brief This API reads the data from the given register address of sensor.
+ *
+ * @param[in] reg_addr  : Register address from where the data to be read
+ * @param[out] reg_data : Pointer to data buffer to store the read data.
+ * @param[in] len       : No of bytes of data to be read.
+ * @param[in] dev       : Structure instance of bmm150_dev.
+ *
+ * @return Result of API execution status
+ * @retval zero -> Success / +ve value -> Warning / -ve value -> Error
+ */
+int8_t bmm150_get_regs(uint8_t reg_addr, uint8_t *reg_data, uint8_t len, const struct bmm150_dev *dev);
+
+/*!
+ * @brief This API is used to perform soft-reset of the sensor
+ * where all the registers are reset to their default values except 0x4B.
+ *
+ * @param[in] dev       : Structure instance of bmm150_dev.
+ *
+ * @return Result of API execution status
+ * @retval zero -> Success / +ve value -> Warning / -ve value -> Error
+ */
+int8_t bmm150_soft_reset(const struct bmm150_dev *dev);
+
+/*!
+ * @brief This API is used to set the power mode of the sensor.
+ *
+ * @param[in] dev       : Structure instance of bmm150_dev.
+ *
+ * dev->settings.pwr_mode  |  Power mode
+ * ------------------------|-----------------------
+ *   0x00                  |   BMM150_NORMAL_MODE
+ *   0x01                  |   BMM150_FORCED_MODE
+ *   0x03                  |   BMM150_SLEEP_MODE
+ *   0x04                  |   BMM150_SUSPEND_MODE
+ *
+ * @return Result of API execution status
+ * @retval zero -> Success / +ve value -> Warning / -ve value -> Error
+ */
+int8_t bmm150_set_op_mode(struct bmm150_dev *dev);
+
+/*!
+ * @brief This API is used to get the power mode of the sensor.
+ *
+ * @param[out] op_mode  : power mode of the sensor.
+ * @param[in] dev       : Structure instance of bmm150_dev.
+ *
+ *   op_mode     |  Power mode
+ *  -------------|-----------------------
+ *   0x00        |   BMM150_NORMAL_MODE
+ *   0x01        |   BMM150_FORCED_MODE
+ *   0x03        |   BMM150_SLEEP_MODE
+ *   0x04        |   BMM150_SUSPEND_MODE
+ *
+ * @return Result of API execution status
+ * @retval zero -> Success / +ve value -> Warning / -ve value -> Error
+ */
+int8_t bmm150_get_op_mode(uint8_t *op_mode, const struct bmm150_dev *dev);
+
+/*!
+ * @brief This API is used to set the preset mode of the sensor.
+ *
+ * @param[in] dev       : Structure instance of bmm150_dev.
+ *
+ *   dev->settings.preset_mode |  Preset mode
+ *  ---------------------------|----------------------------------
+ *   0x01                      |   BMM150_PRESETMODE_LOWPOWER
+ *   0x02                      |   BMM150_PRESETMODE_REGULAR
+ *   0x03                      |   BMM150_PRESETMODE_HIGHACCURACY
+ *   0x04                      |   BMM150_PRESETMODE_ENHANCED
+ *
+ * @return Result of API execution status
+ * @retval zero -> Success / +ve value -> Warning / -ve value -> Error
+ */
+int8_t bmm150_set_presetmode(struct bmm150_dev *dev);
+
+/*!
+ * @brief This API sets the sensor settings based on the desired_settings
+ * and the dev structure configuration
+ *
+ * @param[in] desired_settings  : Selection macro for selecting the setting.
+ * @param[in] dev               : Structure instance of bmm150_dev.
+ *
+ * @note Assign the sensor setting macros (multiple macros can be
+ * set by doing a bitwise-OR operation) to the desired_settings parameter
+ * of this API to perform the corresponding setting.
+ *
+ * @note threshold interrupt for each axes are set by using bitwise AND
+ * operation of the following macros
+ *  - BMM150_THRESHOLD_X
+ *  - BMM150_THRESHOLD_Y
+ *  - BMM150_THRESHOLD_Z
+ *
+ *   desired_settings  |  Selected sensor setting macros
+ *  -------------------|--------------------------------
+ *   0x0001            |  BMM150_DATA_RATE_SEL
+ *   0x0002            |  BMM150_CONTROL_MEASURE_SEL
+ *   0x0004            |  BMM150_XY_REP_SEL
+ *   0x0008            |  BMM150_Z_REP_SEL
+ *   0x0010            |  BMM150_DRDY_PIN_EN_SEL
+ *   0x0020            |  BMM150_INT_PIN_EN_SEL
+ *   0x0040            |  BMM150_DRDY_POLARITY_SEL
+ *   0x0080            |  BMM150_INT_LATCH_SEL
+ *   0x0100            |  BMM150_INT_POLARITY_SEL
+ *   0x0200            |  BMM150_DATA_OVERRUN_INT_SEL
+ *   0x0400            |  BMM150_OVERFLOW_INT_SEL
+ *   0x0800            |  BMM150_HIGH_THRESHOLD_INT_SEL
+ *   0x1000            |  BMM150_LOW_THRESHOLD_INT_SEL
+ *   0x2000            |  BMM150_LOW_THRESHOLD_SETTING_SEL
+ *   0x4000            |  BMM150_HIGH_THRESHOLD_SETTING_SEL
+ *
+ * @return Result of API execution status
+ * @retval zero -> Success / +ve value -> Warning / -ve value -> Error
+ */
+int8_t bmm150_set_sensor_settings(uint16_t desired_settings, const struct bmm150_dev *dev);
+
+/*!
+ * @brief This API gets all the sensor settings and updates the dev structure
+ *
+ * @param[in] dev       : Structure instance of bmm150_dev.
+ *
+ * @return Result of API execution status
+ * @retval zero -> Success / +ve value -> Warning / -ve value -> Error
+ */
+int8_t bmm150_get_sensor_settings(struct bmm150_dev *dev);
+
+/*!
+ * @brief This API reads the magnetometer data from registers 0x42 to 0x49
+ * and updates the dev structure with compensated mag data in micro-tesla
+ * if using floating point, and 16 * micro-teslas if using int16_t for the data
+ *
+ * @param[in,out] dev     :   Structure instance of bmm150_dev.
+ *
+ * @note The output mag data can be obtained either in int16_t or float format
+ *       using this API.
+ * @note Enable the macro "BMM150_USE_FLOATING_POINT" in the bmm150_defs.h
+ *       file and call this API to get the mag data in float,
+ *       disable this macro to get the mag data in int16_t format
+ *
+ *  Mag data output(micro-tesla)   |  Mag data in dev structure(int16_t/float)
+ * --------------------------------|------------------------------------------
+ *   X-axis data                   | dev->data.x
+ *   Y-axis data                   | dev->data.y
+ *   Z-axis data                   | dev->data.z
+ *
+ * @return Result of API execution status
+ * @retval zero -> Success / +ve value -> Warning / -ve value -> Error
+ */
+int8_t bmm150_read_mag_data(struct bmm150_dev *dev);
+
+/*!
+ * @brief This API is used to perform the complete self test
+ * (both normal and advanced) for the BMM150 sensor
+ *
+ * @param[in] self_test_mode    : The type of self test to be performed
+ * @param[in] dev               : Structure instance of bmm150_dev.
+ *
+ *   self_test_mode    |  Self test enabled
+ * --------------------|--------------------------
+ *      0              | BMM150_NORMAL_SELF_TEST
+ *      1              | BMM150_ADVANCED_SELF_TEST
+ *
+ * @note The return value of this API gives us the result of self test.
+ *
+ * @note Performing advanced self test does soft reset of the sensor, User can
+ * set the desired settings after performing the advanced self test.
+ *
+ * @return Result of API execution status and self test result.
+ * @retval zero -> Success / +ve value -> Warning / -ve value -> Error
+ *
+ *   Return value      | Result of self test
+ * --------------------|---------------------------------
+ *      0              | BMM150_OK
+ *      1              | BMM150_W_NORMAL_SELF_TEST_YZ_FAIL
+ *      2              | BMM150_W_NORMAL_SELF_TEST_XZ_FAIL
+ *      3              | BMM150_W_NORMAL_SELF_TEST_Z_FAIL
+ *      4              | BMM150_W_NORMAL_SELF_TEST_XY_FAIL
+ *      5              | BMM150_W_NORMAL_SELF_TEST_Y_FAIL
+ *      6              | BMM150_W_NORMAL_SELF_TEST_X_FAIL
+ *      7              | BMM150_W_NORMAL_SELF_TEST_XYZ_FAIL
+ *      8              | BMM150_W_ADV_SELF_TEST_FAIL
+ *
+ */
+int8_t bmm150_perform_self_test(uint8_t self_test_mode, struct bmm150_dev *dev);
+
+/*!
+ * @brief This API obtains the status flags of all interrupt
+ * which is used to check for the assertion of interrupts
+ *
+ * @param[in,out] dev       : Structure instance of bmm150_dev.
+ *
+ * @note The status flags of all the interrupts are stored in the
+ * dev->int_status.
+ *
+ * @note The value of dev->int_status is performed a bitwise AND operation
+ * with predefined interrupt status macros to find the interrupt status
+ * which is either set or reset.
+ *
+ * Ex.
+ * if (dev->int_status & BMM150_DATA_READY_INT)
+ * {
+ *	Occurrence of data ready interrupt
+ * } else {
+ *	No interrupt occurred
+ * }
+ *
+ * @return Result of API execution status and self test result.
+ * @retval zero -> Success / +ve value -> Warning / -ve value -> Error
+ */
+int8_t bmm150_get_interrupt_status(struct bmm150_dev *dev);
+/*!
+ * @brief This API is used to compensate the raw mag data
+ *
+ * @param[in] aux_data      : Raw mag data obtained from BMI160 registers
+ * @param[in,out] dev       : Structure instance of bmm150_dev.
+ *
+ * @return Result of API execution status and self test result.
+ * @retval zero -> Success / +ve value -> Warning / -ve value -> Error
+ */
+int8_t bmm150_aux_mag_data(uint8_t *aux_data, struct bmm150_dev *dev);
+
+#ifdef __cplusplus
+}
+#endif /* End of CPP guard */
+
+#endif /* BMM150_H__ */
+/** @}*/

lib/bmm150/bmm150_defs.h

@@ -0,0 +1,568 @@
+/**
+ * Copyright (C) 2015 - 2016 Bosch Sensortec GmbH
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * Neither the name of the copyright holder nor the names of the
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER
+ * OR CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
+ * OR CONSEQUENTIAL DAMAGES(INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE
+ *
+ * The information provided is believed to be accurate and reliable.
+ * The copyright holder assumes no responsibility
+ * for the consequences of use
+ * of such information nor for any infringement of patents or
+ * other rights of third parties which may result from its use.
+ * No license is granted by implication or otherwise under any patent or
+ * patent rights of the copyright holder.
+ *
+ * @file	bmm150_defs.h
+ * @date	12 Sep 2017
+ * @version	1.0.0
+ *
+ */
+
+/*! \file bmm150_defs.h */
+/*!
+ * @defgroup BMM150 SENSOR API
+ * @brief
+ * @{*/
+#ifndef BMM150_DEFS_H_
+#define BMM150_DEFS_H_
+
+/*********************************************************************/
+/**\ header files */
+#ifdef __KERNEL__
+#include <linux/types.h>
+#else
+#include <stdint.h>
+#include <stddef.h>
+#endif
+
+/*********************************************************************/
+/* macro definitions */
+#ifdef __KERNEL__
+#if (LONG_MAX) > 0x7fffffff
+#define __have_long64	1
+#elif (LONG_MAX) == 0x7fffffff
+#define __have_long32	1
+#endif
+#endif
+
+#if !defined(UINT8_C)
+#define INT8_C(x)       x
+#if (INT_MAX) > 0x7f
+#define UINT8_C(x)      x
+#else
+#define UINT8_C(x)      x##U
+#endif
+#endif
+
+#if !defined(UINT16_C)
+#define INT16_C(x)      x
+#if (INT_MAX) > 0x7fff
+#define UINT16_C(x)     x
+#else
+#define UINT16_C(x)     x##U
+#endif
+#endif
+
+#if !defined(INT32_C) && !defined(UINT32_C)
+#if __have_long32
+#define INT32_C(x)      x##L
+#define UINT32_C(x)     x##UL
+#else
+#define INT32_C(x)      x
+#define UINT32_C(x)     x##U
+#endif
+#endif
+
+#if !defined(INT64_C) && !defined(UINT64_C)
+#if __have_long64
+#define INT64_C(x)      x##L
+#define UINT64_C(x)     x##UL
+#else
+#define INT64_C(x)      x##LL
+#define UINT64_C(x)     x##ULL
+#endif
+#endif
+
+/**\name C standard macros */
+#ifndef NULL
+#ifdef __cplusplus
+#define NULL   0
+#else
+#define NULL   ((void *) 0)
+#endif
+#endif
+
+#define TRUE           UINT8_C(1)
+#define FALSE          UINT8_C(0)
+
+/********************************************************/
+/**\name Compiler switch macros */
+/**\name Comment the below line to use fixed-point compensation and
+un-comment it to obtain the output data in float */
+#define BMM150_USE_FLOATING_POINT
+
+/********************************************************/
+/**\name Macro definitions */
+
+/**\name API success code */
+#define BMM150_OK	INT8_C(0)
+
+/**\name API error codes */
+#define BMM150_E_NULL_PTR		INT8_C(-1)
+#define BMM150_E_DEV_NOT_FOUND		INT8_C(-2)
+#define BMM150_E_INVALID_CONFIG		INT8_C(-3)
+
+/**\name API warning codes */
+#define BMM150_W_NORMAL_SELF_TEST_YZ_FAIL	INT8_C(1)
+#define BMM150_W_NORMAL_SELF_TEST_XZ_FAIL	INT8_C(2)
+#define BMM150_W_NORMAL_SELF_TEST_Z_FAIL	INT8_C(3)
+#define BMM150_W_NORMAL_SELF_TEST_XY_FAIL	INT8_C(4)
+#define BMM150_W_NORMAL_SELF_TEST_Y_FAIL	INT8_C(5)
+#define BMM150_W_NORMAL_SELF_TEST_X_FAIL	INT8_C(6)
+#define BMM150_W_NORMAL_SELF_TEST_XYZ_FAIL	INT8_C(7)
+#define BMM150_W_ADV_SELF_TEST_FAIL		INT8_C(8)
+
+/**\name CHIP ID & SOFT RESET VALUES      */
+#define BMM150_CHIP_ID                  UINT8_C(0x32)
+#define BMM150_SET_SOFT_RESET		UINT8_C(0x82)
+
+/**\name POWER MODE DEFINTIONS      */
+#define BMM150_NORMAL_MODE		UINT8_C(0x00)
+#define BMM150_FORCED_MODE		UINT8_C(0x01)
+#define BMM150_SLEEP_MODE		UINT8_C(0x03)
+#define BMM150_SUSPEND_MODE		UINT8_C(0x04)
+
+/**\name I2C ADDRESS       */
+#define BMM150_DEFAULT_I2C_ADDRESS		UINT8_C(0x10)
+#define BMM150_I2C_ADDRESS_CSB_LOW_SDO_HIGH	UINT8_C(0x11)
+#define BMM150_I2C_ADDRESS_CSB_HIGH_SDO_LOW	UINT8_C(0x12)
+#define BMM150_I2C_ADDRESS_CSB_HIGH_SDO_HIGH	UINT8_C(0x13)
+
+/**\name Power mode settings  */
+#define	BMM150_POWER_CNTRL_DISABLE	UINT8_C(0x00)
+#define	BMM150_POWER_CNTRL_ENABLE	UINT8_C(0x01)
+
+/**\name Sensor delay time settings  */
+#define BMM150_SOFT_RESET_DELAY		UINT8_C(1)
+#define BMM150_NORMAL_SELF_TEST_DELAY	UINT8_C(2)
+#define BMM150_START_UP_TIME		UINT8_C(3)
+#define BMM150_ADV_SELF_TEST_DELAY	UINT8_C(4)
+
+/**\name ENABLE/DISABLE DEFINITIONS  */
+#define BMM150_XY_CHANNEL_ENABLE	UINT8_C(0x00)
+#define BMM150_XY_CHANNEL_DISABLE	UINT8_C(0x03)
+
+/**\name Register Address */
+#define BMM150_CHIP_ID_ADDR		UINT8_C(0x40)
+#define BMM150_DATA_X_LSB		UINT8_C(0x42)
+#define BMM150_DATA_READY_STATUS	UINT8_C(0x48)
+#define BMM150_INTERRUPT_STATUS		UINT8_C(0x4A)
+#define BMM150_POWER_CONTROL_ADDR	UINT8_C(0x4B)
+#define BMM150_OP_MODE_ADDR		UINT8_C(0x4C)
+#define BMM150_INT_CONFIG_ADDR		UINT8_C(0x4D)
+#define BMM150_AXES_ENABLE_ADDR		UINT8_C(0x4E)
+#define BMM150_LOW_THRESHOLD_ADDR	UINT8_C(0x4F)
+#define BMM150_HIGH_THRESHOLD_ADDR	UINT8_C(0x50)
+#define BMM150_REP_XY_ADDR		UINT8_C(0x51)
+#define BMM150_REP_Z_ADDR		UINT8_C(0x52)
+
+/**\name Macros to select the sensor settings to be set by the user
+   These values are internal for API implementation. Don't relate this to
+   data sheet. */
+#define BMM150_DATA_RATE_SEL			UINT16_C(1)
+#define BMM150_CONTROL_MEASURE_SEL		UINT16_C(1 << 1)
+#define BMM150_XY_REP_SEL			UINT16_C(1 << 2)
+#define BMM150_Z_REP_SEL			UINT16_C(1 << 3)
+#define BMM150_DRDY_PIN_EN_SEL			UINT16_C(1 << 4)
+#define BMM150_INT_PIN_EN_SEL			UINT16_C(1 << 5)
+#define BMM150_DRDY_POLARITY_SEL		UINT16_C(1 << 6)
+#define BMM150_INT_LATCH_SEL			UINT16_C(1 << 7)
+#define BMM150_INT_POLARITY_SEL			UINT16_C(1 << 8)
+#define BMM150_DATA_OVERRUN_INT_SEL		UINT16_C(1 << 9)
+#define BMM150_OVERFLOW_INT_SEL			UINT16_C(1 << 10)
+#define BMM150_HIGH_THRESHOLD_INT_SEL		UINT16_C(1 << 11)
+#define BMM150_LOW_THRESHOLD_INT_SEL		UINT16_C(1 << 12)
+#define BMM150_LOW_THRESHOLD_SETTING_SEL	UINT16_C(1 << 13)
+#define BMM150_HIGH_THRESHOLD_SETTING_SEL	UINT16_C(1 << 14)
+
+/**\name DATA RATE DEFINITIONS  */
+#define BMM150_DATA_RATE_10HZ        UINT8_C(0x00)
+#define BMM150_DATA_RATE_02HZ        UINT8_C(0x01)
+#define BMM150_DATA_RATE_06HZ        UINT8_C(0x02)
+#define BMM150_DATA_RATE_08HZ        UINT8_C(0x03)
+#define BMM150_DATA_RATE_15HZ        UINT8_C(0x04)
+#define BMM150_DATA_RATE_20HZ        UINT8_C(0x05)
+#define BMM150_DATA_RATE_25HZ        UINT8_C(0x06)
+#define BMM150_DATA_RATE_30HZ        UINT8_C(0x07)
+
+/**\name TRIM REGISTERS      */
+/* Trim Extended Registers */
+#define BMM150_DIG_X1               UINT8_C(0x5D)
+#define BMM150_DIG_Y1               UINT8_C(0x5E)
+#define BMM150_DIG_Z4_LSB           UINT8_C(0x62)
+#define BMM150_DIG_Z4_MSB           UINT8_C(0x63)
+#define BMM150_DIG_X2               UINT8_C(0x64)
+#define BMM150_DIG_Y2               UINT8_C(0x65)
+#define BMM150_DIG_Z2_LSB           UINT8_C(0x68)
+#define BMM150_DIG_Z2_MSB           UINT8_C(0x69)
+#define BMM150_DIG_Z1_LSB           UINT8_C(0x6A)
+#define BMM150_DIG_Z1_MSB           UINT8_C(0x6B)
+#define BMM150_DIG_XYZ1_LSB         UINT8_C(0x6C)
+#define BMM150_DIG_XYZ1_MSB         UINT8_C(0x6D)
+#define BMM150_DIG_Z3_LSB           UINT8_C(0x6E)
+#define BMM150_DIG_Z3_MSB           UINT8_C(0x6F)
+#define BMM150_DIG_XY2              UINT8_C(0x70)
+#define BMM150_DIG_XY1              UINT8_C(0x71)
+
+/**\name Threshold interrupt setting macros for x,y,z axes selection */
+#define BMM150_THRESHOLD_X	UINT8_C(0x06)
+#define BMM150_THRESHOLD_Y	UINT8_C(0x05)
+#define BMM150_THRESHOLD_Z	UINT8_C(0x03)
+
+/**\name User configurable interrupt setting macros */
+#define BMM150_INT_ENABLE		UINT8_C(0x01)
+#define BMM150_INT_DISABLE		UINT8_C(0x00)
+#define BMM150_ACTIVE_HIGH_POLARITY	UINT8_C(0x01)
+#define BMM150_ACTIVE_LOW_POLARITY	UINT8_C(0x00)
+#define BMM150_LATCHED			UINT8_C(0x01)
+#define BMM150_NON_LATCHED		UINT8_C(0x00)
+
+/**\name Interrupt status */
+#define BMM150_LOW_THRESHOLD_INT_X	UINT16_C(0x0001)
+#define BMM150_LOW_THRESHOLD_INT_Y	UINT16_C(0x0002)
+#define BMM150_LOW_THRESHOLD_INT_Z	UINT16_C(0x0004)
+#define BMM150_HIGH_THRESHOLD_INT_X	UINT16_C(0x0008)
+#define BMM150_HIGH_THRESHOLD_INT_Y	UINT16_C(0x0010)
+#define BMM150_HIGH_THRESHOLD_INT_Z	UINT16_C(0x0020)
+#define BMM150_DATA_OVERFLOW_INT	UINT16_C(0x0040)
+#define BMM150_DATA_OVERRUN_INT		UINT16_C(0x0080)
+#define BMM150_DATA_READY_INT		UINT16_C(0x0100)
+
+/**\name Macros for bit masking */
+#define	BMM150_PWR_CNTRL_MSK		UINT8_C(0x01)
+
+#define	BMM150_CONTROL_MEASURE_MSK	UINT8_C(0x38)
+#define	BMM150_CONTROL_MEASURE_POS	UINT8_C(0x03)
+
+#define BMM150_POWER_CONTROL_BIT_MSK	UINT8_C(0x01)
+#define BMM150_POWER_CONTROL_BIT_POS	UINT8_C(0x00)
+
+#define BMM150_OP_MODE_MSK		UINT8_C(0x06)
+#define BMM150_OP_MODE_POS		UINT8_C(0x01)
+
+#define BMM150_ODR_MSK			UINT8_C(0x38)
+#define BMM150_ODR_POS			UINT8_C(0x03)
+
+#define BMM150_DATA_X_MSK		UINT8_C(0xF8)
+#define BMM150_DATA_X_POS		UINT8_C(0x03)
+
+#define BMM150_DATA_Y_MSK		UINT8_C(0xF8)
+#define BMM150_DATA_Y_POS		UINT8_C(0x03)
+
+#define BMM150_DATA_Z_MSK		UINT8_C(0xFE)
+#define BMM150_DATA_Z_POS		UINT8_C(0x01)
+
+#define BMM150_DATA_RHALL_MSK		UINT8_C(0xFC)
+#define BMM150_DATA_RHALL_POS		UINT8_C(0x02)
+
+#define	BMM150_SELF_TEST_MSK		UINT8_C(0x01)
+
+#define	BMM150_ADV_SELF_TEST_MSK	UINT8_C(0xC0)
+#define	BMM150_ADV_SELF_TEST_POS	UINT8_C(0x06)
+
+#define	BMM150_DRDY_EN_MSK		UINT8_C(0x80)
+#define	BMM150_DRDY_EN_POS		UINT8_C(0x07)
+
+#define	BMM150_INT_PIN_EN_MSK		UINT8_C(0x40)
+#define	BMM150_INT_PIN_EN_POS		UINT8_C(0x06)
+
+#define	BMM150_DRDY_POLARITY_MSK	UINT8_C(0x04)
+#define	BMM150_DRDY_POLARITY_POS	UINT8_C(0x02)
+
+#define	BMM150_INT_LATCH_MSK		UINT8_C(0x02)
+#define	BMM150_INT_LATCH_POS		UINT8_C(0x01)
+
+#define	BMM150_INT_POLARITY_MSK		UINT8_C(0x01)
+
+#define	BMM150_DATA_OVERRUN_INT_MSK	UINT8_C(0x80)
+#define	BMM150_DATA_OVERRUN_INT_POS	UINT8_C(0x07)
+
+#define	BMM150_OVERFLOW_INT_MSK		UINT8_C(0x40)
+#define	BMM150_OVERFLOW_INT_POS		UINT8_C(0x06)
+
+#define	BMM150_HIGH_THRESHOLD_INT_MSK	UINT8_C(0x38)
+#define	BMM150_HIGH_THRESHOLD_INT_POS	UINT8_C(0x03)
+
+#define	BMM150_LOW_THRESHOLD_INT_MSK	UINT8_C(0x07)
+
+#define	BMM150_DRDY_STATUS_MSK		UINT8_C(0x01)
+
+
+/**\name OVERFLOW DEFINITIONS  */
+#define BMM150_XYAXES_FLIP_OVERFLOW_ADCVAL	INT16_C(-4096)
+#define BMM150_ZAXIS_HALL_OVERFLOW_ADCVAL	INT16_C(-16384)
+#define BMM150_OVERFLOW_OUTPUT			INT16_C(-32768)
+#define BMM150_NEGATIVE_SATURATION_Z            INT16_C(-32767)
+#define BMM150_POSITIVE_SATURATION_Z            UINT16_C(32767)
+#ifdef BMM150_USE_FLOATING_POINT
+#define BMM150_OVERFLOW_OUTPUT_FLOAT		0.0f
+#endif
+
+/**\name PRESET MODE DEFINITIONS  */
+#define BMM150_PRESETMODE_LOWPOWER                 UINT8_C(0x01)
+#define BMM150_PRESETMODE_REGULAR                  UINT8_C(0x02)
+#define BMM150_PRESETMODE_HIGHACCURACY             UINT8_C(0x03)
+#define BMM150_PRESETMODE_ENHANCED                 UINT8_C(0x04)
+
+/**\name PRESET MODES - REPETITIONS-XY RATES */
+#define BMM150_LOWPOWER_REPXY                    UINT8_C(1)
+#define BMM150_REGULAR_REPXY                     UINT8_C(4)
+#define BMM150_ENHANCED_REPXY                    UINT8_C(7)
+#define BMM150_HIGHACCURACY_REPXY                UINT8_C(23)
+
+/**\name PRESET MODES - REPETITIONS-Z RATES */
+#define BMM150_LOWPOWER_REPZ                     UINT8_C(2)
+#define BMM150_REGULAR_REPZ                      UINT8_C(14)
+#define BMM150_ENHANCED_REPZ                     UINT8_C(26)
+#define BMM150_HIGHACCURACY_REPZ                 UINT8_C(82)
+
+/**\name Register read lengths	*/
+#define BMM150_SELF_TEST_LEN			UINT8_C(5)
+#define BMM150_SETTING_DATA_LEN			UINT8_C(8)
+#define BMM150_XYZR_DATA_LEN			UINT8_C(8)
+
+/**\name Self test selection macros */
+#define BMM150_NORMAL_SELF_TEST			UINT8_C(0)
+#define BMM150_ADVANCED_SELF_TEST		UINT8_C(1)
+
+/**\name Self test settings */
+#define BMM150_DISABLE_XY_AXIS			UINT8_C(0x03)
+#define BMM150_SELF_TEST_REP_Z			UINT8_C(0x04)
+
+/**\name Advanced self-test current settings */
+#define BMM150_DISABLE_SELF_TEST_CURRENT	UINT8_C(0x00)
+#define BMM150_ENABLE_NEGATIVE_CURRENT		UINT8_C(0x02)
+#define BMM150_ENABLE_POSITIVE_CURRENT		UINT8_C(0x03)
+
+/**\name Normal self-test status */
+#define BMM150_SELF_TEST_STATUS_XYZ_FAIL	UINT8_C(0x00)
+#define BMM150_SELF_TEST_STATUS_SUCCESS		UINT8_C(0x07)
+
+
+/**\name Macro to SET and GET BITS of a register*/
+#define BMM150_SET_BITS(reg_data, bitname, data) \
+				((reg_data & ~(bitname##_MSK)) | \
+				((data << bitname##_POS) & bitname##_MSK))
+
+#define BMM150_GET_BITS(reg_data, bitname)  ((reg_data & (bitname##_MSK)) >> \
+							(bitname##_POS))
+
+#define BMM150_SET_BITS_POS_0(reg_data, bitname, data) \
+				((reg_data & ~(bitname##_MSK)) | \
+				(data & bitname##_MSK))
+
+#define BMM150_GET_BITS_POS_0(reg_data, bitname)  (reg_data & (bitname##_MSK))
+
+/********************************************************/
+
+/*!
+ * @brief Interface selection Enums
+ */
+enum bmm150_intf {
+	/*! SPI interface */
+	BMM150_SPI_INTF,
+	/*! I2C interface */
+	BMM150_I2C_INTF
+};
+
+/********************************************************/
+/**\name	TYPE DEFINITIONS */
+/*!
+ * @brief Bus communication function pointer which should be mapped to
+ * the platform specific read and write functions of the user
+ */
+typedef int8_t (*bmm150_com_fptr_t)(uint8_t dev_id, uint8_t reg_addr, uint8_t *read_data, uint16_t len);
+
+/*!	delay function pointer */
+typedef void (*bmm150_delay_fptr_t)(uint32_t);
+
+/********************************************************/
+/**\name	STRUCTURE DEFINITIONS*/
+
+/*!
+ * @brief bmm150 trim data structure
+ */
+struct bmm150_trim_registers {
+	/*! trim x1 data */
+	int8_t dig_x1;
+	/*! trim y1 data */
+	int8_t dig_y1;
+	/*! trim x2 data */
+	int8_t dig_x2;
+	/*! trim y2 data */
+	int8_t dig_y2;
+	/*! trim z1 data */
+	uint16_t dig_z1;
+	/*! trim z2 data */
+	int16_t dig_z2;
+	/*! trim z3 data */
+	int16_t dig_z3;
+	/*! trim z4 data */
+	int16_t dig_z4;
+	/*! trim xy1 data */
+	uint8_t dig_xy1;
+	/*! trim xy2 data */
+	int8_t dig_xy2;
+	/*! trim xyz1 data */
+	uint16_t dig_xyz1;
+};
+
+/*!
+ * @brief bmm150 interrupt pin settings
+ */
+struct bmm150_int_ctrl_settings {
+	/*! Data ready interrupt enable */
+	uint8_t drdy_pin_en;
+	/*! Threshold and overflow interrupts enable */
+	uint8_t int_pin_en;
+	/*! Data ready interrupt polarity Active high/low */
+	uint8_t drdy_polarity;
+	/*! Interrupt pin - Latched or Non-latched */
+	uint8_t int_latch;
+	/*! Interrupt polarity Active high/low */
+	uint8_t int_polarity;
+	/*! Data overrun interrupt enable */
+	uint8_t data_overrun_en;
+	/*! Overflow interrupt enable */
+	uint8_t overflow_int_en;
+	/*! high interrupt enable/disable axis selection */
+	uint8_t high_int_en;
+	/*! low interrupt enable/disable axis selection */
+	uint8_t low_int_en;
+	/*! low threshold limit */
+	uint8_t low_threshold;
+	/*! high threshold limit */
+	uint8_t high_threshold;
+};
+
+/*!
+ * @brief bmm150 sensor settings
+ */
+struct bmm150_settings {
+	/*! Control measurement of XYZ axes */
+	uint8_t xyz_axes_control;
+	/*! Power control bit value */
+	uint8_t pwr_cntrl_bit;
+	/*! Power control bit value */
+	uint8_t pwr_mode;
+	/*! Data rate value (ODR) */
+	uint8_t data_rate;
+	/*! XY Repetitions */
+	uint8_t xy_rep;
+	/*! Z Repetitions */
+	uint8_t z_rep;
+	/*! Preset mode of sensor */
+	uint8_t preset_mode;
+	/*! Interrupt configuration settings */
+	struct bmm150_int_ctrl_settings int_settings;
+};
+
+/*!
+ * @brief bmm150 un-compensated (raw) magnetometer data
+ */
+struct bmm150_raw_mag_data {
+	/*! Raw mag X data */
+	int16_t raw_datax;
+	/*! Raw mag Y data */
+	int16_t raw_datay;
+	/*! Raw mag Z data */
+	int16_t raw_dataz;
+	/*! Raw mag resistance value */
+	uint16_t raw_data_r;
+};
+
+#ifdef BMM150_USE_FLOATING_POINT
+/*!
+ * @brief bmm150 compensated magnetometer data in float
+ */
+struct bmm150_mag_data {
+	/*! compensated mag X data */
+	float x;
+	/*! compensated mag Y data */
+	float y;
+	/*! compensated mag Z data */
+	float z;
+};
+
+#else
+
+/*!
+ * @brief bmm150 compensated magnetometer data in int16_t format
+ */
+struct bmm150_mag_data {
+	/*! compensated mag X data */
+	int16_t x;
+	/*! compensated mag Y data */
+	int16_t y;
+	/*! compensated mag Z data */
+	int16_t z;
+};
+
+#endif
+
+/*!
+ * @brief bmm150 device structure
+ */
+struct bmm150_dev {
+	/*! Chip Id */
+	uint8_t chip_id;
+	/*! Device Id */
+	uint8_t dev_id;
+	/*! SPI/I2C Interface */
+	enum bmm150_intf intf;
+	/*! Bus read function pointer */
+	bmm150_com_fptr_t read;
+	/*! Bus write function pointer */
+	bmm150_com_fptr_t write;
+	/*! delay(in ms) function pointer */
+	bmm150_delay_fptr_t delay_ms;
+	/*! Trim registers */
+	struct bmm150_trim_registers trim_data;
+	/*! Sensor settings */
+	struct bmm150_settings settings;
+	/*! Interrupt status */
+	uint16_t int_status;
+	/*! Structure containing mag data */
+	struct bmm150_mag_data data;
+};
+
+#endif /* BMM150_DEFS_H_ */
+/** @}*/
+/** @}*/

src/main.cpp

@@ -7,22 +7,58 @@ OSC configuration custom page
 
 */
 
+/*  TODO
+- auto change osc target to latest message source (simplify configuration)
+- rework UI with graphical views
+- sensor management
+- sensor calibration
+- smoothing | passage 180-> -180
+
+*/
+
 #include "wifimgr.h"
 #include "oscmgr.h"
 
-///////////////////////      TFT   ////////////////////////////////////
-
-///////////////////////   SENSOR   ////////////////////////////////////
 // #define M5STACK_MPU6886 
 #define M5STACK_MPU9250 
 // #define M5STACK_MPU6050
 // #define M5STACK_200Q
 
 #include <M5Stack.h>
+#include <BMM150class.h>
+#include <utility/quaternionFilters.h>
+
+#include <Smoothed.h>
+
+///////////////////////   SENSOR   ////////////////////////////////////
 
-int sensor_rate = 50 ;
+Smoothed <float> yaw_smooth;
+Smoothed <float> pitch_smooth;
+Smoothed <float> roll_smooth;
+
+int sensor_rate = 10 ;
 long int last_sent ;
 
+// float accX = 0.0F;
+// float accY = 0.0F;
+// float accZ = 0.0F;
+
+// float gyroX = 0.0F;
+// float gyroY = 0.0F;
+// float gyroZ = 0.0F;
+
+// float pitch = 0.0F;
+// float roll  = 0.0F;
+// float yaw   = 0.0F;
+
+// float temp = 0.0F;
+
+#define MADGWICK
+//#define MAHONY
+
+BMM150class bmm150;
+//TaskHandle_t lcdTaskHandle = NULL;
+
 float accX = 0.0F;
 float accY = 0.0F;
 float accZ = 0.0F;
@@ -31,12 +67,132 @@ float gyroX = 0.0F;
 float gyroY = 0.0F;
 float gyroZ = 0.0F;
 
+int AVERAGENUM_GY = 256;
+float init_gyroX = 0.0F;
+float init_gyroY = 0.0F;
+float init_gyroZ = 0.0F;
+
+float magnetX = 0.0F;
+float magnetY = 0.0F;
+float magnetZ = 0.0F;
+
+//for hard iron correction
+float magoffsetX = 0.0F;
+float magoffsetY = 0.0F;
+float magoffsetZ = 0.0F;
+
+//for soft iron correction
+float magscaleX = 0.0F;
+float magscaleY = 0.0F;
+float magscaleZ = 0.0F;
+
 float pitch = 0.0F;
-float roll  = 0.0F;
-float yaw   = 0.0F;
+float roll = 0.0F;
+float yaw = 0.0F;
+
+float pitch_ahrs = 0.0F;
+float roll_ahrs = 0.0F;
+float yaw_ahrs = 0.0F;
 
 float temp = 0.0F;
 
+uint32_t Now = 0;
+uint32_t lastUpdate = 0;
+float deltat = 0.0f;
+
+//moved to global
+float magnetX1, magnetY1, magnetZ1;
+float head_dir;
+
+void initGyro() {
+  M5.Lcd.fillScreen(BLACK);
+  M5.Lcd.setTextColor(WHITE);
+  M5.Lcd.setTextSize(1);
+  M5.Lcd.setCursor(0, 0);
+  M5.Lcd.print("begin gyro calibration");
+
+  for (int i = 0;i < AVERAGENUM_GY;i++) {
+    M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
+    init_gyroX += gyroX;
+    init_gyroY += gyroY;
+    init_gyroZ += gyroZ;
+    delay(5);
+  }
+  init_gyroX /= AVERAGENUM_GY;
+  init_gyroY /= AVERAGENUM_GY;
+  init_gyroZ /= AVERAGENUM_GY;
+}
+
+void calibGyro(){
+  M5.Lcd.fillScreen(BLACK);
+  M5.Lcd.setTextColor(WHITE);
+  M5.Lcd.setTextSize(1);
+  M5.Lcd.setCursor(0, 0);
+  M5.Lcd.print("begin calibration in 3 seconds");
+  delay(3000);
+  M5.Lcd.setCursor(0, 10);
+  M5.Lcd.print("Flip + rotate core calibration");
+  bmm150.bmm150_calibrate(15000);
+  delay(100);
+
+  bmm150.Init();
+  bmm150.getMagnetOffset(&magoffsetX, &magoffsetY, &magoffsetZ);
+  bmm150.getMagnetScale(&magscaleX, &magscaleY, &magscaleZ);
+
+  M5.Lcd.fillScreen(BLACK);
+  M5.Lcd.setTextColor(GREEN, BLACK);
+  M5.Lcd.setTextSize(2);
+}
+
+
+///////////////////////      TFT   ////////////////////////////////////
+
+void drawNetworkInfo(int offsetX, int offsetY){
+  
+  String printString = "Connected to ";
+  printString += WiFi.SSID();
+  printString += " with IP ";
+  printString += WiFi.localIP().toString();
+  M5.Lcd.drawString(printString, offsetX, offsetY);
+  M5.Lcd.setTextWrap(true, true);
+}
+
+void drawOSCInfo(int offsetX, int offsetY){
+
+}
+
+int barWidth = 60;
+int barHeight = 10;
+int barShift = 15;
+void drawSensorInfo(int offsetX, int offsetY){
+  String printString = "";
+  //printString += gyroX + ", " + gyroY + ", " + gyroZ;
+  float temp = map(yaw, -180, 180, 0, 100);
+  M5.Lcd.fillRect(offsetX, offsetY, barWidth, barHeight,0);
+  M5.Lcd.progressBar(offsetX, offsetY, barWidth, barHeight,temp);
+  M5.Lcd.fillRect(offsetX, offsetY + barShift, barWidth, barHeight,0);
+  M5.Lcd.progressBar(offsetX, offsetY + barShift, barWidth, barHeight,30);
+  M5.Lcd.fillRect(offsetX, offsetY + 2*barShift, barWidth, barHeight,0);
+  M5.Lcd.progressBar(offsetX, offsetY + 2*barShift, barWidth, barHeight,30);
+  // M5.Lcd.printf("%6.2f  %6.2f  %6.2f      ", gyroX, gyroY, gyroZ);
+  // M5.Lcd.setCursor(220, 80);
+  // M5.Lcd.print(" o/s");
+  // M5.Lcd.setCursor(0, 120);
+  // M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", accX, accY, accZ);
+  // M5.Lcd.setCursor(220, 135);
+  // M5.Lcd.print(" G");
+  // M5.Lcd.setCursor(0, 165);
+  // M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", pitch, roll, yaw);
+  // M5.Lcd.setCursor(220, 180);
+  // M5.Lcd.print(" degree");
+  // M5.Lcd.setCursor(0, 220);
+  // M5.Lcd.printf("Temperature : %.2f C", temp);
+
+  M5.Lcd.drawString(printString, offsetX, offsetY);
+  M5.Lcd.setTextWrap(true, true); 
+}
+
+
 
 
 ///////////////////////     SETUP     ////////////////////////////////////
@@ -47,6 +203,10 @@ void setup() {
   Serial.begin(115200);
   Serial.println();
 
+  yaw_smooth.begin(SMOOTHED_AVERAGE, 100);
+  pitch_smooth.begin(SMOOTHED_EXPONENTIAL, 10);
+  roll_smooth.begin(SMOOTHED_EXPONENTIAL, 10);
+
   M5.begin();
   /*
     Power chip connected to gpio21, gpio22, I2C device
@@ -55,7 +215,14 @@ void setup() {
   */
   M5.Power.begin();
   M5.IMU.Init();
+  initGyro();
+  bmm150.Init();
+
+  bmm150.getMagnetOffset(&magoffsetX, &magoffsetY, &magoffsetZ);
+  bmm150.getMagnetScale(&magscaleX, &magscaleY, &magscaleZ);
+
   M5.Lcd.fillScreen(BLACK);
+  M5.Lcd.setTextWrap(true, true);
   M5.Lcd.setTextColor(BLUE , BLACK);
   M5.Lcd.setTextSize(1);
 
@@ -132,6 +299,66 @@ void loop() {
     idle_offset=millis();
     }
 
+  
+
+  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
+  gyroX -= init_gyroX;
+  gyroY -= init_gyroY;
+  gyroZ -= init_gyroZ;
+  M5.IMU.getAccelData(&accX,&accY,&accZ);
+  M5.IMU.getAhrsData(&pitch_ahrs,&roll_ahrs,&yaw_ahrs);
+  M5.IMU.getTempData(&temp);
+  bmm150.getMagnetData(&magnetX, &magnetY, &magnetZ);
+
+  magnetX1 = (magnetX - magoffsetX) * magscaleX;
+  magnetY1 = (magnetY - magoffsetY) * magscaleY;
+  magnetZ1 = (magnetZ - magoffsetZ) * magscaleZ;
+
+  head_dir = atan2(magnetX1, magnetY1);
+  if(head_dir < 0)
+    head_dir += 2*M_PI;
+  if(head_dir > 2*M_PI)
+    head_dir -= 2*M_PI;
+  head_dir *= RAD_TO_DEG;
+  Now = micros();
+  deltat = ((Now - lastUpdate) / 1000000.0f);//0.09
+  lastUpdate = Now;
+
+#ifdef MADGWICK
+  MadgwickQuaternionUpdate(accX, accY, accZ, gyroX * DEG_TO_RAD,
+                           gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD,
+                           -magnetX1, magnetY1, -magnetZ1, deltat);
+#endif
+
+#ifdef MAHONY
+  MahonyQuaternionUpdate(accX, accY, accZ, gyroX * DEG_TO_RAD,
+                           gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD,
+                           -magnetX1, magnetY1, -magnetZ1, deltat);
+  //delay(10); // adjust sampleFreq = 50Hz
+#endif
+
+  yaw = atan2(2.0f * (*(getQ() + 1) * *(getQ() + 2) + *getQ() *
+                                                          *(getQ() + 3)),
+              *getQ() * *getQ() + *(getQ() + 1) * *(getQ() + 1) - *(getQ() + 2) * *(getQ() + 2) - *(getQ() + 3) * *(getQ() + 3));
+  pitch = -asin(2.0f * (*(getQ() + 1) * *(getQ() + 3) - *getQ() *
+                                                            *(getQ() + 2)));
+  roll = atan2(2.0f * (*getQ() * *(getQ() + 1) + *(getQ() + 2) *
+                                                     *(getQ() + 3)),
+               *getQ() * *getQ() - *(getQ() + 1) * *(getQ() + 1) - *(getQ() + 2) * *(getQ() + 2) + *(getQ() + 3) * *(getQ() + 3));
+  yaw = -0.5*M_PI-yaw;
+  if(yaw < 0)
+    yaw += 2*M_PI;
+  if(yaw > 2*M_PI)
+    yaw -= 2*M_PI;
+  pitch *= RAD_TO_DEG;
+  yaw *= RAD_TO_DEG;
+  roll *= RAD_TO_DEG;
+
+  yaw_smooth.add(yaw);
+  pitch_smooth.add(pitch);
+  roll_smooth.add(roll);
+
+
   portal.handleClient();
   handle_OSC(); 
 
@@ -140,27 +367,30 @@ void loop() {
     last_sent = millis();
     sendOSC_3f("/gyro", gyroX, gyroY, gyroZ);
     sendOSC_3f("/acc", accX, accY, accZ);
-    sendOSC_3f("/ahrs", yaw, pitch, roll);
-    
+    sendOSC_3f("/ahrs", yaw_ahrs, pitch_ahrs, roll_ahrs);
+    sendOSC_3f("/ahrs_fixed", yaw, pitch, roll);
+    sendOSC_3f("/ahrs_smooth", yaw_smooth.get(), pitch_smooth.get(), roll_smooth.get());
   }
 
-  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
-  M5.IMU.getAccelData(&accX,&accY,&accZ);
-  M5.IMU.getAhrsData(&pitch,&roll,&yaw);
-  M5.IMU.getTempData(&temp);
-  
-  M5.Lcd.setCursor(0, 65);
-  M5.Lcd.printf("%6.2f  %6.2f  %6.2f      ", gyroX, gyroY, gyroZ);
-  M5.Lcd.setCursor(220, 80);
-  M5.Lcd.print(" o/s");
-  M5.Lcd.setCursor(0, 120);
-  M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", accX, accY, accZ);
-  M5.Lcd.setCursor(220, 135);
-  M5.Lcd.print(" G");
-  M5.Lcd.setCursor(0, 165);
-  M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", pitch, roll, yaw);
-  M5.Lcd.setCursor(220, 180);
-  M5.Lcd.print(" degree");
+  if(calib_flag){
+    calib_flag = false ;
+    calibGyro();
+    }
+
+  drawNetworkInfo(0, 65);
+  drawSensorInfo(0, 120);
+  // M5.Lcd.setCursor(0, 65);
+  // M5.Lcd.printf("%6.2f  %6.2f  %6.2f      ", gyroX, gyroY, gyroZ);
+  // M5.Lcd.setCursor(220, 80);
+  // M5.Lcd.print(" o/s");
+  // M5.Lcd.setCursor(0, 120);
+  // M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", accX, accY, accZ);
+  // M5.Lcd.setCursor(220, 135);
+  // M5.Lcd.print(" G");
+  // M5.Lcd.setCursor(0, 165);
+  // M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", pitch, roll, yaw);
+  // M5.Lcd.setCursor(220, 180);
+  // M5.Lcd.print(" degree");
   // M5.Lcd.setCursor(0, 220);
   // M5.Lcd.printf("Temperature : %.2f C", temp);
 }

src/oscmgr.cpp

@@ -37,12 +37,15 @@ void sendOSC_3f(const char * adress, float parameter, float parameter2, float pa
 }
 
 void printTest(OSCMessage &msg) {
+  Serial.print("OSC type : ");
+  Serial.println (msg.getType(0));
+  sendOSC("/received", (int) msg.getType(0));
   
-    Serial.print("OSC type : ");
-    Serial.println (msg.getType(0));
-    sendOSC("/received", (int) msg.getType(0));
-    
-  
+}
+
+bool calib_flag;
+void OSCcb_calibGyro(OSCMessage &msg){
+  calib_flag = true;
 }
 
 
@@ -83,6 +86,7 @@ void handle_OSC() {
     //Declare valid OSC messages here 
     if (!OSCin.hasError()) {
       OSCin.dispatch("/test", printTest);
+      OSCin.dispatch("/calibration",OSCcb_calibGyro);
       // if (OSCin.match(ESP_NAME)){
       //   Serial.println("matched");
       //   OSCin.dispatch("/test", printTest);

src/wifimgr.cpp

@@ -6,7 +6,7 @@ fs::SPIFFSFS& FlashFS = SPIFFS;
 
 const char hostname[] = "esp32";
 
-std::vector<String> const& config_elements = { "hostname", "hostname", "OSC_TARGET_IP", "OSC_TARGET_PORT", "OSC_INPUT_PORT"} ;
+std::vector<String> const& config_elements = { "HOSTNAME", "OSC_TARGET_IP", "OSC_TARGET_PORT", "OSC_INPUT_PORT"} ;
  
 
 static const char PAGE_CONFIG[] PROGMEM = R"(
@@ -20,13 +20,7 @@ static const char PAGE_CONFIG[] PROGMEM = R"(
       "type": "ACStyle",
       "value": "table{font-family:arial,sans-serif;border-collapse:collapse;width:100%;color:black;}td,th{border:1px solid #dddddd;text-align:center;padding:8px;}tr:nth-child(even){background-color:#dddddd;}"
     },
-    {
-      "name": "hostname",
-      "type": "ACInput",
-      "label": "Hostname",
-      "placeholder": "This area accepts hostname patterns",
-      "pattern": "^(([a-zA-Z0-9]|[a-zA-Z0-9][a-zA-Z0-9\\-]*[a-zA-Z0-9])\\.)*([A-Za-z0-9]|[A-Za-z0-9][A-Za-z0-9\\-]*[A-Za-z0-9])$"
-    },
+    
     {
       "name": "hr",
       "type": "ACElement",
@@ -34,6 +28,13 @@ static const char PAGE_CONFIG[] PROGMEM = R"(
       "posterior": "par"
     },
     {
+      "name": "HOSTNAME",
+      "type": "ACInput",
+      "label": "Hostname",
+      "placeholder": "This area accepts hostname patterns",
+      "pattern": "^(([a-zA-Z0-9]|[a-zA-Z0-9][a-zA-Z0-9\\-]*[a-zA-Z0-9])\\.)*([A-Za-z0-9]|[A-Za-z0-9][A-Za-z0-9\\-]*[A-Za-z0-9])$"
+    },
+    {
       "name": "OSC_SECTION_TEXT",
       "type": "ACText",
       "value": "OSC CONFIGURATION",