diff --git a/cmake/modules/FindMotionDriver.cmake b/cmake/modules/FindMotionDriver.cmake deleted file mode 100644 index b24511161b..0000000000 --- a/cmake/modules/FindMotionDriver.cmake +++ /dev/null @@ -1,44 +0,0 @@ -# Try to find the MotionDriver library to access the InvenSense gyros -# -# You must provide a MOTIONDRIVER_ROOT_DIR which contains lib and include directories -# -# Once done this will define -# -# MOTIONDRIVER_FOUND - system found MotionDriver -# MOTIONDRIVER_INCLUDE_DIRS - the MotionDriver include directory -# MOTIONDRIVER_LIBRARIES - Link this to use MotionDriver -# -# Created on 7/9/2013 by Andrzej Kapolka -# Copyright (c) 2013 High Fidelity -# - -if (MOTIONDRIVER_LIBRARIES AND MOTIONDRIVER_INCLUDE_DIRS) - # in cache already - set(MOTIONDRIVER_FOUND TRUE) -else (MOTIONDRIVER_LIBRARIES AND MOTIONDRIVER_INCLUDE_DIRS) - find_path(MOTIONDRIVER_INCLUDE_DIRS inv_mpu.h ${MOTIONDRIVER_ROOT_DIR}/include) - - if (APPLE) - find_library(MOTIONDRIVER_LIBRARIES libMotionDriver.a ${MOTIONDRIVER_ROOT_DIR}/lib/MacOS/) - elseif (UNIX) - find_library(MOTIONDRIVER_LIBRARIES libMotionDriver.a ${MOTIONDRIVER_ROOT_DIR}/lib/UNIX/) - endif () - - if (MOTIONDRIVER_INCLUDE_DIRS AND MOTIONDRIVER_LIBRARIES) - set(MOTIONDRIVER_FOUND TRUE) - endif (MOTIONDRIVER_INCLUDE_DIRS AND MOTIONDRIVER_LIBRARIES) - - if (MOTIONDRIVER_FOUND) - if (NOT MOTIONDRIVER_FIND_QUIETLY) - message(STATUS "Found MotionDriver: ${MOTIONDRIVER_LIBRARIES}") - endif (NOT MOTIONDRIVER_FIND_QUIETLY) - else (MOTIONDRIVER_FOUND) - if (MOTIONDRIVER_FIND_REQUIRED) - message(FATAL_ERROR "Could not find MotionDriver") - endif (MOTIONDRIVER_FIND_REQUIRED) - endif (MOTIONDRIVER_FOUND) - - # show the MOTIONDRIVER_INCLUDE_DIRS and MOTIONDRIVER_LIBRARIES variables only in the advanced view - mark_as_advanced(MOTIONDRIVER_INCLUDE_DIRS MOTIONDRIVER_LIBRARIES) - -endif (MOTIONDRIVER_LIBRARIES AND MOTIONDRIVER_INCLUDE_DIRS) diff --git a/interface/CMakeLists.txt b/interface/CMakeLists.txt index 65fd74bc98..b1e4305cbf 100644 --- a/interface/CMakeLists.txt +++ b/interface/CMakeLists.txt @@ -10,7 +10,6 @@ project(${TARGET_NAME}) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/../cmake/modules/") set(FACESHIFT_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/faceshift) set(LIBOVR_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/LibOVR) -set(MOTIONDRIVER_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/MotionDriver) set(SIXENSE_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/Sixense) if (DEFINED ENV{JOB_ID}) @@ -128,7 +127,6 @@ link_hifi_library(script-engine ${TARGET_NAME} ${ROOT_DIR}) find_package(Faceshift) find_package(GLM REQUIRED) find_package(LibOVR) -find_package(MotionDriver) find_package(Sixense) find_package(ZLIB) @@ -166,7 +164,6 @@ include_directories( SYSTEM ${FACESHIFT_INCLUDE_DIRS} ${GLM_INCLUDE_DIRS} - ${MOTIONDRIVER_INCLUDE_DIRS} ) MESSAGE("here..." ${FACESHIFT_LIBRARIES} ) @@ -174,7 +171,6 @@ MESSAGE("here..." ${FACESHIFT_LIBRARIES} ) target_link_libraries( ${TARGET_NAME} ${FACESHIFT_LIBRARIES} - ${MOTIONDRIVER_LIBRARIES} ${ZLIB_LIBRARIES} ) diff --git a/interface/external/MotionDriver/CMakeLists.txt b/interface/external/MotionDriver/CMakeLists.txt deleted file mode 100644 index 521f3f8096..0000000000 --- a/interface/external/MotionDriver/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 2.8) - -set(TARGET_NAME MotionDriver) -project(${TARGET_NAME}) - -# let the library know which device we're using -add_definitions(-DMPU9150) - -# grab the implemenation and header files -file(GLOB MOTION_DRIVER_SRCS include/*.h src/*.c) - -include_directories(include) - -add_library(${TARGET_NAME} ${MOTION_DRIVER_SRCS}) diff --git a/interface/external/MotionDriver/License.txt b/interface/external/MotionDriver/License.txt deleted file mode 100644 index ed2e2e1f24..0000000000 --- a/interface/external/MotionDriver/License.txt +++ /dev/null @@ -1,218 +0,0 @@ -SOFTWARE LICENSE AGREEMENT - -Unless you and InvenSense Corporation ("InvenSense") execute a separate written -software license agreement governing use of the accompanying software, this -software is licensed to you under the terms of this Software License -Agreement ("Agreement"). - -ANY USE, REPRODUCTION OR DISTRIBUTION OF THE SOFTWARE CONSTITUTES YOUR -ACCEPTANCE OF THIS AGREEMENT. - -1. DEFINITIONS. - -1.1. "InvenSense Product" means any of the proprietary integrated circuit -product(s) sold by InvenSense with which the Software was designed to be used, -or their successors. - -1.2. "Licensee" means you or if you are accepting on behalf of an entity -then the entity and its affiliates exercising rights under, and complying -with all of the terms of this Agreement. - -1.3. "Software" shall mean that software made available by InvenSense to -Licensee in binary code form with this Agreement. - -2. LICENSE GRANT; OWNERSHIP - -2.1. License Grants. Subject to the terms and conditions of this Agreement, -InvenSense hereby grants to Licensee a non-exclusive, non-transferable, -royalty-free license (i) to use and integrate the Software in conjunction -with any other software; and (ii) to reproduce and distribute the Software -complete, unmodified and only for use with a InvenSense Product. - -2.2. Restriction on Modification. If and to the extent that the Software is -designed to be compliant with any published communications standard -(including, without limitation, DOCSIS, HomePNA, IEEE, and ITU standards), -Licensee may not make any modifications to the Software that would cause the -Software or the accompanying InvenSense Products to be incompatible with such -standard. - -2.3. Restriction on Distribution. Licensee shall only distribute the -Software (a) under the terms of this Agreement and a copy of this Agreement -accompanies such distribution, and (b) agrees to defend and indemnify -InvenSense and its licensors from and against any damages, costs, liabilities, -settlement amounts and/or expenses (including attorneys' fees) incurred in -connection with any claim, lawsuit or action by any third party that arises -or results from the use or distribution of any and all Software by the -Licensee except as contemplated herein. - -2.4. Proprietary Notices. Licensee shall not remove, efface or obscure any -copyright or trademark notices from the Software. Licensee shall include -reproductions of the InvenSense copyright notice with each copy of the -Software, except where such Software is embedded in a manner not readily -accessible to the end user. Licensee acknowledges that any symbols, -trademarks, tradenames, and service marks adopted by InvenSense to identify the -Software belong to InvenSense and that Licensee shall have no rights therein. - -2.5. Ownership. InvenSense shall retain all right, title and interest, -including all intellectual property rights, in and to the Software. Licensee -hereby covenants that it will not assert any claim that the Software created -by or for InvenSense infringe any intellectual property right owned or -controlled by Licensee. - -2.6. No Other Rights Granted; Restrictions. Apart from the license rights -expressly set forth in this Agreement, InvenSense does not grant and Licensee -does not receive any ownership right, title or interest nor any security -interest or other interest in any intellectual property rights relating to -the Software, nor in any copy of any part of the foregoing. No license is -granted to Licensee in any human readable code of the Software (source code). -Licensee shall not (i) use, license, sell or otherwise distribute the -Software except as provided in this Agreement, (ii) attempt to reverse -engineer, decompile or disassemble any portion of the Software; or (iii) use -the Software or other material in violation of any applicable law or -regulation, including but not limited to any regulatory agency, such as FCC, -rules. - -3. NO WARRANTY OR SUPPORT - -3.1. No Warranty. THE SOFTWARE IS OFFERED "AS IS," AND INVENSENSE GRANTS AND -LICENSEE RECEIVES NO WARRANTIES OF ANY KIND, EXPRESS OR IMPLIED, BY STATUTE, -COMMUNICATION OR CONDUCT WITH LICENSEE, OR OTHERWISE. INVENSENSE SPECIFICALLY -DISCLAIMS ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A SPECIFIC -PURPOSE OR NONINFRINGEMENT CONCERNING THE SOFTWARE OR ANY UPGRADES TO OR -DOCUMENTATION FOR THE SOFTWARE. WITHOUT LIMITATION OF THE ABOVE, INVENSENSE -GRANTS NO WARRANTY THAT THE SOFTWARE IS ERROR-FREE OR WILL OPERATE WITHOUT -INTERRUPTION, AND GRANTS NO WARRANTY REGARDING ITS USE OR THE RESULTS -THEREFROM INCLUDING, WITHOUT LIMITATION, ITS CORRECTNESS, ACCURACY OR -RELIABILITY. - -3.2. No Support. Nothing in this agreement shall obligate InvenSense to -provide any support for the Software. InvenSense may, but shall be under no -obligation to, correct any defects in the Software and/or provide updates to -licensees of the Software. Licensee shall make reasonable efforts to -promptly report to InvenSense any defects it finds in the Software, as an aid -to creating improved revisions of the Software. - -3.3. Dangerous Applications. The Software is not designed, intended, or -certified for use in components of systems intended for the operation of -weapons, weapons systems, nuclear installations, means of mass -transportation, aviation, life-support computers or equipment (including -resuscitation equipment and surgical implants), pollution control, hazardous -substances management, or for any other dangerous application in which the -failure of the Software could create a situation where personal injury or -death may occur. Licensee understands that use of the Software in such -applications is fully at the risk of Licensee. - -4. TERM AND TERMINATION - -4.1. Termination. This Agreement will automatically terminate if Licensee -fails to comply with any of the terms and conditions hereof. In such event, -Licensee must destroy all copies of the Software and all of its component -parts. - -4.2. Effect Of Termination. Upon any termination of this Agreement, the -rights and licenses granted to Licensee under this Agreement shall -immediately terminate. - -4.3. Survival. The rights and obligations under this Agreement which by -their nature should survive termination will remain in effect after -expiration or termination of this Agreement. - -5. CONFIDENTIALITY - -5.1. Obligations. Licensee acknowledges and agrees that any documentation -relating to the Software, and any other information (if such other -information is identified as confidential or should be recognized as -confidential under the circumstances) provided to Licensee by InvenSense -hereunder (collectively, "Confidential Information") constitute the -confidential and proprietary information of InvenSense, and that Licensee's -protection thereof is an essential condition to Licensee's use and possession -of the Software. Licensee shall retain all Confidential Information in -strict confidence and not disclose it to any third party or use it in any way -except under a written agreement with terms and conditions at least as -protective as the terms of this Section. Licensee will exercise at least the -same amount of diligence in preserving the secrecy of the Confidential -Information as it uses in preserving the secrecy of its own most valuable -confidential information, but in no event less than reasonable diligence. -Information shall not be considered Confidential Information if and to the -extent that it: (i) was in the public domain at the time it was disclosed or -has entered the public domain through no fault of Licensee; (ii) was known to -Licensee, without restriction, at the time of disclosure as proven by the -files of Licensee in existence at the time of disclosure; or (iii) becomes -known to Licensee, without restriction, from a source other than InvenSense -without breach of this Agreement by Licensee and otherwise not in violation -of InvenSense's rights. - -5.2. Return of Confidential Information. Notwithstanding the foregoing, all -documents and other tangible objects containing or representing InvenSense -Confidential Information and all copies thereof which are in the possession -of Licensee shall be and remain the property of InvenSense, and shall be -promptly returned to InvenSense upon written request by InvenSense or upon -termination of this Agreement. - -6. LIMITATION OF LIABILITY -TO THE MAXIMUM EXTENT PERMITTED BY LAW, IN NO EVENT SHALL INVENSENSE OR ANY OF -INVENSENSE'S LICENSORS HAVE ANY LIABILITY FOR ANY INDIRECT, INCIDENTAL, -SPECIAL, OR CONSEQUENTIAL DAMAGES, HOWEVER CAUSED AND ON ANY THEORY OF -LIABILITY, WHETHER FOR BREACH OF CONTRACT, TORT (INCLUDING NEGLIGENCE) OR -OTHERWISE, ARISING OUT OF THIS AGREEMENT, INCLUDING BUT NOT LIMITED TO LOSS -OF PROFITS, EVEN IF SUCH PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH -DAMAGES. IN NO EVENT WILL INVENSENSE'S LIABILITY WHETHER IN CONTRACT, TORT -(INCLUDING NEGLIGENCE), OR OTHERWISE, EXCEED THE AMOUNT PAID BY LICENSEE FOR -SOFTWARE UNDER THIS AGREEMENT. THESE LIMITATIONS SHALL APPLY NOTWITHSTANDING -ANY FAILURE OF ESSENTIAL PURPOSE OF ANY LIMITED REMEDY. - -7. MISCELLANEOUS - -7.1. Export Regulations. YOU UNDERSTAND AND AGREE THAT THE SOFTWARE IS -SUBJECT TO UNITED STATES AND OTHER APPLICABLE EXPORT-RELATED LAWS AND -REGULATIONS AND THAT YOU MAY NOT EXPORT, RE-EXPORT OR TRANSFER THE SOFTWARE -OR ANY DIRECT PRODUCT OF THE SOFTWARE EXCEPT AS PERMITTED UNDER THOSE LAWS. -WITHOUT LIMITING THE FOREGOING, EXPORT, RE-EXPORT OR TRANSFER OF THE SOFTWARE -TO CUBA, IRAN, NORTH KOREA, SUDAN AND SYRIA IS PROHIBITED. - -7.2 Assignment. This Agreement shall be binding upon and inure to the -benefit of the parties and their respective successors and assigns, provided, -however that Licensee may not assign this Agreement or any rights or -obligation hereunder, directly or indirectly, by operation of law or -otherwise, without the prior written consent of InvenSense, and any such -attempted assignment shall be void. Notwithstanding the foregoing, Licensee -may assign this Agreement to a successor to all or substantially all of its -business or assets to which this Agreement relates that is not a competitor -of InvenSense. - -7.3. Governing Law; Venue. This Agreement shall be governed by the laws of -California without regard to any conflict-of-laws rules, and the United -Nations Convention on Contracts for the International Sale of Goods is hereby -excluded. The sole jurisdiction and venue for actions related to the subject -matter hereof shall be the state and federal courts located in the County of -Orange, California, and both parties hereby consent to such jurisdiction and -venue. - -7.4. Severability. All terms and provisions of this Agreement shall, if -possible, be construed in a manner which makes them valid, but in the event -any term or provision of this Agreement is found by a court of competent -jurisdiction to be illegal or unenforceable, the validity or enforceability -of the remainder of this Agreement shall not be affected if the illegal or -unenforceable provision does not materially affect the intent of this -Agreement. If the illegal or unenforceable provision materially affects the -intent of the parties to this Agreement, this Agreement shall become -terminated. - -7.5. Equitable Relief. Licensee hereby acknowledges that its breach of this -Agreement would cause irreparable harm and significant injury to InvenSense -that may be difficult to ascertain and that a remedy at law would be -inadequate. Accordingly, Licensee agrees that InvenSense shall have the right -to seek and obtain immediate injunctive relief to enforce obligations under -the Agreement in addition to any other rights and remedies it may have. - -7.6. Waiver. The waiver of, or failure to enforce, any breach or default -hereunder shall not constitute the waiver of any other or subsequent breach -or default. - -7.7. Entire Agreement. This Agreement sets forth the entire Agreement -between the parties and supersedes any and all prior proposals, agreements -and representations between them, whether written or oral concerning the -Software. This Agreement may be changed only by mutual agreement of the -parties in writing. - - diff --git a/interface/external/MotionDriver/include/dmpKey.h b/interface/external/MotionDriver/include/dmpKey.h deleted file mode 100644 index 72f95d69d4..0000000000 --- a/interface/external/MotionDriver/include/dmpKey.h +++ /dev/null @@ -1,494 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -#ifndef DMPKEY_H__ -#define DMPKEY_H__ - -#define KEY_CFG_25 (0) -#define KEY_CFG_24 (KEY_CFG_25 + 1) -#define KEY_CFG_26 (KEY_CFG_24 + 1) -#define KEY_CFG_27 (KEY_CFG_26 + 1) -#define KEY_CFG_21 (KEY_CFG_27 + 1) -#define KEY_CFG_20 (KEY_CFG_21 + 1) -#define KEY_CFG_TAP4 (KEY_CFG_20 + 1) -#define KEY_CFG_TAP5 (KEY_CFG_TAP4 + 1) -#define KEY_CFG_TAP6 (KEY_CFG_TAP5 + 1) -#define KEY_CFG_TAP7 (KEY_CFG_TAP6 + 1) -#define KEY_CFG_TAP0 (KEY_CFG_TAP7 + 1) -#define KEY_CFG_TAP1 (KEY_CFG_TAP0 + 1) -#define KEY_CFG_TAP2 (KEY_CFG_TAP1 + 1) -#define KEY_CFG_TAP3 (KEY_CFG_TAP2 + 1) -#define KEY_CFG_TAP_QUANTIZE (KEY_CFG_TAP3 + 1) -#define KEY_CFG_TAP_JERK (KEY_CFG_TAP_QUANTIZE + 1) -#define KEY_CFG_DR_INT (KEY_CFG_TAP_JERK + 1) -#define KEY_CFG_AUTH (KEY_CFG_DR_INT + 1) -#define KEY_CFG_TAP_SAVE_ACCB (KEY_CFG_AUTH + 1) -#define KEY_CFG_TAP_CLEAR_STICKY (KEY_CFG_TAP_SAVE_ACCB + 1) -#define KEY_CFG_FIFO_ON_EVENT (KEY_CFG_TAP_CLEAR_STICKY + 1) -#define KEY_FCFG_ACCEL_INPUT (KEY_CFG_FIFO_ON_EVENT + 1) -#define KEY_FCFG_ACCEL_INIT (KEY_FCFG_ACCEL_INPUT + 1) -#define KEY_CFG_23 (KEY_FCFG_ACCEL_INIT + 1) -#define KEY_FCFG_1 (KEY_CFG_23 + 1) -#define KEY_FCFG_3 (KEY_FCFG_1 + 1) -#define KEY_FCFG_2 (KEY_FCFG_3 + 1) -#define KEY_CFG_3D (KEY_FCFG_2 + 1) -#define KEY_CFG_3B (KEY_CFG_3D + 1) -#define KEY_CFG_3C (KEY_CFG_3B + 1) -#define KEY_FCFG_5 (KEY_CFG_3C + 1) -#define KEY_FCFG_4 (KEY_FCFG_5 + 1) -#define KEY_FCFG_7 (KEY_FCFG_4 + 1) -#define KEY_FCFG_FSCALE (KEY_FCFG_7 + 1) -#define KEY_FCFG_AZ (KEY_FCFG_FSCALE + 1) -#define KEY_FCFG_6 (KEY_FCFG_AZ + 1) -#define KEY_FCFG_LSB4 (KEY_FCFG_6 + 1) -#define KEY_CFG_12 (KEY_FCFG_LSB4 + 1) -#define KEY_CFG_14 (KEY_CFG_12 + 1) -#define KEY_CFG_15 (KEY_CFG_14 + 1) -#define KEY_CFG_16 (KEY_CFG_15 + 1) -#define KEY_CFG_18 (KEY_CFG_16 + 1) -#define KEY_CFG_6 (KEY_CFG_18 + 1) -#define KEY_CFG_7 (KEY_CFG_6 + 1) -#define KEY_CFG_4 (KEY_CFG_7 + 1) -#define KEY_CFG_5 (KEY_CFG_4 + 1) -#define KEY_CFG_2 (KEY_CFG_5 + 1) -#define KEY_CFG_3 (KEY_CFG_2 + 1) -#define KEY_CFG_1 (KEY_CFG_3 + 1) -#define KEY_CFG_EXTERNAL (KEY_CFG_1 + 1) -#define KEY_CFG_8 (KEY_CFG_EXTERNAL + 1) -#define KEY_CFG_9 (KEY_CFG_8 + 1) -#define KEY_CFG_ORIENT_3 (KEY_CFG_9 + 1) -#define KEY_CFG_ORIENT_2 (KEY_CFG_ORIENT_3 + 1) -#define KEY_CFG_ORIENT_1 (KEY_CFG_ORIENT_2 + 1) -#define KEY_CFG_GYRO_SOURCE (KEY_CFG_ORIENT_1 + 1) -#define KEY_CFG_ORIENT_IRQ_1 (KEY_CFG_GYRO_SOURCE + 1) -#define KEY_CFG_ORIENT_IRQ_2 (KEY_CFG_ORIENT_IRQ_1 + 1) -#define KEY_CFG_ORIENT_IRQ_3 (KEY_CFG_ORIENT_IRQ_2 + 1) -#define KEY_FCFG_MAG_VAL (KEY_CFG_ORIENT_IRQ_3 + 1) -#define KEY_FCFG_MAG_MOV (KEY_FCFG_MAG_VAL + 1) -#define KEY_CFG_LP_QUAT (KEY_FCFG_MAG_MOV + 1) - -/* MPU6050 keys */ -#define KEY_CFG_ACCEL_FILTER (KEY_CFG_LP_QUAT + 1) -#define KEY_CFG_MOTION_BIAS (KEY_CFG_ACCEL_FILTER + 1) -#define KEY_TEMPLABEL (KEY_CFG_MOTION_BIAS + 1) - -#define KEY_D_0_22 (KEY_TEMPLABEL + 1) -#define KEY_D_0_24 (KEY_D_0_22 + 1) -#define KEY_D_0_36 (KEY_D_0_24 + 1) -#define KEY_D_0_52 (KEY_D_0_36 + 1) -#define KEY_D_0_96 (KEY_D_0_52 + 1) -#define KEY_D_0_104 (KEY_D_0_96 + 1) -#define KEY_D_0_108 (KEY_D_0_104 + 1) -#define KEY_D_0_163 (KEY_D_0_108 + 1) -#define KEY_D_0_188 (KEY_D_0_163 + 1) -#define KEY_D_0_192 (KEY_D_0_188 + 1) -#define KEY_D_0_224 (KEY_D_0_192 + 1) -#define KEY_D_0_228 (KEY_D_0_224 + 1) -#define KEY_D_0_232 (KEY_D_0_228 + 1) -#define KEY_D_0_236 (KEY_D_0_232 + 1) - -#define KEY_DMP_PREVPTAT (KEY_D_0_236 + 1) -#define KEY_D_1_2 (KEY_DMP_PREVPTAT + 1) -#define KEY_D_1_4 (KEY_D_1_2 + 1) -#define KEY_D_1_8 (KEY_D_1_4 + 1) -#define KEY_D_1_10 (KEY_D_1_8 + 1) -#define KEY_D_1_24 (KEY_D_1_10 + 1) -#define KEY_D_1_28 (KEY_D_1_24 + 1) -#define KEY_D_1_36 (KEY_D_1_28 + 1) -#define KEY_D_1_40 (KEY_D_1_36 + 1) -#define KEY_D_1_44 (KEY_D_1_40 + 1) -#define KEY_D_1_72 (KEY_D_1_44 + 1) -#define KEY_D_1_74 (KEY_D_1_72 + 1) -#define KEY_D_1_79 (KEY_D_1_74 + 1) -#define KEY_D_1_88 (KEY_D_1_79 + 1) -#define KEY_D_1_90 (KEY_D_1_88 + 1) -#define KEY_D_1_92 (KEY_D_1_90 + 1) -#define KEY_D_1_96 (KEY_D_1_92 + 1) -#define KEY_D_1_98 (KEY_D_1_96 + 1) -#define KEY_D_1_100 (KEY_D_1_98 + 1) -#define KEY_D_1_106 (KEY_D_1_100 + 1) -#define KEY_D_1_108 (KEY_D_1_106 + 1) -#define KEY_D_1_112 (KEY_D_1_108 + 1) -#define KEY_D_1_128 (KEY_D_1_112 + 1) -#define KEY_D_1_152 (KEY_D_1_128 + 1) -#define KEY_D_1_160 (KEY_D_1_152 + 1) -#define KEY_D_1_168 (KEY_D_1_160 + 1) -#define KEY_D_1_175 (KEY_D_1_168 + 1) -#define KEY_D_1_176 (KEY_D_1_175 + 1) -#define KEY_D_1_178 (KEY_D_1_176 + 1) -#define KEY_D_1_179 (KEY_D_1_178 + 1) -#define KEY_D_1_218 (KEY_D_1_179 + 1) -#define KEY_D_1_232 (KEY_D_1_218 + 1) -#define KEY_D_1_236 (KEY_D_1_232 + 1) -#define KEY_D_1_240 (KEY_D_1_236 + 1) -#define KEY_D_1_244 (KEY_D_1_240 + 1) -#define KEY_D_1_250 (KEY_D_1_244 + 1) -#define KEY_D_1_252 (KEY_D_1_250 + 1) -#define KEY_D_2_12 (KEY_D_1_252 + 1) -#define KEY_D_2_96 (KEY_D_2_12 + 1) -#define KEY_D_2_108 (KEY_D_2_96 + 1) -#define KEY_D_2_208 (KEY_D_2_108 + 1) -#define KEY_FLICK_MSG (KEY_D_2_208 + 1) -#define KEY_FLICK_COUNTER (KEY_FLICK_MSG + 1) -#define KEY_FLICK_LOWER (KEY_FLICK_COUNTER + 1) -#define KEY_CFG_FLICK_IN (KEY_FLICK_LOWER + 1) -#define KEY_FLICK_UPPER (KEY_CFG_FLICK_IN + 1) -#define KEY_CGNOTICE_INTR (KEY_FLICK_UPPER + 1) -#define KEY_D_2_224 (KEY_CGNOTICE_INTR + 1) -#define KEY_D_2_244 (KEY_D_2_224 + 1) -#define KEY_D_2_248 (KEY_D_2_244 + 1) -#define KEY_D_2_252 (KEY_D_2_248 + 1) - -#define KEY_D_GYRO_BIAS_X (KEY_D_2_252 + 1) -#define KEY_D_GYRO_BIAS_Y (KEY_D_GYRO_BIAS_X + 1) -#define KEY_D_GYRO_BIAS_Z (KEY_D_GYRO_BIAS_Y + 1) -#define KEY_D_ACC_BIAS_X (KEY_D_GYRO_BIAS_Z + 1) -#define KEY_D_ACC_BIAS_Y (KEY_D_ACC_BIAS_X + 1) -#define KEY_D_ACC_BIAS_Z (KEY_D_ACC_BIAS_Y + 1) -#define KEY_D_GYRO_ENABLE (KEY_D_ACC_BIAS_Z + 1) -#define KEY_D_ACCEL_ENABLE (KEY_D_GYRO_ENABLE + 1) -#define KEY_D_QUAT_ENABLE (KEY_D_ACCEL_ENABLE +1) -#define KEY_D_OUTPUT_ENABLE (KEY_D_QUAT_ENABLE + 1) -#define KEY_D_CR_TIME_G (KEY_D_OUTPUT_ENABLE + 1) -#define KEY_D_CR_TIME_A (KEY_D_CR_TIME_G + 1) -#define KEY_D_CR_TIME_Q (KEY_D_CR_TIME_A + 1) -#define KEY_D_CS_TAX (KEY_D_CR_TIME_Q + 1) -#define KEY_D_CS_TAY (KEY_D_CS_TAX + 1) -#define KEY_D_CS_TAZ (KEY_D_CS_TAY + 1) -#define KEY_D_CS_TGX (KEY_D_CS_TAZ + 1) -#define KEY_D_CS_TGY (KEY_D_CS_TGX + 1) -#define KEY_D_CS_TGZ (KEY_D_CS_TGY + 1) -#define KEY_D_CS_TQ0 (KEY_D_CS_TGZ + 1) -#define KEY_D_CS_TQ1 (KEY_D_CS_TQ0 + 1) -#define KEY_D_CS_TQ2 (KEY_D_CS_TQ1 + 1) -#define KEY_D_CS_TQ3 (KEY_D_CS_TQ2 + 1) - -/* Compass keys */ -#define KEY_CPASS_BIAS_X (KEY_D_CS_TQ3 + 1) -#define KEY_CPASS_BIAS_Y (KEY_CPASS_BIAS_X + 1) -#define KEY_CPASS_BIAS_Z (KEY_CPASS_BIAS_Y + 1) -#define KEY_CPASS_MTX_00 (KEY_CPASS_BIAS_Z + 1) -#define KEY_CPASS_MTX_01 (KEY_CPASS_MTX_00 + 1) -#define KEY_CPASS_MTX_02 (KEY_CPASS_MTX_01 + 1) -#define KEY_CPASS_MTX_10 (KEY_CPASS_MTX_02 + 1) -#define KEY_CPASS_MTX_11 (KEY_CPASS_MTX_10 + 1) -#define KEY_CPASS_MTX_12 (KEY_CPASS_MTX_11 + 1) -#define KEY_CPASS_MTX_20 (KEY_CPASS_MTX_12 + 1) -#define KEY_CPASS_MTX_21 (KEY_CPASS_MTX_20 + 1) -#define KEY_CPASS_MTX_22 (KEY_CPASS_MTX_21 + 1) - -/* Gesture Keys */ -#define KEY_DMP_TAPW_MIN (KEY_CPASS_MTX_22 + 1) -#define KEY_DMP_TAP_THR_X (KEY_DMP_TAPW_MIN + 1) -#define KEY_DMP_TAP_THR_Y (KEY_DMP_TAP_THR_X + 1) -#define KEY_DMP_TAP_THR_Z (KEY_DMP_TAP_THR_Y + 1) -#define KEY_DMP_SH_TH_Y (KEY_DMP_TAP_THR_Z + 1) -#define KEY_DMP_SH_TH_X (KEY_DMP_SH_TH_Y + 1) -#define KEY_DMP_SH_TH_Z (KEY_DMP_SH_TH_X + 1) -#define KEY_DMP_ORIENT (KEY_DMP_SH_TH_Z + 1) -#define KEY_D_ACT0 (KEY_DMP_ORIENT + 1) -#define KEY_D_ACSX (KEY_D_ACT0 + 1) -#define KEY_D_ACSY (KEY_D_ACSX + 1) -#define KEY_D_ACSZ (KEY_D_ACSY + 1) - -#define KEY_X_GRT_Y_TMP (KEY_D_ACSZ + 1) -#define KEY_SKIP_X_GRT_Y_TMP (KEY_X_GRT_Y_TMP + 1) -#define KEY_SKIP_END_COMPARE (KEY_SKIP_X_GRT_Y_TMP + 1) -#define KEY_END_COMPARE_Y_X_TMP2 (KEY_SKIP_END_COMPARE + 1) -#define KEY_CFG_ANDROID_ORIENT_INT (KEY_END_COMPARE_Y_X_TMP2 + 1) -#define KEY_NO_ORIENT_INTERRUPT (KEY_CFG_ANDROID_ORIENT_INT + 1) -#define KEY_END_COMPARE_Y_X_TMP (KEY_NO_ORIENT_INTERRUPT + 1) -#define KEY_END_ORIENT_1 (KEY_END_COMPARE_Y_X_TMP + 1) -#define KEY_END_COMPARE_Y_X (KEY_END_ORIENT_1 + 1) -#define KEY_END_ORIENT (KEY_END_COMPARE_Y_X + 1) -#define KEY_X_GRT_Y (KEY_END_ORIENT + 1) -#define KEY_NOT_TIME_MINUS_1 (KEY_X_GRT_Y + 1) -#define KEY_END_COMPARE_Y_X_TMP3 (KEY_NOT_TIME_MINUS_1 + 1) -#define KEY_X_GRT_Y_TMP2 (KEY_END_COMPARE_Y_X_TMP3 + 1) - -/* Authenticate Keys */ -#define KEY_D_AUTH_OUT (KEY_X_GRT_Y_TMP2 + 1) -#define KEY_D_AUTH_IN (KEY_D_AUTH_OUT + 1) -#define KEY_D_AUTH_A (KEY_D_AUTH_IN + 1) -#define KEY_D_AUTH_B (KEY_D_AUTH_A + 1) - -/* Pedometer standalone only keys */ -#define KEY_D_PEDSTD_BP_B (KEY_D_AUTH_B + 1) -#define KEY_D_PEDSTD_HP_A (KEY_D_PEDSTD_BP_B + 1) -#define KEY_D_PEDSTD_HP_B (KEY_D_PEDSTD_HP_A + 1) -#define KEY_D_PEDSTD_BP_A4 (KEY_D_PEDSTD_HP_B + 1) -#define KEY_D_PEDSTD_BP_A3 (KEY_D_PEDSTD_BP_A4 + 1) -#define KEY_D_PEDSTD_BP_A2 (KEY_D_PEDSTD_BP_A3 + 1) -#define KEY_D_PEDSTD_BP_A1 (KEY_D_PEDSTD_BP_A2 + 1) -#define KEY_D_PEDSTD_INT_THRSH (KEY_D_PEDSTD_BP_A1 + 1) -#define KEY_D_PEDSTD_CLIP (KEY_D_PEDSTD_INT_THRSH + 1) -#define KEY_D_PEDSTD_SB (KEY_D_PEDSTD_CLIP + 1) -#define KEY_D_PEDSTD_SB_TIME (KEY_D_PEDSTD_SB + 1) -#define KEY_D_PEDSTD_PEAKTHRSH (KEY_D_PEDSTD_SB_TIME + 1) -#define KEY_D_PEDSTD_TIML (KEY_D_PEDSTD_PEAKTHRSH + 1) -#define KEY_D_PEDSTD_TIMH (KEY_D_PEDSTD_TIML + 1) -#define KEY_D_PEDSTD_PEAK (KEY_D_PEDSTD_TIMH + 1) -#define KEY_D_PEDSTD_TIMECTR (KEY_D_PEDSTD_PEAK + 1) -#define KEY_D_PEDSTD_STEPCTR (KEY_D_PEDSTD_TIMECTR + 1) -#define KEY_D_PEDSTD_WALKTIME (KEY_D_PEDSTD_STEPCTR + 1) -#define KEY_D_PEDSTD_DECI (KEY_D_PEDSTD_WALKTIME + 1) - -/*Host Based No Motion*/ -#define KEY_D_HOST_NO_MOT (KEY_D_PEDSTD_DECI + 1) - -/* EIS keys */ -#define KEY_P_EIS_FIFO_FOOTER (KEY_D_HOST_NO_MOT + 1) -#define KEY_P_EIS_FIFO_YSHIFT (KEY_P_EIS_FIFO_FOOTER + 1) -#define KEY_P_EIS_DATA_RATE (KEY_P_EIS_FIFO_YSHIFT + 1) -#define KEY_P_EIS_FIFO_XSHIFT (KEY_P_EIS_DATA_RATE + 1) -#define KEY_P_EIS_FIFO_SYNC (KEY_P_EIS_FIFO_XSHIFT + 1) -#define KEY_P_EIS_FIFO_ZSHIFT (KEY_P_EIS_FIFO_SYNC + 1) -#define KEY_P_EIS_FIFO_READY (KEY_P_EIS_FIFO_ZSHIFT + 1) -#define KEY_DMP_FOOTER (KEY_P_EIS_FIFO_READY + 1) -#define KEY_DMP_INTX_HC (KEY_DMP_FOOTER + 1) -#define KEY_DMP_INTX_PH (KEY_DMP_INTX_HC + 1) -#define KEY_DMP_INTX_SH (KEY_DMP_INTX_PH + 1) -#define KEY_DMP_AINV_SH (KEY_DMP_INTX_SH + 1) -#define KEY_DMP_A_INV_XH (KEY_DMP_AINV_SH + 1) -#define KEY_DMP_AINV_PH (KEY_DMP_A_INV_XH + 1) -#define KEY_DMP_CTHX_H (KEY_DMP_AINV_PH + 1) -#define KEY_DMP_CTHY_H (KEY_DMP_CTHX_H + 1) -#define KEY_DMP_CTHZ_H (KEY_DMP_CTHY_H + 1) -#define KEY_DMP_NCTHX_H (KEY_DMP_CTHZ_H + 1) -#define KEY_DMP_NCTHY_H (KEY_DMP_NCTHX_H + 1) -#define KEY_DMP_NCTHZ_H (KEY_DMP_NCTHY_H + 1) -#define KEY_DMP_CTSQ_XH (KEY_DMP_NCTHZ_H + 1) -#define KEY_DMP_CTSQ_YH (KEY_DMP_CTSQ_XH + 1) -#define KEY_DMP_CTSQ_ZH (KEY_DMP_CTSQ_YH + 1) -#define KEY_DMP_INTX_H (KEY_DMP_CTSQ_ZH + 1) -#define KEY_DMP_INTY_H (KEY_DMP_INTX_H + 1) -#define KEY_DMP_INTZ_H (KEY_DMP_INTY_H + 1) -//#define KEY_DMP_HPX_H (KEY_DMP_INTZ_H + 1) -//#define KEY_DMP_HPY_H (KEY_DMP_HPX_H + 1) -//#define KEY_DMP_HPZ_H (KEY_DMP_HPY_H + 1) - -/* Stream keys */ -#define KEY_STREAM_P_GYRO_Z (KEY_DMP_INTZ_H + 1) -#define KEY_STREAM_P_GYRO_Y (KEY_STREAM_P_GYRO_Z + 1) -#define KEY_STREAM_P_GYRO_X (KEY_STREAM_P_GYRO_Y + 1) -#define KEY_STREAM_P_TEMP (KEY_STREAM_P_GYRO_X + 1) -#define KEY_STREAM_P_AUX_Y (KEY_STREAM_P_TEMP + 1) -#define KEY_STREAM_P_AUX_X (KEY_STREAM_P_AUX_Y + 1) -#define KEY_STREAM_P_AUX_Z (KEY_STREAM_P_AUX_X + 1) -#define KEY_STREAM_P_ACCEL_Y (KEY_STREAM_P_AUX_Z + 1) -#define KEY_STREAM_P_ACCEL_X (KEY_STREAM_P_ACCEL_Y + 1) -#define KEY_STREAM_P_FOOTER (KEY_STREAM_P_ACCEL_X + 1) -#define KEY_STREAM_P_ACCEL_Z (KEY_STREAM_P_FOOTER + 1) - -#define NUM_KEYS (KEY_STREAM_P_ACCEL_Z + 1) - -typedef struct { - unsigned short key; - unsigned short addr; -} tKeyLabel; - -#define DINA0A 0x0a -#define DINA22 0x22 -#define DINA42 0x42 -#define DINA5A 0x5a - -#define DINA06 0x06 -#define DINA0E 0x0e -#define DINA16 0x16 -#define DINA1E 0x1e -#define DINA26 0x26 -#define DINA2E 0x2e -#define DINA36 0x36 -#define DINA3E 0x3e -#define DINA46 0x46 -#define DINA4E 0x4e -#define DINA56 0x56 -#define DINA5E 0x5e -#define DINA66 0x66 -#define DINA6E 0x6e -#define DINA76 0x76 -#define DINA7E 0x7e - -#define DINA00 0x00 -#define DINA08 0x08 -#define DINA10 0x10 -#define DINA18 0x18 -#define DINA20 0x20 -#define DINA28 0x28 -#define DINA30 0x30 -#define DINA38 0x38 -#define DINA40 0x40 -#define DINA48 0x48 -#define DINA50 0x50 -#define DINA58 0x58 -#define DINA60 0x60 -#define DINA68 0x68 -#define DINA70 0x70 -#define DINA78 0x78 - -#define DINA04 0x04 -#define DINA0C 0x0c -#define DINA14 0x14 -#define DINA1C 0x1C -#define DINA24 0x24 -#define DINA2C 0x2c -#define DINA34 0x34 -#define DINA3C 0x3c -#define DINA44 0x44 -#define DINA4C 0x4c -#define DINA54 0x54 -#define DINA5C 0x5c -#define DINA64 0x64 -#define DINA6C 0x6c -#define DINA74 0x74 -#define DINA7C 0x7c - -#define DINA01 0x01 -#define DINA09 0x09 -#define DINA11 0x11 -#define DINA19 0x19 -#define DINA21 0x21 -#define DINA29 0x29 -#define DINA31 0x31 -#define DINA39 0x39 -#define DINA41 0x41 -#define DINA49 0x49 -#define DINA51 0x51 -#define DINA59 0x59 -#define DINA61 0x61 -#define DINA69 0x69 -#define DINA71 0x71 -#define DINA79 0x79 - -#define DINA25 0x25 -#define DINA2D 0x2d -#define DINA35 0x35 -#define DINA3D 0x3d -#define DINA4D 0x4d -#define DINA55 0x55 -#define DINA5D 0x5D -#define DINA6D 0x6d -#define DINA75 0x75 -#define DINA7D 0x7d - -#define DINADC 0xdc -#define DINAF2 0xf2 -#define DINAAB 0xab -#define DINAAA 0xaa -#define DINAF1 0xf1 -#define DINADF 0xdf -#define DINADA 0xda -#define DINAB1 0xb1 -#define DINAB9 0xb9 -#define DINAF3 0xf3 -#define DINA8B 0x8b -#define DINAA3 0xa3 -#define DINA91 0x91 -#define DINAB6 0xb6 -#define DINAB4 0xb4 - - -#define DINC00 0x00 -#define DINC01 0x01 -#define DINC02 0x02 -#define DINC03 0x03 -#define DINC08 0x08 -#define DINC09 0x09 -#define DINC0A 0x0a -#define DINC0B 0x0b -#define DINC10 0x10 -#define DINC11 0x11 -#define DINC12 0x12 -#define DINC13 0x13 -#define DINC18 0x18 -#define DINC19 0x19 -#define DINC1A 0x1a -#define DINC1B 0x1b - -#define DINC20 0x20 -#define DINC21 0x21 -#define DINC22 0x22 -#define DINC23 0x23 -#define DINC28 0x28 -#define DINC29 0x29 -#define DINC2A 0x2a -#define DINC2B 0x2b -#define DINC30 0x30 -#define DINC31 0x31 -#define DINC32 0x32 -#define DINC33 0x33 -#define DINC38 0x38 -#define DINC39 0x39 -#define DINC3A 0x3a -#define DINC3B 0x3b - -#define DINC40 0x40 -#define DINC41 0x41 -#define DINC42 0x42 -#define DINC43 0x43 -#define DINC48 0x48 -#define DINC49 0x49 -#define DINC4A 0x4a -#define DINC4B 0x4b -#define DINC50 0x50 -#define DINC51 0x51 -#define DINC52 0x52 -#define DINC53 0x53 -#define DINC58 0x58 -#define DINC59 0x59 -#define DINC5A 0x5a -#define DINC5B 0x5b - -#define DINC60 0x60 -#define DINC61 0x61 -#define DINC62 0x62 -#define DINC63 0x63 -#define DINC68 0x68 -#define DINC69 0x69 -#define DINC6A 0x6a -#define DINC6B 0x6b -#define DINC70 0x70 -#define DINC71 0x71 -#define DINC72 0x72 -#define DINC73 0x73 -#define DINC78 0x78 -#define DINC79 0x79 -#define DINC7A 0x7a -#define DINC7B 0x7b - -#define DIND40 0x40 - - -#define DINA80 0x80 -#define DINA90 0x90 -#define DINAA0 0xa0 -#define DINAC9 0xc9 -#define DINACB 0xcb -#define DINACD 0xcd -#define DINACF 0xcf -#define DINAC8 0xc8 -#define DINACA 0xca -#define DINACC 0xcc -#define DINACE 0xce -#define DINAD8 0xd8 -#define DINADD 0xdd -#define DINAF8 0xf0 -#define DINAFE 0xfe - -#define DINBF8 0xf8 -#define DINAC0 0xb0 -#define DINAC1 0xb1 -#define DINAC2 0xb4 -#define DINAC3 0xb5 -#define DINAC4 0xb8 -#define DINAC5 0xb9 -#define DINBC0 0xc0 -#define DINBC2 0xc2 -#define DINBC4 0xc4 -#define DINBC6 0xc6 - - - -#endif // DMPKEY_H__ diff --git a/interface/external/MotionDriver/include/dmpmap.h b/interface/external/MotionDriver/include/dmpmap.h deleted file mode 100644 index f63e140398..0000000000 --- a/interface/external/MotionDriver/include/dmpmap.h +++ /dev/null @@ -1,264 +0,0 @@ -/* - $License: - Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. - $ - */ -#ifndef DMPMAP_H -#define DMPMAP_H - -#ifdef __cplusplus -extern "C" -{ -#endif - -#define DMP_PTAT 0 -#define DMP_XGYR 2 -#define DMP_YGYR 4 -#define DMP_ZGYR 6 -#define DMP_XACC 8 -#define DMP_YACC 10 -#define DMP_ZACC 12 -#define DMP_ADC1 14 -#define DMP_ADC2 16 -#define DMP_ADC3 18 -#define DMP_BIASUNC 20 -#define DMP_FIFORT 22 -#define DMP_INVGSFH 24 -#define DMP_INVGSFL 26 -#define DMP_1H 28 -#define DMP_1L 30 -#define DMP_BLPFSTCH 32 -#define DMP_BLPFSTCL 34 -#define DMP_BLPFSXH 36 -#define DMP_BLPFSXL 38 -#define DMP_BLPFSYH 40 -#define DMP_BLPFSYL 42 -#define DMP_BLPFSZH 44 -#define DMP_BLPFSZL 46 -#define DMP_BLPFMTC 48 -#define DMP_SMC 50 -#define DMP_BLPFMXH 52 -#define DMP_BLPFMXL 54 -#define DMP_BLPFMYH 56 -#define DMP_BLPFMYL 58 -#define DMP_BLPFMZH 60 -#define DMP_BLPFMZL 62 -#define DMP_BLPFC 64 -#define DMP_SMCTH 66 -#define DMP_0H2 68 -#define DMP_0L2 70 -#define DMP_BERR2H 72 -#define DMP_BERR2L 74 -#define DMP_BERR2NH 76 -#define DMP_SMCINC 78 -#define DMP_ANGVBXH 80 -#define DMP_ANGVBXL 82 -#define DMP_ANGVBYH 84 -#define DMP_ANGVBYL 86 -#define DMP_ANGVBZH 88 -#define DMP_ANGVBZL 90 -#define DMP_BERR1H 92 -#define DMP_BERR1L 94 -#define DMP_ATCH 96 -#define DMP_BIASUNCSF 98 -#define DMP_ACT2H 100 -#define DMP_ACT2L 102 -#define DMP_GSFH 104 -#define DMP_GSFL 106 -#define DMP_GH 108 -#define DMP_GL 110 -#define DMP_0_5H 112 -#define DMP_0_5L 114 -#define DMP_0_0H 116 -#define DMP_0_0L 118 -#define DMP_1_0H 120 -#define DMP_1_0L 122 -#define DMP_1_5H 124 -#define DMP_1_5L 126 -#define DMP_TMP1AH 128 -#define DMP_TMP1AL 130 -#define DMP_TMP2AH 132 -#define DMP_TMP2AL 134 -#define DMP_TMP3AH 136 -#define DMP_TMP3AL 138 -#define DMP_TMP4AH 140 -#define DMP_TMP4AL 142 -#define DMP_XACCW 144 -#define DMP_TMP5 146 -#define DMP_XACCB 148 -#define DMP_TMP8 150 -#define DMP_YACCB 152 -#define DMP_TMP9 154 -#define DMP_ZACCB 156 -#define DMP_TMP10 158 -#define DMP_DZH 160 -#define DMP_DZL 162 -#define DMP_XGCH 164 -#define DMP_XGCL 166 -#define DMP_YGCH 168 -#define DMP_YGCL 170 -#define DMP_ZGCH 172 -#define DMP_ZGCL 174 -#define DMP_YACCW 176 -#define DMP_TMP7 178 -#define DMP_AFB1H 180 -#define DMP_AFB1L 182 -#define DMP_AFB2H 184 -#define DMP_AFB2L 186 -#define DMP_MAGFBH 188 -#define DMP_MAGFBL 190 -#define DMP_QT1H 192 -#define DMP_QT1L 194 -#define DMP_QT2H 196 -#define DMP_QT2L 198 -#define DMP_QT3H 200 -#define DMP_QT3L 202 -#define DMP_QT4H 204 -#define DMP_QT4L 206 -#define DMP_CTRL1H 208 -#define DMP_CTRL1L 210 -#define DMP_CTRL2H 212 -#define DMP_CTRL2L 214 -#define DMP_CTRL3H 216 -#define DMP_CTRL3L 218 -#define DMP_CTRL4H 220 -#define DMP_CTRL4L 222 -#define DMP_CTRLS1 224 -#define DMP_CTRLSF1 226 -#define DMP_CTRLS2 228 -#define DMP_CTRLSF2 230 -#define DMP_CTRLS3 232 -#define DMP_CTRLSFNLL 234 -#define DMP_CTRLS4 236 -#define DMP_CTRLSFNL2 238 -#define DMP_CTRLSFNL 240 -#define DMP_TMP30 242 -#define DMP_CTRLSFJT 244 -#define DMP_TMP31 246 -#define DMP_TMP11 248 -#define DMP_CTRLSF2_2 250 -#define DMP_TMP12 252 -#define DMP_CTRLSF1_2 254 -#define DMP_PREVPTAT 256 -#define DMP_ACCZB 258 -#define DMP_ACCXB 264 -#define DMP_ACCYB 266 -#define DMP_1HB 272 -#define DMP_1LB 274 -#define DMP_0H 276 -#define DMP_0L 278 -#define DMP_ASR22H 280 -#define DMP_ASR22L 282 -#define DMP_ASR6H 284 -#define DMP_ASR6L 286 -#define DMP_TMP13 288 -#define DMP_TMP14 290 -#define DMP_FINTXH 292 -#define DMP_FINTXL 294 -#define DMP_FINTYH 296 -#define DMP_FINTYL 298 -#define DMP_FINTZH 300 -#define DMP_FINTZL 302 -#define DMP_TMP1BH 304 -#define DMP_TMP1BL 306 -#define DMP_TMP2BH 308 -#define DMP_TMP2BL 310 -#define DMP_TMP3BH 312 -#define DMP_TMP3BL 314 -#define DMP_TMP4BH 316 -#define DMP_TMP4BL 318 -#define DMP_STXG 320 -#define DMP_ZCTXG 322 -#define DMP_STYG 324 -#define DMP_ZCTYG 326 -#define DMP_STZG 328 -#define DMP_ZCTZG 330 -#define DMP_CTRLSFJT2 332 -#define DMP_CTRLSFJTCNT 334 -#define DMP_PVXG 336 -#define DMP_TMP15 338 -#define DMP_PVYG 340 -#define DMP_TMP16 342 -#define DMP_PVZG 344 -#define DMP_TMP17 346 -#define DMP_MNMFLAGH 352 -#define DMP_MNMFLAGL 354 -#define DMP_MNMTMH 356 -#define DMP_MNMTML 358 -#define DMP_MNMTMTHRH 360 -#define DMP_MNMTMTHRL 362 -#define DMP_MNMTHRH 364 -#define DMP_MNMTHRL 366 -#define DMP_ACCQD4H 368 -#define DMP_ACCQD4L 370 -#define DMP_ACCQD5H 372 -#define DMP_ACCQD5L 374 -#define DMP_ACCQD6H 376 -#define DMP_ACCQD6L 378 -#define DMP_ACCQD7H 380 -#define DMP_ACCQD7L 382 -#define DMP_ACCQD0H 384 -#define DMP_ACCQD0L 386 -#define DMP_ACCQD1H 388 -#define DMP_ACCQD1L 390 -#define DMP_ACCQD2H 392 -#define DMP_ACCQD2L 394 -#define DMP_ACCQD3H 396 -#define DMP_ACCQD3L 398 -#define DMP_XN2H 400 -#define DMP_XN2L 402 -#define DMP_XN1H 404 -#define DMP_XN1L 406 -#define DMP_YN2H 408 -#define DMP_YN2L 410 -#define DMP_YN1H 412 -#define DMP_YN1L 414 -#define DMP_YH 416 -#define DMP_YL 418 -#define DMP_B0H 420 -#define DMP_B0L 422 -#define DMP_A1H 424 -#define DMP_A1L 426 -#define DMP_A2H 428 -#define DMP_A2L 430 -#define DMP_SEM1 432 -#define DMP_FIFOCNT 434 -#define DMP_SH_TH_X 436 -#define DMP_PACKET 438 -#define DMP_SH_TH_Y 440 -#define DMP_FOOTER 442 -#define DMP_SH_TH_Z 444 -#define DMP_TEMP29 448 -#define DMP_TEMP30 450 -#define DMP_XACCB_PRE 452 -#define DMP_XACCB_PREL 454 -#define DMP_YACCB_PRE 456 -#define DMP_YACCB_PREL 458 -#define DMP_ZACCB_PRE 460 -#define DMP_ZACCB_PREL 462 -#define DMP_TMP22 464 -#define DMP_TAP_TIMER 466 -#define DMP_TAP_THX 468 -#define DMP_TAP_THY 472 -#define DMP_TAP_THZ 476 -#define DMP_TAPW_MIN 478 -#define DMP_TMP25 480 -#define DMP_TMP26 482 -#define DMP_TMP27 484 -#define DMP_TMP28 486 -#define DMP_ORIENT 488 -#define DMP_THRSH 490 -#define DMP_ENDIANH 492 -#define DMP_ENDIANL 494 -#define DMP_BLPFNMTCH 496 -#define DMP_BLPFNMTCL 498 -#define DMP_BLPFNMXH 500 -#define DMP_BLPFNMXL 502 -#define DMP_BLPFNMYH 504 -#define DMP_BLPFNMYL 506 -#define DMP_BLPFNMZH 508 -#define DMP_BLPFNMZL 510 -#ifdef __cplusplus -} -#endif -#endif // DMPMAP_H diff --git a/interface/external/MotionDriver/include/inv_mpu.h b/interface/external/MotionDriver/include/inv_mpu.h deleted file mode 100644 index 9610124f04..0000000000 --- a/interface/external/MotionDriver/include/inv_mpu.h +++ /dev/null @@ -1,127 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -/** - * @addtogroup DRIVERS Sensor Driver Layer - * @brief Hardware drivers to communicate with sensors via I2C. - * - * @{ - * @file inv_mpu.h - * @brief An I2C-based driver for Invensense gyroscopes. - * @details This driver currently works for the following devices: - * MPU6050 - * MPU6500 - * MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus) - * MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus) - */ - -#ifndef _INV_MPU_H_ -#define _INV_MPU_H_ - -#define INV_X_GYRO (0x40) -#define INV_Y_GYRO (0x20) -#define INV_Z_GYRO (0x10) -#define INV_XYZ_GYRO (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO) -#define INV_XYZ_ACCEL (0x08) -#define INV_XYZ_COMPASS (0x01) - -struct int_param_s { -#if defined EMPL_TARGET_MSP430 || defined MOTION_DRIVER_TARGET_MSP430 - void (*cb)(void); - unsigned short pin; - unsigned char lp_exit; - unsigned char active_low; -#elif defined EMPL_TARGET_UC3L0 - unsigned long pin; - void (*cb)(volatile void*); - void *arg; -#endif -}; - -#define MPU_INT_STATUS_DATA_READY (0x0001) -#define MPU_INT_STATUS_DMP (0x0002) -#define MPU_INT_STATUS_PLL_READY (0x0004) -#define MPU_INT_STATUS_I2C_MST (0x0008) -#define MPU_INT_STATUS_FIFO_OVERFLOW (0x0010) -#define MPU_INT_STATUS_ZMOT (0x0020) -#define MPU_INT_STATUS_MOT (0x0040) -#define MPU_INT_STATUS_FREE_FALL (0x0080) -#define MPU_INT_STATUS_DMP_0 (0x0100) -#define MPU_INT_STATUS_DMP_1 (0x0200) -#define MPU_INT_STATUS_DMP_2 (0x0400) -#define MPU_INT_STATUS_DMP_3 (0x0800) -#define MPU_INT_STATUS_DMP_4 (0x1000) -#define MPU_INT_STATUS_DMP_5 (0x2000) - -/* Set up APIs */ -int mpu_init(struct int_param_s *int_param); -int mpu_init_slave(void); -int mpu_set_bypass(unsigned char bypass_on); - -/* Configuration APIs */ -int mpu_lp_accel_mode(unsigned char rate); -int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time, - unsigned char lpa_freq); -int mpu_set_int_level(unsigned char active_low); -int mpu_set_int_latched(unsigned char enable); - -int mpu_set_dmp_state(unsigned char enable); -int mpu_get_dmp_state(unsigned char *enabled); - -int mpu_get_lpf(unsigned short *lpf); -int mpu_set_lpf(unsigned short lpf); - -int mpu_get_gyro_fsr(unsigned short *fsr); -int mpu_set_gyro_fsr(unsigned short fsr); - -int mpu_get_accel_fsr(unsigned char *fsr); -int mpu_set_accel_fsr(unsigned char fsr); - -int mpu_get_compass_fsr(unsigned short *fsr); - -int mpu_get_gyro_sens(float *sens); -int mpu_get_accel_sens(unsigned short *sens); - -int mpu_get_sample_rate(unsigned short *rate); -int mpu_set_sample_rate(unsigned short rate); -int mpu_get_compass_sample_rate(unsigned short *rate); -int mpu_set_compass_sample_rate(unsigned short rate); - -int mpu_get_fifo_config(unsigned char *sensors); -int mpu_configure_fifo(unsigned char sensors); - -int mpu_get_power_state(unsigned char *power_on); -int mpu_set_sensors(unsigned char sensors); - -int mpu_set_accel_bias(const long *accel_bias); - -/* Data getter/setter APIs */ -int mpu_get_gyro_reg(short *data, unsigned long *timestamp); -int mpu_get_accel_reg(short *data, unsigned long *timestamp); -int mpu_get_compass_reg(short *data, unsigned long *timestamp); -int mpu_get_temperature(long *data, unsigned long *timestamp); - -int mpu_get_int_status(short *status); -int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp, - unsigned char *sensors, unsigned char *more); -int mpu_read_fifo_stream(unsigned short length, unsigned char *data, - unsigned char *more); -int mpu_reset_fifo(void); - -int mpu_write_mem(unsigned short mem_addr, unsigned short length, - unsigned char *data); -int mpu_read_mem(unsigned short mem_addr, unsigned short length, - unsigned char *data); -int mpu_load_firmware(unsigned short length, const unsigned char *firmware, - unsigned short start_addr, unsigned short sample_rate); - -int mpu_reg_dump(void); -int mpu_read_reg(unsigned char reg, unsigned char *data); -int mpu_run_self_test(long *gyro, long *accel); -int mpu_register_tap_cb(void (*func)(unsigned char, unsigned char)); - -#endif /* #ifndef _INV_MPU_H_ */ - diff --git a/interface/external/MotionDriver/include/inv_mpu_dmp_motion_driver.h b/interface/external/MotionDriver/include/inv_mpu_dmp_motion_driver.h deleted file mode 100644 index b0afea6caf..0000000000 --- a/interface/external/MotionDriver/include/inv_mpu_dmp_motion_driver.h +++ /dev/null @@ -1,97 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -/** - * @addtogroup DRIVERS Sensor Driver Layer - * @brief Hardware drivers to communicate with sensors via I2C. - * - * @{ - * @file inv_mpu_dmp_motion_driver.h - * @brief DMP image and interface functions. - * @details All functions are preceded by the dmp_ prefix to - * differentiate among MPL and general driver function calls. - */ -#ifndef _INV_MPU_DMP_MOTION_DRIVER_H_ -#define _INV_MPU_DMP_MOTION_DRIVER_H_ - -#define TAP_X (0x01) -#define TAP_Y (0x02) -#define TAP_Z (0x04) -#define TAP_XYZ (0x07) - -#define TAP_X_UP (0x01) -#define TAP_X_DOWN (0x02) -#define TAP_Y_UP (0x03) -#define TAP_Y_DOWN (0x04) -#define TAP_Z_UP (0x05) -#define TAP_Z_DOWN (0x06) - -#define ANDROID_ORIENT_PORTRAIT (0x00) -#define ANDROID_ORIENT_LANDSCAPE (0x01) -#define ANDROID_ORIENT_REVERSE_PORTRAIT (0x02) -#define ANDROID_ORIENT_REVERSE_LANDSCAPE (0x03) - -#define DMP_INT_GESTURE (0x01) -#define DMP_INT_CONTINUOUS (0x02) - -#define DMP_FEATURE_TAP (0x001) -#define DMP_FEATURE_ANDROID_ORIENT (0x002) -#define DMP_FEATURE_LP_QUAT (0x004) -#define DMP_FEATURE_PEDOMETER (0x008) -#define DMP_FEATURE_6X_LP_QUAT (0x010) -#define DMP_FEATURE_GYRO_CAL (0x020) -#define DMP_FEATURE_SEND_RAW_ACCEL (0x040) -#define DMP_FEATURE_SEND_RAW_GYRO (0x080) -#define DMP_FEATURE_SEND_CAL_GYRO (0x100) - -#define INV_WXYZ_QUAT (0x100) - -/* Set up functions. */ -int dmp_load_motion_driver_firmware(void); -int dmp_set_fifo_rate(unsigned short rate); -int dmp_get_fifo_rate(unsigned short *rate); -int dmp_enable_feature(unsigned short mask); -int dmp_get_enabled_features(unsigned short *mask); -int dmp_set_interrupt_mode(unsigned char mode); -int dmp_set_orientation(unsigned short orient); -int dmp_set_gyro_bias(long *bias); -int dmp_set_accel_bias(long *bias); - -/* Tap functions. */ -int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char)); -int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh); -int dmp_set_tap_axes(unsigned char axis); -int dmp_set_tap_count(unsigned char min_taps); -int dmp_set_tap_time(unsigned short time); -int dmp_set_tap_time_multi(unsigned short time); -int dmp_set_shake_reject_thresh(long sf, unsigned short thresh); -int dmp_set_shake_reject_time(unsigned short time); -int dmp_set_shake_reject_timeout(unsigned short time); - -/* Android orientation functions. */ -int dmp_register_android_orient_cb(void (*func)(unsigned char)); - -/* LP quaternion functions. */ -int dmp_enable_lp_quat(unsigned char enable); -int dmp_enable_6x_lp_quat(unsigned char enable); - -/* Pedometer functions. */ -int dmp_get_pedometer_step_count(unsigned long *count); -int dmp_set_pedometer_step_count(unsigned long count); -int dmp_get_pedometer_walk_time(unsigned long *time); -int dmp_set_pedometer_walk_time(unsigned long time); - -/* DMP gyro calibration functions. */ -int dmp_enable_gyro_cal(unsigned char enable); - -/* Read function. This function should be called whenever the MPU interrupt is - * detected. - */ -int dmp_read_fifo(short *gyro, short *accel, long *quat, - unsigned long *timestamp, short *sensors, unsigned char *more); - -#endif /* #ifndef _INV_MPU_DMP_MOTION_DRIVER_H_ */ - diff --git a/interface/external/MotionDriver/include/inv_tty.h b/interface/external/MotionDriver/include/inv_tty.h deleted file mode 100644 index d92ece2017..0000000000 --- a/interface/external/MotionDriver/include/inv_tty.h +++ /dev/null @@ -1,22 +0,0 @@ -// -// inv_tty.h -// interface -// -// Created by Andrzej Kapolka on 7/9/13. -// Copyright (c) 2013 High Fidelity, Inc. All rights reserved. -// - -#ifndef __interface__inv_tty__ -#define __interface__inv_tty__ - -void tty_set_file_descriptor(int file_descriptor); - -int tty_i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data); - -int tty_i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data); - -void tty_delay_ms(unsigned long num_ms); - -void tty_get_ms(unsigned long *count); - -#endif /* defined(__interface__inv_tty__) */ diff --git a/interface/external/MotionDriver/lib/MacOS/libMotionDriver.a b/interface/external/MotionDriver/lib/MacOS/libMotionDriver.a deleted file mode 100644 index 5911eb9f99..0000000000 Binary files a/interface/external/MotionDriver/lib/MacOS/libMotionDriver.a and /dev/null differ diff --git a/interface/external/MotionDriver/lib/UNIX/libMotionDriver.a b/interface/external/MotionDriver/lib/UNIX/libMotionDriver.a deleted file mode 100644 index 82ca139b50..0000000000 Binary files a/interface/external/MotionDriver/lib/UNIX/libMotionDriver.a and /dev/null differ diff --git a/interface/external/MotionDriver/src/inv_mpu.c b/interface/external/MotionDriver/src/inv_mpu.c deleted file mode 100644 index 9969465596..0000000000 --- a/interface/external/MotionDriver/src/inv_mpu.c +++ /dev/null @@ -1,2798 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -/** - * @addtogroup DRIVERS Sensor Driver Layer - * @brief Hardware drivers to communicate with sensors via I2C. - * - * @{ - * @file inv_mpu.c - * @brief An I2C-based driver for Invensense gyroscopes. - * @details This driver currently works for the following devices: - * MPU6050 - * MPU6500 - * MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus) - * MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus) - */ -#include -#include -#include -#include -#include -#include "inv_mpu.h" -#include "inv_tty.h" - -/* The following functions must be defined for this platform: - * i2c_write(unsigned char slave_addr, unsigned char reg_addr, - * unsigned char length, unsigned char const *data) - * i2c_read(unsigned char slave_addr, unsigned char reg_addr, - * unsigned char length, unsigned char *data) - * delay_ms(unsigned long num_ms) - * get_ms(unsigned long *count) - * reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin) - * labs(long x) - * fabsf(float x) - * min(int a, int b) - */ -#if defined MOTION_DRIVER_TARGET_MSP430 -#include "msp430.h" -#include "msp430_i2c.h" -#include "msp430_clock.h" -#include "msp430_interrupt.h" -#define i2c_write msp430_i2c_write -#define i2c_read msp430_i2c_read -#define delay_ms msp430_delay_ms -#define get_ms msp430_get_clock_ms -static inline int reg_int_cb(struct int_param_s *int_param) -{ - return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit, - int_param->active_low); -} -#define log_i(...) do {} while (0) -#define log_e(...) do {} while (0) -/* labs is already defined by TI's toolchain. */ -/* fabs is for doubles. fabsf is for floats. */ -#define fabs fabsf -#define min(a,b) ((acb, int_param->pin, int_param->lp_exit, - int_param->active_low); -} -#define log_i MPL_LOGI -#define log_e MPL_LOGE -/* labs is already defined by TI's toolchain. */ -/* fabs is for doubles. fabsf is for floats. */ -#define fabs fabsf -#define min(a,b) ((apin, int_param->cb, int_param->arg); - return 0; -} -#define log_i MPL_LOGI -#define log_e MPL_LOGE -/* UC3 is a 32-bit processor, so abs and labs are equivalent. */ -#define labs abs -#define fabs(x) (((x)>0)?(x):-(x)) -#else -#define i2c_write tty_i2c_write -#define i2c_read tty_i2c_read -#define delay_ms tty_delay_ms -#define get_ms tty_get_ms -#define min(a,b) ((a> 3 & 0x03 */ - unsigned char gyro_fsr; - /* Matches accel_cfg >> 3 & 0x03 */ - unsigned char accel_fsr; - /* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */ - unsigned char sensors; - /* Matches config register. */ - unsigned char lpf; - unsigned char clk_src; - /* Sample rate, NOT rate divider. */ - unsigned short sample_rate; - /* Matches fifo_en register. */ - unsigned char fifo_enable; - /* Matches int enable register. */ - unsigned char int_enable; - /* 1 if devices on auxiliary I2C bus appear on the primary. */ - unsigned char bypass_mode; - /* 1 if half-sensitivity. - * NOTE: This doesn't belong here, but everything else in hw_s is const, - * and this allows us to save some precious RAM. - */ - unsigned char accel_half; - /* 1 if device in low-power accel-only mode. */ - unsigned char lp_accel_mode; - /* 1 if interrupts are only triggered on motion events. */ - unsigned char int_motion_only; - struct motion_int_cache_s cache; - /* 1 for active low interrupts. */ - unsigned char active_low_int; - /* 1 for latched interrupts. */ - unsigned char latched_int; - /* 1 if DMP is enabled. */ - unsigned char dmp_on; - /* Ensures that DMP will only be loaded once. */ - unsigned char dmp_loaded; - /* Sampling rate used when DMP is enabled. */ - unsigned short dmp_sample_rate; -#ifdef AK89xx_SECONDARY - /* Compass sample rate. */ - unsigned short compass_sample_rate; - unsigned char compass_addr; - short mag_sens_adj[3]; -#endif -}; - -/* Information for self-test. */ -struct test_s { - unsigned long gyro_sens; - unsigned long accel_sens; - unsigned char reg_rate_div; - unsigned char reg_lpf; - unsigned char reg_gyro_fsr; - unsigned char reg_accel_fsr; - unsigned short wait_ms; - unsigned char packet_thresh; - float min_dps; - float max_dps; - float max_gyro_var; - float min_g; - float max_g; - float max_accel_var; -}; - -/* Gyro driver state variables. */ -struct gyro_state_s { - const struct gyro_reg_s *reg; - const struct hw_s *hw; - struct chip_cfg_s chip_cfg; - const struct test_s *test; -}; - -/* Filter configurations. */ -enum lpf_e { - INV_FILTER_256HZ_NOLPF2 = 0, - INV_FILTER_188HZ, - INV_FILTER_98HZ, - INV_FILTER_42HZ, - INV_FILTER_20HZ, - INV_FILTER_10HZ, - INV_FILTER_5HZ, - INV_FILTER_2100HZ_NOLPF, - NUM_FILTER -}; - -/* Full scale ranges. */ -enum gyro_fsr_e { - INV_FSR_250DPS = 0, - INV_FSR_500DPS, - INV_FSR_1000DPS, - INV_FSR_2000DPS, - NUM_GYRO_FSR -}; - -/* Full scale ranges. */ -enum accel_fsr_e { - INV_FSR_2G = 0, - INV_FSR_4G, - INV_FSR_8G, - INV_FSR_16G, - NUM_ACCEL_FSR -}; - -/* Clock sources. */ -enum clock_sel_e { - INV_CLK_INTERNAL = 0, - INV_CLK_PLL, - NUM_CLK -}; - -/* Low-power accel wakeup rates. */ -enum lp_accel_rate_e { -#if defined MPU6050 - INV_LPA_1_25HZ, - INV_LPA_5HZ, - INV_LPA_20HZ, - INV_LPA_40HZ -#elif defined MPU6500 - INV_LPA_0_3125HZ, - INV_LPA_0_625HZ, - INV_LPA_1_25HZ, - INV_LPA_2_5HZ, - INV_LPA_5HZ, - INV_LPA_10HZ, - INV_LPA_20HZ, - INV_LPA_40HZ, - INV_LPA_80HZ, - INV_LPA_160HZ, - INV_LPA_320HZ, - INV_LPA_640HZ -#endif -}; - -#define BIT_I2C_MST_VDDIO (0x80) -#define BIT_FIFO_EN (0x40) -#define BIT_DMP_EN (0x80) -#define BIT_FIFO_RST (0x04) -#define BIT_DMP_RST (0x08) -#define BIT_FIFO_OVERFLOW (0x10) -#define BIT_DATA_RDY_EN (0x01) -#define BIT_DMP_INT_EN (0x02) -#define BIT_MOT_INT_EN (0x40) -#define BITS_FSR (0x18) -#define BITS_LPF (0x07) -#define BITS_HPF (0x07) -#define BITS_CLK (0x07) -#define BIT_FIFO_SIZE_1024 (0x40) -#define BIT_FIFO_SIZE_2048 (0x80) -#define BIT_FIFO_SIZE_4096 (0xC0) -#define BIT_RESET (0x80) -#define BIT_SLEEP (0x40) -#define BIT_S0_DELAY_EN (0x01) -#define BIT_S2_DELAY_EN (0x04) -#define BITS_SLAVE_LENGTH (0x0F) -#define BIT_SLAVE_BYTE_SW (0x40) -#define BIT_SLAVE_GROUP (0x10) -#define BIT_SLAVE_EN (0x80) -#define BIT_I2C_READ (0x80) -#define BITS_I2C_MASTER_DLY (0x1F) -#define BIT_AUX_IF_EN (0x20) -#define BIT_ACTL (0x80) -#define BIT_LATCH_EN (0x20) -#define BIT_ANY_RD_CLR (0x10) -#define BIT_BYPASS_EN (0x02) -#define BITS_WOM_EN (0xC0) -#define BIT_LPA_CYCLE (0x20) -#define BIT_STBY_XA (0x20) -#define BIT_STBY_YA (0x10) -#define BIT_STBY_ZA (0x08) -#define BIT_STBY_XG (0x04) -#define BIT_STBY_YG (0x02) -#define BIT_STBY_ZG (0x01) -#define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA) -#define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG) - -#if defined AK8975_SECONDARY -#define SUPPORTS_AK89xx_HIGH_SENS (0x00) -#define AK89xx_FSR (9830) -#elif defined AK8963_SECONDARY -#define SUPPORTS_AK89xx_HIGH_SENS (0x10) -#define AK89xx_FSR (4915) -#endif - -#ifdef AK89xx_SECONDARY -#define AKM_REG_WHOAMI (0x00) - -#define AKM_REG_ST1 (0x02) -#define AKM_REG_HXL (0x03) -#define AKM_REG_ST2 (0x09) - -#define AKM_REG_CNTL (0x0A) -#define AKM_REG_ASTC (0x0C) -#define AKM_REG_ASAX (0x10) -#define AKM_REG_ASAY (0x11) -#define AKM_REG_ASAZ (0x12) - -#define AKM_DATA_READY (0x01) -#define AKM_DATA_OVERRUN (0x02) -#define AKM_OVERFLOW (0x80) -#define AKM_DATA_ERROR (0x40) - -#define AKM_BIT_SELF_TEST (0x40) - -#define AKM_POWER_DOWN (0x00 | SUPPORTS_AK89xx_HIGH_SENS) -#define AKM_SINGLE_MEASUREMENT (0x01 | SUPPORTS_AK89xx_HIGH_SENS) -#define AKM_FUSE_ROM_ACCESS (0x0F | SUPPORTS_AK89xx_HIGH_SENS) -#define AKM_MODE_SELF_TEST (0x08 | SUPPORTS_AK89xx_HIGH_SENS) - -#define AKM_WHOAMI (0x48) -#endif - -#if defined MPU6050 -const struct gyro_reg_s reg = { - .who_am_i = 0x75, - .rate_div = 0x19, - .lpf = 0x1A, - .prod_id = 0x0C, - .user_ctrl = 0x6A, - .fifo_en = 0x23, - .gyro_cfg = 0x1B, - .accel_cfg = 0x1C, - .motion_thr = 0x1F, - .motion_dur = 0x20, - .fifo_count_h = 0x72, - .fifo_r_w = 0x74, - .raw_gyro = 0x43, - .raw_accel = 0x3B, - .temp = 0x41, - .int_enable = 0x38, - .dmp_int_status = 0x39, - .int_status = 0x3A, - .pwr_mgmt_1 = 0x6B, - .pwr_mgmt_2 = 0x6C, - .int_pin_cfg = 0x37, - .mem_r_w = 0x6F, - .accel_offs = 0x06, - .i2c_mst = 0x24, - .bank_sel = 0x6D, - .mem_start_addr = 0x6E, - .prgm_start_h = 0x70 -#ifdef AK89xx_SECONDARY - ,.raw_compass = 0x49, - .yg_offs_tc = 0x01, - .s0_addr = 0x25, - .s0_reg = 0x26, - .s0_ctrl = 0x27, - .s1_addr = 0x28, - .s1_reg = 0x29, - .s1_ctrl = 0x2A, - .s4_ctrl = 0x34, - .s0_do = 0x63, - .s1_do = 0x64, - .i2c_delay_ctrl = 0x67 -#endif -}; -const struct hw_s hw = { - .addr = 0x68, - .max_fifo = 1024, - .num_reg = 118, - .temp_sens = 340, - .temp_offset = -521, - .bank_size = 256 -#if defined AK89xx_SECONDARY - ,.compass_fsr = AK89xx_FSR -#endif -}; - -const struct test_s test = { - .gyro_sens = 32768/250, - .accel_sens = 32768/16, - .reg_rate_div = 0, /* 1kHz. */ - .reg_lpf = 1, /* 188Hz. */ - .reg_gyro_fsr = 0, /* 250dps. */ - .reg_accel_fsr = 0x18, /* 16g. */ - .wait_ms = 50, - .packet_thresh = 5, /* 5% */ - .min_dps = 10.f, - .max_dps = 105.f, - .max_gyro_var = 0.14f, - .min_g = 0.3f, - .max_g = 0.95f, - .max_accel_var = 0.14f -}; - -static struct gyro_state_s st = { - .reg = ®, - .hw = &hw, - .test = &test -}; -#elif defined MPU6500 -const struct gyro_reg_s reg = { - .who_am_i = 0x75, - .rate_div = 0x19, - .lpf = 0x1A, - .prod_id = 0x0C, - .user_ctrl = 0x6A, - .fifo_en = 0x23, - .gyro_cfg = 0x1B, - .accel_cfg = 0x1C, - .accel_cfg2 = 0x1D, - .lp_accel_odr = 0x1E, - .motion_thr = 0x1F, - .motion_dur = 0x20, - .fifo_count_h = 0x72, - .fifo_r_w = 0x74, - .raw_gyro = 0x43, - .raw_accel = 0x3B, - .temp = 0x41, - .int_enable = 0x38, - .dmp_int_status = 0x39, - .int_status = 0x3A, - .accel_intel = 0x69, - .pwr_mgmt_1 = 0x6B, - .pwr_mgmt_2 = 0x6C, - .int_pin_cfg = 0x37, - .mem_r_w = 0x6F, - .accel_offs = 0x77, - .i2c_mst = 0x24, - .bank_sel = 0x6D, - .mem_start_addr = 0x6E, - .prgm_start_h = 0x70 -#ifdef AK89xx_SECONDARY - ,.raw_compass = 0x49, - .s0_addr = 0x25, - .s0_reg = 0x26, - .s0_ctrl = 0x27, - .s1_addr = 0x28, - .s1_reg = 0x29, - .s1_ctrl = 0x2A, - .s4_ctrl = 0x34, - .s0_do = 0x63, - .s1_do = 0x64, - .i2c_delay_ctrl = 0x67 -#endif -}; -const struct hw_s hw = { - .addr = 0x68, - .max_fifo = 1024, - .num_reg = 128, - .temp_sens = 321, - .temp_offset = 0, - .bank_size = 256 -#if defined AK89xx_SECONDARY - ,.compass_fsr = AK89xx_FSR -#endif -}; - -const struct test_s test = { - .gyro_sens = 32768/250, - .accel_sens = 32768/16, - .reg_rate_div = 0, /* 1kHz. */ - .reg_lpf = 1, /* 188Hz. */ - .reg_gyro_fsr = 0, /* 250dps. */ - .reg_accel_fsr = 0x18, /* 16g. */ - .wait_ms = 50, - .packet_thresh = 5, /* 5% */ - .min_dps = 10.f, - .max_dps = 105.f, - .max_gyro_var = 0.14f, - .min_g = 0.3f, - .max_g = 0.95f, - .max_accel_var = 0.14f -}; - -static struct gyro_state_s st = { - .reg = ®, - .hw = &hw, - .test = &test -}; -#endif - -#define MAX_PACKET_LENGTH (12) - -#ifdef AK89xx_SECONDARY -static int setup_compass(void); -#define MAX_COMPASS_SAMPLE_RATE (100) -#endif - -/** - * @brief Enable/disable data ready interrupt. - * If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready - * interrupt is used. - * @param[in] enable 1 to enable interrupt. - * @return 0 if successful. - */ -static int set_int_enable(unsigned char enable) -{ - unsigned char tmp; - - if (st.chip_cfg.dmp_on) { - if (enable) - tmp = BIT_DMP_INT_EN; - else - tmp = 0x00; - if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp)) - return -1; - st.chip_cfg.int_enable = tmp; - } else { - if (!st.chip_cfg.sensors) - return -1; - if (enable && st.chip_cfg.int_enable) - return 0; - if (enable) - tmp = BIT_DATA_RDY_EN; - else - tmp = 0x00; - if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp)) - return -1; - st.chip_cfg.int_enable = tmp; - } - return 0; -} - -/** - * @brief Register dump for testing. - * @return 0 if successful. - */ -int mpu_reg_dump(void) -{ - unsigned char ii; - unsigned char data; - - for (ii = 0; ii < st.hw->num_reg; ii++) { - if (ii == st.reg->fifo_r_w || ii == st.reg->mem_r_w) - continue; - if (i2c_read(st.hw->addr, ii, 1, &data)) - return -1; - log_i("%#5x: %#5x\r\n", ii, data); - } - return 0; -} - -/** - * @brief Read from a single register. - * NOTE: The memory and FIFO read/write registers cannot be accessed. - * @param[in] reg Register address. - * @param[out] data Register data. - * @return 0 if successful. - */ -int mpu_read_reg(unsigned char reg, unsigned char *data) -{ - if (reg == st.reg->fifo_r_w || reg == st.reg->mem_r_w) - return -1; - if (reg >= st.hw->num_reg) - return -1; - return i2c_read(st.hw->addr, reg, 1, data); -} - -/** - * @brief Initialize hardware. - * Initial configuration:\n - * Gyro FSR: +/- 2000DPS\n - * Accel FSR +/- 2G\n - * DLPF: 42Hz\n - * FIFO rate: 50Hz\n - * Clock source: Gyro PLL\n - * FIFO: Disabled.\n - * Data ready interrupt: Disabled, active low, unlatched. - * @param[in] int_param Platform-specific parameters to interrupt API. - * @return 0 if successful. - */ -int mpu_init(struct int_param_s *int_param) -{ - unsigned char data[6], rev; - - /* Reset device. */ - data[0] = BIT_RESET; - if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) - return -1; - delay_ms(100); - - /* Wake up chip. */ - data[0] = 0x00; - if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) - return -1; - -#if defined MPU6050 - /* Check product revision. */ - if (i2c_read(st.hw->addr, st.reg->accel_offs, 6, data)) - return -1; - rev = ((data[5] & 0x01) << 2) | ((data[3] & 0x01) << 1) | - (data[1] & 0x01); - - if (rev) { - /* Congrats, these parts are better. */ - if (rev == 1) - st.chip_cfg.accel_half = 1; - else if (rev == 2) - st.chip_cfg.accel_half = 0; - else { - log_e("Unsupported software product rev %d.\n", rev); - return -1; - } - } else { - if (i2c_read(st.hw->addr, st.reg->prod_id, 1, data)) - return -1; - rev = data[0] & 0x0F; - if (!rev) { - log_e("Product ID read as 0 indicates device is either " - "incompatible or an MPU3050.\n"); - return -1; - } else if (rev == 4) { - log_i("Half sensitivity part found.\n"); - st.chip_cfg.accel_half = 1; - } else - st.chip_cfg.accel_half = 0; - } -#elif defined MPU6500 -#define MPU6500_MEM_REV_ADDR (0x17) - if (mpu_read_mem(MPU6500_MEM_REV_ADDR, 1, &rev)) - return -1; - if (rev == 0x1) - st.chip_cfg.accel_half = 0; - else { - log_e("Unsupported software product rev %d.\n", rev); - return -1; - } - - /* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the - * first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO. - */ - data[0] = BIT_FIFO_SIZE_1024 | 0x8; - if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, data)) - return -1; -#endif - - /* Set to invalid values to ensure no I2C writes are skipped. */ - st.chip_cfg.sensors = 0xFF; - st.chip_cfg.gyro_fsr = 0xFF; - st.chip_cfg.accel_fsr = 0xFF; - st.chip_cfg.lpf = 0xFF; - st.chip_cfg.sample_rate = 0xFFFF; - st.chip_cfg.fifo_enable = 0xFF; - st.chip_cfg.bypass_mode = 0xFF; -#ifdef AK89xx_SECONDARY - st.chip_cfg.compass_sample_rate = 0xFFFF; -#endif - /* mpu_set_sensors always preserves this setting. */ - st.chip_cfg.clk_src = INV_CLK_PLL; - /* Handled in next call to mpu_set_bypass. */ - st.chip_cfg.active_low_int = 1; - st.chip_cfg.latched_int = 0; - st.chip_cfg.int_motion_only = 0; - st.chip_cfg.lp_accel_mode = 0; - memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache)); - st.chip_cfg.dmp_on = 0; - st.chip_cfg.dmp_loaded = 0; - st.chip_cfg.dmp_sample_rate = 0; - - if (mpu_set_gyro_fsr(2000)) - return -1; - if (mpu_set_accel_fsr(2)) - return -1; - if (mpu_set_lpf(42)) - return -1; - if (mpu_set_sample_rate(50)) - return -1; - if (mpu_configure_fifo(0)) - return -1; - - if (int_param) - reg_int_cb(int_param); - -#ifdef AK89xx_SECONDARY - setup_compass(); - if (mpu_set_compass_sample_rate(10)) - return -1; -#else - /* Already disabled by setup_compass. */ - if (mpu_set_bypass(0)) - return -1; -#endif - - mpu_set_sensors(0); - return 0; -} - -/** - * @brief Enter low-power accel-only mode. - * In low-power accel mode, the chip goes to sleep and only wakes up to sample - * the accelerometer at one of the following frequencies: - * \n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz - * \n MPU6500: 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz - * \n If the requested rate is not one listed above, the device will be set to - * the next highest rate. Requesting a rate above the maximum supported - * frequency will result in an error. - * \n To select a fractional wake-up frequency, round down the value passed to - * @e rate. - * @param[in] rate Minimum sampling rate, or zero to disable LP - * accel mode. - * @return 0 if successful. - */ -int mpu_lp_accel_mode(unsigned char rate) -{ - unsigned char tmp[2]; - - if (rate > 40) - return -1; - - if (!rate) { - mpu_set_int_latched(0); - tmp[0] = 0; - tmp[1] = BIT_STBY_XYZG; - if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp)) - return -1; - st.chip_cfg.lp_accel_mode = 0; - return 0; - } - /* For LP accel, we automatically configure the hardware to produce latched - * interrupts. In LP accel mode, the hardware cycles into sleep mode before - * it gets a chance to deassert the interrupt pin; therefore, we shift this - * responsibility over to the MCU. - * - * Any register read will clear the interrupt. - */ - mpu_set_int_latched(1); -#if defined MPU6050 - tmp[0] = BIT_LPA_CYCLE; - if (rate == 1) { - tmp[1] = INV_LPA_1_25HZ; - mpu_set_lpf(5); - } else if (rate <= 5) { - tmp[1] = INV_LPA_5HZ; - mpu_set_lpf(5); - } else if (rate <= 20) { - tmp[1] = INV_LPA_20HZ; - mpu_set_lpf(10); - } else { - tmp[1] = INV_LPA_40HZ; - mpu_set_lpf(20); - } - tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG; - if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp)) - return -1; -#elif defined MPU6500 - /* Set wake frequency. */ - if (rate == 1) - tmp[0] = INV_LPA_1_25HZ; - else if (rate == 2) - tmp[0] = INV_LPA_2_5HZ; - else if (rate <= 5) - tmp[0] = INV_LPA_5HZ; - else if (rate <= 10) - tmp[0] = INV_LPA_10HZ; - else if (rate <= 20) - tmp[0] = INV_LPA_20HZ; - else if (rate <= 40) - tmp[0] = INV_LPA_40HZ; - else if (rate <= 80) - tmp[0] = INV_LPA_80HZ; - else if (rate <= 160) - tmp[0] = INV_LPA_160HZ; - else if (rate <= 320) - tmp[0] = INV_LPA_320HZ; - else - tmp[0] = INV_LPA_640HZ; - if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp)) - return -1; - tmp[0] = BIT_LPA_CYCLE; - if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp)) - return -1; -#endif - st.chip_cfg.sensors = INV_XYZ_ACCEL; - st.chip_cfg.clk_src = 0; - st.chip_cfg.lp_accel_mode = 1; - mpu_configure_fifo(0); - - return 0; -} - -/** - * @brief Read raw gyro data directly from the registers. - * @param[out] data Raw data in hardware units. - * @param[out] timestamp Timestamp in milliseconds. Null if not needed. - * @return 0 if successful. - */ -int mpu_get_gyro_reg(short *data, unsigned long *timestamp) -{ - unsigned char tmp[6]; - - if (!(st.chip_cfg.sensors & INV_XYZ_GYRO)) - return -1; - - if (i2c_read(st.hw->addr, st.reg->raw_gyro, 6, tmp)) - return -1; - data[0] = (tmp[0] << 8) | tmp[1]; - data[1] = (tmp[2] << 8) | tmp[3]; - data[2] = (tmp[4] << 8) | tmp[5]; - if (timestamp) - get_ms(timestamp); - return 0; -} - -/** - * @brief Read raw accel data directly from the registers. - * @param[out] data Raw data in hardware units. - * @param[out] timestamp Timestamp in milliseconds. Null if not needed. - * @return 0 if successful. - */ -int mpu_get_accel_reg(short *data, unsigned long *timestamp) -{ - unsigned char tmp[6]; - - if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL)) - return -1; - - if (i2c_read(st.hw->addr, st.reg->raw_accel, 6, tmp)) - return -1; - data[0] = (tmp[0] << 8) | tmp[1]; - data[1] = (tmp[2] << 8) | tmp[3]; - data[2] = (tmp[4] << 8) | tmp[5]; - if (timestamp) - get_ms(timestamp); - return 0; -} - -/** - * @brief Read temperature data directly from the registers. - * @param[out] data Data in q16 format. - * @param[out] timestamp Timestamp in milliseconds. Null if not needed. - * @return 0 if successful. - */ -int mpu_get_temperature(long *data, unsigned long *timestamp) -{ - unsigned char tmp[2]; - short raw; - - if (!(st.chip_cfg.sensors)) - return -1; - - if (i2c_read(st.hw->addr, st.reg->temp, 2, tmp)) - return -1; - raw = (tmp[0] << 8) | tmp[1]; - if (timestamp) - get_ms(timestamp); - - data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) / st.hw->temp_sens)) * 65536L); - return 0; -} - -/** - * @brief Push biases to the accel bias registers. - * This function expects biases relative to the current sensor output, and - * these biases will be added to the factory-supplied values. - * @param[in] accel_bias New biases. - * @return 0 if successful. - */ -int mpu_set_accel_bias(const long *accel_bias) -{ - unsigned char data[6]; - short accel_hw[3]; - short got_accel[3]; - short fg[3]; - - if (!accel_bias) - return -1; - if (!accel_bias[0] && !accel_bias[1] && !accel_bias[2]) - return 0; - - if (i2c_read(st.hw->addr, 3, 3, data)) - return -1; - fg[0] = ((data[0] >> 4) + 8) & 0xf; - fg[1] = ((data[1] >> 4) + 8) & 0xf; - fg[2] = ((data[2] >> 4) + 8) & 0xf; - - accel_hw[0] = (short)(accel_bias[0] * 2 / (64 + fg[0])); - accel_hw[1] = (short)(accel_bias[1] * 2 / (64 + fg[1])); - accel_hw[2] = (short)(accel_bias[2] * 2 / (64 + fg[2])); - - if (i2c_read(st.hw->addr, 0x06, 6, data)) - return -1; - - got_accel[0] = ((short)data[0] << 8) | data[1]; - got_accel[1] = ((short)data[2] << 8) | data[3]; - got_accel[2] = ((short)data[4] << 8) | data[5]; - - accel_hw[0] += got_accel[0]; - accel_hw[1] += got_accel[1]; - accel_hw[2] += got_accel[2]; - - data[0] = (accel_hw[0] >> 8) & 0xff; - data[1] = (accel_hw[0]) & 0xff; - data[2] = (accel_hw[1] >> 8) & 0xff; - data[3] = (accel_hw[1]) & 0xff; - data[4] = (accel_hw[2] >> 8) & 0xff; - data[5] = (accel_hw[2]) & 0xff; - - if (i2c_write(st.hw->addr, 0x06, 6, data)) - return -1; - return 0; -} - -/** - * @brief Reset FIFO read/write pointers. - * @return 0 if successful. - */ -int mpu_reset_fifo(void) -{ - unsigned char data; - - if (!(st.chip_cfg.sensors)) - return -1; - - data = 0; - if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data)) - return -1; - if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data)) - return -1; - if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) - return -1; - - if (st.chip_cfg.dmp_on) { - data = BIT_FIFO_RST | BIT_DMP_RST; - if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) - return -1; - delay_ms(50); - data = BIT_DMP_EN | BIT_FIFO_EN; - if (st.chip_cfg.sensors & INV_XYZ_COMPASS) - data |= BIT_AUX_IF_EN; - if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) - return -1; - if (st.chip_cfg.int_enable) - data = BIT_DMP_INT_EN; - else - data = 0; - if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data)) - return -1; - data = 0; - if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data)) - return -1; - } else { - data = BIT_FIFO_RST; - if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) - return -1; - if (st.chip_cfg.bypass_mode || !(st.chip_cfg.sensors & INV_XYZ_COMPASS)) - data = BIT_FIFO_EN; - else - data = BIT_FIFO_EN | BIT_AUX_IF_EN; - if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data)) - return -1; - delay_ms(50); - if (st.chip_cfg.int_enable) - data = BIT_DATA_RDY_EN; - else - data = 0; - if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data)) - return -1; - if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable)) - return -1; - } - return 0; -} - -/** - * @brief Get the gyro full-scale range. - * @param[out] fsr Current full-scale range. - * @return 0 if successful. - */ -int mpu_get_gyro_fsr(unsigned short *fsr) -{ - switch (st.chip_cfg.gyro_fsr) { - case INV_FSR_250DPS: - fsr[0] = 250; - break; - case INV_FSR_500DPS: - fsr[0] = 500; - break; - case INV_FSR_1000DPS: - fsr[0] = 1000; - break; - case INV_FSR_2000DPS: - fsr[0] = 2000; - break; - default: - fsr[0] = 0; - break; - } - return 0; -} - -/** - * @brief Set the gyro full-scale range. - * @param[in] fsr Desired full-scale range. - * @return 0 if successful. - */ -int mpu_set_gyro_fsr(unsigned short fsr) -{ - unsigned char data; - - if (!(st.chip_cfg.sensors)) - return -1; - - switch (fsr) { - case 250: - data = INV_FSR_250DPS << 3; - break; - case 500: - data = INV_FSR_500DPS << 3; - break; - case 1000: - data = INV_FSR_1000DPS << 3; - break; - case 2000: - data = INV_FSR_2000DPS << 3; - break; - default: - return -1; - } - - if (st.chip_cfg.gyro_fsr == (data >> 3)) - return 0; - if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, &data)) - return -1; - st.chip_cfg.gyro_fsr = data >> 3; - return 0; -} - -/** - * @brief Get the accel full-scale range. - * @param[out] fsr Current full-scale range. - * @return 0 if successful. - */ -int mpu_get_accel_fsr(unsigned char *fsr) -{ - switch (st.chip_cfg.accel_fsr) { - case INV_FSR_2G: - fsr[0] = 2; - break; - case INV_FSR_4G: - fsr[0] = 4; - break; - case INV_FSR_8G: - fsr[0] = 8; - break; - case INV_FSR_16G: - fsr[0] = 16; - break; - default: - return -1; - } - if (st.chip_cfg.accel_half) - fsr[0] <<= 1; - return 0; -} - -/** - * @brief Set the accel full-scale range. - * @param[in] fsr Desired full-scale range. - * @return 0 if successful. - */ -int mpu_set_accel_fsr(unsigned char fsr) -{ - unsigned char data; - - if (!(st.chip_cfg.sensors)) - return -1; - - switch (fsr) { - case 2: - data = INV_FSR_2G << 3; - break; - case 4: - data = INV_FSR_4G << 3; - break; - case 8: - data = INV_FSR_8G << 3; - break; - case 16: - data = INV_FSR_16G << 3; - break; - default: - return -1; - } - - if (st.chip_cfg.accel_fsr == (data >> 3)) - return 0; - if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, &data)) - return -1; - st.chip_cfg.accel_fsr = data >> 3; - return 0; -} - -/** - * @brief Get the current DLPF setting. - * @param[out] lpf Current LPF setting. - * 0 if successful. - */ -int mpu_get_lpf(unsigned short *lpf) -{ - switch (st.chip_cfg.lpf) { - case INV_FILTER_188HZ: - lpf[0] = 188; - break; - case INV_FILTER_98HZ: - lpf[0] = 98; - break; - case INV_FILTER_42HZ: - lpf[0] = 42; - break; - case INV_FILTER_20HZ: - lpf[0] = 20; - break; - case INV_FILTER_10HZ: - lpf[0] = 10; - break; - case INV_FILTER_5HZ: - lpf[0] = 5; - break; - case INV_FILTER_256HZ_NOLPF2: - case INV_FILTER_2100HZ_NOLPF: - default: - lpf[0] = 0; - break; - } - return 0; -} - -/** - * @brief Set digital low pass filter. - * The following LPF settings are supported: 188, 98, 42, 20, 10, 5. - * @param[in] lpf Desired LPF setting. - * @return 0 if successful. - */ -int mpu_set_lpf(unsigned short lpf) -{ - unsigned char data; - - if (!(st.chip_cfg.sensors)) - return -1; - - if (lpf >= 188) - data = INV_FILTER_188HZ; - else if (lpf >= 98) - data = INV_FILTER_98HZ; - else if (lpf >= 42) - data = INV_FILTER_42HZ; - else if (lpf >= 20) - data = INV_FILTER_20HZ; - else if (lpf >= 10) - data = INV_FILTER_10HZ; - else - data = INV_FILTER_5HZ; - - if (st.chip_cfg.lpf == data) - return 0; - if (i2c_write(st.hw->addr, st.reg->lpf, 1, &data)) - return -1; - st.chip_cfg.lpf = data; - return 0; -} - -/** - * @brief Get sampling rate. - * @param[out] rate Current sampling rate (Hz). - * @return 0 if successful. - */ -int mpu_get_sample_rate(unsigned short *rate) -{ - if (st.chip_cfg.dmp_on) - return -1; - else - rate[0] = st.chip_cfg.sample_rate; - return 0; -} - -/** - * @brief Set sampling rate. - * Sampling rate must be between 4Hz and 1kHz. - * @param[in] rate Desired sampling rate (Hz). - * @return 0 if successful. - */ -int mpu_set_sample_rate(unsigned short rate) -{ - unsigned char data; - - if (!(st.chip_cfg.sensors)) - return -1; - - if (st.chip_cfg.dmp_on) - return -1; - else { - if (st.chip_cfg.lp_accel_mode) { - if (rate && (rate <= 40)) { - /* Just stay in low-power accel mode. */ - mpu_lp_accel_mode(rate); - return 0; - } - /* Requested rate exceeds the allowed frequencies in LP accel mode, - * switch back to full-power mode. - */ - mpu_lp_accel_mode(0); - } - if (rate < 4) - rate = 4; - else if (rate > 1000) - rate = 1000; - - data = 1000 / rate - 1; - if (i2c_write(st.hw->addr, st.reg->rate_div, 1, &data)) - return -1; - - st.chip_cfg.sample_rate = 1000 / (1 + data); - -#ifdef AK89xx_SECONDARY - mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE)); -#endif - - /* Automatically set LPF to 1/2 sampling rate. */ - mpu_set_lpf(st.chip_cfg.sample_rate >> 1); - return 0; - } -} - -/** - * @brief Get compass sampling rate. - * @param[out] rate Current compass sampling rate (Hz). - * @return 0 if successful. - */ -int mpu_get_compass_sample_rate(unsigned short *rate) -{ -#ifdef AK89xx_SECONDARY - rate[0] = st.chip_cfg.compass_sample_rate; - return 0; -#else - rate[0] = 0; - return -1; -#endif -} - -/** - * @brief Set compass sampling rate. - * The compass on the auxiliary I2C bus is read by the MPU hardware at a - * maximum of 100Hz. The actual rate can be set to a fraction of the gyro - * sampling rate. - * - * \n WARNING: The new rate may be different than what was requested. Call - * mpu_get_compass_sample_rate to check the actual setting. - * @param[in] rate Desired compass sampling rate (Hz). - * @return 0 if successful. - */ -int mpu_set_compass_sample_rate(unsigned short rate) -{ -#ifdef AK89xx_SECONDARY - unsigned char div; - if (!rate || rate > st.chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE) - return -1; - - div = st.chip_cfg.sample_rate / rate - 1; - if (i2c_write(st.hw->addr, st.reg->s4_ctrl, 1, &div)) - return -1; - st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate / (div + 1); - return 0; -#else - return -1; -#endif -} - -/** - * @brief Get gyro sensitivity scale factor. - * @param[out] sens Conversion from hardware units to dps. - * @return 0 if successful. - */ -int mpu_get_gyro_sens(float *sens) -{ - switch (st.chip_cfg.gyro_fsr) { - case INV_FSR_250DPS: - sens[0] = 131.f; - break; - case INV_FSR_500DPS: - sens[0] = 65.5f; - break; - case INV_FSR_1000DPS: - sens[0] = 32.8f; - break; - case INV_FSR_2000DPS: - sens[0] = 16.4f; - break; - default: - return -1; - } - return 0; -} - -/** - * @brief Get accel sensitivity scale factor. - * @param[out] sens Conversion from hardware units to g's. - * @return 0 if successful. - */ -int mpu_get_accel_sens(unsigned short *sens) -{ - switch (st.chip_cfg.accel_fsr) { - case INV_FSR_2G: - sens[0] = 16384; - break; - case INV_FSR_4G: - sens[0] = 8092; - break; - case INV_FSR_8G: - sens[0] = 4096; - break; - case INV_FSR_16G: - sens[0] = 2048; - break; - default: - return -1; - } - if (st.chip_cfg.accel_half) - sens[0] >>= 1; - return 0; -} - -/** - * @brief Get current FIFO configuration. - * @e sensors can contain a combination of the following flags: - * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO - * \n INV_XYZ_GYRO - * \n INV_XYZ_ACCEL - * @param[out] sensors Mask of sensors in FIFO. - * @return 0 if successful. - */ -int mpu_get_fifo_config(unsigned char *sensors) -{ - sensors[0] = st.chip_cfg.fifo_enable; - return 0; -} - -/** - * @brief Select which sensors are pushed to FIFO. - * @e sensors can contain a combination of the following flags: - * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO - * \n INV_XYZ_GYRO - * \n INV_XYZ_ACCEL - * @param[in] sensors Mask of sensors to push to FIFO. - * @return 0 if successful. - */ -int mpu_configure_fifo(unsigned char sensors) -{ - unsigned char prev; - int result = 0; - - /* Compass data isn't going into the FIFO. Stop trying. */ - sensors &= ~INV_XYZ_COMPASS; - - if (st.chip_cfg.dmp_on) - return 0; - else { - if (!(st.chip_cfg.sensors)) - return -1; - prev = st.chip_cfg.fifo_enable; - st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors; - if (st.chip_cfg.fifo_enable != sensors) - /* You're not getting what you asked for. Some sensors are - * asleep. - */ - result = -1; - else - result = 0; - if (sensors || st.chip_cfg.lp_accel_mode) - set_int_enable(1); - else - set_int_enable(0); - if (sensors) { - if (mpu_reset_fifo()) { - st.chip_cfg.fifo_enable = prev; - return -1; - } - } - } - - return result; -} - -/** - * @brief Get current power state. - * @param[in] power_on 1 if turned on, 0 if suspended. - * @return 0 if successful. - */ -int mpu_get_power_state(unsigned char *power_on) -{ - if (st.chip_cfg.sensors) - power_on[0] = 1; - else - power_on[0] = 0; - return 0; -} - -/** - * @brief Turn specific sensors on/off. - * @e sensors can contain a combination of the following flags: - * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO - * \n INV_XYZ_GYRO - * \n INV_XYZ_ACCEL - * \n INV_XYZ_COMPASS - * @param[in] sensors Mask of sensors to wake. - * @return 0 if successful. - */ -int mpu_set_sensors(unsigned char sensors) -{ - unsigned char data; -#ifdef AK89xx_SECONDARY - unsigned char user_ctrl; -#endif - - if (sensors & INV_XYZ_GYRO) - data = INV_CLK_PLL; - else if (sensors) - data = 0; - else - data = BIT_SLEEP; - if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) { - st.chip_cfg.sensors = 0; - return -1; - } - st.chip_cfg.clk_src = data & ~BIT_SLEEP; - - data = 0; - if (!(sensors & INV_X_GYRO)) - data |= BIT_STBY_XG; - if (!(sensors & INV_Y_GYRO)) - data |= BIT_STBY_YG; - if (!(sensors & INV_Z_GYRO)) - data |= BIT_STBY_ZG; - if (!(sensors & INV_XYZ_ACCEL)) - data |= BIT_STBY_XYZA; - if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) { - st.chip_cfg.sensors = 0; - return -1; - } - - if (sensors && (sensors != INV_XYZ_ACCEL)) - /* Latched interrupts only used in LP accel mode. */ - mpu_set_int_latched(0); - -#ifdef AK89xx_SECONDARY -#ifdef AK89xx_BYPASS - if (sensors & INV_XYZ_COMPASS) - mpu_set_bypass(1); - else - mpu_set_bypass(0); -#else - if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl)) - return -1; - /* Handle AKM power management. */ - if (sensors & INV_XYZ_COMPASS) { - data = AKM_SINGLE_MEASUREMENT; - user_ctrl |= BIT_AUX_IF_EN; - } else { - data = AKM_POWER_DOWN; - user_ctrl &= ~BIT_AUX_IF_EN; - } - if (st.chip_cfg.dmp_on) - user_ctrl |= BIT_DMP_EN; - else - user_ctrl &= ~BIT_DMP_EN; - if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data)) - return -1; - /* Enable/disable I2C master mode. */ - if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl)) - return -1; -#endif -#endif - - st.chip_cfg.sensors = sensors; - st.chip_cfg.lp_accel_mode = 0; - delay_ms(50); - return 0; -} - -/** - * @brief Read the MPU interrupt status registers. - * @param[out] status Mask of interrupt bits. - * @return 0 if successful. - */ -int mpu_get_int_status(short *status) -{ - unsigned char tmp[2]; - if (!st.chip_cfg.sensors) - return -1; - if (i2c_read(st.hw->addr, st.reg->dmp_int_status, 2, tmp)) - return -1; - status[0] = (tmp[0] << 8) | tmp[1]; - return 0; -} - -/** - * @brief Get one packet from the FIFO. - * If @e sensors does not contain a particular sensor, disregard the data - * returned to that pointer. - * \n @e sensors can contain a combination of the following flags: - * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO - * \n INV_XYZ_GYRO - * \n INV_XYZ_ACCEL - * \n If the FIFO has no new data, @e sensors will be zero. - * \n If the FIFO is disabled, @e sensors will be zero and this function will - * return a non-zero error code. - * @param[out] gyro Gyro data in hardware units. - * @param[out] accel Accel data in hardware units. - * @param[out] timestamp Timestamp in milliseconds. - * @param[out] sensors Mask of sensors read from FIFO. - * @param[out] more Number of remaining packets. - * @return 0 if successful. - */ -int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp, - unsigned char *sensors, unsigned char *more) -{ - /* Assumes maximum packet size is gyro (6) + accel (6). */ - unsigned char data[MAX_PACKET_LENGTH]; - unsigned char packet_size = 0; - unsigned short fifo_count, index = 0; - - if (st.chip_cfg.dmp_on) - return -1; - - sensors[0] = 0; - if (!st.chip_cfg.sensors) - return -1; - if (!st.chip_cfg.fifo_enable) - return -1; - - if (st.chip_cfg.fifo_enable & INV_X_GYRO) - packet_size += 2; - if (st.chip_cfg.fifo_enable & INV_Y_GYRO) - packet_size += 2; - if (st.chip_cfg.fifo_enable & INV_Z_GYRO) - packet_size += 2; - if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) - packet_size += 6; - - if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) - return -1; - fifo_count = (data[0] << 8) | data[1]; - if (fifo_count < packet_size) - return 0; -// log_i("FIFO count: %hd\n", fifo_count); - if (fifo_count > (st.hw->max_fifo >> 1)) { - /* FIFO is 50% full, better check overflow bit. */ - if (i2c_read(st.hw->addr, st.reg->int_status, 1, data)) - return -1; - if (data[0] & BIT_FIFO_OVERFLOW) { - mpu_reset_fifo(); - return -2; - } - } - get_ms((unsigned long*)timestamp); - - if (i2c_read(st.hw->addr, st.reg->fifo_r_w, packet_size, data)) - return -1; - more[0] = fifo_count / packet_size - 1; - sensors[0] = 0; - - if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) { - accel[0] = (data[index+0] << 8) | data[index+1]; - accel[1] = (data[index+2] << 8) | data[index+3]; - accel[2] = (data[index+4] << 8) | data[index+5]; - sensors[0] |= INV_XYZ_ACCEL; - index += 6; - } - if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) { - gyro[0] = (data[index+0] << 8) | data[index+1]; - sensors[0] |= INV_X_GYRO; - index += 2; - } - if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) { - gyro[1] = (data[index+0] << 8) | data[index+1]; - sensors[0] |= INV_Y_GYRO; - index += 2; - } - if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) { - gyro[2] = (data[index+0] << 8) | data[index+1]; - sensors[0] |= INV_Z_GYRO; - index += 2; - } - - return 0; -} - -/** - * @brief Get one unparsed packet from the FIFO. - * This function should be used if the packet is to be parsed elsewhere. - * @param[in] length Length of one FIFO packet. - * @param[in] data FIFO packet. - * @param[in] more Number of remaining packets. - */ -int mpu_read_fifo_stream(unsigned short length, unsigned char *data, - unsigned char *more) -{ - unsigned char tmp[2]; - unsigned short fifo_count; - if (!st.chip_cfg.dmp_on) - return -1; - if (!st.chip_cfg.sensors) - return -1; - - if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp)) - return -1; - fifo_count = (tmp[0] << 8) | tmp[1]; - if (fifo_count < length) { - more[0] = 0; - return -1; - } - if (fifo_count > (st.hw->max_fifo >> 1)) { - /* FIFO is 50% full, better check overflow bit. */ - if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp)) - return -1; - if (tmp[0] & BIT_FIFO_OVERFLOW) { - mpu_reset_fifo(); - return -2; - } - } - - if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data)) - return -1; - more[0] = fifo_count / length - 1; - return 0; -} - -/** - * @brief Set device to bypass mode. - * @param[in] bypass_on 1 to enable bypass mode. - * @return 0 if successful. - */ -int mpu_set_bypass(unsigned char bypass_on) -{ - unsigned char tmp; - - if (st.chip_cfg.bypass_mode == bypass_on) - return 0; - - if (bypass_on) { - if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) - return -1; - tmp &= ~BIT_AUX_IF_EN; - if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) - return -1; - delay_ms(3); - tmp = BIT_BYPASS_EN; - if (st.chip_cfg.active_low_int) - tmp |= BIT_ACTL; - if (st.chip_cfg.latched_int) - tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR; - if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) - return -1; - } else { - /* Enable I2C master mode if compass is being used. */ - if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) - return -1; - if (st.chip_cfg.sensors & INV_XYZ_COMPASS) - tmp |= BIT_AUX_IF_EN; - else - tmp &= ~BIT_AUX_IF_EN; - if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp)) - return -1; - delay_ms(3); - if (st.chip_cfg.active_low_int) - tmp = BIT_ACTL; - else - tmp = 0; - if (st.chip_cfg.latched_int) - tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR; - if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) - return -1; - } - st.chip_cfg.bypass_mode = bypass_on; - return 0; -} - -/** - * @brief Set interrupt level. - * @param[in] active_low 1 for active low, 0 for active high. - * @return 0 if successful. - */ -int mpu_set_int_level(unsigned char active_low) -{ - st.chip_cfg.active_low_int = active_low; - return 0; -} - -/** - * @brief Enable latched interrupts. - * Any MPU register will clear the interrupt. - * @param[in] enable 1 to enable, 0 to disable. - * @return 0 if successful. - */ -int mpu_set_int_latched(unsigned char enable) -{ - unsigned char tmp; - if (st.chip_cfg.latched_int == enable) - return 0; - - if (enable) - tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR; - else - tmp = 0; - if (st.chip_cfg.bypass_mode) - tmp |= BIT_BYPASS_EN; - if (st.chip_cfg.active_low_int) - tmp |= BIT_ACTL; - if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp)) - return -1; - st.chip_cfg.latched_int = enable; - return 0; -} - -#ifdef MPU6050 -static int get_accel_prod_shift(float *st_shift) -{ - unsigned char tmp[4], shift_code[3], ii; - - if (i2c_read(st.hw->addr, 0x0D, 4, tmp)) - return 0x07; - - shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4); - shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2); - shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03); - for (ii = 0; ii < 3; ii++) { - if (!shift_code[ii]) { - st_shift[ii] = 0.f; - continue; - } - /* Equivalent to.. - * st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f) - */ - st_shift[ii] = 0.34f; - while (--shift_code[ii]) - st_shift[ii] *= 1.034f; - } - return 0; -} - -static int accel_self_test(long *bias_regular, long *bias_st) -{ - int jj, result = 0; - float st_shift[3], st_shift_cust, st_shift_var; - - get_accel_prod_shift(st_shift); - for(jj = 0; jj < 3; jj++) { - st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f; - if (st_shift[jj]) { - st_shift_var = st_shift_cust / st_shift[jj] - 1.f; - if (fabs(st_shift_var) > test.max_accel_var) - result |= 1 << jj; - } else if ((st_shift_cust < test.min_g) || - (st_shift_cust > test.max_g)) - result |= 1 << jj; - } - - return result; -} - -static int gyro_self_test(long *bias_regular, long *bias_st) -{ - int jj, result = 0; - unsigned char tmp[3]; - float st_shift, st_shift_cust, st_shift_var; - - if (i2c_read(st.hw->addr, 0x0D, 3, tmp)) - return 0x07; - - tmp[0] &= 0x1F; - tmp[1] &= 0x1F; - tmp[2] &= 0x1F; - - for (jj = 0; jj < 3; jj++) { - st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f; - if (tmp[jj]) { - st_shift = 3275.f / test.gyro_sens; - while (--tmp[jj]) - st_shift *= 1.046f; - st_shift_var = st_shift_cust / st_shift - 1.f; - if (fabs(st_shift_var) > test.max_gyro_var) - result |= 1 << jj; - } else if ((st_shift_cust < test.min_dps) || - (st_shift_cust > test.max_dps)) - result |= 1 << jj; - } - return result; -} - -#ifdef AK89xx_SECONDARY -static int compass_self_test(void) -{ - unsigned char tmp[6]; - unsigned char tries = 10; - int result = 0x07; - short data; - - mpu_set_bypass(1); - - tmp[0] = AKM_POWER_DOWN; - if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp)) - return 0x07; - tmp[0] = AKM_BIT_SELF_TEST; - if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp)) - goto AKM_restore; - tmp[0] = AKM_MODE_SELF_TEST; - if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp)) - goto AKM_restore; - - do { - delay_ms(10); - if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp)) - goto AKM_restore; - if (tmp[0] & AKM_DATA_READY) - break; - } while (tries--); - if (!(tmp[0] & AKM_DATA_READY)) - goto AKM_restore; - - if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp)) - goto AKM_restore; - - result = 0; - data = (short)(tmp[1] << 8) | tmp[0]; - if ((data > 100) || (data < -100)) - result |= 0x01; - data = (short)(tmp[3] << 8) | tmp[2]; - if ((data > 100) || (data < -100)) - result |= 0x02; - data = (short)(tmp[5] << 8) | tmp[4]; - if ((data > -300) || (data < -1000)) - result |= 0x04; - -AKM_restore: - tmp[0] = 0 | SUPPORTS_AK89xx_HIGH_SENS; - i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp); - tmp[0] = SUPPORTS_AK89xx_HIGH_SENS; - i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp); - mpu_set_bypass(0); - return result; -} -#endif -#endif - -static int get_st_biases(long *gyro, long *accel, unsigned char hw_test) -{ - unsigned char data[MAX_PACKET_LENGTH]; - unsigned char packet_count, ii; - unsigned short fifo_count; - - data[0] = 0x01; - data[1] = 0; - if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data)) - return -1; - delay_ms(200); - data[0] = 0; - if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) - return -1; - if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) - return -1; - if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) - return -1; - if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data)) - return -1; - if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) - return -1; - data[0] = BIT_FIFO_RST | BIT_DMP_RST; - if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) - return -1; - delay_ms(15); - data[0] = st.test->reg_lpf; - if (i2c_write(st.hw->addr, st.reg->lpf, 1, data)) - return -1; - data[0] = st.test->reg_rate_div; - if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data)) - return -1; - if (hw_test) - data[0] = st.test->reg_gyro_fsr | 0xE0; - else - data[0] = st.test->reg_gyro_fsr; - if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data)) - return -1; - - if (hw_test) - data[0] = st.test->reg_accel_fsr | 0xE0; - else - data[0] = test.reg_accel_fsr; - if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data)) - return -1; - if (hw_test) - delay_ms(200); - - /* Fill FIFO for test.wait_ms milliseconds. */ - data[0] = BIT_FIFO_EN; - if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data)) - return -1; - - data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL; - if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) - return -1; - delay_ms(test.wait_ms); - data[0] = 0; - if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data)) - return -1; - - if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data)) - return -1; - - fifo_count = (data[0] << 8) | data[1]; - packet_count = fifo_count / MAX_PACKET_LENGTH; - gyro[0] = gyro[1] = gyro[2] = 0; - accel[0] = accel[1] = accel[2] = 0; - - for (ii = 0; ii < packet_count; ii++) { - short accel_cur[3], gyro_cur[3]; - if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data)) - return -1; - accel_cur[0] = ((short)data[0] << 8) | data[1]; - accel_cur[1] = ((short)data[2] << 8) | data[3]; - accel_cur[2] = ((short)data[4] << 8) | data[5]; - accel[0] += (long)accel_cur[0]; - accel[1] += (long)accel_cur[1]; - accel[2] += (long)accel_cur[2]; - gyro_cur[0] = (((short)data[6] << 8) | data[7]); - gyro_cur[1] = (((short)data[8] << 8) | data[9]); - gyro_cur[2] = (((short)data[10] << 8) | data[11]); - gyro[0] += (long)gyro_cur[0]; - gyro[1] += (long)gyro_cur[1]; - gyro[2] += (long)gyro_cur[2]; - } -#ifdef EMPL_NO_64BIT - gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count); - gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count); - gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count); - if (has_accel) { - accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens / - packet_count); - accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens / - packet_count); - accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens / - packet_count); - /* Don't remove gravity! */ - accel[2] -= 65536L; - } -#else - gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count); - gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count); - gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count); - accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens / - packet_count); - accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens / - packet_count); - accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens / - packet_count); - /* Don't remove gravity! */ - if (accel[2] > 0L) - accel[2] -= 65536L; - else - accel[2] += 65536L; -#endif - - return 0; -} - -/** - * @brief Trigger gyro/accel/compass self-test. - * On success/error, the self-test returns a mask representing the sensor(s) - * that failed. For each bit, a one (1) represents a "pass" case; conversely, - * a zero (0) indicates a failure. - * - * \n The mask is defined as follows: - * \n Bit 0: Gyro. - * \n Bit 1: Accel. - * \n Bit 2: Compass. - * - * \n Currently, the hardware self-test is unsupported for MPU6500. However, - * this function can still be used to obtain the accel and gyro biases. - * - * \n This function must be called with the device either face-up or face-down - * (z-axis is parallel to gravity). - * @param[out] gyro Gyro biases in q16 format. - * @param[out] accel Accel biases (if applicable) in q16 format. - * @return Result mask (see above). - */ -int mpu_run_self_test(long *gyro, long *accel) -{ -#ifdef MPU6050 - const unsigned char tries = 2; - long gyro_st[3], accel_st[3]; - unsigned char accel_result, gyro_result; -#ifdef AK89xx_SECONDARY - unsigned char compass_result; -#endif - int ii; -#endif - int result; - unsigned char accel_fsr, fifo_sensors, sensors_on; - unsigned short gyro_fsr, sample_rate, lpf; - unsigned char dmp_was_on; - - if (st.chip_cfg.dmp_on) { - mpu_set_dmp_state(0); - dmp_was_on = 1; - } else - dmp_was_on = 0; - - /* Get initial settings. */ - mpu_get_gyro_fsr(&gyro_fsr); - mpu_get_accel_fsr(&accel_fsr); - mpu_get_lpf(&lpf); - mpu_get_sample_rate(&sample_rate); - sensors_on = st.chip_cfg.sensors; - mpu_get_fifo_config(&fifo_sensors); - - /* For older chips, the self-test will be different. */ -#if defined MPU6050 - for (ii = 0; ii < tries; ii++) - if (!get_st_biases(gyro, accel, 0)) - break; - if (ii == tries) { - /* If we reach this point, we most likely encountered an I2C error. - * We'll just report an error for all three sensors. - */ - result = 0; - goto restore; - } - for (ii = 0; ii < tries; ii++) - if (!get_st_biases(gyro_st, accel_st, 1)) - break; - if (ii == tries) { - /* Again, probably an I2C error. */ - result = 0; - goto restore; - } - accel_result = accel_self_test(accel, accel_st); - gyro_result = gyro_self_test(gyro, gyro_st); - - result = 0; - if (!gyro_result) - result |= 0x01; - if (!accel_result) - result |= 0x02; - -#ifdef AK89xx_SECONDARY - compass_result = compass_self_test(); - if (!compass_result) - result |= 0x04; -#endif -restore: -#elif defined MPU6500 - /* For now, this function will return a "pass" result for all three sensors - * for compatibility with current test applications. - */ - get_st_biases(gyro, accel, 0); - result = 0x7; -#endif - /* Set to invalid values to ensure no I2C writes are skipped. */ - st.chip_cfg.gyro_fsr = 0xFF; - st.chip_cfg.accel_fsr = 0xFF; - st.chip_cfg.lpf = 0xFF; - st.chip_cfg.sample_rate = 0xFFFF; - st.chip_cfg.sensors = 0xFF; - st.chip_cfg.fifo_enable = 0xFF; - st.chip_cfg.clk_src = INV_CLK_PLL; - mpu_set_gyro_fsr(gyro_fsr); - mpu_set_accel_fsr(accel_fsr); - mpu_set_lpf(lpf); - mpu_set_sample_rate(sample_rate); - mpu_set_sensors(sensors_on); - mpu_configure_fifo(fifo_sensors); - - if (dmp_was_on) - mpu_set_dmp_state(1); - - return result; -} - -/** - * @brief Write to the DMP memory. - * This function prevents I2C writes past the bank boundaries. The DMP memory - * is only accessible when the chip is awake. - * @param[in] mem_addr Memory location (bank << 8 | start address) - * @param[in] length Number of bytes to write. - * @param[in] data Bytes to write to memory. - * @return 0 if successful. - */ -int mpu_write_mem(unsigned short mem_addr, unsigned short length, - unsigned char *data) -{ - unsigned char tmp[2]; - - if (!data) - return -1; - if (!st.chip_cfg.sensors) - return -1; - - tmp[0] = (unsigned char)(mem_addr >> 8); - tmp[1] = (unsigned char)(mem_addr & 0xFF); - - /* Check bank boundaries. */ - if (tmp[1] + length > st.hw->bank_size) - return -1; - - if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp)) - return -1; - if (i2c_write(st.hw->addr, st.reg->mem_r_w, length, data)) - return -1; - return 0; -} - -/** - * @brief Read from the DMP memory. - * This function prevents I2C reads past the bank boundaries. The DMP memory - * is only accessible when the chip is awake. - * @param[in] mem_addr Memory location (bank << 8 | start address) - * @param[in] length Number of bytes to read. - * @param[out] data Bytes read from memory. - * @return 0 if successful. - */ -int mpu_read_mem(unsigned short mem_addr, unsigned short length, - unsigned char *data) -{ - unsigned char tmp[2]; - - if (!data) - return -1; - if (!st.chip_cfg.sensors) - return -1; - - tmp[0] = (unsigned char)(mem_addr >> 8); - tmp[1] = (unsigned char)(mem_addr & 0xFF); - - /* Check bank boundaries. */ - if (tmp[1] + length > st.hw->bank_size) - return -1; - - if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp)) - return -1; - if (i2c_read(st.hw->addr, st.reg->mem_r_w, length, data)) - return -1; - return 0; -} - -/** - * @brief Load and verify DMP image. - * @param[in] length Length of DMP image. - * @param[in] firmware DMP code. - * @param[in] start_addr Starting address of DMP code memory. - * @param[in] sample_rate Fixed sampling rate used when DMP is enabled. - * @return 0 if successful. - */ -int mpu_load_firmware(unsigned short length, const unsigned char *firmware, - unsigned short start_addr, unsigned short sample_rate) -{ - unsigned short ii; - unsigned short this_write; - /* Must divide evenly into st.hw->bank_size to avoid bank crossings. */ -#define LOAD_CHUNK (16) - unsigned char cur[LOAD_CHUNK], tmp[2]; - - if (st.chip_cfg.dmp_loaded) - /* DMP should only be loaded once. */ - return -1; - - if (!firmware) - return -1; - for (ii = 0; ii < length; ii += this_write) { - this_write = min(LOAD_CHUNK, length - ii); - if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii])) - return -1; - if (mpu_read_mem(ii, this_write, cur)) - return -1; - if (memcmp(firmware+ii, cur, this_write)) - return -2; - } - - /* Set program start address. */ - tmp[0] = start_addr >> 8; - tmp[1] = start_addr & 0xFF; - if (i2c_write(st.hw->addr, st.reg->prgm_start_h, 2, tmp)) - return -1; - - st.chip_cfg.dmp_loaded = 1; - st.chip_cfg.dmp_sample_rate = sample_rate; - return 0; -} - -/** - * @brief Enable/disable DMP support. - * @param[in] enable 1 to turn on the DMP. - * @return 0 if successful. - */ -int mpu_set_dmp_state(unsigned char enable) -{ - unsigned char tmp; - if (st.chip_cfg.dmp_on == enable) - return 0; - - if (enable) { - if (!st.chip_cfg.dmp_loaded) - return -1; - /* Disable data ready interrupt. */ - set_int_enable(0); - /* Disable bypass mode. */ - mpu_set_bypass(0); - /* Keep constant sample rate, FIFO rate controlled by DMP. */ - mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate); - /* Remove FIFO elements. */ - tmp = 0; - i2c_write(st.hw->addr, 0x23, 1, &tmp); - st.chip_cfg.dmp_on = 1; - /* Enable DMP interrupt. */ - set_int_enable(1); - mpu_reset_fifo(); - } else { - /* Disable DMP interrupt. */ - set_int_enable(0); - /* Restore FIFO settings. */ - tmp = st.chip_cfg.fifo_enable; - i2c_write(st.hw->addr, 0x23, 1, &tmp); - st.chip_cfg.dmp_on = 0; - mpu_reset_fifo(); - } - return 0; -} - -/** - * @brief Get DMP state. - * @param[out] enabled 1 if enabled. - * @return 0 if successful. - */ -int mpu_get_dmp_state(unsigned char *enabled) -{ - enabled[0] = st.chip_cfg.dmp_on; - return 0; -} - - -/* This initialization is similar to the one in ak8975.c. */ -int setup_compass(void) -{ -#ifdef AK89xx_SECONDARY - unsigned char data[4], akm_addr; - - mpu_set_bypass(1); - - /* Find compass. Possible addresses range from 0x0C to 0x0F. */ - for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) { - int result; - result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data); - if (!result && (data[0] == AKM_WHOAMI)) - break; - } - - if (akm_addr > 0x0F) { - /* TODO: Handle this case in all compass-related functions. */ - log_e("Compass not found.\n"); - return -1; - } - - st.chip_cfg.compass_addr = akm_addr; - - data[0] = AKM_POWER_DOWN; - if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) - return -1; - delay_ms(1); - - data[0] = AKM_FUSE_ROM_ACCESS; - if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) - return -1; - delay_ms(1); - - /* Get sensitivity adjustment data from fuse ROM. */ - if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data)) - return -1; - st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128; - st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128; - st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128; - - data[0] = AKM_POWER_DOWN; - if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data)) - return -1; - delay_ms(1); - - mpu_set_bypass(0); - - /* Set up master mode, master clock, and ES bit. */ - data[0] = 0x40; - if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data)) - return -1; - - /* Slave 0 reads from AKM data registers. */ - data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr; - if (i2c_write(st.hw->addr, st.reg->s0_addr, 1, data)) - return -1; - - /* Compass reads start at this register. */ - data[0] = AKM_REG_ST1; - if (i2c_write(st.hw->addr, st.reg->s0_reg, 1, data)) - return -1; - - /* Enable slave 0, 8-byte reads. */ - data[0] = BIT_SLAVE_EN | 8; - if (i2c_write(st.hw->addr, st.reg->s0_ctrl, 1, data)) - return -1; - - /* Slave 1 changes AKM measurement mode. */ - data[0] = st.chip_cfg.compass_addr; - if (i2c_write(st.hw->addr, st.reg->s1_addr, 1, data)) - return -1; - - /* AKM measurement mode register. */ - data[0] = AKM_REG_CNTL; - if (i2c_write(st.hw->addr, st.reg->s1_reg, 1, data)) - return -1; - - /* Enable slave 1, 1-byte writes. */ - data[0] = BIT_SLAVE_EN | 1; - if (i2c_write(st.hw->addr, st.reg->s1_ctrl, 1, data)) - return -1; - - /* Set slave 1 data. */ - data[0] = AKM_SINGLE_MEASUREMENT; - if (i2c_write(st.hw->addr, st.reg->s1_do, 1, data)) - return -1; - - /* Trigger slave 0 and slave 1 actions at each sample. */ - data[0] = 0x03; - if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data)) - return -1; - -#ifdef MPU9150 - /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */ - data[0] = BIT_I2C_MST_VDDIO; - if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data)) - return -1; -#endif - - return 0; -#else - return -1; -#endif -} - -/** - * @brief Read raw compass data. - * @param[out] data Raw data in hardware units. - * @param[out] timestamp Timestamp in milliseconds. Null if not needed. - * @return 0 if successful. - */ -int mpu_get_compass_reg(short *data, unsigned long *timestamp) -{ -#ifdef AK89xx_SECONDARY - unsigned char tmp[9]; - - if (!(st.chip_cfg.sensors & INV_XYZ_COMPASS)) - return -1; - -#ifdef AK89xx_BYPASS - if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp)) - return -1; - tmp[8] = AKM_SINGLE_MEASUREMENT; - if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8)) - return -1; -#else - if (i2c_read(st.hw->addr, st.reg->raw_compass, 8, tmp)) - return -1; -#endif - -#if defined AK8975_SECONDARY - /* AK8975 doesn't have the overrun error bit. */ - if (!(tmp[0] & AKM_DATA_READY)) - return -2; - if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR)) - return -3; -#elif defined AK8963_SECONDARY - /* AK8963 doesn't have the data read error bit. */ - if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN)) - return -2; - if (tmp[7] & AKM_OVERFLOW) - return -3; -#endif - data[0] = (tmp[2] << 8) | tmp[1]; - data[1] = (tmp[4] << 8) | tmp[3]; - data[2] = (tmp[6] << 8) | tmp[5]; - - data[0] = ((long)data[0] * st.chip_cfg.mag_sens_adj[0]) >> 8; - data[1] = ((long)data[1] * st.chip_cfg.mag_sens_adj[1]) >> 8; - data[2] = ((long)data[2] * st.chip_cfg.mag_sens_adj[2]) >> 8; - - if (timestamp) - get_ms(timestamp); - return 0; -#else - return -1; -#endif -} - -/** - * @brief Get the compass full-scale range. - * @param[out] fsr Current full-scale range. - * @return 0 if successful. - */ -int mpu_get_compass_fsr(unsigned short *fsr) -{ -#ifdef AK89xx_SECONDARY - fsr[0] = st.hw->compass_fsr; - return 0; -#else - return -1; -#endif -} - -/** - * @brief Enters LP accel motion interrupt mode. - * The behavior of this feature is very different between the MPU6050 and the - * MPU6500. Each chip's version of this feature is explained below. - * - * \n MPU6050: - * \n When this mode is first enabled, the hardware captures a single accel - * sample, and subsequent samples are compared with this one to determine if - * the device is in motion. Therefore, whenever this "locked" sample needs to - * be changed, this function must be called again. - * - * \n The hardware motion threshold can be between 32mg and 8160mg in 32mg - * increments. - * - * \n Low-power accel mode supports the following frequencies: - * \n 1.25Hz, 5Hz, 20Hz, 40Hz - * - * \n MPU6500: - * \n Unlike the MPU6050 version, the hardware does not "lock in" a reference - * sample. The hardware monitors the accel data and detects any large change - * over a short period of time. - * - * \n The hardware motion threshold can be between 4mg and 1020mg in 4mg - * increments. - * - * \n MPU6500 Low-power accel mode supports the following frequencies: - * \n 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz - * - * \n\n NOTES: - * \n The driver will round down @e thresh to the nearest supported value if - * an unsupported threshold is selected. - * \n To select a fractional wake-up frequency, round down the value passed to - * @e lpa_freq. - * \n The MPU6500 does not support a delay parameter. If this function is used - * for the MPU6500, the value passed to @e time will be ignored. - * \n To disable this mode, set @e lpa_freq to zero. The driver will restore - * the previous configuration. - * - * @param[in] thresh Motion threshold in mg. - * @param[in] time Duration in milliseconds that the accel data must - * exceed @e thresh before motion is reported. - * @param[in] lpa_freq Minimum sampling rate, or zero to disable. - * @return 0 if successful. - */ -int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time, - unsigned char lpa_freq) -{ - unsigned char data[3]; - - if (lpa_freq) { - unsigned char thresh_hw; - -#if defined MPU6050 - /* TODO: Make these const/#defines. */ - /* 1LSb = 32mg. */ - if (thresh > 8160) - thresh_hw = 255; - else if (thresh < 32) - thresh_hw = 1; - else - thresh_hw = thresh >> 5; -#elif defined MPU6500 - /* 1LSb = 4mg. */ - if (thresh > 1020) - thresh_hw = 255; - else if (thresh < 4) - thresh_hw = 1; - else - thresh_hw = thresh >> 2; -#endif - - if (!time) - /* Minimum duration must be 1ms. */ - time = 1; - -#if defined MPU6050 - if (lpa_freq > 40) -#elif defined MPU6500 - if (lpa_freq > 640) -#endif - /* At this point, the chip has not been re-configured, so the - * function can safely exit. - */ - return -1; - - if (!st.chip_cfg.int_motion_only) { - /* Store current settings for later. */ - if (st.chip_cfg.dmp_on) { - mpu_set_dmp_state(0); - st.chip_cfg.cache.dmp_on = 1; - } else - st.chip_cfg.cache.dmp_on = 0; - mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr); - mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr); - mpu_get_lpf(&st.chip_cfg.cache.lpf); - mpu_get_sample_rate(&st.chip_cfg.cache.sample_rate); - st.chip_cfg.cache.sensors_on = st.chip_cfg.sensors; - mpu_get_fifo_config(&st.chip_cfg.cache.fifo_sensors); - } - -#ifdef MPU6050 - /* Disable hardware interrupts for now. */ - set_int_enable(0); - - /* Enter full-power accel-only mode. */ - mpu_lp_accel_mode(0); - - /* Override current LPF (and HPF) settings to obtain a valid accel - * reading. - */ - data[0] = INV_FILTER_256HZ_NOLPF2; - if (i2c_write(st.hw->addr, st.reg->lpf, 1, data)) - return -1; - - /* NOTE: Digital high pass filter should be configured here. Since this - * driver doesn't modify those bits anywhere, they should already be - * cleared by default. - */ - - /* Configure the device to send motion interrupts. */ - /* Enable motion interrupt. */ - data[0] = BIT_MOT_INT_EN; - if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) - goto lp_int_restore; - - /* Set motion interrupt parameters. */ - data[0] = thresh_hw; - data[1] = time; - if (i2c_write(st.hw->addr, st.reg->motion_thr, 2, data)) - goto lp_int_restore; - - /* Force hardware to "lock" current accel sample. */ - delay_ms(5); - data[0] = (st.chip_cfg.accel_fsr << 3) | BITS_HPF; - if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data)) - goto lp_int_restore; - - /* Set up LP accel mode. */ - data[0] = BIT_LPA_CYCLE; - if (lpa_freq == 1) - data[1] = INV_LPA_1_25HZ; - else if (lpa_freq <= 5) - data[1] = INV_LPA_5HZ; - else if (lpa_freq <= 20) - data[1] = INV_LPA_20HZ; - else - data[1] = INV_LPA_40HZ; - data[1] = (data[1] << 6) | BIT_STBY_XYZG; - if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data)) - goto lp_int_restore; - - st.chip_cfg.int_motion_only = 1; - return 0; -#elif defined MPU6500 - /* Disable hardware interrupts. */ - set_int_enable(0); - - /* Enter full-power accel-only mode, no FIFO/DMP. */ - data[0] = 0; - data[1] = 0; - data[2] = BIT_STBY_XYZG; - if (i2c_write(st.hw->addr, st.reg->user_ctrl, 3, data)) - goto lp_int_restore; - - /* Set motion threshold. */ - data[0] = thresh_hw; - if (i2c_write(st.hw->addr, st.reg->motion_thr, 1, data)) - goto lp_int_restore; - - /* Set wake frequency. */ - if (lpa_freq == 1) - data[0] = INV_LPA_1_25HZ; - else if (lpa_freq == 2) - data[0] = INV_LPA_2_5HZ; - else if (lpa_freq <= 5) - data[0] = INV_LPA_5HZ; - else if (lpa_freq <= 10) - data[0] = INV_LPA_10HZ; - else if (lpa_freq <= 20) - data[0] = INV_LPA_20HZ; - else if (lpa_freq <= 40) - data[0] = INV_LPA_40HZ; - else if (lpa_freq <= 80) - data[0] = INV_LPA_80HZ; - else if (lpa_freq <= 160) - data[0] = INV_LPA_160HZ; - else if (lpa_freq <= 320) - data[0] = INV_LPA_320HZ; - else - data[0] = INV_LPA_640HZ; - if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, data)) - goto lp_int_restore; - - /* Enable motion interrupt (MPU6500 version). */ - data[0] = BITS_WOM_EN; - if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data)) - goto lp_int_restore; - - /* Enable cycle mode. */ - data[0] = BIT_LPA_CYCLE; - if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data)) - goto lp_int_restore; - - /* Enable interrupt. */ - data[0] = BIT_MOT_INT_EN; - if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data)) - goto lp_int_restore; - - st.chip_cfg.int_motion_only = 1; - return 0; -#endif - } else { - /* Don't "restore" the previous state if no state has been saved. */ - int ii; - char *cache_ptr = (char*)&st.chip_cfg.cache; - for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++) { - if (cache_ptr[ii] != 0) - goto lp_int_restore; - } - /* If we reach this point, motion interrupt mode hasn't been used yet. */ - return -1; - } -lp_int_restore: - /* Set to invalid values to ensure no I2C writes are skipped. */ - st.chip_cfg.gyro_fsr = 0xFF; - st.chip_cfg.accel_fsr = 0xFF; - st.chip_cfg.lpf = 0xFF; - st.chip_cfg.sample_rate = 0xFFFF; - st.chip_cfg.sensors = 0xFF; - st.chip_cfg.fifo_enable = 0xFF; - st.chip_cfg.clk_src = INV_CLK_PLL; - mpu_set_sensors(st.chip_cfg.cache.sensors_on); - mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr); - mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr); - mpu_set_lpf(st.chip_cfg.cache.lpf); - mpu_set_sample_rate(st.chip_cfg.cache.sample_rate); - mpu_configure_fifo(st.chip_cfg.cache.fifo_sensors); - - if (st.chip_cfg.cache.dmp_on) - mpu_set_dmp_state(1); - -#ifdef MPU6500 - /* Disable motion interrupt (MPU6500 version). */ - data[0] = 0; - if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data)) - goto lp_int_restore; -#endif - - st.chip_cfg.int_motion_only = 0; - return 0; -} - -/** - * @} - */ - diff --git a/interface/external/MotionDriver/src/inv_mpu_dmp_motion_driver.c b/interface/external/MotionDriver/src/inv_mpu_dmp_motion_driver.c deleted file mode 100644 index db71b4b74c..0000000000 --- a/interface/external/MotionDriver/src/inv_mpu_dmp_motion_driver.c +++ /dev/null @@ -1,1373 +0,0 @@ -/* - $License: - Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. - See included License.txt for License information. - $ - */ -/** - * @addtogroup DRIVERS Sensor Driver Layer - * @brief Hardware drivers to communicate with sensors via I2C. - * - * @{ - * @file inv_mpu_dmp_motion_driver.c - * @brief DMP image and interface functions. - * @details All functions are preceded by the dmp_ prefix to - * differentiate among MPL and general driver function calls. - */ -#include -#include -#include -#include -#include -#include "inv_mpu.h" -#include "inv_mpu_dmp_motion_driver.h" -#include "dmpKey.h" -#include "dmpmap.h" -#include "inv_tty.h" - -/* The following functions must be defined for this platform: - * i2c_write(unsigned char slave_addr, unsigned char reg_addr, - * unsigned char length, unsigned char const *data) - * i2c_read(unsigned char slave_addr, unsigned char reg_addr, - * unsigned char length, unsigned char *data) - * delay_ms(unsigned long num_ms) - * get_ms(unsigned long *count) - */ -#if defined MOTION_DRIVER_TARGET_MSP430 -#include "msp430.h" -#include "msp430_clock.h" -#define delay_ms msp430_delay_ms -#define get_ms msp430_get_clock_ms -#define log_i(...) do {} while (0) -#define log_e(...) do {} while (0) - -#elif defined EMPL_TARGET_MSP430 -#include "msp430.h" -#include "msp430_clock.h" -#include "log.h" -#define delay_ms msp430_delay_ms -#define get_ms msp430_get_clock_ms -#define log_i MPL_LOGI -#define log_e MPL_LOGE - -#elif defined EMPL_TARGET_UC3L0 -/* Instead of using the standard TWI driver from the ASF library, we're using - * a TWI driver that follows the slave address + register address convention. - */ -#include "delay.h" -#include "sysclk.h" -#include "log.h" -#include "uc3l0_clock.h" -/* delay_ms is a function already defined in ASF. */ -#define get_ms uc3l0_get_clock_ms -#define log_i MPL_LOGI -#define log_e MPL_LOGE - -#else -#define i2c_write tty_i2c_write -#define i2c_read tty_i2c_read -#define delay_ms tty_delay_ms -#define get_ms tty_get_clock_ms -#endif - -/* These defines are copied from dmpDefaultMPU6050.c in the general MPL - * releases. These defines may change for each DMP image, so be sure to modify - * these values when switching to a new image. - */ -#define CFG_LP_QUAT (2712) -#define END_ORIENT_TEMP (1866) -#define CFG_27 (2742) -#define CFG_20 (2224) -#define CFG_23 (2745) -#define CFG_FIFO_ON_EVENT (2690) -#define END_PREDICTION_UPDATE (1761) -#define CGNOTICE_INTR (2620) -#define X_GRT_Y_TMP (1358) -#define CFG_DR_INT (1029) -#define CFG_AUTH (1035) -#define UPDATE_PROP_ROT (1835) -#define END_COMPARE_Y_X_TMP2 (1455) -#define SKIP_X_GRT_Y_TMP (1359) -#define SKIP_END_COMPARE (1435) -#define FCFG_3 (1088) -#define FCFG_2 (1066) -#define FCFG_1 (1062) -#define END_COMPARE_Y_X_TMP3 (1434) -#define FCFG_7 (1073) -#define FCFG_6 (1106) -#define FLAT_STATE_END (1713) -#define SWING_END_4 (1616) -#define SWING_END_2 (1565) -#define SWING_END_3 (1587) -#define SWING_END_1 (1550) -#define CFG_8 (2718) -#define CFG_15 (2727) -#define CFG_16 (2746) -#define CFG_EXT_GYRO_BIAS (1189) -#define END_COMPARE_Y_X_TMP (1407) -#define DO_NOT_UPDATE_PROP_ROT (1839) -#define CFG_7 (1205) -#define FLAT_STATE_END_TEMP (1683) -#define END_COMPARE_Y_X (1484) -#define SKIP_SWING_END_1 (1551) -#define SKIP_SWING_END_3 (1588) -#define SKIP_SWING_END_2 (1566) -#define TILTG75_START (1672) -#define CFG_6 (2753) -#define TILTL75_END (1669) -#define END_ORIENT (1884) -#define CFG_FLICK_IN (2573) -#define TILTL75_START (1643) -#define CFG_MOTION_BIAS (1208) -#define X_GRT_Y (1408) -#define TEMPLABEL (2324) -#define CFG_ANDROID_ORIENT_INT (1853) -#define CFG_GYRO_RAW_DATA (2722) -#define X_GRT_Y_TMP2 (1379) - -#define D_0_22 (22+512) -#define D_0_24 (24+512) - -#define D_0_36 (36) -#define D_0_52 (52) -#define D_0_96 (96) -#define D_0_104 (104) -#define D_0_108 (108) -#define D_0_163 (163) -#define D_0_188 (188) -#define D_0_192 (192) -#define D_0_224 (224) -#define D_0_228 (228) -#define D_0_232 (232) -#define D_0_236 (236) - -#define D_1_2 (256 + 2) -#define D_1_4 (256 + 4) -#define D_1_8 (256 + 8) -#define D_1_10 (256 + 10) -#define D_1_24 (256 + 24) -#define D_1_28 (256 + 28) -#define D_1_36 (256 + 36) -#define D_1_40 (256 + 40) -#define D_1_44 (256 + 44) -#define D_1_72 (256 + 72) -#define D_1_74 (256 + 74) -#define D_1_79 (256 + 79) -#define D_1_88 (256 + 88) -#define D_1_90 (256 + 90) -#define D_1_92 (256 + 92) -#define D_1_96 (256 + 96) -#define D_1_98 (256 + 98) -#define D_1_106 (256 + 106) -#define D_1_108 (256 + 108) -#define D_1_112 (256 + 112) -#define D_1_128 (256 + 144) -#define D_1_152 (256 + 12) -#define D_1_160 (256 + 160) -#define D_1_176 (256 + 176) -#define D_1_178 (256 + 178) -#define D_1_218 (256 + 218) -#define D_1_232 (256 + 232) -#define D_1_236 (256 + 236) -#define D_1_240 (256 + 240) -#define D_1_244 (256 + 244) -#define D_1_250 (256 + 250) -#define D_1_252 (256 + 252) -#define D_2_12 (512 + 12) -#define D_2_96 (512 + 96) -#define D_2_108 (512 + 108) -#define D_2_208 (512 + 208) -#define D_2_224 (512 + 224) -#define D_2_236 (512 + 236) -#define D_2_244 (512 + 244) -#define D_2_248 (512 + 248) -#define D_2_252 (512 + 252) - -#define CPASS_BIAS_X (35 * 16 + 4) -#define CPASS_BIAS_Y (35 * 16 + 8) -#define CPASS_BIAS_Z (35 * 16 + 12) -#define CPASS_MTX_00 (36 * 16) -#define CPASS_MTX_01 (36 * 16 + 4) -#define CPASS_MTX_02 (36 * 16 + 8) -#define CPASS_MTX_10 (36 * 16 + 12) -#define CPASS_MTX_11 (37 * 16) -#define CPASS_MTX_12 (37 * 16 + 4) -#define CPASS_MTX_20 (37 * 16 + 8) -#define CPASS_MTX_21 (37 * 16 + 12) -#define CPASS_MTX_22 (43 * 16 + 12) -#define D_EXT_GYRO_BIAS_X (61 * 16) -#define D_EXT_GYRO_BIAS_Y (61 * 16) + 4 -#define D_EXT_GYRO_BIAS_Z (61 * 16) + 8 -#define D_ACT0 (40 * 16) -#define D_ACSX (40 * 16 + 4) -#define D_ACSY (40 * 16 + 8) -#define D_ACSZ (40 * 16 + 12) - -#define FLICK_MSG (45 * 16 + 4) -#define FLICK_COUNTER (45 * 16 + 8) -#define FLICK_LOWER (45 * 16 + 12) -#define FLICK_UPPER (46 * 16 + 12) - -#define D_AUTH_OUT (992) -#define D_AUTH_IN (996) -#define D_AUTH_A (1000) -#define D_AUTH_B (1004) - -#define D_PEDSTD_BP_B (768 + 0x1C) -#define D_PEDSTD_HP_A (768 + 0x78) -#define D_PEDSTD_HP_B (768 + 0x7C) -#define D_PEDSTD_BP_A4 (768 + 0x40) -#define D_PEDSTD_BP_A3 (768 + 0x44) -#define D_PEDSTD_BP_A2 (768 + 0x48) -#define D_PEDSTD_BP_A1 (768 + 0x4C) -#define D_PEDSTD_INT_THRSH (768 + 0x68) -#define D_PEDSTD_CLIP (768 + 0x6C) -#define D_PEDSTD_SB (768 + 0x28) -#define D_PEDSTD_SB_TIME (768 + 0x2C) -#define D_PEDSTD_PEAKTHRSH (768 + 0x98) -#define D_PEDSTD_TIML (768 + 0x2A) -#define D_PEDSTD_TIMH (768 + 0x2E) -#define D_PEDSTD_PEAK (768 + 0X94) -#define D_PEDSTD_STEPCTR (768 + 0x60) -#define D_PEDSTD_TIMECTR (964) -#define D_PEDSTD_DECI (768 + 0xA0) - -#define D_HOST_NO_MOT (976) -#define D_ACCEL_BIAS (660) - -#define D_ORIENT_GAP (76) - -#define D_TILT0_H (48) -#define D_TILT0_L (50) -#define D_TILT1_H (52) -#define D_TILT1_L (54) -#define D_TILT2_H (56) -#define D_TILT2_L (58) -#define D_TILT3_H (60) -#define D_TILT3_L (62) - -#define DMP_CODE_SIZE (3062) - -static const unsigned char dmp_memory[DMP_CODE_SIZE] = { - /* bank # 0 */ - 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, - 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, - 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, - 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, - 0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, - 0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2, - 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, - 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, - 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, - 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, - 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, - 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, - 0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00, - 0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - /* bank # 1 */ - 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f, - 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00, - 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, - 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, - 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00, - 0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00, - 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28, - 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, - 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, - /* bank # 2 */ - 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, - 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, - 0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c, - 0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, - 0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - /* bank # 3 */ - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, - 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - - /* bank # 4 */ - 0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, - 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, - 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9, - 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96, - 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, - 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, - 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, - 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, - 0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, - 0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8, - 0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, - 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, - 0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, - 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2, - 0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, - 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78, - /* bank # 5 */ - 0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba, - 0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8, - 0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, - 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7, - 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1, - 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1, - 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29, - 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a, - 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7, - 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e, - 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, - 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8, - 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88, - 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94, - 0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2, - 0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82, - /* bank # 6 */ - 0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1, - 0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98, - 0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88, - 0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02, - 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf, - 0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1, - 0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39, - 0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9, - 0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9, - 0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde, - 0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0, - 0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8, - 0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2, - 0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3, - 0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb, - 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8, - /* bank # 7 */ - 0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb, - 0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28, - 0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2, - 0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21, 0xd8, 0xf4, 0x0a, - 0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb, - 0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6, - 0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8, - 0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1, - 0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99, - 0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb, - 0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9, - 0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9, - 0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac, - 0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6, - 0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0, - 0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf, - /* bank # 8 */ - 0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30, - 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9, - 0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0, - 0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1, - 0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6, - 0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, - 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda, - 0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde, - 0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde, - 0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88, - 0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf, - 0xd8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8, - 0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5, - 0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8, - 0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7, - 0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8, - /* bank # 9 */ - 0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0, - 0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b, - 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab, - 0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0, - 0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0, - 0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59, - 0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31, - 0x8b, 0x30, 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, - 0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, - 0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, - 0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, - 0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, - 0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0, - 0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf, - 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2, - 0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83, - /* bank # 10 */ - 0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6, - 0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, - 0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb, - 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c, - 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8, - 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9, - 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2, - 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84, - 0xc1, 0xc3, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0, - 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3, - 0xa3, 0xa3, 0xb2, 0x8b, 0xb6, 0x9b, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, - 0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9, - 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, - 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, - 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90, - 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, - /* bank # 11 */ - 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83, - 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19, - 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39, - 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7, - 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80, - 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9, - 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26, - 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89, - 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, - 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, - 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22, - 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2, - 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00, - 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10, - 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88, - 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff -}; - -static const unsigned short sStartAddress = 0x0400; - -/* END OF SECTION COPIED FROM dmpDefaultMPU6050.c */ - -#define INT_SRC_TAP (0x01) -#define INT_SRC_ANDROID_ORIENT (0x08) - -#define DMP_FEATURE_SEND_ANY_GYRO (DMP_FEATURE_SEND_RAW_GYRO | \ - DMP_FEATURE_SEND_CAL_GYRO) - -#define MAX_PACKET_LENGTH (32) - -#define DMP_SAMPLE_RATE (200) -#define GYRO_SF (46850825LL * 200 / DMP_SAMPLE_RATE) - -#define FIFO_CORRUPTION_CHECK -#ifdef FIFO_CORRUPTION_CHECK -#define QUAT_ERROR_THRESH (1L<<24) -#define QUAT_MAG_SQ_NORMALIZED (1L<<28) -#define QUAT_MAG_SQ_MIN (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH) -#define QUAT_MAG_SQ_MAX (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH) -#endif - -struct dmp_s { - void (*tap_cb)(unsigned char count, unsigned char direction); - void (*android_orient_cb)(unsigned char orientation); - unsigned short orient; - unsigned short feature_mask; - unsigned short fifo_rate; - unsigned char packet_length; -}; - -static struct dmp_s dmp = { - .tap_cb = NULL, - .android_orient_cb = NULL, - .orient = 0, - .feature_mask = 0, - .fifo_rate = 0, - .packet_length = 0 -}; - -/** - * @brief Load the DMP with this image. - * @return 0 if successful. - */ -int dmp_load_motion_driver_firmware(void) -{ - return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress, - DMP_SAMPLE_RATE); -} - -/** - * @brief Push gyro and accel orientation to the DMP. - * The orientation is represented here as the output of - * @e inv_orientation_matrix_to_scalar. - * @param[in] orient Gyro and accel orientation in body frame. - * @return 0 if successful. - */ -int dmp_set_orientation(unsigned short orient) -{ - unsigned char gyro_regs[3], accel_regs[3]; - const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C}; - const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C}; - const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76}; - const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66}; - - gyro_regs[0] = gyro_axes[orient & 3]; - gyro_regs[1] = gyro_axes[(orient >> 3) & 3]; - gyro_regs[2] = gyro_axes[(orient >> 6) & 3]; - accel_regs[0] = accel_axes[orient & 3]; - accel_regs[1] = accel_axes[(orient >> 3) & 3]; - accel_regs[2] = accel_axes[(orient >> 6) & 3]; - - /* Chip-to-body, axes only. */ - if (mpu_write_mem(FCFG_1, 3, gyro_regs)) - return -1; - if (mpu_write_mem(FCFG_2, 3, accel_regs)) - return -1; - - memcpy(gyro_regs, gyro_sign, 3); - memcpy(accel_regs, accel_sign, 3); - if (orient & 4) { - gyro_regs[0] |= 1; - accel_regs[0] |= 1; - } - if (orient & 0x20) { - gyro_regs[1] |= 1; - accel_regs[1] |= 1; - } - if (orient & 0x100) { - gyro_regs[2] |= 1; - accel_regs[2] |= 1; - } - - /* Chip-to-body, sign only. */ - if (mpu_write_mem(FCFG_3, 3, gyro_regs)) - return -1; - if (mpu_write_mem(FCFG_7, 3, accel_regs)) - return -1; - dmp.orient = orient; - return 0; -} - -/** - * @brief Push gyro biases to the DMP. - * Because the gyro integration is handled in the DMP, any gyro biases - * calculated by the MPL should be pushed down to DMP memory to remove - * 3-axis quaternion drift. - * \n NOTE: If the DMP-based gyro calibration is enabled, the DMP will - * overwrite the biases written to this location once a new one is computed. - * @param[in] bias Gyro biases in q16. - * @return 0 if successful. - */ -int dmp_set_gyro_bias(long *bias) -{ - long gyro_bias_body[3]; - unsigned char regs[4]; - - gyro_bias_body[0] = bias[dmp.orient & 3]; - if (dmp.orient & 4) - gyro_bias_body[0] *= -1; - gyro_bias_body[1] = bias[(dmp.orient >> 3) & 3]; - if (dmp.orient & 0x20) - gyro_bias_body[1] *= -1; - gyro_bias_body[2] = bias[(dmp.orient >> 6) & 3]; - if (dmp.orient & 0x100) - gyro_bias_body[2] *= -1; - -#ifdef EMPL_NO_64BIT - gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) / 1073741824.f); - gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) / 1073741824.f); - gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) / 1073741824.f); -#else - gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30); - gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30); - gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30); -#endif - - regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF); - regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF); - regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF); - regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF); - if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs)) - return -1; - - regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF); - regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF); - regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF); - regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF); - if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs)) - return -1; - - regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF); - regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF); - regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF); - regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF); - return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs); -} - -/** - * @brief Push accel biases to the DMP. - * These biases will be removed from the DMP 6-axis quaternion. - * @param[in] bias Accel biases in q16. - * @return 0 if successful. - */ -int dmp_set_accel_bias(long *bias) -{ - long accel_bias_body[3]; - unsigned char regs[12]; - long long accel_sf; - unsigned short accel_sens; - - mpu_get_accel_sens(&accel_sens); - accel_sf = (long long)accel_sens << 15; - __no_operation(); - - accel_bias_body[0] = bias[dmp.orient & 3]; - if (dmp.orient & 4) - accel_bias_body[0] *= -1; - accel_bias_body[1] = bias[(dmp.orient >> 3) & 3]; - if (dmp.orient & 0x20) - accel_bias_body[1] *= -1; - accel_bias_body[2] = bias[(dmp.orient >> 6) & 3]; - if (dmp.orient & 0x100) - accel_bias_body[2] *= -1; - -#ifdef EMPL_NO_64BIT - accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) / 1073741824.f); - accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) / 1073741824.f); - accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) / 1073741824.f); -#else - accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30); - accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30); - accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30); -#endif - - regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF); - regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF); - regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF); - regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF); - regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF); - regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF); - regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF); - regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF); - regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF); - regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF); - regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF); - regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF); - return mpu_write_mem(D_ACCEL_BIAS, 12, regs); -} - -/** - * @brief Set DMP output rate. - * Only used when DMP is on. - * @param[in] rate Desired fifo rate (Hz). - * @return 0 if successful. - */ -int dmp_set_fifo_rate(unsigned short rate) -{ - const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB, - 0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF}; - unsigned short div; - unsigned char tmp[8]; - - if (rate > DMP_SAMPLE_RATE) - return -1; - div = DMP_SAMPLE_RATE / rate - 1; - tmp[0] = (unsigned char)((div >> 8) & 0xFF); - tmp[1] = (unsigned char)(div & 0xFF); - if (mpu_write_mem(D_0_22, 2, tmp)) - return -1; - if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end)) - return -1; - - dmp.fifo_rate = rate; - return 0; -} - -/** - * @brief Get DMP output rate. - * @param[out] rate Current fifo rate (Hz). - * @return 0 if successful. - */ -int dmp_get_fifo_rate(unsigned short *rate) -{ - rate[0] = dmp.fifo_rate; - return 0; -} - -/** - * @brief Set tap threshold for a specific axis. - * @param[in] axis 1, 2, and 4 for XYZ accel, respectively. - * @param[in] thresh Tap threshold, in mg/ms. - * @return 0 if successful. - */ -int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh) -{ - unsigned char tmp[4], accel_fsr; - float scaled_thresh; - unsigned short dmp_thresh, dmp_thresh_2; - if (!(axis & TAP_XYZ) || thresh > 1600) - return -1; - - scaled_thresh = (float)thresh / DMP_SAMPLE_RATE; - - mpu_get_accel_fsr(&accel_fsr); - switch (accel_fsr) { - case 2: - dmp_thresh = (unsigned short)(scaled_thresh * 16384); - /* dmp_thresh * 0.75 */ - dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288); - break; - case 4: - dmp_thresh = (unsigned short)(scaled_thresh * 8192); - /* dmp_thresh * 0.75 */ - dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144); - break; - case 8: - dmp_thresh = (unsigned short)(scaled_thresh * 4096); - /* dmp_thresh * 0.75 */ - dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072); - break; - case 16: - dmp_thresh = (unsigned short)(scaled_thresh * 2048); - /* dmp_thresh * 0.75 */ - dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536); - break; - default: - return -1; - } - tmp[0] = (unsigned char)(dmp_thresh >> 8); - tmp[1] = (unsigned char)(dmp_thresh & 0xFF); - tmp[2] = (unsigned char)(dmp_thresh_2 >> 8); - tmp[3] = (unsigned char)(dmp_thresh_2 & 0xFF); - - if (axis & TAP_X) { - if (mpu_write_mem(DMP_TAP_THX, 2, tmp)) - return -1; - if (mpu_write_mem(D_1_36, 2, tmp+2)) - return -1; - } - if (axis & TAP_Y) { - if (mpu_write_mem(DMP_TAP_THY, 2, tmp)) - return -1; - if (mpu_write_mem(D_1_40, 2, tmp+2)) - return -1; - } - if (axis & TAP_Z) { - if (mpu_write_mem(DMP_TAP_THZ, 2, tmp)) - return -1; - if (mpu_write_mem(D_1_44, 2, tmp+2)) - return -1; - } - return 0; -} - -/** - * @brief Set which axes will register a tap. - * @param[in] axis 1, 2, and 4 for XYZ, respectively. - * @return 0 if successful. - */ -int dmp_set_tap_axes(unsigned char axis) -{ - unsigned char tmp = 0; - - if (axis & TAP_X) - tmp |= 0x30; - if (axis & TAP_Y) - tmp |= 0x0C; - if (axis & TAP_Z) - tmp |= 0x03; - return mpu_write_mem(D_1_72, 1, &tmp); -} - -/** - * @brief Set minimum number of taps needed for an interrupt. - * @param[in] min_taps Minimum consecutive taps (1-4). - * @return 0 if successful. - */ -int dmp_set_tap_count(unsigned char min_taps) -{ - unsigned char tmp; - - if (min_taps < 1) - min_taps = 1; - else if (min_taps > 4) - min_taps = 4; - - tmp = min_taps - 1; - return mpu_write_mem(D_1_79, 1, &tmp); -} - -/** - * @brief Set length between valid taps. - * @param[in] time Milliseconds between taps. - * @return 0 if successful. - */ -int dmp_set_tap_time(unsigned short time) -{ - unsigned short dmp_time; - unsigned char tmp[2]; - - dmp_time = time / (1000 / DMP_SAMPLE_RATE); - tmp[0] = (unsigned char)(dmp_time >> 8); - tmp[1] = (unsigned char)(dmp_time & 0xFF); - return mpu_write_mem(DMP_TAPW_MIN, 2, tmp); -} - -/** - * @brief Set max time between taps to register as a multi-tap. - * @param[in] time Max milliseconds between taps. - * @return 0 if successful. - */ -int dmp_set_tap_time_multi(unsigned short time) -{ - unsigned short dmp_time; - unsigned char tmp[2]; - - dmp_time = time / (1000 / DMP_SAMPLE_RATE); - tmp[0] = (unsigned char)(dmp_time >> 8); - tmp[1] = (unsigned char)(dmp_time & 0xFF); - return mpu_write_mem(D_1_218, 2, tmp); -} - -/** - * @brief Set shake rejection threshold. - * If the DMP detects a gyro sample larger than @e thresh, taps are rejected. - * @param[in] sf Gyro scale factor. - * @param[in] thresh Gyro threshold in dps. - * @return 0 if successful. - */ -int dmp_set_shake_reject_thresh(long sf, unsigned short thresh) -{ - unsigned char tmp[4]; - long thresh_scaled = sf / 1000 * thresh; - tmp[0] = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF); - tmp[1] = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF); - tmp[2] = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF); - tmp[3] = (unsigned char)((long)thresh_scaled & 0xFF); - return mpu_write_mem(D_1_92, 4, tmp); -} - -/** - * @brief Set shake rejection time. - * Sets the length of time that the gyro must be outside of the threshold set - * by @e gyro_set_shake_reject_thresh before taps are rejected. A mandatory - * 60 ms is added to this parameter. - * @param[in] time Time in milliseconds. - * @return 0 if successful. - */ -int dmp_set_shake_reject_time(unsigned short time) -{ - unsigned char tmp[2]; - - time /= (1000 / DMP_SAMPLE_RATE); - tmp[0] = time >> 8; - tmp[1] = time & 0xFF; - return mpu_write_mem(D_1_90,2,tmp); -} - -/** - * @brief Set shake rejection timeout. - * Sets the length of time after a shake rejection that the gyro must stay - * inside of the threshold before taps can be detected again. A mandatory - * 60 ms is added to this parameter. - * @param[in] time Time in milliseconds. - * @return 0 if successful. - */ -int dmp_set_shake_reject_timeout(unsigned short time) -{ - unsigned char tmp[2]; - - time /= (1000 / DMP_SAMPLE_RATE); - tmp[0] = time >> 8; - tmp[1] = time & 0xFF; - return mpu_write_mem(D_1_88,2,tmp); -} - -/** - * @brief Get current step count. - * @param[out] count Number of steps detected. - * @return 0 if successful. - */ -int dmp_get_pedometer_step_count(unsigned long *count) -{ - unsigned char tmp[4]; - if (!count) - return -1; - - if (mpu_read_mem(D_PEDSTD_STEPCTR, 4, tmp)) - return -1; - - count[0] = ((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | - ((unsigned long)tmp[2] << 8) | tmp[3]; - return 0; -} - -/** - * @brief Overwrite current step count. - * WARNING: This function writes to DMP memory and could potentially encounter - * a race condition if called while the pedometer is enabled. - * @param[in] count New step count. - * @return 0 if successful. - */ -int dmp_set_pedometer_step_count(unsigned long count) -{ - unsigned char tmp[4]; - - tmp[0] = (unsigned char)((count >> 24) & 0xFF); - tmp[1] = (unsigned char)((count >> 16) & 0xFF); - tmp[2] = (unsigned char)((count >> 8) & 0xFF); - tmp[3] = (unsigned char)(count & 0xFF); - return mpu_write_mem(D_PEDSTD_STEPCTR, 4, tmp); -} - -/** - * @brief Get duration of walking time. - * @param[in] time Walk time in milliseconds. - * @return 0 if successful. - */ -int dmp_get_pedometer_walk_time(unsigned long *time) -{ - unsigned char tmp[4]; - if (!time) - return -1; - - if (mpu_read_mem(D_PEDSTD_TIMECTR, 4, tmp)) - return -1; - - time[0] = (((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | - ((unsigned long)tmp[2] << 8) | tmp[3]) * 20; - return 0; -} - -/** - * @brief Overwrite current walk time. - * WARNING: This function writes to DMP memory and could potentially encounter - * a race condition if called while the pedometer is enabled. - * @param[in] time New walk time in milliseconds. - */ -int dmp_set_pedometer_walk_time(unsigned long time) -{ - unsigned char tmp[4]; - - time /= 20; - - tmp[0] = (unsigned char)((time >> 24) & 0xFF); - tmp[1] = (unsigned char)((time >> 16) & 0xFF); - tmp[2] = (unsigned char)((time >> 8) & 0xFF); - tmp[3] = (unsigned char)(time & 0xFF); - return mpu_write_mem(D_PEDSTD_TIMECTR, 4, tmp); -} - -/** - * @brief Enable DMP features. - * The following \#define's are used in the input mask: - * \n DMP_FEATURE_TAP - * \n DMP_FEATURE_ANDROID_ORIENT - * \n DMP_FEATURE_LP_QUAT - * \n DMP_FEATURE_6X_LP_QUAT - * \n DMP_FEATURE_GYRO_CAL - * \n DMP_FEATURE_SEND_RAW_ACCEL - * \n DMP_FEATURE_SEND_RAW_GYRO - * \n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually - * exclusive. - * \n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also - * mutually exclusive. - * @param[in] mask Mask of features to enable. - * @return 0 if successful. - */ -int dmp_enable_feature(unsigned short mask) -{ - unsigned char tmp[10]; - - /* TODO: All of these settings can probably be integrated into the default - * DMP image. - */ - /* Set integration scale factor. */ - tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF); - tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF); - tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF); - tmp[3] = (unsigned char)(GYRO_SF & 0xFF); - mpu_write_mem(D_0_104, 4, tmp); - - /* Send sensor data to the FIFO. */ - tmp[0] = 0xA3; - if (mask & DMP_FEATURE_SEND_RAW_ACCEL) { - tmp[1] = 0xC0; - tmp[2] = 0xC8; - tmp[3] = 0xC2; - } else { - tmp[1] = 0xA3; - tmp[2] = 0xA3; - tmp[3] = 0xA3; - } - if (mask & DMP_FEATURE_SEND_ANY_GYRO) { - tmp[4] = 0xC4; - tmp[5] = 0xCC; - tmp[6] = 0xC6; - } else { - tmp[4] = 0xA3; - tmp[5] = 0xA3; - tmp[6] = 0xA3; - } - tmp[7] = 0xA3; - tmp[8] = 0xA3; - tmp[9] = 0xA3; - mpu_write_mem(CFG_15,10,tmp); - - /* Send gesture data to the FIFO. */ - if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) - tmp[0] = DINA20; - else - tmp[0] = 0xD8; - mpu_write_mem(CFG_27,1,tmp); - - if (mask & DMP_FEATURE_GYRO_CAL) - dmp_enable_gyro_cal(1); - else - dmp_enable_gyro_cal(0); - - if (mask & DMP_FEATURE_SEND_ANY_GYRO) { - if (mask & DMP_FEATURE_SEND_CAL_GYRO) { - tmp[0] = 0xB2; - tmp[1] = 0x8B; - tmp[2] = 0xB6; - tmp[3] = 0x9B; - } else { - tmp[0] = DINAC0; - tmp[1] = DINA80; - tmp[2] = DINAC2; - tmp[3] = DINA90; - } - mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp); - } - - if (mask & DMP_FEATURE_TAP) { - /* Enable tap. */ - tmp[0] = 0xF8; - mpu_write_mem(CFG_20, 1, tmp); - dmp_set_tap_thresh(TAP_XYZ, 250); - dmp_set_tap_axes(TAP_XYZ); - dmp_set_tap_count(1); - dmp_set_tap_time(100); - dmp_set_tap_time_multi(500); - - dmp_set_shake_reject_thresh(GYRO_SF, 200); - dmp_set_shake_reject_time(40); - dmp_set_shake_reject_timeout(10); - } else { - tmp[0] = 0xD8; - mpu_write_mem(CFG_20, 1, tmp); - } - - if (mask & DMP_FEATURE_ANDROID_ORIENT) { - tmp[0] = 0xD9; - } else - tmp[0] = 0xD8; - mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp); - - if (mask & DMP_FEATURE_LP_QUAT) - dmp_enable_lp_quat(1); - else - dmp_enable_lp_quat(0); - - if (mask & DMP_FEATURE_6X_LP_QUAT) - dmp_enable_6x_lp_quat(1); - else - dmp_enable_6x_lp_quat(0); - - /* Pedometer is always enabled. */ - dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER; - mpu_reset_fifo(); - - dmp.packet_length = 0; - if (mask & DMP_FEATURE_SEND_RAW_ACCEL) - dmp.packet_length += 6; - if (mask & DMP_FEATURE_SEND_ANY_GYRO) - dmp.packet_length += 6; - if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) - dmp.packet_length += 16; - if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) - dmp.packet_length += 4; - - return 0; -} - -/** - * @brief Get list of currently enabled DMP features. - * @param[out] Mask of enabled features. - * @return 0 if successful. - */ -int dmp_get_enabled_features(unsigned short *mask) -{ - mask[0] = dmp.feature_mask; - return 0; -} - -/** - * @brief Calibrate the gyro data in the DMP. - * After eight seconds of no motion, the DMP will compute gyro biases and - * subtract them from the quaternion output. If @e dmp_enable_feature is - * called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be - * subtracted from the gyro output. - * @param[in] enable 1 to enable gyro calibration. - * @return 0 if successful. - */ -int dmp_enable_gyro_cal(unsigned char enable) -{ - if (enable) { - unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d}; - return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); - } else { - unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7}; - return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); - } -} - -/** - * @brief Generate 3-axis quaternions from the DMP. - * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually - * exclusive. - * @param[in] enable 1 to enable 3-axis quaternion. - * @return 0 if successful. - */ -int dmp_enable_lp_quat(unsigned char enable) -{ - unsigned char regs[4]; - if (enable) { - regs[0] = DINBC0; - regs[1] = DINBC2; - regs[2] = DINBC4; - regs[3] = DINBC6; - } - else - memset(regs, 0x8B, 4); - - mpu_write_mem(CFG_LP_QUAT, 4, regs); - - return mpu_reset_fifo(); -} - -/** - * @brief Generate 6-axis quaternions from the DMP. - * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually - * exclusive. - * @param[in] enable 1 to enable 6-axis quaternion. - * @return 0 if successful. - */ -int dmp_enable_6x_lp_quat(unsigned char enable) -{ - unsigned char regs[4]; - if (enable) { - regs[0] = DINA20; - regs[1] = DINA28; - regs[2] = DINA30; - regs[3] = DINA38; - } else - memset(regs, 0xA3, 4); - - mpu_write_mem(CFG_8, 4, regs); - - return mpu_reset_fifo(); -} - -/** - * @brief Decode the four-byte gesture data and execute any callbacks. - * @param[in] gesture Gesture data from DMP packet. - * @return 0 if successful. - */ -static int decode_gesture(unsigned char *gesture) -{ - unsigned char tap, android_orient; - - android_orient = gesture[3] & 0xC0; - tap = 0x3F & gesture[3]; - - if (gesture[1] & INT_SRC_TAP) { - unsigned char direction, count; - direction = tap >> 3; - count = (tap % 8) + 1; - if (dmp.tap_cb) - dmp.tap_cb(direction, count); - } - - if (gesture[1] & INT_SRC_ANDROID_ORIENT) { - if (dmp.android_orient_cb) - dmp.android_orient_cb(android_orient >> 6); - } - - return 0; -} - -/** - * @brief Specify when a DMP interrupt should occur. - * A DMP interrupt can be configured to trigger on either of the two - * conditions below: - * \n a. One FIFO period has elapsed (set by @e mpu_set_sample_rate). - * \n b. A tap event has been detected. - * @param[in] mode DMP_INT_GESTURE or DMP_INT_CONTINUOUS. - * @return 0 if successful. - */ -int dmp_set_interrupt_mode(unsigned char mode) -{ - const unsigned char regs_continuous[11] = - {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9}; - const unsigned char regs_gesture[11] = - {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda}; - - switch (mode) { - case DMP_INT_CONTINUOUS: - return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, - (unsigned char*)regs_continuous); - case DMP_INT_GESTURE: - return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, - (unsigned char*)regs_gesture); - default: - return -1; - } -} - -/** - * @brief Get one packet from the FIFO. - * If @e sensors does not contain a particular sensor, disregard the data - * returned to that pointer. - * \n @e sensors can contain a combination of the following flags: - * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO - * \n INV_XYZ_GYRO - * \n INV_XYZ_ACCEL - * \n INV_WXYZ_QUAT - * \n If the FIFO has no new data, @e sensors will be zero. - * \n If the FIFO is disabled, @e sensors will be zero and this function will - * return a non-zero error code. - * @param[out] gyro Gyro data in hardware units. - * @param[out] accel Accel data in hardware units. - * @param[out] quat 3-axis quaternion data in hardware units. - * @param[out] timestamp Timestamp in milliseconds. - * @param[out] sensors Mask of sensors read from FIFO. - * @param[out] more Number of remaining packets. - * @return 0 if successful. - */ -int dmp_read_fifo(short *gyro, short *accel, long *quat, - unsigned long *timestamp, short *sensors, unsigned char *more) -{ - unsigned char fifo_data[MAX_PACKET_LENGTH]; - unsigned char ii = 0; - - /* TODO: sensors[0] only changes when dmp_enable_feature is called. We can - * cache this value and save some cycles. - */ - sensors[0] = 0; - - /* Get a packet. */ - if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more)) - return -1; - - /* Parse DMP packet. */ - if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) { -#ifdef FIFO_CORRUPTION_CHECK - long quat_q14[4], quat_mag_sq; -#endif - quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) | - ((long)fifo_data[2] << 8) | fifo_data[3]; - quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) | - ((long)fifo_data[6] << 8) | fifo_data[7]; - quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) | - ((long)fifo_data[10] << 8) | fifo_data[11]; - quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) | - ((long)fifo_data[14] << 8) | fifo_data[15]; - ii += 16; -#ifdef FIFO_CORRUPTION_CHECK - /* We can detect a corrupted FIFO by monitoring the quaternion data and - * ensuring that the magnitude is always normalized to one. This - * shouldn't happen in normal operation, but if an I2C error occurs, - * the FIFO reads might become misaligned. - * - * Let's start by scaling down the quaternion data to avoid long long - * math. - */ - quat_q14[0] = quat[0] >> 16; - quat_q14[1] = quat[1] >> 16; - quat_q14[2] = quat[2] >> 16; - quat_q14[3] = quat[3] >> 16; - quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] + - quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3]; - if ((quat_mag_sq < QUAT_MAG_SQ_MIN) || - (quat_mag_sq > QUAT_MAG_SQ_MAX)) { - /* Quaternion is outside of the acceptable threshold. */ - mpu_reset_fifo(); - sensors[0] = 0; - return -1; - } - sensors[0] |= INV_WXYZ_QUAT; -#endif - } - - if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) { - accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; - accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; - accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; - ii += 6; - sensors[0] |= INV_XYZ_ACCEL; - } - - if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) { - gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; - gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; - gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; - ii += 6; - sensors[0] |= INV_XYZ_GYRO; - } - - /* Gesture data is at the end of the DMP packet. Parse it and call - * the gesture callbacks (if registered). - */ - if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) - decode_gesture(fifo_data + ii); - - get_ms(timestamp); - return 0; -} - -/** - * @brief Register a function to be executed on a tap event. - * The tap direction is represented by one of the following: - * \n TAP_X_UP - * \n TAP_X_DOWN - * \n TAP_Y_UP - * \n TAP_Y_DOWN - * \n TAP_Z_UP - * \n TAP_Z_DOWN - * @param[in] func Callback function. - * @return 0 if successful. - */ -int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char)) -{ - dmp.tap_cb = func; - return 0; -} - -/** - * @brief Register a function to be executed on a android orientation event. - * @param[in] func Callback function. - * @return 0 if successful. - */ -int dmp_register_android_orient_cb(void (*func)(unsigned char)) -{ - dmp.android_orient_cb = func; - return 0; -} - -/** - * @} - */ - diff --git a/interface/external/MotionDriver/src/inv_tty.c b/interface/external/MotionDriver/src/inv_tty.c deleted file mode 100644 index 30a3fceb55..0000000000 --- a/interface/external/MotionDriver/src/inv_tty.c +++ /dev/null @@ -1,113 +0,0 @@ -// -// inv_tty.c -// interface -// -// Created by Andrzej Kapolka on 7/9/13. -// Copyright (c) 2013 High Fidelity, Inc. All rights reserved. - -#include -#include -#include - -#include "inv_tty.h" - -// the file descriptor of the tty -static int ttyFileDescriptor; - -void tty_set_file_descriptor(int file_descriptor) { - ttyFileDescriptor = file_descriptor; -} - -static char to_hex_digit(unsigned char value) { - return (value < 10) ? '0' + value : 'A' + (value - 10); -} - -static unsigned char from_hex_digit(char digit) { - return (digit < 'A') ? digit - '0' : (digit - 'A') + 10; -} - -static int write_byte(unsigned char value) { - char chars[] = { to_hex_digit(value / 16), to_hex_digit(value % 16) }; - return write(ttyFileDescriptor, chars, 2) != 2; -} - -static int read_byte(unsigned char* value) { - char chars[2]; - if (read(ttyFileDescriptor, chars, 2) != 2) { - return 1; - } - *value = from_hex_digit(chars[0]) * 16 + from_hex_digit(chars[1]); - return 0; -} - -int tty_i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data) { - if (write(ttyFileDescriptor, "WR", 2) != 2) { - return 1; - } - if (write_byte(slave_addr)) { - return 1; - } - if (write_byte(reg_addr)) { - return 1; - } - int i; - for (i = 0; i < length; i++) { - if (write_byte(data[i])) { - return 1; - } - } - if (write(ttyFileDescriptor, "\n", 1) != 1) { - return 1; - } - - char response[8]; - return read(ttyFileDescriptor, response, 8) != 8; -} - -int tty_i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data) { - if (write(ttyFileDescriptor, "RD", 2) != 2) { - return 1; - } - if (write_byte(slave_addr)) { - return 1; - } - if (write_byte(reg_addr)) { - return 1; - } - if (write_byte(length)) { - return 1; - } - if (write(ttyFileDescriptor, "\n", 1) != 1) { - return 1; - } - - char prefix[6]; - if (read(ttyFileDescriptor, prefix, 6) != 6) { - return 1; - } - int i; - for (i = 0; i < length; i++) { - if (read_byte(data + i)) { - return 1; - } - } - - char suffix[2]; - return read(ttyFileDescriptor, suffix, 2) != 2; -} - -void tty_delay_ms(unsigned long num_ms) { - struct timespec required, remaining; - required.tv_sec = 0; - const long NANOSECONDS_PER_MILLISECOND = 1000000; - required.tv_nsec = num_ms * NANOSECONDS_PER_MILLISECOND; - nanosleep(&required, &remaining); -} - -void tty_get_ms(unsigned long *count) { - struct timeval time; - gettimeofday(&time, 0); - const long MILLISECONDS_PER_SECOND = 1000; - const long MICROSECONDS_PER_MILLISECOND = 1000; - *count = time.tv_sec * MILLISECONDS_PER_SECOND + time.tv_usec / MICROSECONDS_PER_MILLISECOND; -}