21ic官方微信-->
后使用快捷导航没有帐号？
请完成以下验证码
查看: 10155|回复: 19
51上实现了MPU6050的DMP，但是yaw角度不准确
&&已结帖(20)
主题帖子积分
高级技术员, 积分 723, 距离下一级还需 277 积分
高级技术员, 积分 723, 距离下一级还需 277 积分
主题帖子积分
专家等级：结帖率：90%
主题帖子积分
高级技术员, 积分 723, 距离下一级还需 277 积分
高级技术员, 积分 723, 距离下一级还需 277 积分
如题，我本质就是想测试这个航向角yaw，但是通过四元数得到的原始数据计算出欧拉角中yaw不准确啊，另外两个角度我测试是比较准确的，和手机上的对比过了，但是yaw测试相当不准确啊，网上查了资料说是必须要有磁阻才行啊，那难道必须得换MPU9150？
主题帖子积分
中级技术员, 积分 247, 距离下一级还需 53 积分
中级技术员, 积分 247, 距离下一级还需 53 积分
主题帖子积分
专家等级：结帖率：0%
主题帖子积分
中级技术员, 积分 247, 距离下一级还需 53 积分
中级技术员, 积分 247, 距离下一级还需 53 积分
源程序能共享吗，正想看啊，能发一份给我吗？
主题帖子积分
高级技术员, 积分 723, 距离下一级还需 277 积分
高级技术员, 积分 723, 距离下一级还需 277 积分
主题帖子积分
专家等级：结帖率：90%
主题帖子积分
高级技术员, 积分 723, 距离下一级还需 277 积分
高级技术员, 积分 723, 距离下一级还需 277 积分
本帖子中包含更多资源
才可以下载或查看，没有帐号？
主题帖子积分
主题帖子积分
专家等级：结帖率：0%打赏：0.00受赏：15.00
主题帖子积分
yaw没有磁传感器的校正，肯定和手机上的航向是对不上的．如果你只要ＹＡＷ，用指南针模块，５８８３就行了．
烈火DIY Mini四轴飞行器群:&&
主题帖子积分
高级技术员, 积分 723, 距离下一级还需 277 积分
高级技术员, 积分 723, 距离下一级还需 277 积分
主题帖子积分
专家等级：结帖率：90%
主题帖子积分
高级技术员, 积分 723, 距离下一级还需 277 积分
高级技术员, 积分 723, 距离下一级还需 277 积分
怎么回帖不显示？
主题帖子积分
实习生, 积分 15, 距离下一级还需 35 积分
实习生, 积分 15, 距离下一级还需 35 积分
主题帖子积分
专家等级：结帖率：0%
主题帖子积分
实习生, 积分 15, 距离下一级还需 35 积分
实习生, 积分 15, 距离下一级还需 35 积分
你确定你上传的源码可以在51上跑？
主题帖子积分
初级工程师, 积分 2374, 距离下一级还需 626 积分
初级工程师, 积分 2374, 距离下一级还需 626 积分
主题帖子积分
专家等级：结帖率：96%
主题帖子积分
初级工程师, 积分 2374, 距离下一级还需 626 积分
初级工程师, 积分 2374, 距离下一级还需 626 积分
能否指点一些问题啊............
努力学习，积极回复，只为申请版主.......
主题帖子积分
实习生, 积分 15, 距离下一级还需 35 积分
实习生, 积分 15, 距离下一级还需 35 积分
主题帖子积分
专家等级：结帖率：0%
主题帖子积分
实习生, 积分 15, 距离下一级还需 35 积分
实习生, 积分 15, 距离下一级还需 35 积分
额 最近也在做& &可是显示的数根本都是不变的
主题帖子积分
实习生, 积分 6, 距离下一级还需 44 积分
实习生, 积分 6, 距离下一级还需 44 积分
主题帖子积分
专家等级：结帖率：0%
主题帖子积分
实习生, 积分 6, 距离下一级还需 44 积分
实习生, 积分 6, 距离下一级还需 44 积分
初学者表示完全看不懂
主题帖子积分
实习生, 积分 3, 距离下一级还需 47 积分
实习生, 积分 3, 距离下一级还需 47 积分
主题帖子积分
专家等级：结帖率：0%
主题帖子积分
实习生, 积分 3, 距离下一级还需 47 积分
实习生, 积分 3, 距离下一级还需 47 积分
这源码不是51的。。。
主题帖子积分
资深工程师, 积分 11113, 距离下一级还需 8887 积分
资深工程师, 积分 11113, 距离下一级还需 8887 积分
主题帖子积分
专家等级：结帖率：22%打赏：0.20受赏：568.10
主题帖子积分
资深工程师, 积分 11113, 距离下一级还需 8887 积分
资深工程师, 积分 11113, 距离下一级还需 8887 积分
主题帖子积分
实习生, 积分 3, 距离下一级还需 47 积分
实习生, 积分 3, 距离下一级还需 47 积分
主题帖子积分
专家等级：结帖率：0%
主题帖子积分
实习生, 积分 3, 距离下一级还需 47 积分
实习生, 积分 3, 距离下一级还需 47 积分
确实不是51的
主题帖子积分
中级技术员, 积分 229, 距离下一级还需 71 积分
中级技术员, 积分 229, 距离下一级还需 71 积分
主题帖子积分
专家等级：结帖率：100%
主题帖子积分
中级技术员, 积分 229, 距离下一级还需 71 积分
中级技术员, 积分 229, 距离下一级还需 71 积分
YAW的解算需要加速度计和磁力计相结合，你可以自己推倒一下，看一些理论
主题帖子积分
初级工程师, 积分 2164, 距离下一级还需 836 积分
初级工程师, 积分 2164, 距离下一级还需 836 积分
主题帖子积分
专家等级：结帖率：0%
主题帖子积分
初级工程师, 积分 2164, 距离下一级还需 836 积分
初级工程师, 积分 2164, 距离下一级还需 836 积分
主题帖子积分
高级技术员, 积分 723, 距离下一级还需 277 积分
高级技术员, 积分 723, 距离下一级还需 277 积分
主题帖子积分
专家等级：结帖率：90%
主题帖子积分
高级技术员, 积分 723, 距离下一级还需 277 积分
高级技术员, 积分 723, 距离下一级还需 277 积分
这源码不是51的。。。
源码就是源码，谁说是51的？移植了才叫51的代码，那就不叫源码！搞程序的，怎么这个概念都混淆？
主题帖子积分
高级技术员, 积分 723, 距离下一级还需 277 积分
高级技术员, 积分 723, 距离下一级还需 277 积分
主题帖子积分
专家等级：结帖率：90%
主题帖子积分
高级技术员, 积分 723, 距离下一级还需 277 积分
高级技术员, 积分 723, 距离下一级还需 277 积分
确实不是51的
源码就是源码，谁说是51的？移植了才叫51的代码，那就不叫源码！搞程序的，怎么这个概念都混淆？
主题帖子积分
实习生, 积分 6, 距离下一级还需 44 积分
实习生, 积分 6, 距离下一级还需 44 积分
主题帖子积分
专家等级：结帖率：0%
主题帖子积分
实习生, 积分 6, 距离下一级还需 44 积分
实习生, 积分 6, 距离下一级还需 44 积分
你好&&可以分享下你51移值的程序吗？ @@.COM&&我移值数据全是3&&
主题帖子积分
技术达人, 积分 9193, 距离下一级还需 807 积分
技术达人, 积分 9193, 距离下一级还需 807 积分
主题帖子积分
专家等级：结帖率：5%打赏：0.00受赏：9.00
主题帖子积分
技术达人, 积分 9193, 距离下一级还需 807 积分
技术达人, 积分 9193, 距离下一级还需 807 积分
YAW的解算需要加速度计和磁力计相结合，你可以自己推倒一下，看一些理论
我发现用磁力计，横滚对YAW影响很大，俯仰影响很小(但也有微小的影响)，水平放置时测出的YAW很准确，倾斜后就会有变化，特别是横滚影响很大，是不是要用加速度去补偿？有没有具体算法？
主题帖子积分
中级技术员, 积分 229, 距离下一级还需 71 积分
中级技术员, 积分 229, 距离下一级还需 71 积分
主题帖子积分
专家等级：结帖率：100%
主题帖子积分
中级技术员, 积分 229, 距离下一级还需 71 积分
中级技术员, 积分 229, 距离下一级还需 71 积分
不对吧，解算不存在任何问题，只有在俯仰角为+-90度的时候，由于万向节锁，解不出来横滚和偏航，只能用四元数算
主题帖子积分
实习生, 积分 3, 距离下一级还需 47 积分
实习生, 积分 3, 距离下一级还需 47 积分
主题帖子积分
专家等级：结帖率：0%
主题帖子积分
实习生, 积分 3, 距离下一级还需 47 积分
实习生, 积分 3, 距离下一级还需 47 积分
大神，能不能给一下在51上跑的代码，我自己写的能输出原始数据，使用卡尔曼滤波函数和四元数软解后，输出全是0
技术高手奖章
人才类勋章
时间类勋章
涓涓之细流
发帖类勋章
时间类勋章
技术奇才奖章
人才类勋章
甘甜之泉水
发帖类勋章
希望之星奖章
等级类勋章
技术新星奖章
人才类勋章
时间类勋章
湍急之河流
发帖类勋章
精华达人奖章
等级类勋章
无冕之王奖章
等级类勋章
奔腾之江水
发帖类勋章
时间类勋章
技术导师奖章
人才类勋章
精英会员奖章
等级类勋章
热门推荐 /3查看: 10058|回复: 6
MPU6050 DMP已输出四元数，但是如何输出加速度和角速度？求各位大神解答，谢了！
这是MPU6050 DMP读取四元数程序
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);& & & &
if (sensors & INV_WXYZ_QUAT )
& &&&PrintChar(&in Calculating quaternion steps.....\n&);
& &&&q0=quat[0] / 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;
& &&&printf(&\r\npitch:%f\ roll:%f yaw:%f\r\n&,Pitch,Roll,Yaw);
& & delay_ms(500);
这是输出四元数和欧拉角。但是
if(sensors & INV_XYZ_GYRO)
if(sensors & INV_XYZ_ACCEL)
这两个函数怎么填写？
希望知道的人们帮一下小弟，再次谢过。
是基于STM32的，忘说了……
不是AVR吗？？？那算法可以直接用于AVR上面吗？
楼主解决这个问题了没，最近在做空中鼠标，也遇到了这个问题
等高人吧{:soso_e107:}
谢谢楼主，最近在用arduino做
貌似还原不了
Powered by之前做四轴的时候调试过姿态传感器，由于是穷逼学生，没钱买好的陀螺仪，买的是GY86模块，上面集成了一块六轴传感器MPU6050和一块HMC5883L磁力计。mpu6050计算出pitch和roll，用HMC5883L去结合pitch和roll修正yaw，完成姿态角的输出。
当时第一次接触姿态解算的时候使用的是原子中的例程，里面使用mpu6050自带的DMP进行解算，直接输出四元数，然后通过四元数转化转换为对应的姿态角，但是没有Z轴上的参考量，所以Yaw是不准的。讲真，个人感觉算出的姿态角还是有点稳定的，除了动态性能不好，恢复较慢，有时会挂之外。但是有一个很大缺点就是DMP库是不开源的，也没有参考资料，不知道里面怎么写的，解算出的姿态角是以上电时的平面为基准面的，又不能自己修改。这就很蛋疼，难道我每次上电都要把四轴摆放到绝对水平的位置吗？那岂不是稍微有点不平，起飞就炸机了= =。
后来就尝试使用了各种解算算法，互补滤波，一二阶，梯度向下等等，虽然解决了DMP解算的很多问题，但这个问题还是让我膈应了很久。
这段时间做比赛又需要调试陀螺仪了，所以想着之前没解决这个问题，索性就研究了一下午。大胆的拆开DMP库源码看了之后发现了问题。
在DMP初始化后面有一个函数run_self_test()，进去查看之后发现了这么一段代码
mpu_get_gyro_sens(&sens);
gyro[0] = (long)(gyro[0] * sens);
gyro[1] = (long)(gyro[1] * sens);
gyro[2] = (long)(gyro[2] * sens);
dmp_set_gyro_bias(gyro);
mpu_get_accel_sens(&accel_sens);
accel[0] *= accel_
accel[1] *= accel_
accel[2] *= accel_
dmp_set_accel_bias(accel);
研究了一下发现这应该是DMP修正偏置量（就是我们说的零飘）的过程，只不过把读出来的AD值转化了一下存到了寄存器中。
了解了这个，那么把基准坐标系转化到水平面上就很简单了（这个不清楚的话请查阅一下 “载体坐标系” 和 “世界坐标系”），测量出置于水平面上的偏置量，然后传进去就可以了
比如我是这样写的：
mpu_get_gyro_sens(&sens);
gyro[0] = (long)(GX_OFFSET * sens);
gyro[1] = (long)(GY_OFFSET * sens);
gyro[2] = (long)(GZ_OFFSET * sens);
dmp_set_gyro_bias(gyro);
mpu_get_accel_sens(&accel_sens);
accel[0] = AX_OFFSET * accel_
accel[1] = AY_OFFSET * accel_
accel[2] = AZ_OFFSET * accel_
dmp_set_accel_bias(accel);
大概思路就是这样，这样不管上电位置是怎样的，算出的角度仍旧是相对于水平面的坐标系。
至于怎么测量水平的偏置量之前没有说，在自检函数里有个获取上电水平偏置的函数，大家可以研究一下，很容易就可以获取到DMP寄存器中的水平偏置量，也可以参考下我的代码。
最后贴上自己的渣渣代码供大家参考一下（使用的战舰开发板，LCD屏幕显示，在git上自己下载~）。
本文已收录于以下专栏：
相关文章推荐
Motion Driver 是一款传感器驱动层程序，其配置简单，是InvenSense运动传感设备的片上数字移动处理器（DMP）的实际应用。Motion Driver 是嵌入式运动应用软件的一个子集，...
前些天，根据原子哥和火哥的MPU6050例程，自己仿写了一个IIC读MPU6050的程序，因为我要做的是小四轴，要姿态融合解算，只是单纯的原始数据没有实际价值，所以一开始我仿照crazepony的程序...
MCU：初步选型是STM32F103C8T6，64 Pin，淘宝价格大概8.xRMB一片
教你如何使用Arduino与惯性测量单元（IMU）：3D可视化 Arduino教程：MPU6050的数据获取、分析与处理
下载i2cdev库
/jrowberg...
玩四轴的都知道, MWC V2.5 飞控主板,板载陀螺仪传感器就是MPU6050.(不信你可以拿放大镜看). 
而默认的MWC开源程序是自己读取MPU6050的原始数据,经过,自己的一套算法.算出来...
MPU6050内带3轴陀螺仪和3轴加速度传感器，并且含有一个第二IIC接口，可用于连接外部磁力传感器，利用自带数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，...
玩四轴的都知道, MWC V2.5 飞控主板,板载陀螺仪传感器就是MPU6050.(不信你可以拿放大镜看).
而默认的MWC开源程序是自己读取MPU6050的原始数据,经过,自己的一套算法.算出来四...
要想知道MPU6050工作原理，得先了解下面俩个传感器：
①陀螺仪传感器：
陀螺仪的原理就是，一个旋转物体的旋转轴所指的方向在不受外力影响时，是不会改变的。人们根据这个道理，用它来保持方向。然后用多种...
一、MPU6050初始化
/**************************实现函数*****************************************...
他的最新文章
讲师：韦玮
讲师：陈守元
您举报文章：
举报原因：
原文地址：
原因补充：
(最多只允许输入30个字)3865人阅读
/*****************************************************
功能: 采集MPU6050 DMP 数据
CPU: STC89C54RD+
晶震: 11.0592
环境: Keli2.0
作者：XW1005
来源：移植 Jeff Jrowberg 公开的程序。
sda 或则scl 为高时是:释放总线
*****************************************************/
#include &reg52.h&
#include &intrins.h&
//应答标志位 0:ack 1：nak
sbit scl = P2^0;
sbit sda = P2^1;
unsigned char dmpdatas[42]; //DMP数据
#define SlaveAddress 0xD0 //IIC写入时的地址字节数据，+1为读取
//#define SlaveAddress 0xae //i2c测试地址
void SendByte(unsigned char dat);
unsigned char i2cread(unsigned char addr,unsigned char *Data);
unsigned char i2creads(unsigned char addr,unsigned char length,unsigned char *Data);
//以下的 firmware 及 config update 数据来自于 Jeff Jrowberg 公开的程序
code unsigned char dmpmemorydata[1929]={
// bank 0, 256 bytes
0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01,
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
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,
0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC,
0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10,
// bank 1, 256 bytes
0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8,
0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7,
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0,
// bank 2, 256 bytes
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 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, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 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, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
// bank 3, 256 bytes
0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F,
0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2,
0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF,
0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C,
0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1,
0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01,
0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80,
0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C,
0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80,
0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E,
0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9,
0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24,
0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0,
0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86,
0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86,
// bank 4, 256 bytes
0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA,
0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8,
0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3,
0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5,
0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3,
0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5,
0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D,
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D,
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,
0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8,
0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87,
// bank 5, 256 bytes
0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68,
0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D,
0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA,
0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56,
0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA,
0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A,
0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60,
0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97,
0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04,
0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78,
0x78, 0x9B, 0xF1, 0x1A, 0xB0, 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, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68,
// bank 6, 256 bytes
0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 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, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56,
0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD,
0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91,
0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8,
0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE,
0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9,
0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD,
0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E,
0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8,
0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89,
0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79,
// bank 7, 138 bytes (remainder)
0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8,
0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA,
0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB,
0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3,
0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3,
0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3,
0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC,
0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF
code unsigned char dmpcfgupddata[192] = {
dmp config
0x4C, 0xCD, 0x6C,
0x36, 0x56, 0x76,
0x02, 0xCB, 0x47, 0xA2,
0x00, 0x05, 0x8B, 0xC1,
0x00, 0x00, 0x00, 0x00,
0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97,
0x26, 0x46, 0x66,
0x20, 0x00,
0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x40, 0x00,
0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97,
0x0D, 0x35, 0x5D,
0x87, 0x2D, 0x35, 0x3D,
//这里是开启DMP的特殊中断的
//原程序中此行代码为(这里不一定错)
即LENGTH=0x00，有错
0xF1, 0x20, 0x28, 0x30, 0x38,
0xF1, 0x28, 0x30, 0x38,
0xF1, 0x28, 0x30, 0x38,
0x00, 0x01,
/* 上行最后一个数据调整FIFO rate :0x01=100HZ,0x02=66HZ,0x03=50HZ ,0x04=40HZ,0x05=33.33HZ,
// 可从 datasheet 公式推算
//dmp updates
0xFF, 0xFF,
0x09, 0x23, 0xA1, 0x35,
0x06, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x40, 0x00, 0x00, 0x00,
0x00, 0x00,
0x00, 0x40, 0x00, 0x00*/
code unsigned char dmpUpdates[47]={
0xFF, 0xFF,
0x09, 0x23, 0xA1, 0x35,
0x06, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x40, 0x00, 0x00, 0x00,
0x00, 0x00,
0x00, 0x40, 0x00, 0x00
void DelayUs2x(unsigned char t) //延时1
while(--t);
void DelayMs(unsigned char t) //延时2
while(t--)
//大致延时1mS
DelayUs2x(245);
DelayUs2x(245);
/*i2c基本协议*/
/*i2c基本协议*/
void i2_qs(void) //启始信号
_nop_();_nop_();
_nop_();_nop_();
void i2_tz() //停止信号
_nop_();_nop_();
_nop_();_nop_();
void i2_fs(unsigned char Data) //发送8位数据
unsigned char i=0;
for(;i&8;i++)
Data &&= 1;
_nop_();_nop_();
_nop_();_nop_();
//下面是接收从设备发回的应答信号
_nop_();_nop_();
_nop_();_nop_();
ack = //接收设备的ack _n
unsigned char i2_js(bit _ack) //接收8位数据
unsigned char i = 0,Data = 0;
for(;i&8;i++)
Data &&= 1;
_nop_();_nop_();
_nop_();_nop_();
Data |= //接收数据
//下面时主控器发送ACK或则NAK
sda = _ //ack或者nak
scl = 1; //拉高时钟产生第9个时钟
_nop_();_nop_();
_nop_();_nop_();
scl = 0; //拉底时钟
sda = 1; //释放总线
/*i2c协议基本*/
/*写入8位寄存器一个字节
参数 addr 寄存器地址
参数 Data 写入数据
返回值 (1=成功,0=失败)
unsigned char i2cwrite(unsigned char addr,unsigned char Data) //写入8位有效数据
i2_qs(); //起始信号
i2_fs(SlaveAddress); //设备地址+写信号
if(ack) return 0; //判断是否是ACK如果不是返回0
i2_fs(addr); //设备内部地址
if(ack) return 0;
i2_fs(Data); //写入的数据
if(ack) return 0;
i2_tz(); //停止信号
/*读取8位寄存器一个字节
参数 addr 寄存器地址
参数 *Data 数据存储地址
返回值 (1=成功,0=失败)
unsigned char i2cread(unsigned char addr,unsigned char *Data)
/*unsigned char Data1;
i2_qs(); //起始信号
i2_fs(SlaveAddress); //设备地址+写信号
SendByte(ack);
i2_fs(addr); //设备内部地址
i2_qs(); //起始信号
i2_fs(SlaveAddress+1); //设备地址+读信号
*Data = i2_js(1); //读取数据
i2_tz(); //停止信号*/
return i2creads(addr,1,Data);
/*读取8位寄存器多个字节
* @参数 addr
I2C从器件内部地址
* @参数 length
写入数据长度
* @参数 Data
保存数据的地址
* @返回值 返回状态 (1=成功)
unsigned char i2creads(unsigned char addr,unsigned char length,unsigned char *Data)
length --;
i2_qs(); //起始信号
i2_fs(SlaveAddress); //设备地址+写信号
if(ack) return 0;
i2_fs(addr); //设备内部地址
if(ack) return 0;
i2_qs(); //起始信号
i2_fs(SlaveAddress+1); //设备地址+读信号
if(ack) return 0;
for(i=0;i&i++)
Data[i]= i2_js(0); //读取数据,发送ACK
Data[length] = i2_js(1);//读取数据,发送NAK
i2_tz(); //停止信号
/**写入8位寄存器的一个位。
* @参数 addr
I2C从器件内部地址
* @参数 bitNum
写入的比特位(0-7)
* @参数 data
* @返回值 返回状态 (1=成功)
unsigned char writeBit(unsigned char addr,unsigned char bitNum,unsigned char Data)
if (i2cread(addr,&b))
b = (Data != 0) ? (b | (1 && bitNum)):(b & ~(1 && bitNum));
return i2cwrite(addr,b); //写入数据
/**写入8位寄存器的多个位。
* @参数 addr
I2C从器件内部地址
* @参数 bitStart 第一位的写入位置（0-7）
* @参数 length
写的比特数(不超过8)
* @参数 Data
* @返回值 返回状态 (1=成功)
unsigned char writeBits(unsigned char addr,unsigned char bitStart,unsigned char length,unsigned char Data)
010 要写入的值
args: bitStart=4, length=3
原始值（样本）
原始值 & ~掩码
掩码 | 原始值
unsigned char b,mask=0;
if (i2cread(addr,&b))
mask = (((1&&length) - 1) && (bitStart-length + 1)); //掩码
Data &&=(bitStart - length + 1); //把写入的数据移动到位
b &= ~(mask);
i2cwrite(addr,b); //写入数据
/**读取一个位从8位器件的寄存器。
* @参数 addr
I2C从器件内部地址
* @参数 bitNum
位的位置来读取（0-7）
* @参数 *data
数据存储地址
* @返回值（1=成功）
unsigned char readBit(unsigned char addr,unsigned char bitNum,unsigned char *Data)
if(i2cread(addr,&b))
*Data = b & (1 && bitNum);
/**读取8位寄存器的多个位。
* @参数 addr
I2C从器件内部地址
* @参数 bitStart第一位的位置读取（0-7）
* @参数 length
位读取@参数长度数（不超过8）
* @参数 *data
数据存储地址（即'101'任何bitStart位置读取将等于0X05）
* @返回值（1=成功）
unsigned char readBits(unsigned char addr,unsigned char bitStart,unsigned char length,unsigned char *Data)
args: bitStart=4, length=3
-& 010 shifted
unsigned char b,mask=0;
if(i2cread(addr,&b))
mask = ((1 && length) - 1) && (bitStart - length + 1);
b &&= (bitStart - length + 1);
/*void init6050() //初始化 6050
i2cwrite(0x6b,0x80);
//电源管理（0x00 正常启用）
i2cwrite(0x6b ,0x01);
//No sleep
, 3 PLL with X axis gyroscope reference电源管理
DelayMs(200);
i2cwrite(0x23 ,0x00);
,temp,gx,gy,gz,accel？
i2cwrite(0x19 ,0x05);
//陀螺仪采样率
i2cwrite(0x1a ,0x03);
//低通滤波
i2cwrite(0x1b ,0x18);
//陀螺仪自检，量程范围 2000 /s
i2cwrite(0x1c ,0x00);
//加速计自检，量程范围及 高通滤波频率 +-2g/s
加载 DMP代码到
(1=成功,0=失败)
unsigned char loadfirmware(void)
unsigned int datanum=0; //DMP固件写入标志位
unsigned char ye,i,j;
unsigned char bank=0; //段（256个数据一段）
unsigned char addr=0;
for(;bank&8;bank++)
if(bank == 7) //这里的作用就是区分最后一段数据
for(ye=0;ye&i;ye++)
i2cwrite(0x6d,bank);
i2cwrite(0x6e,addr);
i2_qs(); //起始信号
i2_fs(SlaveAddress); //设备地址+写信号
i2_fs(0x6f); //设备内部地址
for(j=0;j&16;j++)
i2_fs(dmpmemorydata[datanum++]); //写入DMP的数据
if(ack) return 0;
addr += 16;
i2_tz(); //停止信号
i2cwrite(0x6d,7);
i2cwrite(0x6e,addr);
i2_qs(); //起始信号
i2_fs(SlaveAddress); //设备地址+写信号
i2_fs(0x6f); //设备内部地址
for(i=0;i&9;i++)
i2_fs(dmpmemorydata[datanum++]); //写入DMP的数据
if(ack) return 0;
i2_tz(); //停止信号
unsigned char loadcfgupd(void) //DMP设置
//一共需要写入30条设置数据
unsigned char datacounts=0; //DMP设置数据标志位
unsigned char bytes2 //数据长度。
//偏移地址
unsigne //数据写入标志与bytes2write一同使用
for (line=0;line&30;line++)
bank=dmpcfgupddata[datacounts++];
offset=dmpcfgupddata[datacounts++];
bytes2write=dmpcfgupddata[datacounts++];
i2cwrite(0x6d,bank);
i2cwrite(0x6e,offset);
i2_qs(); //起始信号
i2_fs(SlaveAddress); //设备地址+写信号
i2_fs(0x6f); //设备内部地址
for(writingcounts=0;writingcounts&bytes2writingcounts++)
i2_fs(dmpcfgupddata[datacounts++]); //写入DMP配置数据
if(ack) return 0;
if(0 == bytes2write)
special=dmpcfgupddata[datacounts++];
if(0x01 == special)
//设置零运动中断启用（真）;
//设置FIFO缓冲区溢出启用（真）;
//设置DMP启用（真）;
i2cwrite(0x38,0x32);
i2_tz(); //停止信号
/*最后更新DMP*/
unsigned char xdmpUpdates(unsigned char datacounts)
unsigned char writingcounts,bank,offset,bytes2
bank=dmpUpdates[datacounts++];
offset=dmpUpdates[datacounts++];
bytes2write=dmpUpdates[datacounts++];
i2cwrite(0x6d,bank);
i2cwrite(0x6e,offset);
i2_qs(); //起始信号
i2_fs(SlaveAddress); //设备地址+写信号
i2_fs(0x6f); //设备内部地址
for(writingcounts=0;writingcounts&bytes2writingcounts++)
i2_fs(dmpUpdates[datacounts++]); //写入DMP配置数据
if(ack) return 0;
i2_tz(); //停止信号
/*读取 FIFO 计数*/
unsigned int getFIFOCount()
unsigned char i[2];
i2creads(0x72,2,i);
return ((i[0]&&8)+i[1]);
/*FIFO数据读取
参数 *Data 存储数据的地址
返回值 (1=读取成功,0读取失败)
unsigned char readdmp(unsigned char *Data)
return i2creads(0x74,42,Data);
//加载并配置 DMP 数字运动处理引擎
unsigned char dmpInitialize(void)
unsigned char hwRevision,otpValid,mpuIntStatus/*fifoBuffer[128]*/;
unsigned char xgOffsetTC,ygOffsetTC,zgOffsetTC;
unsigned int fifoC
writeBit(0x6B,7,1); //复位 MPU6050
DelayMs(30);
writeBit(0x6B,6,0); //禁止睡眠模式
i2cwrite(0x6D,0x70); //写入一个字节数据到0x6d寄存器(选择用户 bank)
i2cwrite(0x6E,0x06); //写入一个字节数据到0x6e寄存器(选择存储字节)
i2cread(0x6F,&hwRevision); //读取
i2cwrite(0x6D,0); //重置内存 bank 选择
readBit(0x00,0,&otpValid); //读取 OTP bank 有效标志
readBits(0x00,6,6,&xgOffsetTC); //读陀螺偏置TC值 X
readBits(0x01,6,6,&ygOffsetTC); //读陀螺偏置TC值 Y)
readBits(0x02,6,6,&zgOffsetTC); //读陀螺偏置TC值 Z
i2cwrite(0x25,0x7f); //设置从0地址 0x7
writeBit(0x6A,5,0); //禁用I2C主模式
i2cwrite(0x25,0x68); //这里可能要改。还没有弄明白这里
writeBit(0x6A,1,1); //I2C总线主控复位
DelayMs(20);
if((loadfirmware()) == 0 ) return 0; //加载 DMP代码到内存
if((loadcfgupd()) == 0 ) return 0; //配制DMP
writeBits(0x6B,2,3,0x03); //设置时钟脉冲源Z陀螺
i2cwrite(0x38,0x12); //设置DMP和FIFO_OFLOW启用中断
i2cwrite(0x19,4); //设置采样率为200 hz
(1khz / (1 + 4) = 200 Hz)
writeBits(0x1A,5,3,0x1); //设置外部帧同步TEMP_OUT_L[0]
writeBits(0x1A,2,3,0x03); //设置DLPF带宽42赫兹
writeBits(0x1B,4,2,0x03); //陀螺灵敏度设置为+ / - 2000 deg/sec
i2cwrite(0x70,0x03); //设置DMP配置字节（功能未知）
i2cwrite(0x71,0x00); //设置DMP配置字节（功能未知）
writeBit(0x00,0,0); //清除OTP Bank 标志
writeBits(0x00,6,6,xgOffsetTC); //设置X 陀螺抵消TCs之前的值
writeBits(0x01,6,6,ygOffsetTC); //设置Y 陀螺抵消TCs之前的值
writeBits(0x02,6,6,zgOffsetTC); //设置Z 陀螺抵消TCs之前的值
xdmpUpdates(0); //最后更新1/7(函数未知)dmpUpdates数组第一行
xdmpUpdates(5); //最后更新2/7(函数未知)dmpUpdates数组第二行
writeBit(0x6A,2,1); //复位 FIFO
fifoCount = getFIFOCount(); //读取 FIFO 计数
//readdmp(fifoCount,fifoBuffer); //读取FIFO里的数据
writeBit(0x6A,2,1); //复位 FIFO
i2cwrite(0x1F,2); //运动检测阈值设置为2
i2cwrite(0x21,156); //零运动检测阈值为156
i2cwrite(0x20,80); //设置运动检测持续时间至80
i2cwrite(0x22,0); //设置零运动检测时间0
writeBit(0x6A,2,1); //复位 FIFO
writeBit(0x6A,6,1); //使能 FIFO
writeBit(0x6A,7,1); //使能 DMP
writeBit(0x6A,3,1); //复位 DMP
xdmpUpdates(12); //最后更新3/7(函数未知)dmpUpdates数组第三行
xdmpUpdates(17); //最后更新4/7(函数未知)dmpUpdates数组第四行
xdmpUpdates(28); //最后更新5/7(函数未知)dmpUpdates数组第五行
while((fifoCount = getFIFOCount()) & 3); //等待 FIFO 计数 & 2
writeBit(0x6A,2,1); //复位 FIFO
//readdmp(fifoCount,fifoBuffer); //读取FIFO里的数据
i2cread(0x3A,&mpuIntStatus); //读取中断状态
xdmpUpdates(35); //最后更新6/7(函数未知)dmpUpdates数组第六行
while((fifoCount = getFIFOCount()) & 3); //等待 FIFO 计数 & 2
writeBit(0x6A,2,1); //复位 FIFO
//readdmp(fifoCount,fifoBuffer); //读取FIFO里的数据
i2cread(0x3A,&mpuIntStatus); //读取中断状态
xdmpUpdates(40); //最后更新7/7(函数未知)dmpUpdates数组第七行
writeBit(0x6A,7,0); //禁用DMP(稍后您打开它)
writeBit(0x6A,2,1); //复位 FIFO
i2cread(0x3A,&mpuIntStatus);
//星期六 ()
/*初始化MPU6050*/
void initMPU6050(void)
writeBits (0x6B,2,3,0x01); //电源管理
writeBits (0x1B,4,2,0x00); //设置陀螺仪量程 250/s
writeBits (0x1C,4,2,0x00); //设置加速度量程 2G
writeBit (0x6B,6,1); //电源管理MUP进入睡眠模式
/*验证MPU6050连接*/
unsigned char getDeviceID(void)
unsigned char b=0; //临时变量
readBits(0x75,6,6,&b); //读取i2c固定地址，去掉最高位和最低位这两位数据
return b == 0x34; //判断B是否等于0x34，如果等于返回1，不等于返回0(库的是0x38)
/**************************************串口**********************************/
(void) //初始化串口
// SCON: 模式 1, 8-bit UART, 使能接收
TMOD |= 0x20;
// TMOD: timer 1, mode 2, 8-bit 重装
重装值 9600 波特率 晶振 11.0592MHz
timer 1 打开
//打开总中断
//打开串口中断
void SendByte(unsigned char dat) //发送一个字符
SBUF = //SBUF 串行数据缓冲器
while(!TI);
//TI发送中断标志位 (当数据发送完毕后由硬件置 1 否则等待硬件置 1)
/************************************************************************************/
void main(void)
unsigned char zd,i;
CSH(); //初始化串口
initMPU6050(); //初始化
if (getDeviceID()) //验证连接是否正常(读取MPU6050的I2C地址)
if(!(dmpInitialize())) //加载并配置运动库
writeBit(0x6A,2,1); //复位 FIFO
writeBit(0x6A,7,1); //使能DMP
i=getFIFOCount();//读取FIFO计数
i2cread(0x3A,&zd); //读取中断状态
if((zd & 0x10)||i==1024) //判断FIFO是否溢出
writeBit(0x6A,2,1); //复位 FIFO
else if (zd & 0x02)
while(i&42) i=getFIFOCount();
readdmp(dmpdatas); //读取FIFO数据(四元数+其他的数据)
SendByte(dmpdatas[0]);
SendByte(dmpdatas[1]);
SendByte(dmpdatas[4]);
SendByte(dmpdatas[5]);
SendByte(dmpdatas[8]);
SendByte(dmpdatas[9]);
SendByte(dmpdatas[12]);
SendByte(dmpdatas[13]);
&/pre&&pre name=&code& class=&objc&&完整代码资料：/page/view?key=7c13c486bb08d948-
&/pre&&pre name=&code& class=&objc&&
&&相关文章推荐
* 以上用户言论只代表其个人观点，不代表CSDN网站的观点或立场
访问：3888次
排名：千里之外
(window.slotbydup = window.slotbydup || []).push({
id: '4740881',
container: s,
size: '200,200',
display: 'inlay-fix'