From a116f5a636196549d0d91074763277ca252de770 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Tue, 9 Jul 2013 17:28:23 -0700 Subject: [PATCH 1/6] Use InvenSense code to handle the rather complicated business of talking the sensor into giving us the compass reading. We're getting it now, but I still have to figure out how to incorporate it. --- cmake/modules/FindMotionDriver.cmake | 44 + interface/CMakeLists.txt | 8 +- .../external/MotionDriver/CMakeLists.txt | 14 + interface/external/MotionDriver/License.txt | 218 ++ .../external/MotionDriver/include/dmpKey.h | 494 +++ .../external/MotionDriver/include/dmpmap.h | 264 ++ .../external/MotionDriver/include/inv_mpu.h | 127 + .../include/inv_mpu_dmp_motion_driver.h | 97 + .../external/MotionDriver/include/inv_tty.h | 22 + .../MotionDriver/lib/UNIX/libMotionDriver.a | Bin 0 -> 61870 bytes interface/external/MotionDriver/src/inv_mpu.c | 2798 +++++++++++++++++ .../src/inv_mpu_dmp_motion_driver.c | 1373 ++++++++ interface/external/MotionDriver/src/inv_tty.c | 88 + interface/src/SerialInterface.cpp | 118 +- interface/src/SerialInterface.h | 4 +- 15 files changed, 5603 insertions(+), 66 deletions(-) create mode 100644 cmake/modules/FindMotionDriver.cmake create mode 100644 interface/external/MotionDriver/CMakeLists.txt create mode 100644 interface/external/MotionDriver/License.txt create mode 100644 interface/external/MotionDriver/include/dmpKey.h create mode 100644 interface/external/MotionDriver/include/dmpmap.h create mode 100644 interface/external/MotionDriver/include/inv_mpu.h create mode 100644 interface/external/MotionDriver/include/inv_mpu_dmp_motion_driver.h create mode 100644 interface/external/MotionDriver/include/inv_tty.h create mode 100644 interface/external/MotionDriver/lib/UNIX/libMotionDriver.a create mode 100644 interface/external/MotionDriver/src/inv_mpu.c create mode 100644 interface/external/MotionDriver/src/inv_mpu_dmp_motion_driver.c create mode 100644 interface/external/MotionDriver/src/inv_tty.c diff --git a/cmake/modules/FindMotionDriver.cmake b/cmake/modules/FindMotionDriver.cmake new file mode 100644 index 0000000000..b24511161b --- /dev/null +++ b/cmake/modules/FindMotionDriver.cmake @@ -0,0 +1,44 @@ +# 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 cccb9b96b0..345a307b9b 100755 --- a/interface/CMakeLists.txt +++ b/interface/CMakeLists.txt @@ -10,6 +10,7 @@ project(${TARGET_NAME}) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/../cmake/modules/") set(LIBOVR_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/LibOVR) set(LEAP_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/Leap) +set(MOTIONDRIVER_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/MotionDriver) set(PORTAUDIO_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/PortAudio) set(OPENCV_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/OpenCV) set(UVCCAMERACONTROL_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/UVCCameraControl) @@ -92,10 +93,11 @@ link_hifi_library(audio ${TARGET_NAME} ${ROOT_DIR}) find_package(GLM REQUIRED) find_package(LibOVR) find_package(Leap) +find_package(MotionDriver) find_package(OpenCV) -find_package(ZLIB) -find_package(UVCCameraControl) find_package(OpenNI) +find_package(UVCCameraControl) +find_package(ZLIB) # let the source know that we have OpenNI/NITE for Kinect if (OPENNI_FOUND) @@ -118,6 +120,7 @@ include_directories( ${GLM_INCLUDE_DIRS} ${LIBOVR_INCLUDE_DIRS} ${LEAP_INCLUDE_DIRS} + ${MOTIONDRIVER_INCLUDE_DIRS} ${OPENCV_INCLUDE_DIRS} ) @@ -125,6 +128,7 @@ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -isystem ${OPENCV_INCLUDE_DIRS}") target_link_libraries( ${TARGET_NAME} ${QT_LIBRARIES} + ${MOTIONDRIVER_LIBRARIES} ${OPENCV_LIBRARIES} ${ZLIB_LIBRARIES} fervor diff --git a/interface/external/MotionDriver/CMakeLists.txt b/interface/external/MotionDriver/CMakeLists.txt new file mode 100644 index 0000000000..521f3f8096 --- /dev/null +++ b/interface/external/MotionDriver/CMakeLists.txt @@ -0,0 +1,14 @@ +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 new file mode 100644 index 0000000000..ed2e2e1f24 --- /dev/null +++ b/interface/external/MotionDriver/License.txt @@ -0,0 +1,218 @@ +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 new file mode 100644 index 0000000000..72f95d69d4 --- /dev/null +++ b/interface/external/MotionDriver/include/dmpKey.h @@ -0,0 +1,494 @@ +/* + $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 new file mode 100644 index 0000000000..f63e140398 --- /dev/null +++ b/interface/external/MotionDriver/include/dmpmap.h @@ -0,0 +1,264 @@ +/* + $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 new file mode 100644 index 0000000000..9610124f04 --- /dev/null +++ b/interface/external/MotionDriver/include/inv_mpu.h @@ -0,0 +1,127 @@ +/* + $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 new file mode 100644 index 0000000000..b0afea6caf --- /dev/null +++ b/interface/external/MotionDriver/include/inv_mpu_dmp_motion_driver.h @@ -0,0 +1,97 @@ +/* + $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 new file mode 100644 index 0000000000..d92ece2017 --- /dev/null +++ b/interface/external/MotionDriver/include/inv_tty.h @@ -0,0 +1,22 @@ +// +// 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/UNIX/libMotionDriver.a b/interface/external/MotionDriver/lib/UNIX/libMotionDriver.a new file mode 100644 index 0000000000000000000000000000000000000000..4915e9910be513af4bd2e5e0a02d55684dd0ff2c GIT binary patch literal 61870 zcmd^o33yaR_HT6(AYkMMMFrQm{=q>6aY5Xoa-BO4C@yiCaV7-PfsENqIxK3&Kqt~| zjN%@Z8IicOh!7Dwfc~We*olf`gE-R(Ou7ZsK<5qV1f+vVzwgwobMNU&H;Xgxz3;tO z-eO;iZKuw?Hm7v@%9j=O5yoKMdVvPNku>p7f@B6#;d)DXgcjvMHOa5{TOR@`!b8@o_i*5PEMcKKf`LnF0*?IY; zg|l-?tqlCjthVgp(tK-?EeFCF*o?WQ#o5#HbIJ@PXZmz&K`fM)pI4k+nq$*R&9K@e z1c=*mO0sP;ORZ%y4N3sz%(0rJ0hnG~USx|2V#_ZydE{jB3$59OSkTQ}AFc|7OHkKWkCW zv;u2(o;Al-E{a4zyacA_6!5$Xte_o)Ck6#Rbe~)#YRWmk$Yw1qEibWU z7Z&I0vMsgdTFlN^zOc4|S_{stREKzR3kMj*#HWJiXih+1H zpo`K7R#H5hw;<>ofOQ}Sf+@4**vfTkX4TX4CMsffnXS~CQ^-Nc?zFj37RD==u635R zK#x^klwD>m$jde=4n*M{n~^lHh1rExog_~cUwH3m^oPi2Y;-CjT3#qN6m>xf_OnJa zW0%UGyQ8>qmtlaZgYt`JWdldRO-u(qdiv<%O9u_QRR2&KPXCgQ8*|0w85v{8j2T1z zUwI{9S6z0w9%bsF$J9x;Bv~vBorPV`%s10HMw-u={4tZA$Of@2&H0_u*bfk;(X*z7 zG3H$V3B0cR@dU;?;OP$=Ae3hBEOAEioa!R4>Y3zKo4)j_>kGW9FU_lN{>-ax8Rb=j zS9;ZN@A0ZTW_i`Fi@a+17O&c!r!=aI?7?KMPH|7{=W(cu0my6WNLCs>6n|ad{Uv-! z3;~}*ZHPy}=TP5|N5JP$S8BEDV$He!lVr%(MJXa*)gIfR)zN4)y1SP#rq!wqMwpBH zhWSPh6I2@0^PE|W6?bNmLtUcPs$DwLpd%}FW>cy?wxNn!U2Jc$K#tvN1ApSI+k+Xx z`*jY!uDI3r`4i8o|Vwpn0$=w8z7wv$;pd+H5Bh> zSiG5QKAk(V_N0kwZmEfy(4d?o*i-fbbwU1}%6LwBS+D^2@Zg5?Z1( zujhOkgn}j^coc+!G9f<_(rU6Ix!Sm)yg+UJcxyI_d#dKFa7o3j)vE29vzZ4}8r62C z(W5m56%Zn?E5ZZ&3$sqR^g7XCmW|!06AgOFpkJRlA>yv4aj{OU*5jgIpE@Dpl6p$( zgh!9<(Q4ENOus^15^>JI_0+VPQB-MzxA?g@C+P#|w! z#ikZEOH{nal*X)0&a9Tz`WK<;;5i2>TI?N`JSS8NwLPz?Bh>+2pV|(ML^O#cVER;K zy4r))7&;|0S51S=HY1XQPH7YEl#Y;LXGKJ5JOz3#twxRHIn#N~G(J*eoClp8{zcUFwf2zMnx!BXAq@M`Hdz3#m^(aHsU0SWW z*Ju-Nb+6*~*dr;n`|XjG#|GIWNw#6Tzh{iuBdNBz_DH%?^#)^X_t%_6`s7_bN;+>Y zJ<9z}J<2Fgo>Se^lfa{M_glXxTBtM;#l%U4uKxvbguKqPaCz-9Yr!6e8de(BP95on zM~B*@3*1uWu}zApB3mtH4G<*1(m$_Vn%$EG^`BRz3;Kq88#EPOj!D%X+buC<{Uo@k zSdZ`kl;()Of$q@>!Z$3I;P$>@FYZHdRNp}Tc$GXt9kzF7I3o_V)2l`yH=TAaq(O>X zjoMqrIMk>PLKX#>W`Mw^0FxbRRI62^ATZGS)@jI#(aY=UR91Ad^8Q}6(-Im~S<#s? zE5)mJrtm~MEHaVX;}OYos+~}(N~0&=^W`SSnmW?+nmUs5oa$kvQ9W!{g~M`((EcB$ z*Vc#L7|mJXcXs7D)vZ#K*J>vFwY6UzxNGN|uWroHoGsdcJzCSj0r1omxj`GbUTIXf zYRZcBnv?g;9p={-9>r%H9>v$TL;l)6fb1NtypJG4E?QHBow5jjhiTAq{Ws{{*O#8= ztmu%1XRqjB<)S6!U{m9I&s0<~%uYU2ajRX5+oL(xYfU?bDQ-0kZ%x5;ZQy#vt#<2g zsrp+FybWBh)uW&t#x^sOf&8vP} z?^Soyd)1vh(1n%yfN8K--L=uHez(c1e(!}~yXw8_clBQN`+BeXL%mn+sQ0S7>oG-` zhFPFb;?SUVpf;fDY%rw)VN^;_ z@~Xj=UiI4=ue!tSRd>D`O82V42Cw>UrB~gt%B${Nh)HsJEQw7LzNE9fYA}L z2=2N8L@xTc;0S`V2h)!%S$j*WP!&0EUanpVHlVhoD{ce{85$Eh@(%-sI1t|Y}b0#;A*e>?NYD0;{&g{v(~HbTIE&0tMjVg zpXgP8xWcP;`~i!9AQaqc9OH*3khn{|>dvKJbytH|{m$W4zu(|hfB4j^b~MM2d&GvU z>kv{q29*NpbC<`fhSz)5?u}lx#~T_1{bDn#si~=n=^^W|Ci61;H+WcTJ5o&8yky{a1^(C z$kWkZaeJK2(Jj2v85%$))STWh@PH}zii@6UMEf4IG> zf1p=wUFB8VTD@wp&8vRh4uubx)h(RyWxr z7Ta0&NK*M2tycAc&UpgVzxgQY&{)nq~B#hS9hr`1e` zST0Rj(WKR0o}|@e!88lZyMP}=he??xP?^C~>iXms$O2f-6#Y{;L_5Z#4Y z(E5z+P74hGIjS@==laUOS+F)Z(`oT$R(q7HmHMFZD}4$YO1UYR>I9|qE4#;HqP##C z=J5husMStM(nBPfAs}9pj*1Cwpf<22&zYpv@HjB4`pWJ}i5sV%iSjs=>R0_~Hte6L z#><9KaE>!0b$UkXenCbc&cB(Fl6V=>zosXHh_u=)9~8Z_xufi0G^*98m724ecX7PT zkKqdo^FJDy$rf`!&Wbobpqs^YC4gSQ7)xTXJYZ#aNB1&zu#%l{(tKt~P8+jf@RLvO z9{NpY(#a1gPmQ-n>||`OW5_v zV_>}{>GPcfudie*L+}9DRCtDYzO-RQjP-wZ(5aq4G@5kJ&rg0mJNoU$7lWUEvJ`?u zqbK}|u~YA5Ect$Y?dGh5EUDlu@Vg-z2(jxwaI+K7o5R?p*K+d1Z~Bn2tCukLfwgt; zYoDF<)uhYEFvg}Z#;$Qc+`&Kx1f5T3lclqR&!)5U{>V-|?=`-7bQxpOTNs7|T`B_weDSrY5VK<8wfuI~D=ek|9>EO)>;2$)?Ksbef^Wz4QH7Jc=Q z?tM%y9TV=)*vdaKm?N_MWM?pHXg$Op4M**qj1_oEhJ1o5dm&(_E{6Z<`roDSY}v=( zM}U0QdC}-W$d~RW0{?5h?ALRaKtLVr4?ja0yKpkQw4{)+e#==}l261i-5KU1cOdIj z3Hwhvn%-bGn6W|442BMjT^5Z-7cmBPN0$^xCHh^T#P$4%Trx|g`FFuZFpH_X{w$rn z$n^R}T-u=9W`6nA?158GopHZ+_}}IeuNZgQKdSW77q0oar~h*H8D|d1ciAN+533f{ z3@Qz;INXp??7mT|5(Y0?h zJmG%kvU_H&ta|>9meeo(t5-FIU!L!}B&(v%HTcxCmyEjm(P!W28Z~83xXM+x@ykR0 z6SuhzUDDIK_N_?U+PB*MYgf7-fAK9}Ps@#MZ?#1H4XakqcRjm*)ZLD{@D;85hWW!w ze34#PRcmt~60v{U7RV3SoBaDML&L85x4Ei%hHRd{zxCM>FFC>k_nmpBzsl9)>D}D= z(){qqeJA>RdV4pw&fm8_+20;G-5*|Zu-8>p$0-=Y3`yUNECVch$twnX)^2fHl*WKRhU+{vf``pcKZ}#px ztu?&-DW7M<^7-DX4X)}JTHbVYd&2uHgTqh#)#bhZp8a=+9i{s$r-bX&>Z1Km{WZlA zzNzkaPyMyOBY&Ue%y8Y=LsEVFQ^IGzl$2C=<%QdNTdUvfX=zjez zwQuzt+;>v9|AiAbx59JVo83)o-|7kY!=fxd@A*yJTVL?9^uJft?FodR{$hg*%DJWK ziw!*us~mN%bGJQI`X%$2TKA>;yK6_b?d#XtUH!)TZojMcdCTV3 zeM7qa3(ocL8|3eOLi5KeIudQ#5_#c3OkHWu5 z@8&mK18oQW?^lmG(Aw^gjCrrE-QT_H$(QRd$*Ndv4+H`qKC}48k*z=Z7atngdeFbv zcSmc)f9Q_ZUjJMGeTU|@9`wK73vO{6G>O-FqRZMN@3q^XTQ=(MN5if_An@Z~Mzt*t zv>x1cNyP7((ze*w-s+1)BHOYee%G9~#lAqR?;u3nRuS37BXccAt!??r~SbuSP254?A9OzRu%z5edy?FW19Z?|svv^nf}_3ek>JJ23@ zBU1Hz>f0~%u;tx#!{2at>z{Z0cv+y^{@k{Kt=&rle*f}F@8-6iH`?D0J6?U*-&6Jc z^5MO0OTz)bYo2Grr_J@x+J7AJUVCfAUq9tN_&*2!M|wBE-qyb1(`Ntc^PZ0k>uzu^ zgG8ZUcuVHo96N?~D&B#0EK5;?G z{WTf)J@mxH{P8tcXU=>gR~uV1<~LIv;bol4lR6Ctr?Bivh~MSpv5gepaSY73zvSH^ z<1_PJ!^dhao zZCJ2zX*dx0Xhk5f#{Yh}{-vj;1{$imTOz%!?f&Pg!?(U~uTTBZ{z__k<7?^uaKqZ= z4Nt6J>3-_3;h$Rfo#pRsO$+-Yk@s?1m;dD7r#}yGUH((RzwgwrKk{B}PV2sX%l0o? zR2%r!TKDpUk$orl!^?e<_wHz0elQaDuX*2HZ(o$U=6(AsA1?L$JjUMmTKb1I&vhpy zZGN|{-5+kKT>GwP!^*0r4gUAn^wiWp;~u!G_1*S{1&h|y|1q2 z_m1$wQ47-Ep7+wU=eizxXlnKImXUYNUHE2r#H4}+X)g?`o>OmMIAT(Pqrl&@WIpRo zcRf42a!GgVMgFkEHMv#uzv#NUvh`a33ze>`-+H{RbyVAew@3M1&KHKiQ`tJ+?{Z%K zPG#G*e%EN%)eBqG{H_&0*@TlVClv@y%teeF^IlZ%%5(wDVYKD(rQ(tKC=^X@CcFFM|8 zW&Us!G$ie2#wu@NEb|t|*e#5CRK_Y*#xhmLn97)EBK!5tH&4hIku|Nn$X0&or2L|s z(&CKEM_)O5+$CenIe5iL{hcd@+*uyJlUe4ZTh2%ukh;*q*qKcK19uVpyK!*(<4HH4 z*gq$20H}rFk4jYlbTIT!ZnHoL@PB~EW^8Ya{~p8tQQ`ktjQ>W%f06JH)AW-%IRild zW8u#S6qx>fCV$(7|J$H%qkr;}B;*e@cu>O)|D@>#{WFDb34%5KGY$W1g+Gi>O#j~+ z{;Y)Dlj-cguwhKy=u@uB>anG(*Kj;eTH*g(jQ>-H{{zDRaE$zW4f#JR{1cVmBH_P0hWyk2C z&h!VF9?d#4JFhgS5JsB+5eM@EP>uK~lQ>fry#>5cMbTACmhkdcmce(!F0aLP! zG5CRWa7c<|p>G0Qh|+MQ5{%H~v_=V!9bfD@DAyrz$_`wH*!P=*2?;0j5AU+laGR7K2fkQFCdWM`ntVBs!v<0siD#tGi^zR|8@k^raSp!)9o3FrfrEzN5^p zzL@Qj-6JAA zhT}R1W9;b!;r}AS_Y)cAItXKIMS}1!YY+VF6Xnfy62=%zB*|?a9PFHnhwm43{#z6c z*IgK611viI<)WkH-#o_HaDjgzaIUK`#$a<+Jo?v*@TE|38Pjza#u)5<0{+_s&UF^X zSdPHYfhBR&MHpkH0xyYt>CUn0WuZ#rY_e1Q+)i;+SXf%#*BuNOGiK^SAd7kJiScw@Tm!5AwNcnNek zjOjWBV{E>_&ldF@(|tkUwG#erQv7mRE5hF;=wtbQE$~$*>*;b`f-$y7;PZy+IM*Q< zW03^(PfU)V?%Bz@+&&Zym+KlZi7N1~B%B@LPzOJ3RcI|NE}hHD?y}{S+HTCvEwz^M z<6A7(I=wj8nmxl>W}Fi{($PLRI1+ofCEj5_2xXp*!4Uk+4?p^s4aaq+KbSpz=7ZUJ zIr;igG&o=*olAlvGUQxPc6L#5b}>K9M2{36=V?JajiVEWQ#Q!p|AprVAv4CwBg}_+ zCQv$Q2=X*NV-F01_!IewBZ2>s!-P7=kQzU420Q~>zwB(Gk$qkV!~rijJ;)DN6%+cr*;i@?QMm-QdPta8zf(5x~G#U}%0f+t{aU6@v z_y(bn$oQ@V_(6ihS0B|==x4#f55=LMjfX=&8xMaZ0sc|~yfFc;itjLZQ#m}70B=ix z|BK+zm7_dQ6DkkThb|cLF$9O5EQnu4a4Mg>65z!N@TU{t?kfVbrBcADg8ABr{PZ(4Fl2zmiLKv@hgJEPzmLnM{o!q5#b+` z@IwOsi-co^c1w7QsOXy|JXPR(Bs@*v{ltI)`A--4*%Cff;ExcT*4z37_?86tfdu&Z zLcf#CQA>aq5S*6RR)SMLhYNjBrk|exUy}gWMES^c9w0d7XBojMKUmH(o$G|W0Y8t_ zpG5?x^bZi6(mz4yk3ffppG0s<|6_vF@Ye`^Q|9w_g46KN6P)tXUorbtijPfz-$L;J zB67Gh0saQTDgDn9;C_OGDv9lM2f;z-M7)#WFt>yFj|7LgF~m6&qQ3&28F2-e6ZqK# zA5QRd2!1}n;oFLIZXh^3BL1fY_+tczF*Am*Nq{dVI80e!_+<(3wFHNLQd9%Bj^Gy) zI-e38!i(Vr^Gi6?EpXo^!1pJ>4=2FWMM6-YAzh?DJOTcz1o-6%@W}+H?c$FFr}Akb zIIxUzP7#dB?QR&sDg7dXQ#xe?r}AGwa7yQ&1gCU%5S-Hak>Hfh{bF2*a>jgF6W~uv zILHXxs|j#Tj4x&ROih44LU79GCW6!Yu#4cMNxAeAoR;Gfp%4Q8@V0y=9o69Y9 zfME`5-7203duCZLP4||4?Pcp@Fjp!l?ke zXG~~-{%}=nfXv7A038^hLj*8Rox-_KPe%$rRnW0G8jZq9Zhi_?aW;26&4I8u!caT8 z3+CjQxs_f&fN+;)?;K`?2})rUw;EJF@i?11rbVMs=Z2=8u$%Z>9lAXljavMgedDm- z8>hB2oj%Puxh2z4u|3mK?sxP#$f+mAli}t`&`5z>-R^AeI19Gnx>dikx#Og0G}^3o zLSctauTeX-IxtzGpW1LXck~>NM(t{+MRBWJuTeW6`==lb?S~RJveva0h7e}U+j4RVo@o}c3e6>SeMGr4@j6FhTK6W;DOgYRmv)b9*q3VyToXs5*4o9O$<2ASz zhL0w%&FUsDhcj!`OPWV$n?Mq)u2DBVw#~@0Pby8!a))e7 zLbGqYkCY+U57C-czA08~RwJ1=Dg2B+7*=S}oT^`|%?j#gNB)i{>W$+p{0y&tfW&K_ zqysYS^H;9n2N#{|HDyH>Z0_#hJICR4q+fHUf3|_$dnX&j?wrW}(|b0!x`Aublg%|} z+_P~m$QS(|F#U1QMk0UQvoV>*PtrI6(Ccx}#w6;0zdgAqZ9ppBv%$+3(@$#V@!{Kw zdp7ty82Zn)CntSu@`&HI>uDNub$_6N-?)j?|1sU4yZ=Yl*2jr&06NrB&?Q=1Uo6Jj z4)N`RSLjB>l!LJbm}qU?CZ&r`PCwDwI>wj7%kPQS)-gWvi%zH#hD)@zj{7ct>Dv0+ z3Gy$epJ;8}CE}ybg-*^-qP6uALJ9VZh){kI7vl@nHR0NNhY<8HU0Yu$5<;B|rdEI@ zIenO7iYF?cm@~q)Fm$r~5;+L>zjAF|Zx3c|#3z`aNMaVfZ)HsHBgHrd@Nt4+-gh#_ z>mT5E3!L|vjPZU6a5!`Wj`x+q#aq({_n%mEzfG)7qvPKQ##kPi-=(e~0sfGn^FKsj z-j_4R9!n4&&Pc*q@ETFTxW<8N^zVy)i1+7=F*rOVrX^zBDnVzj2+#X>Sc6Uw-c*KR zrRSFX0_#L#A+k7orgctsZvKpX8_O##E+nt|isZDpHY?*xX8b9UsY%_$B!uHZOskW9 z)-HhpcpG62Gn7R+Ma5+WR%;0l14~kcWlUd{nqjqZ%b{eg2C@%z6341|I7G%@719G7 zrr$99GJ;b&Tze7Y9y+ISnkYA!{<#8&<}8{tcVMXooSbe6!D+e=OLVZ$m`B1>{udCO z(s_m8l>ZM1PRIUqjf~Rymk5vf!r{(-LWlB$`y^%gTm*oS4bZ}TT_SKV5f{v7KZ2=*`X90>N4@*D{Elkyx0_LK4) z2=+hn90>L^DWtq057^IubtZ6t!5{V~Vrzsnb{==dIBLmKf9Ccq$Bn;g+?Cw=<+0nd z^xvwd4Gzc73QObmEa3nsAF>#uOWlBhFbh%TvE9dw7xee4(IhZe5oO}&z2eRsqPV+^ zH#q69)n-LC=Vm@Y>~om>Dg9t^R5uC{sxXp3t2rgQnFIs34d9%GOb#r+kON_sM**8r z#3qF?o=S&&h02Y*WS%%djoPM~@5A7IisCMh8korM;lBA5soel33>3FdaZir&j5z-m zb7(hYSy-t?Ee65NlnH87Sui^q)q|VdIU}00yo(!J;QfNa89Zc5H{%YQpdLawUSq;3 z6VyYq1wR)k?(##17iYgFt4m_vNR$sT=}ikJRmkrx1cEh*8 zWH>ny!_Y(YVR*J~Y@y2@R0K!mL(LJtPX8_o=P9N9WYb6R8yX-izUV*ExIcN#T$oYG zSAs2qWDG$xD5L2lz?o06Aq6?G1BxbY%-R9!+B)bLx!6oQS&yuP34005TX$Js@D4NY zX7p<{dxf9b()78H=8i!>MWY>;k7N4Ou|!reDew|Jf(;HdNmp6XIRfn>j^U-y$;z`$ zI#A8MYIuNI`U$H#WWG=7!_0(`B4=s$7h?OP&bwFb%BZa9w3MfN)i7HJrO5LXnowEM z*>5(W7J^<-Sp5;6yDdwhIu9*@Uezv}yr{*L2%z zx(SF5uz;wzv$_@cmZlx4PB2E&qqw&W{M__UQQTSki2r`W-(C^a>st^f?T(=?aicm% z&U|X3%q#z|WZwAfVStKZimo;h$m9q`4LEKHbVT@lny&HSC^L zdr--9szGS`61D@35lWbDytjsF7LU@H6;#%3v9Hh6rf<=lSzr}hYdUy?cA%mm53JNc zc0?9<=p9E04O7$*nrD!Q-gSh~8?jtsG7U}slh}4I`V81TpQuXsgvmwh6F?#0b!Cg< zR`(GTIG{LikF83h>T!ZWOrHfRl}Bk**Yo!+{2hEY^7pO$eIta=POzPM5fAz&~LV-+5@I^qnUi*Mg~;J zC${H7?J>&6_)>5I{+naMYt3Buk=z>SU?sggrlpC#9Sj&6Ce^_5+j;hg<+1)?j59PI ztX`jGk0jg94~^nRxSQEQ7W$QV+-C`8h{ulP&?(~4Hy|`fJTkVB_w#go&W+g`0vG)o zoLB{;!`FeWO4~*J=dpp;ftgF&Nud$fbtE%pJLNi1zHLK8!}y4IxL1v)hEDdX(e%)X zUNt%_B(j%lfY*^@lpS1!ilzct0x|JYiqpm-9Dy}|q+uP0Rd>m$7kQ+i`Sdc#3_ zXN4{RiFjF3=sf))B{W=rNDYBawuqOdBl{VKbYP6EMie)z9m zkJDJ_O7VEHSr>;gqb?3&MqTX3j=wH~`10mebbiEULF>1K#^>29I&=6-Qs`IkGLpX} zht7tVlley06O1bfcvK*I)?uB*=<1LTHsm2l(hGd1s zz_nnlE6*OuRerr*X~Y}|&%DhYCMGxbBV0%z1gpzvC=$Cy?JOMtST0_<@Gvp~V#+&E z-YIALg5%2cuLL=z(WBcQ-C|#Fab|U(Na<}0iTeBt%w8)KI*W6m*BTkR9NO7h#*%M# zriVt07yC`l^w7EdW!$)Aco`~Q>^C}%UQE~KM8|K|Q7D|Fe`8o?XavyAYi5hi37sV# zS$gOsd`!jO@_e((o(!JcNMzn-xMd?ET^Yf-;4Kn7<9DX83k7D$teuBV!h`gx{X&eY zAQ}kF$%~NOa6kV-oTql`p9u265XQnVM{0- zr^^pB=)n$Wnhi1D1WKIg;~_ZX!zT~_u8-9Y+k@+Ld3e>s`lkuZb(Vupdf48QqFdGN zL_HX?mbahZ+s(8hh}u7>)g+W6Pe@D z;9V3MuhB$~qOwP?j{1ZN@+m#A zK%P>m29iWb4uY-tJ|LJm?*oD*XaM`0qDqS+svxX!#EME};Gn)75F{h~Zxu1Z->$gT zK;AX#_F1Cu`jU)Q8#9ygHg|w;8L1#ACrUi-)P9ATxNgQ>(RF~sux0>zi1~Dg&1l#2z$U7uSrLRJzySB*aL<*DY~z)2Q0-A z_JC=e2F1;sx*=e-!I7S4SKIoFVr#Qy(Mkn1Z6Z)Vrfjt6*+BA{{QfFV9;_Tgt!T5| zPsu}_)wCM51v4I-jL5aHo*XX+FbEVmNRR}y08E0*>3EaiP^ot z^d_Sljv0hbP&>;-j{^-D)Cr{uM)79S$M+o-8ie>z{!%JnpluyYDRZH~ZUCcq$B8lc ziBZ5YY_b7%lk9XBZPVYlX$2efc{{zc#KJi z@_p2liJ5g6^H%Cx6--df9{v(}U}7+Q6)FYMAoyViINd;|R{AKZTnMCUf$?F~RHI`O z@R}5_K%taPX&qaE9ytS=J??G;BM~S(tyXWgu;=I#I1fv|c5se*ce1W1L;rCh?9OS5 zB$LJcF-?&aSk&ih9PxvsHARvj$hTrK0I9(GpM6I%c7h{+glz*_(~cx9Im_Rpqz{U{{%#0pzk9XmKE-|a zDZ$!XquO+zW6~)>GuaO+)x8$jji5YUfV)N#cj|E3VmvB*&FU@~r0E9?nmfu5!$A&&Y})9JehmVTU@wUJG5F_>8I8SB$V`g*iDcLH|sxIIex#A=^%?`O*B zriWAYU)uxdHL2Ryqok`H`aOcUu=W7IQ?L`R-h+R7JHFdkZBs@!DKB}dJhnS))S#|n zeeG=edZ*JrP~8=}&WJHu?cm|cPczr-($x+l`pI#k+tr}OUJ*=Ep6Z69B|$6Fy9-FuQ2>PF5E2~gf$b`4J}yLsy%ZC@B$2-0)@JHk3DG72CfHaeUEaV zao3|VVsnr3N1rlcJ>2-XHFPSJ&sLPSeskjxb*q_B?UX2gaopW6Xrfh~axav*zeo9_ z3eou?cUW%)vfF_u{5w*iZ;}e#7gOl|IEC(~h2AGR9PqJMY)Vp|dV&|aPqS~-%p*G) z(%P+8-8-mR?SRh5yfX@Re26}u^je_*^DDIa^jdJ=pI7b5Ob`dQxf^km>Ra{DsR=^E zhEEdu9Qb4d#y{B!;>3)Llxm`d@oRg^hrli<<&@|GhDetMGrb{uITZZzud?8WZ$BS6#L{381ut&ECz*}@e%|D#Tgc8 zF(@qDilFlBJjJbLqtN-Z+x}md~;!n$>~{A-I?J-KBUh zZL&iRm>YGnR%`b4=~^?8W?Icj{LU?yJ6NsXzSW@b=gDeF5UfFSrhCA^eqRrmin3#jBsdo^Mo!HI89aX!C$9L$l5hho(8n`&yqbch= zngymqfl>a$_b2iEq-r&Tl*X*}nzC+-W)atoL59;c%rFnI(F~xt)$d7~KVX`$rKp2= z>_!jRQnZJ7gwSIvmoewkXL2c|E_PAz!Vz3hReSz?j;MH~UY(@P+XH7}HD@|su9K?W zN!q;i!u#lxcQCK-(wyasz3NY7o+jN?OuXtrzH#Ot3<0~b$I@!yk}!QOTB}umB+vWs z+339wX|-xEqC&5O4Fr?nRy&tomH3f|4&^{JTJ5nZc9pAhKV74CJoXP=k%Z^+G+~2a zH=M#TDZ!^g!foRR(etu;d+^(k|pZ}=~EI0hD)n>t6dM<%p(hcZg z5vj7`Fe^{7Pwuo-R{X3#C3Qr5AV{>bBB~=P`eM$J_sYlmT9iiE;%6+8yJ16$@z$vR zNc!0E0Ru75NP~2*-8L6`3AhQiJM;$e4Cz(v_b#}o`#(B=koq2FkH|$Bq8Cuv> zD@xGZ0f<|NVlUO!cY^zrMzsU_OI=fF%p9aNdahpoXEFaZ3q9PKXT(v@;!O97h}QvK zg-)mr>k~iTgsQ$M{{Lxir@&gBvn9`2u_@1))sp8_KbFP}M%kh=00+-BdxgiS0Wh_i z*Q{=5Y&Q(Zl*X)0pc6PZk8ExCO&Ml#Q*+_W9 zS3ORUzv?OJ1e0N{XL3+9X;3rxi49oy3e8a()%H9K=qT#;yld1=%KYDfy^=e)@CO@XzI*vF@OEf_b$1(UU>LZ5Sv7t2>n|R)6 ziR#B(^^vb}ovLWrF}pX#q=B(v_LK9?A+%szZ}qpgM5CNuNYy*_>s!DO3U8!zg7)YH zb+*WS^3gVVNHxyvg}mzCw9r^SX(?uF`Md`du*j=Z?cn#qr@|D%wVEwWOkgUrK7-S6 z6?>IgeNH#Q1Ut~g-4Dk+zrwdq%uAMLXA~i%qs`9n*f+<-?i-WO@WF9ckv~&`9jJ2+ z6B~KZM#;Gtcy=3R7 z#=a;1-%)?1k!i|0pZGlR@t?9{z19>-)lD~HGPP6S@?!jjtV2~ zBL=ot`1E6I{ElJhv5z*$;1>rAop1^qu#~q7+B?TM!%knGvtqkrG8ieVtk}wws=Jc- z*5K{IbNDvjXlPiTy<+=0v-N$#C-BdHQ%66pa{F0%idXepDl4`c$1I2Nj+G}|Qx@w5e~nao#K(7@cYTB%R&F?p`UT+rOg%cTM#1_{$9y`s zsJKCs{on1%ho7GIr}NJ^ld*F$N^Lh^bL04{C$I+#t}5<-L19tJg%|zTh>;gxK0SBF zZ!9wzOPR&)wjB7GS@O_KDbp{YCPthrY}jSo@=3;xjBUATPzNiQ4zXxpzm#=peyN85hqfU1qQ`I1jhh5*#I zAKXP``rm5Y=ZLobevY9ZG3eucjEVg54val9@xL?T<9&>eLoaLaH_y0l5btBWCx-t! z4F2&x#s_2Q=Na_zK1P1IIMPr0o8gc5F~VI{rvEI%AMa!2SJ)fzHyHkSALFc;_zxNW zXlLlN82^oiKiNeq7v z8vb}6@^9+Byj}hvULH}Ov%`v=>aU=ClT4%(^`xwDYg%RIs#>e{@e-l$bCK~kv z?E*eb<0q9H^cO+BVT_#t8=o-$$)_6kJ$@|wVV{WUKf{pkcHw_xO#0Uv=^qmQiSCO$ z6Vhdjy$$~`{iG!Zf7c5C88PWk8p(I20;3Rrm+)Ps$}pWC4_{8t(2?-Krr zY^{-3xZ6zUxmgUFKM&|dnc8x4p%Q8f&g z$cAaleR_~#VB}NW?Fi|}HYmZyC*Y!K87>1Z&IDc6Ky-(;&Cr2@B=ZC=_>3L`o#1ah+hYpBTnWY?st@Ff*bLy9?2!*0{>7(GXFP= z^yT|WhY910N3)>{wk;(Vxw{#SH%d+p3pPptlk=~M_!&~gLjKW#&J!>HK9PUy?;!u6 zpJM!@*-)J+?6`4VfjF7}c)WwF17nlbxhO6yDt^?GY=uXCD&ybV_>ikaD2ymFrXU(9Q#vjr!OY3i3+E|!4@m* z&<4lH0^(jxm^_yqtQf?@;W#_cd0o)qV+O|97J;8C8Y~|xFvhkepaT;h@#uqXN1(q# z6g=0t;XY4+|12VLT^eYJ`$n%3Mkl$B4ENLve4Mx+1;+?rAq?o>D()TOIwfQ5W`RE` z24ZL{HCy20#eF1Phh&TuC%DfP?#BT7SBR#;$3%>=X9fOGVI-LAa*VOJ1YRZj13m^~ zj5Q{p50mjgf1bFv48v~~_@6~b!pBN5!6fjR(4jDiu*|? z3i)F{cZR^{3;FZ04rAP=De&{DxTlAYVZa8m2%jS4&&MW=vB?527IMb>Qu75qo*!&x z`dET7ey?O|7&rtG>-M4eE@#`RXQE6 zlQPD};{C0{SSr_r;oe(;|F^gw9q~y5fA&{>KUwsjD7V=He@sj! zpe@xJfsYqPn$gBJ94!U@?-w}NS-}~`R9UHYMs|LYEqnSjR%T;xpC`QO*LcEfc}X^( z<0&g+VEI_TZ55`Dvdd=X=h+zE>spYPZL^ly7{491%y<=X2?A8gZ2SUQYZ)ZZX3l0j zDq3LHZ8qz7g3id!Eicp!FB`_0OG@*LYm@+h8xuC7HtOa_k@*+}2F$%wk*GO8o zJ6>ol)MG$8_!4`Sa-p?wdSM9<1?z^S#gw7kT|_?>#- zpgRNA67IkQ2lv5)LmX{Eqn?I1>e_g>0N~JNWB9oe9nqaLI0FX`>7ah|iiBf$SmpqS z;gSC55{~@rBsk>xK1U?R9Pr8Kb7t_J zBjI5Tg7o2Dc5uiK(*IDxG5n_qaGZmb=^T*4BmHy;DAJ|;k0&^l1GQBSIMT@`;i>#* z6P(@`3HP#Nx|IGx2}kD+7+XX=ozg>if zF(s5Q+B%2tG18&sTTF0xL_RAd9P|4k!6~0>1di$A_@1Bt5&Xlq9_fEAg~#!gpU|h_ z+X)VjnBN}~;Pn1w2*OX6F~;^w;f4BX%z;8NKoHoG4lY?_9C7R)PLOaMOPnm>Lj7c? zOE|Xo^CTSg#fv2z$Dv~-Tnr!CcnQY^wi_fI=NE2~a41V~w@En4`9293YAKs0;bPdz z@+BPm^M?}P6$GdEvA#fXdLQdjg46q0*AX1%q_DniB{=Aih+D)s3Cfq=w|gqVVd#$G zFD3W|1c$>e;D8Ru9>W(B9O@O~k4U&+k3COt+Ml=x4s@`*@Sa9F-ET;EDxXjS+#<$* zK!=7Ok^mn;aGI|xB^>j0JHctbrVGTM}EE|IOQi>jK85ADL-=vPWgF; z;FO=YBpmr!CE>`=mjtK$3=rdfnV(Y$4(TGDixc2C5*&&G?+2YM;kiPe_>$n1eu&_d z&-L)(BHvl$CrEG_{w?w0fP7K<8wpP9c?ZF%{Cf#b<$sd+03m&pe};r3pH~x{rh6N~ zfj-iCK*EtuIl(ENCkRgI)JZtfd7t2vj+fv-2fqug5{`WSAmKSytiw;MwSLaGN(wRVTN{8Mr5B#8>@H-M7aPi8@B^>#;OE{*xh~PBcRRpKy`w_vZ z{CyIR>9$Karh97sGzmvK=LsCT58wy&uF)hsrGGoYA-t&e%p>7~J$8;z zIFTO=KVHHyzqd#@@|jI=%Fk?qQ+{eCTvRjGF5%Euf;*AipAK}8|7jAAbRLm#`|3bo%|6K&9 z{Pz+Z_(%E*xo;hC>_5+ya1otNlyKyq-v3VN6iMN+-abWe7;|90mL$M~1PA&U{$CP~ zau^`y<3Y|;4&w+8bTD5NBpmMi1UH4?RG+_>;FL}-!KvQ+Fu@@_(tkt3ksp3}3eg`B z|3(Ura@Z;1D2GD?r*aq!FLCH!BH_sA%@U6N!v_+M?IKNd;8@SG{a!8Mf<0!HaIA+9 zOE}iIrzITeye#1ueu;!*_;(WEUr0FC+b)8GY*7A(2@d6p^oNU#qa2W*dGhpWCZy^I_F3@raPYCG~JsBPSc%Ea5`@N6Tu-oraMQ% zG2Lef4q3tQct1SykM*RBgs1cmOE}U$O{|Xr9h$F^1gCVal5nImh2WHqmEb@J%l$zK z7sbS85uEBF3kgo?ERk@OPlJRbpPvz&%5$7p*puaXyM!bExe|``d~O2#sRVd^0(^A> z{7VVP{BDzQ%))p zaLjKd!D)U!BRHgs;r}V&NIxXu7(N9)(xN_4`sWdxmg7|fr{$$eIMTm|;FSKe1P40e z^TAe1IP&k2aHQW#a7sTyaLQ+Yv9S)z9qF7yaN3VuMR2Hk$j?m#2l_Zae6NJ#{B41R zBb|pO9Qk>U;FOg_bgq+d zmQ zTt;vR4+fXOO_6Xg-3#t%2}e1+Dd8xG27*Jn;`7A*A>sJ_+9TmYonuo(1pxkG>ooHJ zdxAqf!2?vY2u|zO(*&pCmlB+Y|8IiR@P2~R@OubO`++dQDV;P?F(6;G948Z;hM!4r z8h)tQ*NOE6Sy@5{`7z#D;i~8>Mq0!6}__5{`5x5uDP=B{(g|2MG@J@hsq63CH@gK*BNI z#RRA6en@aie}jZ0{ml}N^!E^)((flGOt8Ko|HBgC7fCp#d%1*Tx|0b`)166hny*I) z4t*-l`&AR1mg930j_EEVI8FC+g41&RhTxR`P6%73|pW4ccfoTgh(a7yQW2}e4g6P)(vd7=ZCPD1cxjlel@{?9~>`e1c$B67=FCi?~LUomSx!u5{~nz zw@5hBzfHoi-rghOIPdod2^Z7jELXy@A1EU@-6u1T;4pN?d_7HY7-Ay+7lMNhi@1y6 zbl;1c;4sv{@Lqz`^8Jb6RQ{KW{o^QStZ$lxBmH|M9K#nAoVH&F!D;(_QNoeV+XSa{ zb`zZPf0o$iF7tn-gd_hGB^=ZJ1Hoy!vj|T4uaa=2^8&$Xzwr-(10A7mu?cW6Pqe%1 zNICw2;IO40`G1_?aMlNL55Xy)f0J{{6+{4bgD(^iD2Im#PW9nA1P3~p?tFsN_EJl52#@)Fm*6zN?-Lxti(v(8A~<|nQO;Wl4&iZ} z)h^+f?jeHHbd$t+3zR3On@Vt+?%4!~bg^6}5uD1ch~Tsyz9ewm7dl0ZLl=?obf0Gf z!69AoS!5q2!2eEgn%{pBoaza^1gG?eLS{s}pmatNoYEOba7yPkf>SvZ5gfvc&oQf% zaPfI!&q}!Hj@Vx$9Obi`;FNzK!72Z31cyhgZ{HCd_(Z%%!m+-ki4FQtuV}jG5}e9s zw1gx5Yb6}%XAzvzFCsYQ^HB*$Ixi5M(pgS$O6Ow^jSoYL7taGKwJ5{~&zf&8hR^=&l4DV=L29O+~cobof5;Pm_QI>CWH>U$qcIF|3{5{~KaBslFKUV@J? zsSnscd@kYGKOB&7>>q}U^Kc*^=(CZZ%Lq>Mbu+;sU&!aj5-ye_S+j)Wci|g?Ll!aJ zZ3GAUV%nDNN`UVXIPRMg%Rp=|2~Woh`=szVAKOd9({aRMDZH3=X34^;63C5?FH!}L z{DADhok+sdamP@C(|U5cgk!qrNc8c$d;tj$WsCA0PjI>q=VpRKx)}Zr3CHsN6TxZy zw-cPwc}BvK&QgL?I?V*9biR>rr1LL=Q#yFS9Lj}$zXphd=YRu!98aAt;V6d@5{~KK zNN}3&lLV*yzarsC=L3RMI{!^@po8<_K7qqH6L75OCkeYGm@gcM4wrBohk}(fa2Ou@ zr4bU2T!Kpm$1gGQQCkPJVvEO)};8f3lmEaH_`>{6(PRGT|2oB+~-&jj<+E0224&kw$ zpaZoT_mqJX+wD1yWd%-DD~{t>0UYjU1UgVxE*8Wxgg%aVhu8->Lc)6l4)zPcVfe!W z$8(5?4;1t>rSL-qj&?9G{5i;vPM-4!e4@jA0S13q52uO%h@*W8Jokn;+KYhO1;8PW z_98qIj`kDyb$BE^+DYJy6CCX!pgfU2+CxA&AddDB@Eil;Xb%DGI8gd%4`H!{qn!$m zgrl8=poF9S3Z5Y0AMGb(03!rPyB3)ej&>E`j$#@f?Ojw#INDoSEa7PH!Xx2mhao88 zXorC}62d3iVaR~SLvXaSktyM5pJA$mqn(XP2}ipPizOWGHh3f)?KT7@9PKvXhXb+) ze`vQMLwryWM|&Na5{`Cerb;;4nW>U+v@_$9aI`biBH?IfrbEKfjzlUsM+Us2y@(MK zj`kun2}gSoGbJ4DQRwz}&320RBIr3dNEhw!v`BQ&jzouqqaBG(T>Fb1ikO} Ah5!Hn literal 0 HcmV?d00001 diff --git a/interface/external/MotionDriver/src/inv_mpu.c b/interface/external/MotionDriver/src/inv_mpu.c new file mode 100644 index 0000000000..9969465596 --- /dev/null +++ b/interface/external/MotionDriver/src/inv_mpu.c @@ -0,0 +1,2798 @@ +/* + $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 new file mode 100644 index 0000000000..db71b4b74c --- /dev/null +++ b/interface/external/MotionDriver/src/inv_mpu_dmp_motion_driver.c @@ -0,0 +1,1373 @@ +/* + $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 new file mode 100644 index 0000000000..62f4e86660 --- /dev/null +++ b/interface/external/MotionDriver/src/inv_tty.c @@ -0,0 +1,88 @@ +// +// inv_tty.c +// interface +// +// Created by Andrzej Kapolka on 7/9/13. +// Copyright (c) 2013 High Fidelity, Inc. All rights reserved. + +#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 void write_byte(unsigned char value) { + char chars[] = { to_hex_digit(value / 16), to_hex_digit(value % 16) }; + write(ttyFileDescriptor, chars, 2); +} + +static unsigned char read_byte() { + char chars[2]; + read(ttyFileDescriptor, chars, 2); + return from_hex_digit(chars[0]) * 16 + from_hex_digit(chars[1]); +} + +int tty_i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data) { + write(ttyFileDescriptor, "WR", 2); + write_byte(slave_addr); + write_byte(reg_addr); + int i; + for (i = 0; i < length; i++) { + write_byte(data[i]); + } + write(ttyFileDescriptor, "\n", 1); + + char response[8]; + read(ttyFileDescriptor, response, 8); + + return 0; +} + +int tty_i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data) { + write(ttyFileDescriptor, "RD", 2); + write_byte(slave_addr); + write_byte(reg_addr); + write_byte(length); + write(ttyFileDescriptor, "\n", 1); + + char prefix[6]; + read(ttyFileDescriptor, prefix, 6); + int i; + for (i = 0; i < length; i++) { + data[i] = read_byte(); + } + + char suffix[2]; + read(ttyFileDescriptor, suffix, 2); + + return 0; +} + +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 timespec time; + clock_gettime(CLOCK_REALTIME, &time); + const long NANOSECONDS_PER_SECOND = 1000000000; + *count = time.tv_sec * NANOSECONDS_PER_SECOND + time.tv_nsec; +} diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 7158cb84b6..38bf43f461 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -5,7 +5,7 @@ // Read interface data from the gyros/accelerometer Invensense board using the SerialUSB // -#ifdef __APPLE__ +#ifndef _WIN32 #include #include #include @@ -15,6 +15,11 @@ #include +extern "C" { + #include + #include +} + #include #include "Application.h" @@ -23,25 +28,31 @@ #include "Webcam.h" const short NO_READ_MAXIMUM_MSECS = 3000; -const int GRAVITY_SAMPLES = 60; // Use the first few samples to baseline values -const int SENSOR_FUSION_SAMPLES = 20; +const int GRAVITY_NORTH_SAMPLES = 60; // Use the first few samples to baseline values +const int ACCELERATION_SENSOR_FUSION_SAMPLES = 20; +const int COMPASS_SENSOR_FUSION_SAMPLES = 200; const int LONG_TERM_RATE_SAMPLES = 1000; const bool USING_INVENSENSE_MPU9150 = 1; void SerialInterface::pair() { -#ifdef __APPLE__ +#ifndef _WIN32 // look for a matching gyro setup DIR *devDir; struct dirent *entry; int matchStatus; regex_t regex; - // for now this only works on OS X, where the usb serial shows up as /dev/tty.usb* + // for now this only works on OS X, where the usb serial shows up as /dev/tty.usb*, + // and (possibly just Ubuntu) Linux, where it shows up as /dev/ttyACM* if((devDir = opendir("/dev"))) { while((entry = readdir(devDir))) { +#ifdef __APPLE__ regcomp(®ex, "tty\\.usb", REG_EXTENDED|REG_NOSUB); +#else + regcomp(®ex, "ttyACM", REG_EXTENDED|REG_NOSUB); +#endif matchStatus = regexec(®ex, entry->d_name, (size_t) 0, NULL, 0); if (matchStatus == 0) { char *serialPortname = new char[100]; @@ -60,7 +71,7 @@ void SerialInterface::pair() { // connect to the serial port void SerialInterface::initializePort(char* portname) { -#ifdef __APPLE__ +#ifndef _WIN32 _serialDescriptor = open(portname, O_RDWR | O_NOCTTY | O_NDELAY); printLog("Opening SerialUSB %s: ", portname); @@ -87,22 +98,14 @@ void SerialInterface::initializePort(char* portname) { int currentFlags = fcntl(_serialDescriptor, F_GETFL); fcntl(_serialDescriptor, F_SETFL, currentFlags & ~O_NONBLOCK); - // there are extra commands to send to the invensense when it fires up - - // this takes it out of SLEEP - write(_serialDescriptor, "WR686B01\n", 9); - - // delay after the wakeup - usleep(10000); + tty_set_file_descriptor(_serialDescriptor); + mpu_init(0); + mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); // this disables streaming so there's no garbage data on reads write(_serialDescriptor, "SD\n", 3); - - // delay after disabling streaming - usleep(10000); - - // flush whatever was produced by the last two commands - tcflush(_serialDescriptor, TCIOFLUSH); + char result[4]; + read(_serialDescriptor, result, 4); } printLog("Connected.\n"); @@ -187,56 +190,43 @@ void SerialInterface::renderLevels(int width, int height) { } } -void convertHexToInt(unsigned char* sourceBuffer, int& destinationInt) { - unsigned int byte[2]; - - for(int i = 0; i < 2; i++) { - sscanf((char*) sourceBuffer + 2 * i, "%2x", &byte[i]); - } - - int16_t result = (byte[0] << 8); - result += byte[1]; - - destinationInt = result; -} void SerialInterface::readData(float deltaTime) { -#ifdef __APPLE__ +#ifndef _WIN32 int initialSamples = totalSamples; if (USING_INVENSENSE_MPU9150) { - unsigned char sensorBuffer[36]; - + // ask the invensense for raw gyro data - write(_serialDescriptor, "RD683B0E\n", 9); - read(_serialDescriptor, sensorBuffer, 36); - - int accelXRate, accelYRate, accelZRate; - - convertHexToInt(sensorBuffer + 6, accelZRate); - convertHexToInt(sensorBuffer + 10, accelYRate); - convertHexToInt(sensorBuffer + 14, accelXRate); + short accelData[3]; + int r1 = mpu_get_accel_reg(accelData, 0); const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * GRAVITY_EARTH; // From MPU-9150 register map, with setting on // highest resolution = +/- 2G - _lastAcceleration = glm::vec3(-accelXRate, -accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2; - - + _lastAcceleration = glm::vec3(-accelData[2], -accelData[1], -accelData[0]) * LSB_TO_METERS_PER_SECOND2; + int rollRate, yawRate, pitchRate; - convertHexToInt(sensorBuffer + 22, rollRate); - convertHexToInt(sensorBuffer + 26, yawRate); - convertHexToInt(sensorBuffer + 30, pitchRate); + short gyroData[3]; + int r2 = mpu_get_gyro_reg(gyroData, 0); // Convert the integer rates to floats const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec. glm::vec3 rotationRates; - rotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND; - rotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND; - rotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND; - + rotationRates[0] = ((float) -gyroData[2]) * LSB_TO_DEGREES_PER_SECOND; + rotationRates[1] = ((float) -gyroData[1]) * LSB_TO_DEGREES_PER_SECOND; + rotationRates[2] = ((float) -gyroData[0]) * LSB_TO_DEGREES_PER_SECOND; + + short compassData[3]; + int r3 = mpu_get_compass_reg(compassData, 0); + + // Convert integer values to floats + _lastCompassHeading = glm::normalize(glm::vec3(compassData[2], compassData[1], compassData[0])); + + printLog("%d %d %d %d %d %d\n", compassData[2], compassData[1], compassData[0], r1, r2, r3); + // update and subtract the long term average _averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates + 1.f/(float)LONG_TERM_RATE_SAMPLES * rotationRates; @@ -251,7 +241,7 @@ void SerialInterface::readData(float deltaTime) { glm::quat(glm::radians(deltaTime * _lastRotationRates)); // Update acceleration estimate: first, subtract gravity as rotated into current frame - _estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() : + _estimatedAcceleration = (totalSamples < GRAVITY_NORTH_SAMPLES) ? glm::vec3() : _lastAcceleration - glm::inverse(estimatedRotation) * _gravity; // update and subtract the long term average @@ -320,22 +310,24 @@ void SerialInterface::readData(float deltaTime) { // Accumulate a set of initial baseline readings for setting gravity if (totalSamples == 0) { _gravity = _lastAcceleration; + _north = _lastCompassHeading; } else { - if (totalSamples < GRAVITY_SAMPLES) { - _gravity = (1.f - 1.f/(float)GRAVITY_SAMPLES) * _gravity + - 1.f/(float)GRAVITY_SAMPLES * _lastAcceleration; + if (totalSamples < GRAVITY_NORTH_SAMPLES) { + float smoothing = 1.0f / GRAVITY_NORTH_SAMPLES; + _gravity = glm::mix(_gravity, _lastAcceleration, smoothing); + _north = glm::mix(_north, _lastCompassHeading, smoothing); + } else { // Use gravity reading to do sensor fusion on the pitch and roll estimation estimatedRotation = safeMix(estimatedRotation, rotationBetween(estimatedRotation * _lastAcceleration, _gravity) * estimatedRotation, - 1.0f / SENSOR_FUSION_SAMPLES); + 1.0f / ACCELERATION_SENSOR_FUSION_SAMPLES); - // Without a compass heading, always decay estimated Yaw slightly - const float YAW_DECAY = 0.999f; - glm::vec3 forward = estimatedRotation * glm::vec3(0.0f, 0.0f, -1.0f); - estimatedRotation = safeMix(glm::angleAxis(glm::degrees(atan2f(forward.x, -forward.z)), - glm::vec3(0.0f, 1.0f, 0.0f)) * estimatedRotation, estimatedRotation, YAW_DECAY); + // Same deal with the compass heading + estimatedRotation = safeMix(estimatedRotation, + rotationBetween(estimatedRotation * _lastCompassHeading, _north) * estimatedRotation, + 1.0f / COMPASS_SENSOR_FUSION_SAMPLES); } } @@ -371,7 +363,7 @@ void SerialInterface::resetAverages() { } void SerialInterface::resetSerial() { -#ifdef __APPLE__ +#ifndef _WIN32 resetAverages(); _active = false; gettimeofday(&lastGoodRead, NULL); diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h index bbe246ba92..46012a3866 100644 --- a/interface/src/SerialInterface.h +++ b/interface/src/SerialInterface.h @@ -13,7 +13,7 @@ #include "Log.h" // These includes are for serial port reading/writing -#ifdef __APPLE__ +#ifndef _WIN32 #include #include #include @@ -69,6 +69,7 @@ private: int totalSamples; timeval lastGoodRead; glm::vec3 _gravity; + glm::vec3 _north; glm::vec3 _averageRotationRates; glm::vec3 _averageAcceleration; glm::vec3 _estimatedRotation; @@ -77,6 +78,7 @@ private: glm::vec3 _estimatedAcceleration; glm::vec3 _lastAcceleration; glm::vec3 _lastRotationRates; + glm::vec3 _lastCompassHeading; glm::mat3 _angularVelocityToLinearAccel; glm::mat3 _angularAccelToLinearAccel; From 6fe9a71868c3934227cdb4dd133acb8b8f6d00d8 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Wed, 10 Jul 2013 14:06:12 -0700 Subject: [PATCH 2/6] Basic compass fusion. --- interface/src/SerialInterface.cpp | 49 +++++++++++++++++++------------ interface/src/SerialInterface.h | 12 ++++++-- 2 files changed, 40 insertions(+), 21 deletions(-) diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 38bf43f461..d4a3e2a2ca 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -28,7 +28,8 @@ extern "C" { #include "Webcam.h" const short NO_READ_MAXIMUM_MSECS = 3000; -const int GRAVITY_NORTH_SAMPLES = 60; // Use the first few samples to baseline values +const int GRAVITY_SAMPLES = 60; // Use the first few samples to baseline values +const int NORTH_SAMPLES = 30; const int ACCELERATION_SENSOR_FUSION_SAMPLES = 20; const int COMPASS_SENSOR_FUSION_SAMPLES = 200; const int LONG_TERM_RATE_SAMPLES = 1000; @@ -98,14 +99,14 @@ void SerialInterface::initializePort(char* portname) { int currentFlags = fcntl(_serialDescriptor, F_GETFL); fcntl(_serialDescriptor, F_SETFL, currentFlags & ~O_NONBLOCK); - tty_set_file_descriptor(_serialDescriptor); - mpu_init(0); - mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); - // this disables streaming so there's no garbage data on reads write(_serialDescriptor, "SD\n", 3); char result[4]; read(_serialDescriptor, result, 4); + + tty_set_file_descriptor(_serialDescriptor); + mpu_init(0); + mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); } printLog("Connected.\n"); @@ -220,12 +221,10 @@ void SerialInterface::readData(float deltaTime) { rotationRates[2] = ((float) -gyroData[0]) * LSB_TO_DEGREES_PER_SECOND; short compassData[3]; - int r3 = mpu_get_compass_reg(compassData, 0); + mpu_get_compass_reg(compassData, 0); - // Convert integer values to floats - _lastCompassHeading = glm::normalize(glm::vec3(compassData[2], compassData[1], compassData[0])); - - printLog("%d %d %d %d %d %d\n", compassData[2], compassData[1], compassData[0], r1, r2, r3); + // Convert integer values to floats, update extents + _lastCompass = glm::vec3(compassData[2], -compassData[0], -compassData[1]); // update and subtract the long term average _averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates + @@ -241,7 +240,7 @@ void SerialInterface::readData(float deltaTime) { glm::quat(glm::radians(deltaTime * _lastRotationRates)); // Update acceleration estimate: first, subtract gravity as rotated into current frame - _estimatedAcceleration = (totalSamples < GRAVITY_NORTH_SAMPLES) ? glm::vec3() : + _estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() : _lastAcceleration - glm::inverse(estimatedRotation) * _gravity; // update and subtract the long term average @@ -310,23 +309,33 @@ void SerialInterface::readData(float deltaTime) { // Accumulate a set of initial baseline readings for setting gravity if (totalSamples == 0) { _gravity = _lastAcceleration; - _north = _lastCompassHeading; } else { - if (totalSamples < GRAVITY_NORTH_SAMPLES) { - float smoothing = 1.0f / GRAVITY_NORTH_SAMPLES; - _gravity = glm::mix(_gravity, _lastAcceleration, smoothing); - _north = glm::mix(_north, _lastCompassHeading, smoothing); + if (totalSamples < GRAVITY_SAMPLES) { + _gravity = glm::mix(_gravity, _lastAcceleration, 1.0f / GRAVITY_SAMPLES); + // North samples start later, because the initial compass readings are screwy + int northSample = totalSamples - (GRAVITY_SAMPLES - NORTH_SAMPLES); + if (northSample == 0) { + _north = _lastCompass; + + } else if (northSample > 0) { + _north = glm::mix(_north, _lastCompass, 1.0f / NORTH_SAMPLES); + } } else { // Use gravity reading to do sensor fusion on the pitch and roll estimation estimatedRotation = safeMix(estimatedRotation, rotationBetween(estimatedRotation * _lastAcceleration, _gravity) * estimatedRotation, 1.0f / ACCELERATION_SENSOR_FUSION_SAMPLES); + // Update the compass extents + _compassMinima = glm::min(_compassMinima, _lastCompass); + _compassMaxima = glm::max(_compassMaxima, _lastCompass); + // Same deal with the compass heading estimatedRotation = safeMix(estimatedRotation, - rotationBetween(estimatedRotation * _lastCompassHeading, _north) * estimatedRotation, + rotationBetween(estimatedRotation * recenterCompass(_lastCompass), + recenterCompass(_north)) * estimatedRotation, 1.0f / COMPASS_SENSOR_FUSION_SAMPLES); } } @@ -370,6 +379,10 @@ void SerialInterface::resetSerial() { #endif } - +glm::vec3 SerialInterface::recenterCompass(const glm::vec3& compass) { + // compensate for "hard iron" distortion by subtracting the midpoint on each axis; see + // http://www.sensorsmag.com/sensors/motion-velocity-displacement/compensating-tilt-hard-iron-and-soft-iron-effects-6475 + return compass - (_compassMinima + _compassMaxima) * 0.5f; +} diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h index 46012a3866..555f2bac58 100644 --- a/interface/src/SerialInterface.h +++ b/interface/src/SerialInterface.h @@ -33,11 +33,13 @@ public: _estimatedVelocity(0, 0, 0), _lastAcceleration(0, 0, 0), _lastRotationRates(0, 0, 0), - _angularVelocityToLinearAccel( // experimentally derived initial values + _compassMinima(-235, -132, -184), // experimentally derived initial values follow + _compassMaxima(83, 155, 120), + _angularVelocityToLinearAccel( 0.003f, -0.001f, -0.006f, -0.005f, -0.001f, -0.006f, 0.010f, 0.004f, 0.007f), - _angularAccelToLinearAccel( // experimentally derived initial values + _angularAccelToLinearAccel( 0.0f, 0.0f, 0.002f, 0.0f, 0.0f, 0.001f, -0.002f, -0.002f, 0.0f) @@ -64,6 +66,8 @@ private: void initializePort(char* portname); void resetSerial(); + glm::vec3 recenterCompass(const glm::vec3& compass); + bool _active; int _serialDescriptor; int totalSamples; @@ -78,7 +82,9 @@ private: glm::vec3 _estimatedAcceleration; glm::vec3 _lastAcceleration; glm::vec3 _lastRotationRates; - glm::vec3 _lastCompassHeading; + glm::vec3 _lastCompass; + glm::vec3 _compassMinima; + glm::vec3 _compassMaxima; glm::mat3 _angularVelocityToLinearAccel; glm::mat3 _angularAccelToLinearAccel; From 53eb5ed0bb984560fa51cda12b1ae03d42f1b2ff Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Wed, 10 Jul 2013 14:21:23 -0700 Subject: [PATCH 3/6] No clock_gettime on OS X; must use gettimeofday. --- .../MotionDriver/lib/UNIX/libMotionDriver.a | Bin 61870 -> 61894 bytes interface/external/MotionDriver/src/inv_tty.c | 10 ++++++---- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/interface/external/MotionDriver/lib/UNIX/libMotionDriver.a b/interface/external/MotionDriver/lib/UNIX/libMotionDriver.a index 4915e9910be513af4bd2e5e0a02d55684dd0ff2c..c29bf3ef8ec5645ce6c8962daa20bcad16293b0a 100644 GIT binary patch delta 514 zcmZ4YnEBXa<_WT_mWD=VW)qcG)>|;@aPmcd6;4HR6ojnj&5=)_ zjmeppjVXwSor8gaVe)E&(@bv6lPe6(7*Ni_c8n_~ zdk5Qdz2E}b&A@PmVX}qEw9Q%}D;PP!&S79+*gx6dOnmaE(EYsWsU;Q5Fg2OvEw;Id zz0w)tl}X+b%q9j3lk=nW83Q(Zdy6rfcyu1y&%nUI@B&159C!V}aB7ydN4M((kIq9L z{Od!1cyznI@L;?E;&!{<@aPmcd3f?lKhw!#x+k#O9o+sObPM3WUG_ diff --git a/interface/external/MotionDriver/src/inv_tty.c b/interface/external/MotionDriver/src/inv_tty.c index 62f4e86660..3fa2bbe17e 100644 --- a/interface/external/MotionDriver/src/inv_tty.c +++ b/interface/external/MotionDriver/src/inv_tty.c @@ -7,6 +7,7 @@ #include #include +#include #include "inv_tty.h" @@ -81,8 +82,9 @@ void tty_delay_ms(unsigned long num_ms) { } void tty_get_ms(unsigned long *count) { - struct timespec time; - clock_gettime(CLOCK_REALTIME, &time); - const long NANOSECONDS_PER_SECOND = 1000000000; - *count = time.tv_sec * NANOSECONDS_PER_SECOND + time.tv_nsec; + 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; } From 6a5fb4f5352d703c7f6ef97f977572f2579bc5e5 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Wed, 10 Jul 2013 14:28:06 -0700 Subject: [PATCH 4/6] Removed unused variables, added motion driver library for OS X. --- .../MotionDriver/lib/MacOS/libMotionDriver.a | Bin 0 -> 58976 bytes interface/src/SerialInterface.cpp | 6 ++---- 2 files changed, 2 insertions(+), 4 deletions(-) create mode 100644 interface/external/MotionDriver/lib/MacOS/libMotionDriver.a diff --git a/interface/external/MotionDriver/lib/MacOS/libMotionDriver.a b/interface/external/MotionDriver/lib/MacOS/libMotionDriver.a new file mode 100644 index 0000000000000000000000000000000000000000..9b2eb2f00a84f95f26bc0c1df9c5b5df5790e8c1 GIT binary patch literal 58976 zcmeFa3wTu3)iAs!7YGQRctO!R8m(Bc1j9uNDw=4H9&4t>mUwBkLri8MZ*wyj#n$*G zB#}88qIi#&Dg`g`22pVWXiWxi5-Dn^)J_5>ky4u=N^X!zK<0baUhAAWGnonWeZT*E zp8tRT^E}C#z4qE`-`2jbz4pb|WE5sDzjoq}XJ0WVDIxI9p8K=8^A^sYJ8ynM!j(z0 z6A}{0dv;RN{CV>d66VaFJ)fwTPPAHQ{rXNzs(fR@uYdWgC2}eNz#UNlrf7f#j{!_& zIFn%t!*quE3|BH-%kVD@I~ab)aB>Xsy@cU>hBq_3i(xTCmEmg)cQE{#;c7`hpL!SG)UeGDg=D8BO;&SH22 z!#fz>!?1**gW;bU)-&`l+{17`!(N6EMe&`*@FxtfV)%20%NYKF;cA9|X1Ix=o8gxX z|IP3y!($9fV*$MX0B|yVhT$s=H!*CD1+Xr^AFR0rCDzRRJbU)Cl0uu+o^8)3+%j9S zH6t_AmSeRS6^5b;ZOcMYMYg=604_5>w;-daC^)Q!3<|9wi!yQxa%|SZjAC1WO;&D! zwWv46&2k4ePP?#Sam%s!?Brw6L%+4zo>`M$O&nd76_!i{fZz~K#4#cP zRsB8L8C*00oGuQCy5|9`X5x(Qh_#8HA+?F9;EF>G)=5rAaprPcR)DQQ+U~RE1W(2YTbCB1FSISoF2Yzc^0Er^v$L%Eh1s^eVr%9-A^yb~1v(PT z(kKqd7jYrkK$IvM5*m$4mu8lA4Noy=pu|($7(Q|_1p9^*19}!M&-lH~T4?((TV}Df zczL0%XnBBdu%}*pqr+wA+VV@d7eUUz^H7lby zBj9Vbrlu@O3Am@@qz3>LMomwrvRCdW82VYQnMK8g*?Ble!Y?#^1+IPvX`PVptm)u5h>HPpn^fQR1)oNRAwHIdO+Q>6B zebetGd@TwF08GCtIG?8hEGHlOkU7>}*9rivm4q^YYvKUuU@_C23n2b5ss9B46O9Z` z@k>|Ke<>A*;yv;)kX-UH#W^_+03_F~F$-Iznl<9LKqEuRb?fAXtx_e%%+!pYy0BHM ziUoikOx;#&N=l7oL~+Ilx4o;&@1f3tiXgFEHpO2v-&ySLlzcKa;pqDn;S`AK&89P%+m zE$Nk9rNV$+;zvLqmD2Hj+%&GfJ7Pj!UjCIi@(}Udc6_EV+&< zs+%)#Tu&^Qcd}MO2Nkun*CQV+Q=E%1qU!OAQxrmS_5D*Mmk;?wDzeql07_$)$Tv_~ z>1;jnLHKX5To?_o6-8dDu^bFko`O?tti~1RrAjVOvZJ&+Rc%hLF6kaCeTSlilpLOh z(%|ljl3bSW%D)zluT2wLB$wq|a!zP*Y;&adl$G{?REeF2b8(RP^swl#aH;Zv9%~dp4`!mLQLncMd5q>f{*&_f&$03tM|nIrO~Ovr z?XXZTs?34(gr;R*&le`n2d)gm(Q<}W+1rEt1vxqSm*+ceoripZ-}17=(Dh(J+U(gTuuxxAn+puirOsX zv9WbL#)WHH@WjqUU~eW!q8O%)?*)KHth-1Oy%h;C(ntijByOBD2Ehs>z{Eu2a69Iq zytEf-s0UJc}{}~?LcZmMKU|gg^JxB1Dhuywthubk3r8cOePq=%#bWyU_ zzb8=t9=15+sdXJw-4#8OR+iXxXk}T^9i?JP2IFExInEEFixZ`YGTyU*O0l9lKC}!+ zv@#sg%5X$0!;!FMh$0}lEF&i=L&M;y^}kex<4;nCBV2|UPiPrxPJmPRmu3?c@PKzoJ_9U5%9gZsZihyG9sKE)L zy;&ZKfgT(rg%7rW=@A^p(Wg6F6r3AQ_;0p973o#^klL%JcdH{QL%oSO?4~%} zsSe9)ze`wgQ zHrv%jqO~`~;!JuGPGF=O9IY6m%3VD;uiT72=%7BZ%dWbm1}Yq>L9lT9O?GuyY8Y1K zeZht1rOr5sriY{ZW9mPeU$UcB4U;t+VrH5t7nSNgGqyEPFR6w_WD-t|<-$4EG;Fu3Fr<1UK}Vj8h( zL^f5qSCmj-6jJ3wWu?bJstg@HvEHiX5lVdXzh6WZ`IYW&gWU-t6S5nP8{ zf-Sc}9+4U*Q^bIHU0CQi}pzo~Ms+TyhI;t=k!t5@RK9dW+I z`!6)G763rGA3Id3k|vWQrXE~V(ul<<;Q8N3a?o@o;w*TIG3~f)rG>9P#^K2_c81d$ z{eC|#DR8v$$Rmh6HZ57-g{nH8drdqp7EAaML4m6z>TLoPxOT-wK2czo9!i0oet-fy z$xsUHkOhU-I3niooRj46K3?c>pNJ)jDHr=lQYF@Pp!-(kK`p+BYqJ<*d`5Z0T!}Kl z+;MEUf8dcvOeiIjv8*sp!(#a6LLQ#hNhNO?)@R39Pa*_%`V@kTe|Xye(x|nBMl6`i zA|(_jj9TdbJe!x{=qz>@@T-=OImE7shc-S$oS_1*&xdH@DCFHQX7oV~_7PkN;Pk8t z_h|#QvK0kNsj&>>O57ulj1M=kP6-UG5!)*^cuprLvFChTG+>2^gnQ)S1WFyEa0yX` zy+$MMBVm0BB2_-rgZtM)iqJ=T)JSAvqVArnH6k&CBO;u2dE^mVXyJ|wNNiK3;BAkHI}bX+Wyc708?FL-${x_{Q{ zQ>+AVQor!?;N6brL;J5Jt^b0uMYyX5Ql;Lr=>107B21;xOKS)x?+(rtWysCX*gRCi@i9tysVEH^P!wk= zEk}bgk4qV?eU8enp=bV&@+1)E>C3#9JgdctI4jWABa#z z%4Rxf1sG7I%bS$VKNh_$Ia{Pg%Ygyu^4**$sZog=kS=ett2+mzyS~t{4azvg4oJWI zVnDiM=YVu)%YY)ybfYjS>Nu7HyPDob&0Opq2AvNj3;EXQa$RnNUiu>45l5uNUrpQ z?t?KE+u|zrOLp~3yISIvT>E4QM$!VF}6q8?-_b94Q zdG_WZpP0Rs91!VuCdZ{pjh6P53U~6&nY$HJlcJ`#D94X*`nd3nFb2 zq)j?$H`h>Vb$p8C8gZ8Fai-IrdYEqOGE-=V_s2=Dl5Z#U+tq*A)zS`;If465!*!)r z%ipI+u2#GH52^7VM4S=vt<;$QO$gB;#2JaclN!^zLx{ddoRR2oU^b4U z`dIQXYB7Na3tE2`AGTdTg{OBozAH7BV|b!Fx^2@A@)?nY@YhBTkwV8t3O(giLPwls z{v*n;je5IbIf_}C6e%$g**UNs>$5qLWRHBTNgMDCQ&}FSkLlCcV|pR}$FZI%G9k1s zw8hS`D4rP{3sg=dz7Z=1Q>3SAtg4MzIk*|!lM})wCQ?Eo7VB)W77z!rJYOJX0GdVw zO0|lXb;pvm%rDiG{~yQs7ZmB>sw@jBW0Mw5pp+s-6S0g@#e);c$E#S6tPzo1JzJsm zGqs_6W3}V`FkKstCt{KtG$D!C_D|JPpX6GELkD)s72VM^8DKp#VPyj%`gHzbA^|14VrqU1zg?HJy;b?**d-lYk*wFM=ARF5_|dLTv|DdJe+ zfw&$jIs6?Tbc~1_9c7E~q=F~G7(jBxxxY3!b~@q%<#qS=H81%6{yM+kzsc|SH{m9* zcK%1@higen@AyFZk{*U~A|1Q7Z-|qMO(QnjzTrKU@SIEYx2=3k6 zonm_jg~6_xjO8O;luG(8`u@D#IhpFC)Mr;EJTRs6QmWrzdE@#;O*o|{ww%h#P`{h; z90xy|ra9%_0qG17dNBi1eC(1$AzOsReP={oZA79*1|lypB8|BV9^mfYzGgMo_h0;e z|KEb^JEDAYdnp~2ey7}PZw5#fiDTCb7YU^u%`aY@m}#!Kpv2+vxGtxnHCOu#h5=OtZ?oXRPiS?isCTRNqd&Ao3`_V&4 z^@=ljZS)Ml)s2dv8(Bur3QJkwEBa439jeUg4>T*n`yv zt8bs2TwU^k2XrOsqWbFyQhU?OxS z@K~e7g>Ef_*coVI9yl?mL`^3V4CP}~+~G3VZC7V&Q6nQm)R^TkQRf(eV)@Yh~1tu-KQ*URV{V2lyJ&*O2v+)cW)OW zZx7M;)%0^v;a5@9yS3?5P}lylP{#@C3y%vW3@5IN#@Y|lx|dVg54)BQOMriXx%#UudnMQIG1f(V%z0q;->@Wt zx|r@{$IpVs#H7IdRi7V8F0ph7w;zv}G-x97W5XIZ;t^8B^=*>j@E?!)M#L#rq;rX6 z$>qW6wc4EKlzUSoS7gTjFySIryLZ9+5hGD!oCuT&g*xM50nYAE?d&k(VZmbD2Dtiv zR77+f7`ous&h|$1ZIlu1Y>&$D)Dj-CVxGaiHn{xsvg0GK#zgqS?gch~xGw83DAF9X9kWzhMiYE?aJo3MzwBD;yoW5a3 zSX@+-DpztQRZwb5AK?i==@Y74rv0u~s~*IqrWl=SN>{5@D-6!Zt0$w5mBaFwT2-v! zo8?-hR;vp9et&ssEp~=d&ChUtBe;ky0)+#lnO(NO6wS)uge;9{Y`6DasOh~CIX+qI^N!Rm6*Ne@Rrn1r( zL8?L}N~ip!IK5ZpC(BA-G9a-mRQXAfE)4poveG{pym9`a%1;WxIGNHtjy(b0dmIH! zQUh0_pcawQ8xs`M6qjm*y-hRY9$~1DBy(Pt*>AmVxcK+ z2x#I!!!BN22uQd1rScKC)UXI|B&afJeSL(7MB4u02%cA`(Xf;vxfZDMAyGFXmvke+ zq{<|#nl2h@k)o1eAi7|dsieS}>X1ixhdxDeE!4D1`I>_&(|lW%_XIV3)ba_MkCY#3 z#TM4e8-|K=f}&b{+6?0hZ8hhRJ3aCjCXc)aLUqAXt8c{HKjGEhXu6JvQA%SAHY}}! z&`RhFkNicr+dpSSyii0eaT2fd2)E3N<@Jo4)-&$0)-&GmCt)^)nKco!rm>hgPsZ$g zX4Xi|8pmRG?a7$MFtd+{*+*kB>l1G6RS95#ZsiYAU@eq61KigTo4~px<4E%-QBS`0qX>* z5*6N=x#UvLEdQhnTfJfT|j zyAFBmsK;~ETPfUpDNWe6P)u9Kim5UtSlCvCW2iOK&BSo? zSPbXlW>14q;}LzRUL6s7kl2$5*gx?EkXKS+B}!XKHBjOj#!B4Nv{CZ~ZPe`1HflbT z8syJZPve_!yzz!2Z5&eE-~U+cO%d9Te^Z<#-AeO~xYh*zEe`GJH!kz>XP1Ele`_RN z3Xyo^-MB@xTXI?chU+z)G>Z#%N=^7|O=fX%QMO!TWeab>Nv=`ZYLQ&>-zlqKYFXVW zx#WKk&a2^eiS#?KO_LhsmNZkdqFzgwhvE>mQ=lG-V_ZDqb`dUKa>;GPtzF}`O>)V5 z3Aazf?MSV*e3c@(+;(-F)VPg&J2YR9A~lw5!|7F;bNtPw-ZXVQVs;SbrZ|DRDGqVC zb<~~ayeXb=@zwHfRO#Q{v-Si|Lga468K=0vj#ub*$?T(wD%UA$i=w8lRn&gPX<4fb z^+~SuzbR^Zt-zGlDnmnxGrdkxqlm+bIK}<%ctw@h;9-#39|+-~5Dp5lMp3Vi;~?=I zq%0)JQ#er;q5dL%iKxmH_4;_lDR(K(1sE=krXC7SmWjj7L%o#Dr*Otb9KfhcLRraq-;Vc!?OhK?%Er^GzY zS=yzjH&GVKNn=G(pGwB*-XnZY*QGd>c%{4tb#oL|iSOYZ3dXxN{8&3RHP;v>koa3E z-+}X>z?MzmyaRheYKyeLU4!~5v;bS+A|>C)iNNRVc9hx{YKu^FU3#3?Mwi>w8T9i z{bn*wbnVhCxAZVBvjiij<-iwR&h)Mc+tl=~p6lpfOl}>J;wR%d6Q*Dl5)Vl6a;r2e zRw$503XZ7~|D5sxUUD7i=@Jb}6sgnFWti1lc4*sYJCq7kArakt$oWadq2$e|F09>8 z`6{$7^y?m^v?6!}9|qxJUe7+-`ub|xMZ8b(mE@8;bXSMuk~?)*C)ccAt+l66t-*Eb z#@cBakbWnsV-MD8*@LlPfmDemv6Rh!#ch185;cia-OBDBMct-U>^CVBb|Qwy) zpUKv5Px$>#xl6wyhI&Y$_nYZHm`6S`v03g#Sr8%;bc5-rM?Px2g`_Rf!pPB$Q2-xd z-D%Si44WMG(v)bjADBXJOCfz+WP0516-`JHTFdxRdU58W#~C?1-j$JD@==4j3nc_g zTrjSdk2u^Y50)d>fK-Lr03iWrZ^MKs$6ifSvK=GVzl+@yy14F=humFpcp?+vdFiWL-ceoC7c7!Tgqar@$MQ|NpAJq>jT(REmI$%mzebf2k}`%bChwuyLT zNC%6=;uWlSo-BtwYW@+=Do^F8880*OYQV5pKh^Fm@dfYIAJX>f4~@N7AFQ6CR0Qi& z|NrIpKPtb)cD2QxqPE&oRJ>)4J&?tDW}2Grv!|+-e$>pjr>gQdc2z!5#(QT$$NOlj zQP;5MO;*~(S!EK}zV{39+%Gj)`f&w=1L9^}6(K@uNZ&tWH=@NgYecJZi&&gX4RXKK zVA(&TMU`8eatn1UU3RBszulSMB{h`n$4R|n8j>2^idx#FHcJitiZk7~ZyS`-Ar#D- z^j5o5-fwqWx}*l5)X*oIX;9ZsssBV!m$c$U9n+zpR&uMY=+YFK?CoJ@w(7t$<31P%{`eu(4}d;4Tnr+gU2Dp)uQ za3PDUU8TnIk8qm@)49v}k`}UGj?}CBh~VtLq39*OXQ=Wrw7T^4^8BoT(Bz_ z2hw8As7-+N3`;I?R^{%BHeAa)60V0FAFhWK{4d5kiSm&IAuYI`CW*&o0GWnCDMCu= zQ4+qRCNd3!c)D1~cuo27C|NQQ5~&vi&Z~p34;~Gd&al@9MLO}s&?DAGntH9U$P{E_ zOHt+SW}15|nR2hP2>UFl0_{o&x6{%seRQ^|951~CF9uKnRwUIa_wqu$w+D5BH7gwN zi->X0II@|pa3_YYMJT>P-lT>_sAH(gH2g{~(VA8HL(P{O$Y#w+En^coU5l_usB*n< zmI)WA@>_g&7^_V9sB#^ZPw;v7Qp0_egpb7jfzZXOu^bZn2ioyOc#9Ka!$4RXa><9L zVfX3wMN2Nr;qtFh%fUSy!&c!Wm;7C@KbF0FIEwraN-p{PV1Hask>|G_xf_R*?nSh7 z^f1KdM_mT#rGh)hnP|FEd?x@@smb-EHv%BZ_p+*c7?%qCkZbi!6xzX1HR9wI0pHvlxH|@t)uDB`z|4*I>>3KyZ1qJzq#kQ=3qI`QX8jnsWD9q0)$t+GN zwB465Eo;_9Sj>oq+UoO0oZqhmP#rC8_ykVbS&9r)_Sp#?y41b?yJ z0wAothvT1sohSeZy+mzC)bRD<{uWx+jle$vn;VKZfl=v8V*lBck5T>wLg5aeBSQGa zxbq1BhjC;JlfGoJIs$M|=lm2=%ScLPR2=!lp;{;BY2#hK}z%CpB@WOdm{_Owf2>xY!!4~iAhYR1t{+!HF z@prNRn-RjV#W4i{a5p0y|D||E2>^r{v@qpU#{P36ly4Ibb^ve%fl=w}Wq-Vf9FG4Q z9OMAtClTbcNob}3d=(-7wWx>!02?C6=i=GqFXp{r@>jtATOx!{VhhgaMu>m0FkK69 zKDFaf`7hwU`QZrR0T1~BV19)7ldmWLx(NQgY!?~zTEpcp39nEAMD#B}gO%8RBJf{x zGxL;1aDJv29(}(j>{7irN6_{4Uo8l*p zk6U8`K;g%l-~1{K&^$^2q?{iAaMTTxx$_ouSddF!BRU=}!qTDYx0mU7)Cfz5+a7D* zBz`=4gr!3>jxBgG2@Tw7s=|hwqGZZy-7zRl>$Q&jhdrFTnZz zejZ)I(%oeOsC|d%G`*K_@mAtqEYtC56PE5RVd3}t zd6WvvZwIzOw1$Lt5~($Yr9->TwQi!*6>h`G!P(IONjSar`~5t6h2?imG(auW@hBFS zE-M-!0WYWe{eB+J!qTDL>lUWtQ7tUprf7gJrsL5qEZx3n058+=C>NG4CI+COnbOCj zU0AxAF#vHbM8~~aSh^+HBQYJ1eqrezi~*SX3Gw4mFf856F#rin$D?6bx?M2|!S{QZjz`U~bWg?t z6#Sj|@#qE?|C=)z0Z zc&~Mg_IB4efaU)pIv!=i@~a#NQ1;Eq>E0R#Q2TG9<54#(zdhps*7OkFSaN!N9DsxA zcoYuH@6z!A$=?z`9*x7&rHuzrn2tx~uyn=a0oEQSempvlqPyu<@vg;`^BFE%3{VJ= zcsGse<{1nV76aS?kT{9m;}(ne6%tRNu~Go@&@Tb*0Z2T|?(Sa#ECopHVt3mw0pICL z+|BNtzXbRVK;kxbfAC9yEPzB8yEia=jp15`wG7uVR2i;hXl6Kp;lM8diU1P#qm^R- zvzOs5jK7NANes_tIPn(%1ptZPS}6Q(3x#iC_yXh0*nJfS>hy% zc-JJ+p9U};AaMvUU;~)@7#?8wRT{v2fW*((-JAw62Ox1XyWdFzI3FN!9lM`T1GpR@ zv4-7M4DVxDkVf@qW%sWbuBF+Xx%w9I21cU&7J%OaEV|xl23Yl+XFt_7G3kod=IDV+UU;N=>FkbS_|<&k>&9a_0CNMwHH1?jH_x&AiFqXd4|BL|#-}ixHjm`cHz;uCWKCIBQ}!7zFbjSEL;*TeknY#Iml&8GhU)7jMTyJu5>UpJfb z@eI>HG@J65#qO&ZPMJ;R7su`sct`+XZe!TUa7z-^>k~;t|1jepWOy}0a}u@Bcy^Ct z*nS0t`;;cy=Isn$WLUxQfh#DUUo-vHjGxVLWES!Nj&_gC|77?M!`sUvoG^>z zr!A4f{{^4$05HGD@JWVfY6a&x7qNQ^!+zS$GPf~&Zzjpjqcf>oFJSyyVc{Ok(-@|T z`;uTDoI!l|2s@Qv{wu?#88n`*pF!y>V!GSe-@dM;3u*rQ zG@@@|_eTuZFwAFoA;ULm_tSjOr8F*C7%pJ=E$yC~TN%F0u$tiqw0mp5kKt(y|40*P z^K}}3ZSI+qR8UCE%0*2=^olrFeFh?`~ zi;E~8532S8m_KCr7~?D1{Rf7(FBXR!MshCVa#`<52|=C2uUWmv)fMeNSHfaK~|v@kUn{fOL|v@kN)(=NaH zIfl9CQ+xR7`9vo%UR(kfB8CHW`e5EUmFUu^65T?^U%@b% z;iq((VSf8u%HLXcKh5x7hPN@inBl>5Xq-5JSBe45UorfW;ihwl-+FdG%P{*K8b^N3 za2CS}3=d2JxCPNm6wU#LwIokzhtrAQ*Jz~&=XVU>V*K;$evIKm3~y)u z8`!;&;k46fUh|{VsosxFCjN&|UjV@THN#ICZedt9nd)@~yR#-!_-i@*e8ykMc;6)A z+e@dB=5H7_O#;XVNZc|>9Ce#>ClTLkCsDc%;?-{e^OvX5xVGaon#aA%@K5Z&oM8sT z+Zo=>Fp1&04F5Wj+RauvT{XWq5#Tm}#5L@Gh~fPVubxQhznt9(3^z`ob@$T@GwBq_ z{Bwr08BSz)Fpl_qMd$kFFBrbb(8O>b>R;fvJf7%ZXZK4Caqk8Fo9F<;T+6U<9MRj@ z{ac1f4DY2=HuJTyG#@p`QoiG30iV)LjEe=-GDvL4Jx#2a7@Duv#!z|PA4Bui+hSsG2g^6ncy$>q_&CXp*{hwm|YKBWq#P23{^LdP!&&$k0Lj}P65>UGE;ZGBQ z86Vxnc_u#jh5qbf|t7CVdRMIAGp>o@T83o|QTxFJ zFfN&40z(uM#COGTc!p~kmN8t)5aj~t6BuGWi}2ALp5a=CWek@xOlFwC5Ff}D;iEV_ z!?g^{7%pXqZ^nrD8Dgc1@Fot=a4o|!hFC91hv#x4JVSJd@Kg`uni%4_A>#481tC5^ zj1bEoAx_s2VkIH`W*orjF`&P(0@F@9h{z*P)8cuBx`XDhWry@KI2EwL9_KGyJi(NSE!nifSK*H0^#@3$OY(~`QG`6n>{ z-*9+M3t}gSFW~yUjq|5z$^4bm-^=-{*7C1u$*})aZZCuEuW5nY&HU>)eYi}<{ApTp zHJrcYoIezO^w+)z**}^6Pq4qHMYWsr7tiJU9>=d~*<8f&ui^APz~!%LX`zZL%1<|! z55BF9@oQRkI8LB{7sro%8TxBlKu0z#FDo&rKCHN)tkK^>A4he?W zv;@Cl|0MQrXMasg59eK2K0fyU7ssz@3Ejf+mvQ|UA-`yVChia0kUko~$8I#+W~hF! zg}6II+-PFnNRP=dx^X#gbmK9R(Jj-ss(-lbG5X^*6{8!hxdkQGLfbNHR!MFFSc{7v zux8K6w4x>|SPKfX^NQ^nO-2?Hk}EqeyEs67e_?j9jhtDwoQwyoxkX^jwdEGsiiy0) zR%~7NKw-YsUQ`&sWMpRAa)MDg1solQD9Xq!$gx=qGmtr^Gc!NWp1rK3&}Ow~+w%jA zghFgl(dYnzi!HAxzYr5tP-4v~;8=6>vzTe24XJ_|-t#~~MiB==-|W0%YfeUS=5kvW z@mZ#2NR$Yc8e&AY3rfUq%A8(c=v{2fEwB}46qgio)X19X?#a&J=yc9{Q6VOzs1T(W z6*@*06(S=R3wny`z_JNsh$0EbVYzU*2M7!D@3$3Ni;DG%L55gHh$>-@C_-GUR`kgH zy)|?B@2$&9G77UG2xZU6&LQS{7;8~+p)DhquzC&H?z3rxC3)5&TaMjYY%4OzSXOR< zmMrqf$+(A;BV-hX(h!sG%E=dLEzCs~Z6Ugu*c!CDG_ciBTG<9wu!hvDE~mhnn_rxr zpNFMmD=aK2CB(@QWuaQY)*ka$ZZf49Ut7N zbTr1K6@Ah(F5yo6#6tw(2ptce;Mkd6*CX&t3U zW-kg9Q6REVtg+ybDqIO5eykGGqr_qgN=Trg1)&XXEeLODZbA4!i__710~(FtCZ&5| z*-<|igc4mHHZ&a@)Yfz?2G`q#j@4V4ju9!!wFMACZA~Y`!WzxaSj3_7pcTEb>cX@) z5kpZiq*-Sb6&Es?&2SFGxuR8yyl376n75JwEdt>bUbei9dva_XWFCmr6%`B7vlW3} zt9q}*U99fbQf%nBFoPrwcrV0Sv^?8h469j^VGL;#DCDUpOJFp4 z*1Zq_JbatUhL=TuaapJ@u&%R>r{Di0IRjwwgM;1{8_C_$XxaGEa9>8Nr6Qf;lpwDW}$u71PX5{d^IF#QD%!e-*Kw5&>C(}RLMhkpa?52wX z2@K`e#Qf&-ZIyxqOj6{}YR$?hMpHoiS*@ulOL+4OKOdcQ;vS~>>Sj{eNBS^*xdj=S z#nzI%`?K@1tkyKvtEVrXH`H)n3O{YVmAsJj%ON=XV71zoQzIby(EP5U^I%xPnM_U! zPTy&O+jD^rs~_`{a20@1A-fL5UJY(%^HvL<|BF9Vtw6OMF3SJt!}n$ykDi9teTC-4 zR;-0BXvI=oHRR{oN7Aw}3OfHZol$F9mFsc2I=rCeP9vt;hR zsrrKWRL9*zgmw={kMi;|bYf#uO!48Ad$n-v)0$(3LqPva=dhBk3>4$X+eADx&?mF6tyR&-DMJkPJ zP&<9Lfihxg;2E6VDUaCISUd&`GRBHlm`BS`sJ$_@v|DNr7U@Y|luWlUYvGJ(R#drF z_YLgjud!IMy(rnT7d2WHRn|z{ zzHwm~bZHC><;)C(>M=gg)Yyv8#A6zjn)F?mwh8~-8Kt;=7h?J_DudgrWXoL!cNe}6 zqPTri72RyjE=I)%P@gg?E`a(>p!7hWkFHNLje%P}jd*?%NTE~qj*bygAu%Es9YS`bjq+w)N>L^Z2(PfTXh4LKQ{oa-haZoYQ%|s*HjkFR7DgFl;S)v~ zWMQX3>GJ+pk-$;y=D#UZZ2D4ho|)#9eQ67~sdCr{FGB2B;d>Xs)}`$#8ty6WFZbhM zsu|{>mWW0z8qdLMz+qmgN$=i_L}J*LXE))<)vwA2gyBtZs=6(OUuLkUsN3wRYI?8K zAosd`aiR=T9NSYJcXuPo;O0}){Xq=w-ekwH!*UGGfl02CW98rA^tT-M zpFt9aeWeE3w~V5ZF4(uRG}Bwm;#9d4qhA?XuzA=Ahnoapuy^K1cDl z?t?R;V+KP;mAEnAh^Jp#n~69Z`6On6Z-QfCnT5bHx?VJ8%Qqs#AJ*NC%8mm$f8bxQ#C=+h*yd} z!g!RL^b?|y1|FCTwQtGk6KV5f7|mdk-z1DaVuBW7bkZaE-y{2DM4lUeX^aG^#H(5& z0mdjt#fR%1ylbV^2%0H8AvIV|q^K<^TRz>jYnMgZ=)T$AeMu@Ber&O;@^Puba@;)} zm*Uu&>PR0!SNYfUsxcytoix7o|9kPF{lJg#-Y@F^UJ4bXZYa6?gY_ihE?dV%jD($VU}vV=F%NBn&$1gE1Ka?`I?r>x3L&BoFL_ z927_%#+p(c@)31N{Y-JDlL0R_dKfyM;Cn<^Kff`Q*YT6rkElMqez3Mq80+VUlh)52 z|M%h(?<0n{h=~Woji@%GP3!6P4em;?0c!S*#M!n>KBTz2rb;f$cf!>7gciwV`Cf5< z9Y;S8D-&9TA!p;`rd06yd#@FT;x`*(97JKkQCx8!o~o4p8--kq-h+&cHzJQPGR}xR zDv*YtLp&AdJ|JYiHAZ=4VR`I0mGZ!=KBrb5*-be0iBK%z&&`MaGS`TKu*0wEP>?c%tbU zKAc>MdbJ+;yC#qPeTzpvyiJTd9{IcV9{KzA9{KQk>@}vL+7(MG1?b)3k^6Reh z(S=NSl%bYF0dc!U3)m%9;;h$baciHj#u>0nC$B6}tMuCatGI|G4Ns?@X2HNlAfG*>s-Bi^^8<8nKP2@(v0(XhH`WEK?eBM*@fWk8nkz87^zY z+aM~NEfY4HRW@3NFJPjo{isxFPxHJb@?e-!p<=8@KGYVdl11YU88Xx*mWEa(b)&1~ zXx{P+VT2wfMrQAhj4oygn)Bk8+E`QdIft>dCmV@38a)k$BZ7UjKg5T5P`DIXi2_E_QbdUT) zl1DzVfZ|&pIldQ9JwD@l7$p{m!l3uUD-I>ld(@Jv^q4wa(PL6e2USb2REf7Uc_}(5 zmaK=QDt*b?xDwNVCkDnPzA7IVuhd8O#h!%c3{F`e`vdlHQ*QgU_GDg-+!SOkxm!_7 z>lL-6j-Cv$)TXKOnl#no@W`uBIM>*lO#rFYW^67gl54L$MeR_W>5des;e?`wdC5T7 zW$6)Wcn$Q#p`qc_c z!o;5hh<65v{~931r(AY1@uvae&jQ4s2Z*OiuJl#q+pwi4JHAYIl&n&m@>*=YxP`_K zkqAt39HYeBC4`D+lz7C1P*WKt-biuMLnhKjg3cNdR@z9=)S@_RL{MoXL4gq=rHuql zEegF>r<99lPp}u0R^#{_K%61r4L0g3^?Gfl+M-A8dJdvJQ>VN}YK$xY1jj^rs9mc@ z>{Gd4+bH@;d6nc!i*0g$9j9*dZ&Nb2Nvfb-$+&1sX42}D>(Fq z;cruZ$72$nKjL~qTR}*!o1&+oay>4|#iBr9aGwJ)L)bS;uA5B8SrM9C5HB!vMdw85ku~rG|2#EHv%NzC~ldMIkPBn2ycTW{=)zxWpLV=E5Q^!DQWxZt5^4& z{rBXkv+k81Sy17tNCIEQv_}Zy*_U+U^)D}l7&w3B18C;y8c|a501GPe*TJ>XN!U`O zcK&n1wPm^*gSZYLRF-j`FllNYz_`aJp3~In_eb6K!|E^Y_kX?P>8_7IdIJm0??3IQ z0O#Be5Pc`|0x*w2RF2R9xX$1CEnNGa3npK(0^kZfa|Af=`u71At^;__)^_?!pO`;S zn}^rI@!b1r*B`pkunXYy6abS6@i6juJY4cSn0(1g*w3J06aS3>nS01T=1YK>UGxVK zk5_0Y8TjGNkXWHwAudSq7YnhFrRi+Yhe5ENz#LyKfZqmCAp!WGKhFNA^2KWrj{|u3 zzr-QG=~#t0`ENT85BB-V3@qvpWJO~>ao#O%HlHI5)KPB9^rC4{;{+=rKy1jKHcyTe^}NikpEA8#`sRhdrA|V zSrKH8-|v3`0A+_t3QNWRuTcp>d_YOqR0jIrrI&r7Q;&mqcp6B)4Bw=QzgYh1=l4%o ze9p2vJ?DLfTe4xlbN^8pq~p^3#G`+@@JAQWt>G|VRP@v;7BdtYAvpL21n<m!Ydkr5v^5TZRIY)lfw`p^I*)x5wcXoEZ(b?JA z|Hj9kHordUlbO%-y{=w%qy1g=+1C@(OY7^a>zGzq7vLiC&n#_{h7iH!7Z(zU0BaE$df4k+F4Mdq?Nq z^5?c}II?tkL0|jUb)9Wn*Ntphzw+_Dv$w8m+q%xzS@m+O`-8VDwtd{Zd0l5`=hn5I zot<89-{v>otJzdL`O3TBb#2|a`swo?ed)E+&$)2ZbDQ5-HFL>>TVJlPc0D%dw)@_# zeDdX%xKF*CH`Vt&yRt4Zy|lLO^m8s;H*?8@kH6eIbMcD4%DUPepC0#4-dlG(aiDGM ztG@QFuXcF1zUzAU=~r6^T2k6yZSi^QH*H>7_xO>SOB}U*SGEmK_4ci6^$pimwl#P9 zd=($JcV>51xV?j>v-|2+{-UmO;EdfXkF-5L{TWB!gux3g@K)9hG!5@=duC+Y*$!mp z?R(o%yQ^-(aNEkq>yA9w=eX+gCx4z-yEl7w``i5;!`@X-)%9PzyZx2n!E@XCHaybW zw0*-$Pv!Qys;63Bar8I!4Vq5xd*sDB&$YK5S<>ey95hYotCg$rjy&>WjHB=R+TT3# z;y6e4py`6X+6&K!YdsRvci}TpQML1@?Hz8bdS$@fUiHdAr?={^wF+IBCE;(&}p;9&YVy8}<&K<{i*Hk#OXAUH0(s z#@6FV*$!8mQeXw{7ff^?CaqeW`BefMY`Y#z+6^^Y*{}OwFXWS2~BczB({6 zcxJ!%smZ(B@OS$w{qC)=4s?3^xGr}N+}!@^UqxMd->&R$>g;>;uiNXeo?G01-9Avi z$x&N(@!orD{&a8EQx~_r-9O^3dZuoA+hClxzh*}JU~F4|)yv!Zy>&HDns&Dhp3(1J zb+LDFqPM@Ywys5ByaO+{e6X>#zr)9$w{P>&_qWtnKefK&n)Piy?_K!sLHy?%-u+5j zXZwivovNgvwhpf^>FxFoZ~vy%&#q5QFI`*F+1dI2V{4DjXglUzdwfRQh<9!4t!+N< z@mt%5y$>L=_4osABi=s`cC@wne7?QuK5yNM_O-2@ZLK31aBr#4`$FC7BT0J~w6%70u5In;^wueR z?`Z36?O5B{+TpERzPF&QvvX}{r?;=pv1aCy2W#tE?^>|Cy??{%&S7u;syes(9Y>vK zc({GOw=T@Tp_)Iv?VH-(zoFAR^!7+n+shrp-u?|8Bf}N1wQc{nxzF+ZYkzoqsH5{` zU*(f=uRSvW8~SU{d)eVx|D@ySoX-A=C-zQg>wlxu>)qfR-rYX%a>r|Zj_3d29jJVA z!+FE)Z}fF~>&lz9f84zO@rt9<-|lGhdDky~8~?As|Gwefe{S#C{&BPS&*e}0ruNsX zZ(*d^FFf;b-R$SbJv;u*XQn%z@12xzc5?lu&AoNi&rM&vqT%_v8Mi)gX~CVT*$c9i z*?k|p>42F_9_)S3xw$T8`r;LR^^Q%?Tvm5y?FVnZ=Zw1SMtl9{-s%@HjC01V4^&@T zaHlijj(e+9vlloQCNHnfQf51oZeHr>drNRxO*|Ze!&|E{e6K1(>oLCWn2>O1!JB6+ zNY1W1Z?>{@b=@^7cO^UseXDBEd$=y|zK33a){&TA>V4<^E$cfwJ2$j;zR}vz|E6!a zt$x*xH~Km|KiJsWxyAcV-}+}BS=w1&+27(DZtL(qQPubJcPiG$yKi%ruFyb3L&D*!3)%W(T?HfjXecmnaxYk#^5Vz%>is#;cqv?mFiiVfs-*-OI z9~HIx&Grs&UwzruH=DMvTQek3Z<^t6%m0bL$75U%!6e*0(yhXlU*8dc7aK*U)?726lZz`*OEurC= zRdsep?*q5?t(m!M(re|<-19{5z4tDydeSuG)(6(S(lK4BGaBbVn_EoRV^wz0Qo%ecK z+X8Q$y72X~_G`R#v+5SEX`AG&dunzqf(dPYuc_VdeQM$B?U(tzYw9Me&(_74)n42_ z%d0+HH@mETmbdQVrwOhiu=4SNl(N3IS>C#p1Luh!E8iMuT32>S->b7;oAvr7wRJPv z-|Fyt*Y#iC_xh}rkJt6jE_?OqzBYJk^$U}d-s))W@OxLk@K$U5TODPOuj@}+S=YC- z|H{6n9j~^5x6hyUY0jiiX3oFuzRPa3pXh&6mlSp;E(TL;&*=p|9bY1R6dKzUJBsOXSni1J1qDeictQ&VpknNEDXcQ&swr`gSHFs z8&&?=y~{}cWn^C`qI|G@9^i@eDE>*TK)yHzEtp4*@-HKML=j(4SjvJ)=7&b}FFT1p zZf=T>+!u++M@d;hoeLw%5z;HY^?P z2K5SU+Oh46qg``f8^z1T5tbj?HSc9QZqH%q(5|`i74c)a4NHgm4NXkP?K>8=jdD+*LKJG8V(xF{*h3zqOzY&%W?V7hT9rq7m z>CmqERJPkZ79H9(U&?f2$tl`3zk})Y1$r3!>}c1#iRrL^5uZl)^NN^9qG=-!?V5KV zq;eU{UOw8|S<7~txxWdUU$kppdlI@Q&B6|lxtQTK41daS_yY3pCRp>}1pqeyB&J;; z`kO@yE&#BgUCs;0{-lZNul^B*D>dhO71mN0d59JT)}R6D)E~%mFz!_ zpGtP3zCV}TJ?DyhQ06t~QuzFHiQh8zUvjS4FirHIL-?U{C>+{ZlK~Q+K8Mm>cn-z0 zoc(`wj<{Q8Rv0d1{Eyjv9>Wt;C|vgx;?u%z_Y_LcCWg;4T+Q%ahFJ`6W_S(5A2FOV zh0-0*?vb;}&L`SoLwldiXNw^=@qLDmpH2DuEyHWhCi|XOoDDD!An}s3$TwZzh@ZEJ4@^`CSJnOe1hvEr^*;+e4k zHvuFbiy{7~EP?#Du^SKTu{>{Ni1$9wofu8=pv?!25Bp$*pD}!$VGYASMiKwT?4A`x ze9+Df^20GT2EhC)hN#3D4PZvQ`3Ui%6w)2V7qtKq(d-b~*J3*mX59W&(2hCvhUWjT z+kZL-?K$B%zyr!9?B2rn7q4fxi`^OQ{*c{e>~3WDT6D(*?mxc5{@d6ek1;X6E_U~@ z`&)LSZ6;wKis!W#q8r~K;QNZxH8!ginKV=+%m+S-sYKo)3whroJ|LGh*E(p+H`)+3bbzELwad>SV@d>AI zIj0Zp&tZ6N9WcQDUF?r@9rV}M5qsHxGyCJ+UG&%1(a&@I37kKCuNnQdb@aRJudqLw zu|a=ronFQMK2HC~TKV%jY6Hig%<tviKVf`%S`dP{T+B&&_{T=Ktt{Y(bcpZ%D-x$80{m+CIP$PJa`Z z4{GpZcx@kGB8Ts0|HGVrZJ(io!`HI^)f`^iCqVmm7=Jv+Z()CJpFr5}83(X{{kMcH zdjD658`o?G@q_gt_IU7(XQMy9i(_=7Su>*>`+uXmEhPLCA^!M0gOMKXVHw>ZZpRCg z2|-unt@ePi5;SWQY`;VpQm_?U^9!?WdBquMfP{>T=;j%SFVYA*0AzmVl+3hfyvZa$ zu-zKX&Hx&fv0C%;t@*;NgqU^v4Rj3rm8rjz*@Y#&{o7&Q37@ngl!d# z8IonJK;sKWOlE#bUU4w0I6K!i8e`2Z$tlhb;ER@L{N83QwEdSYvsjN_;~eZ6#BY>W zehJ5q=`OHk<>%Upg~c2jnw1c#!)nuPV+2wVo>nWUfbaboIlm9dR76iQTVb`^*zyo^ zW1Oh`%#0jL03&k>toN2=a0VDRe?dm7^QMVe?BELbq6A zFn=QiLbCv*8CS~8$N)R^C5`xgWRO{FvS=X__;ywzE;ynr}WM~u?g)%IX#iKbXO2e->Jncs&@SHK7m1v)qTG1ER0{VERI( zT=&Gp1q-_FrOUbj6C}o?@uMVJC&{(t<`+H?=sFCY{VWp6;GWUwFN)wK-bLBV^2Xw> z`v-^uB421*R%Es1Wd-~~ibN1(=I0e>=auB6$uO<$g`-<$DmlESv5&7|IFu z!9vpP)3Ci6`p7=b?78zI*r%C0f4;C+quHk^E`GpZjV8Q38nlh8={BJP3*K*x^c`S> zGiX;AzbC)Iy^tUNOr8KBo{Sh}kLI4DB157=?Y(H}(6-U=<0OhN zG)q=%W>IlrcHT1b9%Y|JOP8kV^fc#F`;5HtJDL5MYdK-K{>AS_=C@*^*dN0$h~>%b z$7uE_QQs6lw419d6@hpICanFKripYGvSt!Mok`H?_G9p<3jgp*4Nmt%vV2M(>Tjy0 z$DO6eb-l)L_o0#{nE|Nvc8}drj>#nKfI(-Cw}_s_w%v zB25nY2Xt2aTS|8LBp2P{36z9VQ}TmDK4DB4-W#hRgDRo-xAgMWb7m+}f2u=1;gEk& zR7jcY;>g5Nlmw>i-rsV)y z&Tzy`G*-)QtQI}hN=?arLsG@PD=g~&UE1}$HW5VONoqtqxF@j(3wrdh(0WiribY&m zkb)HaaS5?)w(Ux7q)nkctk_$Z5)t(1pCQ##FjPnpfAk;mhoQ7Lj+}8Vu>yCaVx9)6>T(*?!2a(Rh zpxN5Y?7XF>)c!iC^;Ck!kn}UR?tE#$4N-Ttb-uRPaQ&b9XZvvMUi|zTeqsgU%Yv~rKtA(kX0Rx!(b;Aq!(*_b1m4(e9*n<6_3bv z^bdV@B(|zSyb|MZh*g0T*Qz?Pja7H(M6lXQ0r)u#2%!_LInip(TCe3dQuX0ncnneM zTX*7Ihd!LH20sU-Eq&Eme&g1dI;zInfLnALo3F8z&}oqL(W|b7CXyLlZ)mFbdNW%7 z+KQd8uTswZKc~sTAG}35 zws@)>{J}^5$aApeQ{`a09y~zqL7&7QxywA?r?6$&zAbNV;9FK>`q|I35=U(!EBw6l z9BnU0oDGZRhgrUx(KvGU59u=li02D?`KOGY7})Qazh?e|^1w&O2Kpx%vD;6xe1hfI z#13ztV-9=Q38m~^!IK*XJ~`_DRoZ{xy~InmVK;%-4>C23|J5z0>4h;QZ?h z`Q}SIPyR*nFOhG)WI(_{pZRiuDS&tkX8r~Ic+i`i_%4v1VTWEJ{h0Jq(l1HF-wgYr z8F@Z~&MDPTJ~p6(Z$_RS@_}=M1ekGsJ)idt`Ayabeqiv;$cb4A^drs(5-tPZq-W$W zkq_LL1Y5yPkfa}V(bzAL*}dRN-dOpI5WIVYCO2CQ{R70RW;jJH%M`qRDvgFOp;)$yVT7N+J)_YxwB ZwWU~1p-$hob&aMvFO!NTZiND+)IS;=y?g)w literal 0 HcmV?d00001 diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index d4a3e2a2ca..1fb5ef8985 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -200,7 +200,7 @@ void SerialInterface::readData(float deltaTime) { // ask the invensense for raw gyro data short accelData[3]; - int r1 = mpu_get_accel_reg(accelData, 0); + mpu_get_accel_reg(accelData, 0); const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * GRAVITY_EARTH; // From MPU-9150 register map, with setting on @@ -208,10 +208,8 @@ void SerialInterface::readData(float deltaTime) { _lastAcceleration = glm::vec3(-accelData[2], -accelData[1], -accelData[0]) * LSB_TO_METERS_PER_SECOND2; - int rollRate, yawRate, pitchRate; - short gyroData[3]; - int r2 = mpu_get_gyro_reg(gyroData, 0); + mpu_get_gyro_reg(gyroData, 0); // Convert the integer rates to floats const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec. From 2ffd151a1fb076e705cc323dfe4aad7242247bd5 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Wed, 10 Jul 2013 14:40:45 -0700 Subject: [PATCH 5/6] Ryan likes the eyelids starting at forty degrees. --- interface/src/Head.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/interface/src/Head.cpp b/interface/src/Head.cpp index 6bf6c65341..39f0da3ca9 100644 --- a/interface/src/Head.cpp +++ b/interface/src/Head.cpp @@ -627,7 +627,7 @@ void Head::renderEyeBalls() { glm::vec3 rotationAxis = glm::axis(orientation); glRotatef(glm::angle(orientation), rotationAxis.x, rotationAxis.y, rotationAxis.z); glScalef(EYELID_RADIUS, EYELID_RADIUS, EYELID_RADIUS); - glRotatef(-90 * _leftEyeBlink, 1, 0, 0); + glRotatef(-40 - 50 * _leftEyeBlink, 1, 0, 0); Application::getInstance()->getGeometryCache()->renderHemisphere(15, 10); glRotatef(180 * _leftEyeBlink, 1, 0, 0); Application::getInstance()->getGeometryCache()->renderHemisphere(15, 10); @@ -640,7 +640,7 @@ void Head::renderEyeBalls() { glm::vec3 rotationAxis = glm::axis(orientation); glRotatef(glm::angle(orientation), rotationAxis.x, rotationAxis.y, rotationAxis.z); glScalef(EYELID_RADIUS, EYELID_RADIUS, EYELID_RADIUS); - glRotatef(-90 * _rightEyeBlink, 1, 0, 0); + glRotatef(-40 - 50 * _rightEyeBlink, 1, 0, 0); Application::getInstance()->getGeometryCache()->renderHemisphere(15, 10); glRotatef(180 * _rightEyeBlink, 1, 0, 0); Application::getInstance()->getGeometryCache()->renderHemisphere(15, 10); From 559dc4bb5d3f156cf3b9c50c45aaeb842426466e Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Wed, 10 Jul 2013 15:07:39 -0700 Subject: [PATCH 6/6] Flush the queue so that we don't get any funny data on the initial read. --- interface/src/SerialInterface.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 1fb5ef8985..733ba0386f 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -99,6 +99,9 @@ void SerialInterface::initializePort(char* portname) { int currentFlags = fcntl(_serialDescriptor, F_GETFL); fcntl(_serialDescriptor, F_SETFL, currentFlags & ~O_NONBLOCK); + // make sure there's nothing queued up to be read + tcflush(_serialDescriptor, TCIOFLUSH); + // this disables streaming so there's no garbage data on reads write(_serialDescriptor, "SD\n", 3); char result[4];