热门标签
热门文章
- 1如何使用OpenAI的GPT-3 API构建一个基于Python的大模型接口应用
- 2python爬取微信小程序,如何爬取小程序数据_微信小程序爬虫
- 3数字三角形【动态规划(含手工运算及多种源码)】_数字三角形问题动态规划
- 4Poison Frogs! Targeted Clean-Label Poisoning Attacks on Neural Networks 多任务学习 多任务关系学习
- 5拉链表和快照表的选择_快照表和拉链表的区别
- 624个Transformer模型高效魔改变体盘点，无bug拿来即用_transformer变体
- 7LangChain 79 LangGraph 从入门到精通一_tavily
- 8Git创建本地项目推送至Gitee_gitte推送代码
- 9text-generation-webui搭建大模型运行环境与踩坑记录
- 10SpringBoot使用自定义注解+AOP+Redis实现接口限流_springboot jedis aop实现ratelimit
当前位置: article > 正文
六轴加速陀螺仪MPU6500/MPU6050使用及DMP库移植，含记步器功能
作者：菜鸟追梦旅行 | 2024-04-22 06:11:54
赞
踩
mpu6500
本篇博客为博主学习使用MPU6500完成后的学习记录，故在这只做主要讲解，如有博友看完后仍不知如何使用，可在下方留言问我，或发邮件问我（dayou1024@qq.com），因邮件有QQ和微信提示，故我能更快看到.
工程代码及开发资料下载连接：https://download.csdn.net/download/dayou1024/12589762
1、MPU6500/MPU6050是一个6轴的传感器，及加速度和角速度各有 x,y,z轴
相关数据手册可在网上找到,如立创商城
2、我这里使用I2C总线驱动MPU6500，要想后面的程序能调通，首先要确保I2C连续读写都正确，能读写MPU6500一两个寄存器正确并不能说明你的I2C一定是可靠了的，因为后面要用到官方给的函数库，这些库要大量连续读写数据。检验你的I2C是否可靠，可能这样操作：
MPU6500中有以下四个寄存器，可读可写，且它自己不会往里写数据，我们可以连续往这写入四个字节，再读出，读出写入一样说明I2C可靠。
DMP库自带的获取6500温度函数是不对的，我们可以参考数据手册自己写
3、我们从MPU6500的寄存器中出来的加速度和角速度值，意义是不大的，主要是想得到欧拉角，但我们自己通过加速度和角速度来计算欧拉角是很麻烦的，官方给了一个DMP解算例程，例程中的core文件夹包含了库函数文件
我们通过DMP函数得能这些角度数据后，用串口发送数据到“匿名四轴”2.6版上位机，可得到3D姿态图
5.1版本解算库包含以下几个文件，比6.12版轻量，适合小工程
我们需要修改两个C文件和inv_mpu.h文件，改动很小，只改到一些宏定义
为了尽量减少改动，inv_mpu_dmp_motion_driver.c 文件只需在头部加上自己的宏定义
inv_mpu_dmp_motion_driver.c 改动：
这段宏定义也是照搬文件下面的，Delay_ms函数提供给DMP库用于毫秒级延时，Get_Timer2_ms函数是毫秒计时，这两个函数要我们写出，printf是DMP库在使用串口输出一些提示信息
inv_mpu.c改动：
这里也是照着文件下面添加的，MPU6500_Write_Len 和 MPU6500_Read_Len函数是DMP库在操作MPU6500时要用的，由我们写一个函数提供给它，函数的形式在文件头部有说明， fsbsf是C编译器的函数库，不用我们写，把math.h头文件包含进去就可以。
inv_mpu.h改动
这里只用加上41和42这两行，其实在函数中实际上也用不到
4，在配置MDK开发环境时，要定义STM32F10X_HD和MPU6500，当然地，也可以在文件中定义，因我为用的平台是STM32F103,所以我定义成这个，也可以定义成你自己的开发平台。
同时还要勾选使用C99标准，因为DMP库中有些语法C99才支持
以下是主要的驱动代码，关于STM32的引脚配置就不放上来了
1，首先是官方给的5.1版本DMP函数库，
（1）inv_mpu_dmp_motion_driver.c
- /*
- $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 <stdio.h>
- #include <stdint.h>
- #include <stdlib.h>
- #include <string.h>
- #include <math.h>
- #include "inv_mpu.h"
- #include "inv_mpu_dmp_motion_driver.h"
- #include "dmpKey.h"
- #include "dmpmap.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)
- */
-
- /***********以下为自己定义的系统平台，在STM32F103ZE上运行***************************/
-
- #if defined STM32F10X_HD //在编译器中定义 STM32F10X_HD 则使用这段,以下的系统平台将不会用到
- #include "delay.h"
- #include "timer.h"
- #include "stdio.h"
- #define delay_ms Delay_ms
- #define get_ms Get_Timer2_ms
- #define log_i printf //打印信息
- #define log_e printf //打印信息
-
- /*********************************************************/
- #elif 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
- #error Gyro driver is missing the system layer implementations.
- #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;
- }
-
- /**
- * @}
- */
-
（2）inv_mpu_dmp_motion_driver.h
- /*
- $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_ */
-
(3) inv_mpu.c
- /*
- $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 <stdio.h>
- #include <stdint.h>
- #include <stdlib.h>
- #include <string.h>
- #include <math.h>
- #include "inv_mpu.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)
- */
-
-
- /***********以下为自己定义的系统平台，在STM32F103ZE上运行***************************/
- #if defined STM32F10X_HD //在编译器中定义 STM32F10X_HD 则使用这段,以下的系统平台将不会用到
-
- #define MPU_Device_Adrr MPU6500_device_addr //定义器件I2C地址,在定义变量st.hw.addr时用到
- #include "mpu6500_driver.h"
- #include "delay.h"
- #include "timer.h"
- #include "stdio.h"
- #define i2c_write MPU6500_Write_Len
- #define i2c_read MPU6500_Read_Len
- #define delay_ms Delay_ms
- #define get_ms Get_Timer2_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 printf //打印信息
- #define log_e printf //打印信息
- /* labs is already defined by TI's toolchain. */
- /* fabs is for doubles. fabsf is for floats. */
- #define fabs fabsf
- #define min(a,b) ((a<b)?a:b)
-
- /***************************************************/
-
- #elif 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) ((a<b)?a:b)
- #elif defined EMPL_TARGET_MSP430
- #include "msp430.h"
- #include "msp430_i2c.h"
- #include "msp430_clock.h"
- #include "msp430_interrupt.h"
- #include "log.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 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) ((a<b)?a:b)
- #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 "twi.h"
- #include "delay.h"
- #include "sysclk.h"
- #include "log.h"
- #include "sensors_xplained.h"
- #include "uc3l0_clock.h"
- #define i2c_write(a, b, c, d) twi_write(a, b, d, c)
- #define i2c_read(a, b, c, d) twi_read(a, b, d, c)
- /* delay_ms is a function already defined in ASF. */
- #define get_ms uc3l0_get_clock_ms
- static inline int reg_int_cb(struct int_param_s *int_param)
- {
- sensor_board_irq_connect(int_param->pin, 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
- #error Gyro driver is missing the system layer implementations.
- #endif
-
- #if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250
- #error Which gyro are you using? Define MPUxxxx in your compiler options.
- #endif
-
- /* Time for some messy macro work. =]
- * #define MPU9150
- * is equivalent to..
- * #define MPU6050
- * #define AK8975_SECONDARY
- *
- * #define MPU9250
- * is equivalent to..
- * #define MPU6500
- * #define AK8963_SECONDARY
- */
- #if defined MPU9150
- #ifndef MPU6050
- #define MPU6050
- #endif /* #ifndef MPU6050 */
- #if defined AK8963_SECONDARY
- #error "MPU9150 and AK8963_SECONDARY cannot both be defined."
- #elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */
- #define AK8975_SECONDARY
- #endif /* #if defined AK8963_SECONDARY */
- #elif defined MPU9250 /* #if defined MPU9150 */
- #ifndef MPU6500
- #define MPU6500
- #endif /* #ifndef MPU6500 */
- #if defined AK8975_SECONDARY
- #error "MPU9250 and AK8975_SECONDARY cannot both be defined."
- #elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */
- #define AK8963_SECONDARY
- #endif /* #if defined AK8975_SECONDARY */
- #endif /* #if defined MPU9150 */
-
- #if defined AK8975_SECONDARY || defined AK8963_SECONDARY
- #define AK89xx_SECONDARY
- #else
- /* #warning "No compass = less profit for Invensense. Lame." */
- #endif
-
- static int set_int_enable(unsigned char enable);
-
- /* Hardware registers needed by driver. */
- struct gyro_reg_s {
- unsigned char who_am_i;
- unsigned char rate_div;
- unsigned char lpf;
- unsigned char prod_id;
- unsigned char user_ctrl;
- unsigned char fifo_en;
- unsigned char gyro_cfg;
- unsigned char accel_cfg;
- unsigned char accel_cfg2;
- unsigned char lp_accel_odr;
- unsigned char motion_thr;
- unsigned char motion_dur;
- unsigned char fifo_count_h;
- unsigned char fifo_r_w;
- unsigned char raw_gyro;
- unsigned char raw_accel;
- unsigned char temp;
- unsigned char int_enable;
- unsigned char dmp_int_status;
- unsigned char int_status;
- unsigned char accel_intel;
- unsigned char pwr_mgmt_1;
- unsigned char pwr_mgmt_2;
- unsigned char int_pin_cfg;
- unsigned char mem_r_w;
- unsigned char accel_offs;
- unsigned char i2c_mst;
- unsigned char bank_sel;
- unsigned char mem_start_addr;
- unsigned char prgm_start_h;
- #if defined AK89xx_SECONDARY
- unsigned char s0_addr;
- unsigned char s0_reg;
- unsigned char s0_ctrl;
- unsigned char s1_addr;
- unsigned char s1_reg;
- unsigned char s1_ctrl;
- unsigned char s4_ctrl;
- unsigned char s0_do;
- unsigned char s1_do;
- unsigned char i2c_delay_ctrl;
- unsigned char raw_compass;
- /* The I2C_MST_VDDIO bit is in this register. */
- unsigned char yg_offs_tc;
- #endif
- };
-
- /* Information specific to a particular device. */
- struct hw_s {
- unsigned char addr;
- unsigned short max_fifo;
- unsigned char num_reg;
- unsigned short temp_sens;
- short temp_offset;
- unsigned short bank_size;
- #if defined AK89xx_SECONDARY
- unsigned short compass_fsr;
- #endif
- };
-
- /* When entering motion interrupt mode, the driver keeps track of the
- * previous state so that it can be restored at a later time.
- * TODO: This is tacky. Fix it.
- */
- struct motion_int_cache_s {
- unsigned short gyro_fsr;
- unsigned char accel_fsr;
- unsigned short lpf;
- unsigned short sample_rate;
- unsigned char sensors_on;
- unsigned char fifo_sensors;
- unsigned char dmp_on;
- };
-
- /* Cached chip configuration data.
- * TODO: A lot of these can be handled with a bitmask.
- */
- struct chip_cfg_s {
- /* Matches gyro_cfg >> 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 = MPU_Device_Adrr,
- .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
- 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];
-
- /* 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;
- }
-
- /**
- * @}
- */
-
(4) inv_mpu.h
- /*
- $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;
- #elif defined STM32F10X_HD //自行添加的一行，避免编译错误，实际没用到
- void (*cb)(void);
- #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_ */
-
(5) dmpKey.h
- /*
- $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__
(6) dmpmap.h
- /*
- $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
2、下面是MPU6500低层通信函数，如I2C读写，直接读写MPU6500寄存器，还有初始化函数
（1） mpu6500_driver.c
- #include "mpu6500_driver.h"
- #include "delay.h"
- #include "inv_mpu.h"
- #include "inv_mpu_dmp_motion_driver.h"
- #include "math_fun.h"
- #include "math.h"
-
-
-
- u8 MPU_EXTI_flag=0; //MPU6500引脚中断
-
-
- /* Platform-specific information. Kinda like a boardfile. */
- struct platform_data_s {
- signed char orientation[9];
- };
-
- /* The sensors can be mounted onto the board in any orientation. The mounting
- * matrix seen below tells the MPL how to rotate the raw data from the
- * driver(s).
- * TODO: The following matrices refer to the configuration on internal test
- * boards at Invensense. If needed, please modify the matrices to match the
- * chip-to-body matrix for your particular set up.
- */
- static struct platform_data_s gyro_pdata = {
-
- .orientation = {0, -1, 0,
- 1, 0, 0,
- 0, 0, 1}
- };
-
-
-
- /* Axis Transformation matrix
- |r11 r12 r13| |vx| |v'x|
- |r21 r22 r23| |vy| = |v'y|
- |r31 r32 r33| |vz| |v'z|
- v'x = {(r11 * vx) +( r12 * vy) +( r13 * vz)}
- v'y = {(r21 * vx) +( r22 * vy) +( r23 * vz)}
- v'z = {(r31 * vx) +( r32 * vy) +( r33 * vz)}
- */
-
-
- void MPU6500_Port_EXIT_Init(void)
- {
- GPIO_InitTypeDef GPIO_InitStrue;
- EXTI_InitTypeDef EXTI_InitStrue;
- NVIC_InitTypeDef NVIC_InitStrue;
-
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO|RCC_APB2Periph_GPIOB,ENABLE);
-
- GPIO_InitStrue.GPIO_Mode=GPIO_Mode_IPU;
- GPIO_InitStrue.GPIO_Pin=GPIO_Pin_14;
- GPIO_Init(GPIOB,&GPIO_InitStrue);
-
- GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource14);
-
-
- EXTI_InitStrue.EXTI_Line=EXTI_Line14;
- EXTI_InitStrue.EXTI_LineCmd=ENABLE;
- EXTI_InitStrue.EXTI_Mode=EXTI_Mode_Interrupt;
- EXTI_InitStrue.EXTI_Trigger=EXTI_Trigger_Falling;
- EXTI_Init(&EXTI_InitStrue);
-
-
- NVIC_InitStrue.NVIC_IRQChannel=EXTI15_10_IRQn;
- NVIC_InitStrue.NVIC_IRQChannelCmd=ENABLE;
- NVIC_InitStrue.NVIC_IRQChannelPreemptionPriority=1;
- NVIC_InitStrue.NVIC_IRQChannelSubPriority=2;
- NVIC_Init(&NVIC_InitStrue);
- }
-
-
- void MPU6500_I2C_PORT_Init(void)
- {
- GPIO_InitTypeDef GPIO_InitStructure;
-
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
-
- GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP;
- GPIO_InitStructure.GPIO_Pin=GPIO_Pin_12|GPIO_Pin_13;
- GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
- GPIO_Init(GPIOB,&GPIO_InitStructure);
- GPIO_SetBits(GPIOB,GPIO_Pin_12|GPIO_Pin_13);
-
-
- }
-
- void MPU6500_I2C_delay(void)
- {
- u16 i;
- /*
- 下面的时间是通过逻辑分析仪测试得到的。
- 工作条件：CPU主频72MHz ，MDK编译环境，1级优化
-
- 循环次数为10时，SCL频率 = 205KHz
- 循环次数为7时，SCL频率 = 347KHz， SCL高电平时间1.5us，SCL低电平时间2.87us
- 循环次数为5时，SCL频率 = 421KHz， SCL高电平时间1.25us，SCL低电平时间2.375us
- */
- for(i=0;i<5;i++);
- }
-
-
- void MPU6500_I2C_start(void)
- {
- MPU6500_I2C_SCL=0;
- MPU6500_I2C_SetOut_Mode();
-
- MPU6500_I2C_SDA_WR=1;
- MPU6500_I2C_SCL=1;
- MPU6500_I2C_delay();
- MPU6500_I2C_SDA_WR=0;
- MPU6500_I2C_SCL=0;
- MPU6500_I2C_delay();
- }
-
- void MPU6500_I2C_stop(void)
- {
- MPU6500_I2C_SCL=0;
- MPU6500_I2C_SetOut_Mode();
-
- MPU6500_I2C_SDA_WR=0;
- MPU6500_I2C_SCL=1;
- MPU6500_I2C_delay();
- MPU6500_I2C_SDA_WR=1;
- MPU6500_I2C_delay();
-
- }
-
- u8 MPU6500_I2C_check_ack(void)
- {
- u16 delay_count=0;
-
- MPU6500_I2C_SCL=0;
- MPU6500_I2C_SetIn_Mode();
- MPU6500_I2C_SDA_WR=1;
- MPU6500_I2C_SCL=1;
- MPU6500_I2C_delay();
- while( MPU6500_I2C_SDA_RE )
- {
- delay_count++;
- if(delay_count>0x0fff)
- {
- MPU6500_I2C_stop();
- return 1;
- }
- }
-
- MPU6500_I2C_SCL=0;
- MPU6500_I2C_delay();
-
- return 0;
- }
-
- void MPU6500_I2C_ack(void)
- {
- MPU6500_I2C_SCL=0;
- MPU6500_I2C_SetOut_Mode();
-
- MPU6500_I2C_SDA_WR=0;
- MPU6500_I2C_SCL=1;
- MPU6500_I2C_delay();
- MPU6500_I2C_SCL=0;
- MPU6500_I2C_delay();
- MPU6500_I2C_SDA_WR=1;
-
- }
-
- void MPU6500_I2C_NoAck(void)
- {
- MPU6500_I2C_SCL=0;
- MPU6500_I2C_SetOut_Mode();
- MPU6500_I2C_SDA_WR=1;
- MPU6500_I2C_delay();
- MPU6500_I2C_SCL=1;
- MPU6500_I2C_delay();
- MPU6500_I2C_SCL=0;
- MPU6500_I2C_delay();
-
- }
-
-
- void MPU6500_I2C_write_char(u8 dat)
- {
- u8 i=0;
-
- MPU6500_I2C_SCL=0;
- MPU6500_I2C_SetOut_Mode();
- for(i=0;i<8;i++)
- {
-
- if(dat&0x80) MPU6500_I2C_SDA_WR=1;
- else MPU6500_I2C_SDA_WR=0;
- MPU6500_I2C_delay();
- MPU6500_I2C_SCL=1;
- MPU6500_I2C_delay();
- MPU6500_I2C_SCL=0;
- dat<<=1;
- }
-
- }
-
- u8 MPU6500_I2C_read_char(void)
- {
- u8 i=0,dat=0;
- MPU6500_I2C_SCL=0;
- MPU6500_I2C_SetIn_Mode();
- for(i=0;i<8;i++)
- {
-
- dat<<=1;
- MPU6500_I2C_SCL=1;
- MPU6500_I2C_delay();
- if(MPU6500_I2C_SDA_RE) dat|=0x01;
- MPU6500_I2C_SCL=0;
- MPU6500_I2C_delay();
-
- }
- return dat;
- }
-
- u8 MPU6500_write_byte(u8 reg,u8 data)
- {
- MPU6500_I2C_start();
- MPU6500_I2C_write_char(MPU6500_device_addr<<1 | 0x00);
- if(MPU6500_I2C_check_ack())
- {
- MPU6500_I2C_stop();
- return 2;
- }
-
- MPU6500_I2C_write_char(reg);
- MPU6500_I2C_check_ack();
- MPU6500_I2C_write_char(data);
- MPU6500_I2C_check_ack();
- MPU6500_I2C_stop();
- return 0;
- }
-
- u8 MPU6500_read_byte(u8 reg)
- {
- u8 data=0;
-
- MPU6500_I2C_start();
- MPU6500_I2C_write_char(MPU6500_device_addr<<1 | 0x00);
- if(MPU6500_I2C_check_ack())
- {
- MPU6500_I2C_stop();
- return 2;
- }
- MPU6500_I2C_write_char(reg);
- MPU6500_I2C_check_ack();
-
- MPU6500_I2C_start();
- MPU6500_I2C_write_char(MPU6500_device_addr<<1 | 0x01);
- MPU6500_I2C_check_ack();
- data=MPU6500_I2C_read_char();
- MPU6500_I2C_ack();
- MPU6500_I2C_stop();
- return data;
- }
-
-
- u8 MPU6500_Read_Len(u8 DeviceAddr,u8 RegAddr,u8 len,u8 *pbuff)
- {
- u8 i;
- MPU6500_I2C_start();
- MPU6500_I2C_write_char(DeviceAddr<<1 | 0x00 );
- if(MPU6500_I2C_check_ack())
- {
- MPU6500_I2C_stop();
- return 2;
- }
-
- MPU6500_I2C_write_char(RegAddr);
- MPU6500_I2C_check_ack();
-
- MPU6500_I2C_start();
- MPU6500_I2C_write_char(DeviceAddr<<1 | 0x01);
- MPU6500_I2C_check_ack();
- for(i=0;i<len;i++)
- {
- *pbuff++ =MPU6500_I2C_read_char();
- if(i+1>=len) {MPU6500_I2C_NoAck(); break;}
- MPU6500_I2C_ack();
- }
-
- MPU6500_I2C_stop();
-
- return 0;
- }
-
-
- u8 MPU6500_Write_Len(u8 DeviceAddr,u8 RegAddr,u8 len,u8 *pbuff)
- {
- u8 i;
-
- MPU6500_I2C_start();
- MPU6500_I2C_write_char(DeviceAddr<<1 | 0x00);
- if(MPU6500_I2C_check_ack())
- {
- MPU6500_I2C_stop();
- return 2;
- }
-
- MPU6500_I2C_write_char(RegAddr);
- MPU6500_I2C_check_ack();
- for(i=0;i<len;i++)
- {
- MPU6500_I2C_write_char(*pbuff++);
- if(MPU6500_I2C_check_ack())
- {
- MPU6500_I2C_stop();
- return 3;
- }
- }
-
- MPU6500_I2C_stop();
- return 0;
- }
-
- u8 InitMPU6050(void)
- {
- int i=0,j=0;
- u8 res=0;
- MPU6500_I2C_PORT_Init();
-
- //在初始化之前要延时一段时间，若没有延时，则断电后再上电数据可能会出错
- for(i=0;i<1000;i++)
- {
- for(j=0;j<1000;j++)
- {
- ;
- }
- }
-
- MPU6500_write_byte(PWR_MGMT_1,0x80);//复位MPU6050
- Delay_ms(100);
- MPU6500_write_byte(PWR_MGMT_1,0);唤醒MPU6050
- MPU6500_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
- MPU6500_Set_Accel_Fsr(0); //加速度传感器,±2g
- MPU6500_Set_Rate(50); //设置采样率50Hz
- MPU6500_write_byte(INT_EN_REG,0X00); //关闭所有中断
- MPU6500_write_byte(USER_CTRL_REG,0X00); //I2C主模式关闭
- MPU6500_write_byte(FIFO_EN_REG,0X00); //关闭FIFO
- MPU6500_write_byte(INTBP_CFG_REG,0X80); //INT引脚低电平有效
- res=MPU6500_read_byte(WHO_AM_I);
- if(res==MPU6500_ID)
- {
- MPU6500_write_byte(PWR_MGMT_1,0X01); //设置CLKSEL,PLL X轴为参考
- MPU6500_write_byte(PWR_MGMT_2,0X00); //加速度与陀螺仪都工作
- MPU6500_Set_Rate(50); //设置采样率为50Hz
-
- }
- else return 1;
-
-
- /*
- temp=0x080;
- MPU6500_Write_Len(MPU6500_device_addr,PWR_MGMT_1,1, &temp); //解除休眠状态
- temp=0x00;
- MPU6500_Write_Len(MPU6500_device_addr,PWR_MGMT_1,1, &temp); //解除休眠状态
-
- MPU6500_Write_Len(MPU6500_device_addr,SMPLRT_DIV,1, &temp);
- temp=0x06;
- MPU6500_Write_Len(MPU6500_device_addr,CONFIG, 1,&temp);
- temp=0x00;
- MPU6500_Write_Len(MPU6500_device_addr,ACCEL_CONFIG,1, &temp);
- temp=0x18;
- MPU6500_Write_Len(MPU6500_device_addr,GYRO_CONFIG,1, &temp);
- */
-
- return 0;
- }
-
- short GetData(u8 REG_Address)
- {
- u8 buff[2];
- MPU6500_Read_Len(MPU6500_device_addr,REG_Address,2,buff);
- return ((buff[0]<<8)|buff[1]); //合成数据
- }
-
- //得到加速度值(原始值)
- //gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
- //返回值:0,成功
- // 其他,错误代码
- u8 MPU6500_Get_Accelerometer(short *ax,short *ay,short *az)
- {
- u8 result;
- u8 acecel[6];
- result=MPU6500_Read_Len(MPU6500_device_addr,ACCEL_XOUT_H,6,acecel);
- if(result==0)
- {
- *ax=((u16)acecel[0]<<8)|acecel[1];
- *ay=((u16)acecel[2]<<8)|acecel[3];
- *az=((u16)acecel[4]<<8)|acecel[5];
- }
-
- return result;
- }
-
- //得到陀螺仪值(原始值)
- //gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
- //返回值:0,成功
- // 其他,错误代码
- u8 MPU6500_Get_Gyroscope(short *gx,short *gy,short *gz)
- {
- u8 result;
- u8 gyro[6];
- result=MPU6500_Read_Len(MPU6500_device_addr,GYRO_XOUT_H,6,gyro);
- if(result==0)
- {
- *gx=((u16)gyro[0]<<8)|gyro[1];
- *gy=((u16)gyro[2]<<8)|gyro[3];
- *gz=((u16)gyro[4]<<8)|gyro[5];
- }
-
- return result;
- }
-
- float MPU6500_GetTemperature(void)
- {
- short temp;
- float dat;
- temp=GetData(TEMP_OUT_H);
-
- dat=((double) (temp-21)/333.87)+21;
-
- return dat;
- }
-
- //设置MPU6050陀螺仪传感器满量程范围
- //fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
- //返回值:0,设置成功
- // 其他,设置失败
- u8 MPU6500_Set_Gyro_Fsr(u8 fsr)
- {
- return MPU6500_write_byte(GYRO_CONFIG,fsr<<3);//设置陀螺仪满量程范围
- }
- //设置MPU6050加速度传感器满量程范围
- //fsr:0,±2g;1,±4g;2,±8g;3,±16g
- //返回值:0,设置成功
- // 其他,设置失败
- u8 MPU6500_Set_Accel_Fsr(u8 fsr)
- {
- return MPU6500_write_byte(ACCEL_CONFIG,fsr<<3);//设置加速度传感器满量程范围
- }
-
- //设置MPU6050的采样率(假定Fs=1KHz)
- //rate:4~1000(Hz)
- //返回值:0,设置成功
- // 其他,设置失败
- u8 MPU6500_Set_Rate(u16 rate)
- {
- u8 data;
- if(rate>1000)rate=1000;
- if(rate<4)rate=4;
- data=1000/rate-1;
- data=MPU6500_write_byte(SMPLRT_DIV,data); //设置数字低通滤波器
- return MPU6500_Set_LPF(rate/2); //自动设置LPF为采样率的一半
- }
-
- //设置MPU6050的数字低通滤波器
- //lpf:数字低通滤波频率(Hz)
- //返回值:0,设置成功
- // 其他,设置失败
- u8 MPU6500_Set_LPF(u16 lpf)
- {
- u8 data=0;
- if(lpf>=188)data=1;
- else if(lpf>=98)data=2;
- else if(lpf>=42)data=3;
- else if(lpf>=20)data=4;
- else if(lpf>=10)data=5;
- else data=6;
- return MPU6500_write_byte(CONFIG,data);//设置数字低通滤波器
- }
-
-
- //MPU6050自测试
- //返回值:0,正常
- // 其他,失败
- u8 MPU6500_run_self_test(void)
- {
- int result;
- //char test_packet[4] = {0};
- long gyro[3], accel[3];
- result = mpu_run_self_test(gyro, accel);
- if (result == 0x07)
- {
- /* Test passed. We can trust the gyro data here, so let's push it down
- * to the DMP.
- */
- float gyro_sens;
- unsigned short accel_sens;
- mpu_get_gyro_sens(&gyro_sens);
- gyro[0] = (long)(gyro[0] * gyro_sens);
- gyro[1] = (long)(gyro[1] * gyro_sens);
- gyro[2] = (long)(gyro[2] * gyro_sens);
- dmp_set_gyro_bias(gyro);
- mpu_get_accel_sens(&accel_sens);
- accel[0] *= accel_sens;
- accel[1] *= accel_sens;
- accel[2] *= accel_sens;
- dmp_set_accel_bias(accel);
-
- return 0;
- }else return 1;
- }
- u8 MPU6500_DMP_Init(void)
- {
- struct int_param_s int_param;
- int result;
-
- result=mpu_init(&int_param);
- if(result) return 1;
-
- result=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
- if(result) return 2;
-
- result=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFO
- if(result) return 3;
-
- result=mpu_set_sample_rate(DEFAULT_MPU_HZ);//设置采样率
- if(result) return 4;
-
- result=dmp_load_motion_driver_firmware();//加载dmp固件
- if(result) return 5;
-
- result=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_pdata.orientation));//设置陀螺仪方向
- if(result) return 6;
-
- result=dmp_enable_feature(DMP_FEATURE_TAP|DMP_FEATURE_ANDROID_ORIENT|
- DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_GYRO_CAL|
- DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_RAW_GYRO);//设置dmp功能
- if(result) return 7;
-
- result=dmp_set_fifo_rate(DEFAULT_MPU_HZ);//设置DMP输出速率(最大不超过200Hz)
- if(result) return 8;
-
- result=MPU6500_run_self_test(); //自检
- if(result) return 9;
-
- result=mpu_set_dmp_state(1);//使能DMP
- if(result) return 10;
-
- result=dmp_set_interrupt_mode(DMP_INT_CONTINUOUS);//设置中断产生方式
- if(result) return 11;
-
- return 0;
- }
- //得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
- //pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
- //roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°
- //yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
- //返回值:0,正常
- // 其他,失败
- u8 MPU6500_dmp_get_euler_angle(short *accel,short *gyro,float *pitch,float *roll,float *yaw)
- {
-
- //q30格式,long转float时的除数.
- #define Q30 ((1<<30)*1.0f)
-
- float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
- unsigned long sensor_timestamp;
- short sensors;
- unsigned char more;
- long quat[4];
- u8 result=0;
- result=dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);
- if(result)return 1;
- /* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
- * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
- **/
- /*if (sensors & INV_XYZ_GYRO )
- send_packet(PACKET_TYPE_GYRO, gyro);
- if (sensors & INV_XYZ_ACCEL)
- send_packet(PACKET_TYPE_ACCEL, accel); */
- /* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
- * The orientation is set by the scalar passed to dmp_set_orientation during initialization.
- **/
- if(sensors&INV_WXYZ_QUAT)
- {
- q0 = quat[0] / Q30; //q30格式转换为浮点数
- q1 = quat[1] / Q30;
- q2 = quat[2] / Q30;
- q3 = quat[3] / Q30;
- //计算得到俯仰角/横滚角/航向角
- *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
- *roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
- *yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
- }else return 2;
- return 0;
- }
(2) mpu6500_driver.h
- #ifndef __MPU6500_DRIVER_H
- #define __MPU6500_DRIVER_H
-
- #include "stm32f10x.h"
-
- #include "sys.h"
-
-
-
- #define MPU6500_I2C_SDA_RE PBin(13)
- #define MPU6500_I2C_SDA_WR PBout(13)
-
- #define MPU6500_I2C_SCL PBout(12)
-
- #define MPU6500_I2C_SetOut_Mode() {GPIOB->CRH&=0XFF0FFFFF;GPIOB->CRH|=0X00300000;}
- #define MPU6500_I2C_SetIn_Mode() {GPIOB->CRH&=0XFF0FFFFF;GPIOB->CRH|=0X00800000;GPIOB->ODR|=0X01<13;}
-
- #define MPU6500_device_addr 0x68
- #define MPU6500_ID 0x70
-
-
- #define DEFAULT_MPU_HZ 100 //100Hz
-
- //****************************************
- // 定义MPU6050内部地址
- //****************************************
- #define SMPLRT_DIV 0x19 //陀螺仪采样率，典型值：0x07(125Hz)
- #define CONFIG 0x1A //低通滤波频率，典型值：0x06(5Hz)
- #define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
- #define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
-
- #define FIFO_EN_REG 0X23 //FIFO使能寄存器
- #define INTBP_CFG_REG 0X37 //中断/旁路设置寄存器
- #define INT_EN_REG 0X38 //中断使能寄存器
- #define ACCEL_XOUT_H 0x3B
- #define ACCEL_XOUT_L 0x3C
- #define ACCEL_YOUT_H 0x3D
- #define ACCEL_YOUT_L 0x3E
- #define ACCEL_ZOUT_H 0x3F
- #define ACCEL_ZOUT_L 0x40
- #define TEMP_OUT_H 0x41
- #define TEMP_OUT_L 0x42
- #define GYRO_XOUT_H 0x43
- #define GYRO_XOUT_L 0x44
- #define GYRO_YOUT_H 0x45
- #define GYRO_YOUT_L 0x46
- #define GYRO_ZOUT_H 0x47
- #define GYRO_ZOUT_L 0x48
- #define USER_CTRL_REG 0X6A //用户控制寄存器
- #define PWR_MGMT_1 0x6B //电源管理，典型值：0x00(正常启用)
- #define PWR_MGMT_2 0X6C //电源管理寄存器2
- #define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x68，只读)
-
- #define SlaveAddress 0xD0 //IIC写入时的地址字节数据，+1为读取
-
-
-
- extern u8 MPU_EXTI_flag;
-
-
-
-
- void MPU6500_Port_EXIT_Init(void);
- void MPU6500_I2C_PORT_Init(void);
- void MPU6500_I2C_delay(void);
- void MPU6500_I2C_start(void);
- void MPU6500_I2C_stop(void);
- u8 MPU6500_I2C_check_ack(void);
- void MPU6500_I2C_ack(void);
- void MPU6500_I2C_NoAck(void);
- void MPU6500_I2C_write_char(u8 dat);
- u8 MPU6500_I2C_read_char(void);
- u8 MPU6500_write_byte(u8 reg,u8 data);
- u8 MPU6500_read_byte(u8 reg);
- u8 MPU6500_Read_Len(u8 DeviceAddr,u8 RegAddr,u8 len,u8 *pbuff);
- u8 MPU6500_Write_Len(u8 DeviceAddr,u8 RegAddr,u8 len,u8 *pbuff);
- u8 InitMPU6050(void);
- short GetData(u8 REG_Address);
- u8 MPU6500_Get_Accelerometer(short *ax,short *ay,short *az);
- u8 MPU6500_Get_Gyroscope(short *gx,short *gy,short *gz);
- float MPU6500_GetTemperature(void);
- u8 MPU6500_Set_Gyro_Fsr(u8 fsr);
- u8 MPU6500_Set_Accel_Fsr(u8 fsr);
- u8 MPU6500_Set_Rate(u16 rate);
- u8 MPU6500_Set_LPF(u16 lpf);
- u8 MPU6500_run_self_test(void);
- u8 MPU6500_DMP_Init(void);
- u8 MPU6500_dmp_get_euler_angle(short *accel,short *gyro,float *pitch,float *roll,float *yaw);
-
-
-
- #endif
3、接下来是两个关于计算方向矩阵的函数，是从DMP6.12版复制过来的，在DMP库设置设计方向矩阵时调用
(1) math_fun.c
- #include "math_fun.h"
-
- /**
- *此文件包含计算MPU6XXX方向矩阵的数学计算函数
- *
- *
- *
- *
- **/
-
-
- unsigned short inv_row_2_scale(const signed char *row)
- {
- unsigned short b;
-
- if (row[0] > 0)
- b = 0;
- else if (row[0] < 0)
- b = 4;
- else if (row[1] > 0)
- b = 1;
- else if (row[1] < 0)
- b = 5;
- else if (row[2] > 0)
- b = 2;
- else if (row[2] < 0)
- b = 6;
- else
- b = 7; // error
- return b;
- }
-
- /** Converts an orientation matrix made up of 0,+1,and -1 to a scalar representation.
- * @param[in] mtx Orientation matrix to convert to a scalar.
- * @return Description of orientation matrix. The lowest 2 bits (0 and 1) represent the column the one is on for the
- * first row, with the bit number 2 being the sign. The next 2 bits (3 and 4) represent
- * the column the one is on for the second row with bit number 5 being the sign.
- * The next 2 bits (6 and 7) represent the column the one is on for the third row with
- * bit number 8 being the sign. In binary the identity matrix would therefor be:
- * 010_001_000 or 0x88 in hex.
- */
- unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
- {
-
- unsigned short scalar;
-
- /*
- XYZ 010_001_000 Identity Matrix
- XZY 001_010_000
- YXZ 010_000_001
- YZX 000_010_001
- ZXY 001_000_010
- ZYX 000_001_010
- */
-
- scalar = inv_row_2_scale(mtx);
- scalar |= inv_row_2_scale(mtx + 3) << 3;
- scalar |= inv_row_2_scale(mtx + 6) << 6;
-
-
- return scalar;
- }
-
(2) math_fun.h
- #ifndef __MATH_FUN_H
- #define __MATH_FUN_H
-
- unsigned short inv_row_2_scale(const signed char *row);
- unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);
-
-
- #endif
4、向上位机“匿名四轴”2.6版发送数据的函数，这些函数的功能是，将加速度，角速度，航向角，俯仰角，横滚角，按照上位机的格式要求，将数据组装后通过串口发送到上位机，格式要求可打开“匿名四轴”2.6版，按F12查看帮助。
（1） send_to_client.c
- #include "send_to_client.h"
-
-
-
- //串口1发送1个字符
- //c:要发送的字符
- void usart1_send_char(u8 c)
- {
- while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET); //循环发送,直到发送完毕
- USART_SendData(USART1,c);
- }
- //传送数据给匿名四轴上位机软件(V2.6版本)
- //fun:功能字. 0XA0~0XAF
- //data:数据缓存区,最多28字节!!
- //len:data区有效数据个数
- void usart1_niming_report(u8 fun,u8*data,u8 len)
- {
- u8 send_buf[32];
- u8 i;
- if(len>28)return; //最多28字节数据
- send_buf[len+3]=0; //校验数置零
- send_buf[0]=0X88; //帧头
- send_buf[1]=fun; //功能字
- send_buf[2]=len; //数据长度
- for(i=0;i<len;i++)send_buf[3+i]=data[i]; //复制数据
- for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i]; //计算校验和
- for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]); //发送数据到串口1
- }
- //发送加速度传感器数据和陀螺仪数据
- //aacx,aacy,aacz:x,y,z三个方向上面的加速度值
- //gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值
- void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)
- {
- u8 tbuf[12];
- tbuf[0]=(aacx>>8)&0XFF;
- tbuf[1]=aacx&0XFF;
- tbuf[2]=(aacy>>8)&0XFF;
- tbuf[3]=aacy&0XFF;
- tbuf[4]=(aacz>>8)&0XFF;
- tbuf[5]=aacz&0XFF;
- tbuf[6]=(gyrox>>8)&0XFF;
- tbuf[7]=gyrox&0XFF;
- tbuf[8]=(gyroy>>8)&0XFF;
- tbuf[9]=gyroy&0XFF;
- tbuf[10]=(gyroz>>8)&0XFF;
- tbuf[11]=gyroz&0XFF;
- usart1_niming_report(0XA1,tbuf,12);//自定义帧,0XA1
- }
- //通过串口1上报结算后的姿态数据给电脑
- //aacx,aacy,aacz:x,y,z三个方向上面的加速度值
- //gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值
- //roll:横滚角.单位0.01度。 -18000 -> 18000 对应 -180.00 -> 180.00度
- //pitch:俯仰角.单位 0.01度。-9000 - 9000 对应 -90.00 -> 90.00 度
- //yaw:航向角.单位为0.1度 0 -> 3600 对应 0 -> 360.0度
- void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz,float roll_f,float pitch_f,float yaw_f)
- {
-
- u8 i;
- short roll,pitch,yaw;
- u8 tbuf[28];
-
- for(i=0;i<28;i++)tbuf[i]=0;//清0
-
- //按上位机格式要求整理数据
- roll = (int)(roll_f*100);
- pitch = (int)(pitch_f*100);
- yaw = (int)(yaw_f*10);
-
- tbuf[0]=(aacx>>8)&0XFF;
- tbuf[1]=aacx&0XFF;
- tbuf[2]=(aacy>>8)&0XFF;
- tbuf[3]=aacy&0XFF;
- tbuf[4]=(aacz>>8)&0XFF;
- tbuf[5]=aacz&0XFF;
- tbuf[6]=(gyrox>>8)&0XFF;
- tbuf[7]=gyrox&0XFF;
- tbuf[8]=(gyroy>>8)&0XFF;
- tbuf[9]=gyroy&0XFF;
- tbuf[10]=(gyroz>>8)&0XFF;
- tbuf[11]=gyroz&0XFF;
- tbuf[18]=(roll>>8)&0XFF;
- tbuf[19]=roll&0XFF;
- tbuf[20]=(pitch>>8)&0XFF;
- tbuf[21]=pitch&0XFF;
- tbuf[22]=(yaw>>8)&0XFF;
- tbuf[23]=yaw&0XFF;
- usart1_niming_report(0XAF,tbuf,28);//飞控显示帧,0XAF
- }
-
(2) send_to_client.h
- #ifndef __SEND_TO_CLIENT_H
- #define __SEND_TO_CLIENT_H
-
- #include "stm32f10x.h"
- #include "sys.h"
-
-
- void usart1_send_char(u8 c);
- void usart1_niming_report(u8 fun,u8*data,u8 len);
- void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz);
- void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz,float roll_f,float pitch_f,float yaw_f);
-
-
-
- #endif
-
5、最后是主函数调用 main.c
- #include "stm32f10x.h"
- #include "variable.h"
- #include "delay.h"
- #include "led.h"
- #include "key.h"
- #include "usart.h"
- #include "timer.h"
- #include "mpu6500_driver.h"
- #include "send_to_client.h"
- #include "inv_mpu_dmp_motion_driver.h"
- #include "inv_mpu.h"
- #include "lcd_driver.h"
-
-
-
-
- #define USART1_Bound 115200
-
- #define save_mpu6500_key 0xaa55
- #define save_mpu6500_addr 98
-
- // unsigned long sensor_timestamp;
- // short gyro[3], accel[3], sensors;
- // unsigned char more;
- // long quat[4];
-
- u8 buffer[100];
- long MPU_buffer[100];
-
- int main(void)
- {
- u8 temp,result;
-
- float pitch,roll,yaw;
-
- short accel[3],gyro[3];
-
- float temperature;
- // unsigned long timestamp;
- unsigned long walk_count;
- unsigned long walk_time;
-
-
- NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
- Delay_Init();
- LED_Init();
- Key_Init();
- MY_USART1_Init(USART1_Bound);
- Timer2_Init(1000-1,72-1);//set timer frequent:1000KHz T=1us
- LCD_Init();
- LCD_Clear(WHITE);
- MPU6500_Port_EXIT_Init();
- if(InitMPU6050()) printf("InitMPU6050 error\r\n");
- if(MPU6500_DMP_Init()) printf("MPU6500_DMP_Init error :%d \r\n",result);
-
-
- LCD_ShowString(10,20,100,16,16,(u8*)"MPU6500 TEST");
- MPU6500_Read_Len(MPU6500_device_addr,WHO_AM_I,1,&temp);
- sprintf((char*)buffer,"MPU6500 ID:0X%x",temp);
- LCD_ShowString(10,50,200,16,16,buffer);
-
-
- while(1)
- {
-
-
- if(MPU_EXTI_flag)
- {
- MPU_EXTI_flag=0;
- LED_D5=!LED_D5;
- result=MPU6500_dmp_get_euler_angle(accel,gyro,&pitch,&roll,&yaw);//获得各角数据
- if(result==0)
- {
-
- mpu6050_send_data(accel[0],accel[1],accel[2],gyro[0],gyro[1],gyro[2]);//将数据发到上位机
- usart1_report_imu(accel[0],accel[1],accel[2],gyro[0],gyro[1],gyro[2],(roll),(pitch),(yaw));
-
- }
- }
-
- if(Timer2_flag_100ms)
- {
- Timer2_flag_100ms=0;
- sprintf((char*)buffer,"yaw:%5.1f",yaw);
- LCD_ShowString(10,70,100,16,16,buffer);
-
- sprintf((char*)buffer,"pitch:%5.1f",pitch);
- LCD_ShowString(10,90,100,16,16,buffer);
-
- sprintf((char*)buffer,"roll:%5.1f",roll);
- LCD_ShowString(10,110,100,16,16,buffer);
-
- }
-
- if(Timer2_flag_1s&LED_Time_flag_MaskBit)
- {
- Timer2_flag_1s&=~LED_Time_flag_MaskBit;
-
- LED_D4=!LED_D4;
-
- dmp_get_pedometer_step_count(&walk_count);//计步器
- dmp_get_pedometer_walk_time(&walk_time);
- temperature=MPU6500_GetTemperature();//内部温度
-
- sprintf((char*)buffer,"step:%d walk_time:%d",walk_count,walk_time);
- LCD_ShowString(10,130,240,16,16,buffer);
-
- sprintf((char*)buffer,"T:%5.1f",temperature);
- LCD_ShowString(10,150,240,16,16,buffer);
-
-
-
- }
-
- /*
- if(USART1_Receive_T_flag&0x01)
- {
- USART_DMACmd(USART1,USART_DMAReq_Tx,ENABLE);
- DMA_Cmd(DMA1_Channel4,ENABLE);
- USART1_Receive_T_flag&=~0x01;
- }
-
- if(DMA_TransferComplete_Flag&0x01)
- {
- LED_D4=0;
- DMA_TransferComplete_Flag&=~0x01;
- }
- */
- }
- }
-
-
声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/菜鸟追梦旅行/article/detail/467194
推荐阅读
相关标签
