MPU9250传感器

MPU9250 内部包括 3 轴陀螺仪、3 轴加速度计和 3 轴磁力计，这3个功能输出都是 16 位的数字量; 可以通过常用的数据总线( IIC) 接口和单片机进行数据交互，传输速率 400 kHz /s。陀螺仪的角速度测量范围±2000（° /s），具有良好的动态响应特性。加速度计的测量范围最大为±16g( g 为重力加速度)，静态测量精度高。磁力计采用高灵度霍尔型传感器进行数据采集，磁感应强度测量范围为±4800μT，可用于对偏航角的辅助测量。
MPU9250 自带的数字运动处理器DMP硬件加速引擎，可以整合九轴传感器数据，向应用端输出完整的 9 轴融合演算数据。 有了 DMP，我们可以使用运动处理库非常方便的实现姿态解算，降低了运动处理运算对操作系统的负荷，同时大大降低了开发难度。

三轴陀螺仪

MPU9250陀螺仪是由三个独立检测X, Y, Z轴的MEMS组成。检测每个轴的转动(一但某个轴发生变化,相应的电容传感器会发生相应的变化,产生的信号被放大,调解,滤波,最后产生个与角速率成正比的电压,然后将每一个轴的电压转换成16位的数据。ADC的采样速率也是可编程的,从每秒3.9-8000个。

三轴加速度

MPU9250的三轴加速度也是单独分开测量的。根据每个轴上的电容来测量轴的偏差度。结构上降低了各种因素造成的测量偏差。加速度计的校准是根据工厂的标准来设定的,电源电压也许和你用的不一样。每一个传感器都有专门的ADC来提供数字性的输出。

三轴磁力计

三轴磁力计采用高精度的霍尔效应传感器,通过驱动电路,信号放大和计算电路来处理信号来采集地磁场在X, Y, Z轴上的电磁强度。

IIC通信

MPU9250的电路图连接如下

 

我们使用IIC让MPU9250和单片机通信，并且输出获取到的传感器值。
IIC数据总线是由两根通信线组成，必要的是包含一个主控制器件和多个从控制器件，不同的从器件通过地址与主器件通信。
实际使用中，一般是单片机作为主机，其它器件作为从机，单片机先向器件发送信息表示要读取数据，之后转变传输方向，器件发送数据到单片机。
 
在通信时，IIC通信线只有只有两根，数据线SDA的高低电平传输2进制的数据，时钟线SCL通过方波信号提供时钟节拍。在时钟的高电平周期内，SDA线上的数据必须保持稳定，数据线仅可以在时钟SCL为低电平时改变。
IIC的通信数据包含起始信号应答信号和结束信号等。
其中起始信号产生的条件是当SCL为高电平的时候，SDA线上由高到低的跳变被定义为起始条件。结束信号产生的条件是SCL为高电平的时候，SDA线上由低到高的跳变被定义为停止条件。
从机应答主机所需要的时钟仍是主机提供的，应答出现在每一次主机完成8个数据位传输后紧跟着的时钟周期，低电平0表示应答，1表示非应答。
关于通信协议具体的内容，可以网上找找详细介绍。作为嵌入式软件工程师，这些常用协议一定要去仔细研究一下，只有理解了协议才能在程序上理清协议实现的逻辑。

程序

由于使用IIC通信协议控制MPU9250，我们需要实现IIC协议。
代码参考正点原子的源码，封装好的函数用起来比较高效。

1. void IIC_Init(void)
2. {
3. GPIO_InitTypeDef GPIO_Initure;
4.  
5. __HAL_RCC_GPIOA_CLK_ENABLE(); //使能GPIOB时钟
6.  
7. //PH4,5初始化设置
8. GPIO_Initure.Pin=GPIO_PIN_4|GPIO_PIN_5;
9. GPIO_Initure.Mode=GPIO_MODE_OUTPUT_PP; //推挽输出
10. GPIO_Initure.Pull=GPIO_PULLUP; //上拉
11. GPIO_Initure.Speed=GPIO_SPEED_FAST; //快速
12. HAL_GPIO_Init(GPIOA,&GPIO_Initure);
13.  
14. IIC_SDA=1;
15. IIC_SCL=1;
16. }
17.  
18. //产生IIC起始信号
19. void IIC_Start(void)
20. {
21. SDA_OUT(); //sda线输出
22. IIC_SDA=1;
23. IIC_SCL=1;
24. delay_us(4);
25. IIC_SDA=0;//START:when CLK is high,DATA change form high to low
26. delay_us(4);
27. IIC_SCL=0;//钳住I2C总线，准备发送或接收数据
28. }
29. //产生IIC停止信号
30. void IIC_Stop(void)
31. {
32. SDA_OUT();//sda线输出
33. IIC_SCL=0;
34. IIC_SDA=0;//STOP:when CLK is high DATA change form low to high
35. delay_us(4);
36. IIC_SCL=1;
37. delay_us(4);
38. IIC_SDA=1;//发送I2C总线结束信号
39. }
40. //等待应答信号到来
41. //返回值：1，接收应答失败
42. // 0，接收应答成功
43. u8 IIC_Wait_Ack(void)
44. {
45. u8 ucErrTime=0;
46. SDA_IN(); //SDA设置为输入
47. IIC_SDA=1;delay_us(1);
48. IIC_SCL=1;delay_us(1);
49. while(READ_SDA)
50. {
51. ucErrTime++;
52. if(ucErrTime>250)
53. {
54. IIC_Stop();
55. return 1;
56. }
57. }
58. IIC_SCL=0;//时钟输出0
59. return 0;
60. }
61. //产生ACK应答
62. void IIC_Ack(void)
63. {
64. IIC_SCL=0;
65. SDA_OUT();
66. IIC_SDA=0;
67. delay_us(2);
68. IIC_SCL=1;
69. delay_us(2);
70. IIC_SCL=0;
71. }
72. //不产生ACK应答
73. void IIC_NAck(void)
74. {
75. IIC_SCL=0;
76. SDA_OUT();
77. IIC_SDA=1;
78. delay_us(2);
79. IIC_SCL=1;
80. delay_us(2);
81. IIC_SCL=0;
82. }
83. //IIC发送一个字节
84. //返回从机有无应答
85. //1，有应答
86. //0，无应答
87. void IIC_Send_Byte(u8 txd)
88. {
89. u8 t;
90. SDA_OUT();
91. IIC_SCL=0;//拉低时钟开始数据传输
92. for(t=0;t<8;t++)
93. {
94. IIC_SDA=(txd&0x80)>>7;
95. txd<<=1;
96. delay_us(2); //对TEA5767这三个延时都是必须的
97. IIC_SCL=1;
98. delay_us(2);
99. IIC_SCL=0;
100. delay_us(2);
101. }
102. }
103. //读1个字节，ack=1时，发送ACK，ack=0，发送nACK
104. u8 IIC_Read_Byte(unsigned char ack)
105. {
106. unsigned char i,receive=0;
107. SDA_IN();//SDA设置为输入
108. for(i=0;i<8;i++ )
109. {
110. IIC_SCL=0;
111. delay_us(2);
112. IIC_SCL=1;
113. receive<<=1;
114. if(READ_SDA)receive++;
115. delay_us(1);
116. }
117. if (!ack)
118. IIC_NAck();//发送nACK
119. else
120. IIC_Ack(); //发送ACK
121. return receive;
122. }
123. u8 MPU9250_Init(void)
124. {
125. u8 res=0;
126. IIC_Init(); //初始化IIC总线
127. MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X80);//复位MPU9250
128. delay_ms(100); //延时100ms
129. MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X00);//唤醒MPU9250
130. MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
131. MPU_Set_Accel_Fsr(0); //加速度传感器,±2g
132. MPU_Set_Rate(50); //设置采样率50Hz
133. MPU_Write_Byte(MPU9250_ADDR,MPU_INT_EN_REG,0X00); //关闭所有中断
134. MPU_Write_Byte(MPU9250_ADDR,MPU_USER_CTRL_REG,0X00);//I2C主模式关闭
135. MPU_Write_Byte(MPU9250_ADDR,MPU_FIFO_EN_REG,0X00); //关闭FIFO
136. MPU_Write_Byte(MPU9250_ADDR,MPU_INTBP_CFG_REG,0X82);//INT引脚低电平有效，开启bypass模式，可以直接读取磁力计
137. res=MPU_Read_Byte(MPU9250_ADDR,MPU_DEVICE_ID_REG); //读取MPU6500的ID
138. if(res==MPU6500_ID) //器件ID正确
139. {
140. MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考
141. MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
142. MPU_Set_Rate(50); //设置采样率为50Hz
143. }else return 1;
144.  
145. res=MPU_Read_Byte(AK8963_ADDR,MAG_WIA); //读取AK8963 ID
146. if(res==AK8963_ID)
147. {
148. MPU_Write_Byte(AK8963_ADDR,MAG_CNTL1,0X11); //设置AK8963为单次测量模式
149. }else return 1;
150.  
151. return 0;
152. }
153.  
154. //设置MPU9250陀螺仪传感器满量程范围
155. //fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
156. //返回值:0,设置成功
157. // 其他,设置失败
158. u8 MPU_Set_Gyro_Fsr(u8 fsr)
159. {
160. return MPU_Write_Byte(MPU9250_ADDR,MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
161. }
162. //设置MPU9250加速度传感器满量程范围
163. //fsr:0,±2g;1,±4g;2,±8g;3,±16g
164. //返回值:0,设置成功
165. // 其他,设置失败
166. u8 MPU_Set_Accel_Fsr(u8 fsr)
167. {
168. return MPU_Write_Byte(MPU9250_ADDR,MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
169. }
170.  
171. //设置MPU9250的数字低通滤波器
172. //lpf:数字低通滤波频率(Hz)
173. //返回值:0,设置成功
174. // 其他,设置失败
175. u8 MPU_Set_LPF(u16 lpf)
176. {
177. u8 data=0;
178. if(lpf>=188)data=1;
179. else if(lpf>=98)data=2;
180. else if(lpf>=42)data=3;
181. else if(lpf>=20)data=4;
182. else if(lpf>=10)data=5;
183. else data=6;
184. return MPU_Write_Byte(MPU9250_ADDR,MPU_CFG_REG,data);//设置数字低通滤波器
185. }
186.  
187. //设置MPU9250的采样率(假定Fs=1KHz)
188. //rate:4~1000(Hz)
189. //返回值:0,设置成功
190. // 其他,设置失败
191. u8 MPU_Set_Rate(u16 rate)
192. {
193. u8 data;
194. if(rate>1000)rate=1000;
195. if(rate<4)rate=4;
196. data=1000/rate-1;
197. data=MPU_Write_Byte(MPU9250_ADDR,MPU_SAMPLE_RATE_REG,data); //设置数字低通滤波器
198. return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
199. }
200. //得到陀螺仪值(原始值)
201. //gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
202. //返回值:0,成功
203. // 其他,错误代码
204. u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
205. {
206. u8 buf[6],res;
207. res=MPU_Read_Len(MPU9250_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
208. if(res==0)
209. {
210. *gx=((u16)buf[0]<<8)|buf[1];
211. *gy=((u16)buf[2]<<8)|buf[3];
212. *gz=((u16)buf[4]<<8)|buf[5];
213. }
214. return res;;
215. }
216. //得到加速度值(原始值)
217. //gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
218. //返回值:0,成功
219. // 其他,错误代码
220. u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
221. {
222. u8 buf[6],res;
223. res=MPU_Read_Len(MPU9250_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
224. if(res==0)
225. {
226. *ax=((u16)buf[0]<<8)|buf[1];
227. *ay=((u16)buf[2]<<8)|buf[3];
228. *az=((u16)buf[4]<<8)|buf[5];
229. }
230. return res;;
231. }
232.  
233. //得到磁力计值(原始值)
234. //mx,my,mz:磁力计x,y,z轴的原始读数(带符号)
235. //返回值:0,成功
236. // 其他,错误代码
237. u8 MPU_Get_Magnetometer(short *mx,short *my,short *mz)
238. {
239. u8 buf[6],res;
240. res=MPU_Read_Len(AK8963_ADDR,MAG_XOUT_L,6,buf);
241. if(res==0)
242. {
243. *mx=((u16)buf[1]<<8)|buf[0];
244. *my=((u16)buf[3]<<8)|buf[2];
245. *mz=((u16)buf[5]<<8)|buf[4];
246. }
247. MPU_Write_Byte(AK8963_ADDR,MAG_CNTL1,0X11); //AK8963每次读完以后都需要重新设置为单次测量模式
248. return res;;
249. }
250.  
251. //IIC连续写
252. //addr:器件地址
253. //reg:寄存器地址
254. //len:写入长度
255. //buf:数据区
256. //返回值:0,正常
257. // 其他,错误代码
258. u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
259. {
260. u8 i;
261. IIC_Start();
262. IIC_Send_Byte((addr<<1)|0); //发送器件地址+写命令
263. if(IIC_Wait_Ack()) //等待应答
264. {
265. IIC_Stop();
266. return 1;
267. }
268. IIC_Send_Byte(reg); //写寄存器地址
269. IIC_Wait_Ack(); //等待应答
270. for(i=0;i<len;i++)
271. {
272. IIC_Send_Byte(buf[i]); //发送数据
273. if(IIC_Wait_Ack()) //等待ACK
274. {
275. IIC_Stop();
276. return 1;
277. }
278. }
279. IIC_Stop();
280. return 0;
281. }
282.  
283. //IIC连续读
284. //addr:器件地址
285. //reg:要读取的寄存器地址
286. //len:要读取的长度
287. //buf:读取到的数据存储区
288. //返回值:0,正常
289. // 其他,错误代码
290. u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
291. {
292. IIC_Start();
293. IIC_Send_Byte((addr<<1)|0); //发送器件地址+写命令
294. if(IIC_Wait_Ack()) //等待应答
295. {
296. IIC_Stop();
297. return 1;
298. }
299. IIC_Send_Byte(reg); //写寄存器地址
300. IIC_Wait_Ack(); //等待应答
301. IIC_Start();
302. IIC_Send_Byte((addr<<1)|1); //发送器件地址+读命令
303. IIC_Wait_Ack(); //等待应答
304. while(len)
305. {
306. if(len==1)*buf=IIC_Read_Byte(0);//读数据,发送nACK
307. else *buf=IIC_Read_Byte(1); //读数据,发送ACK
308. len--;
309. buf++;
310. }
311. IIC_Stop(); //产生一个停止条件
312. return 0;
313. }
314.  
315. //IIC写一个字节
316. //devaddr:器件IIC地址
317. //reg:寄存器地址
318. //data:数据
319. //返回值:0,正常
320. // 其他,错误代码
321. u8 MPU_Write_Byte(u8 addr,u8 reg,u8 data)
322. {
323. IIC_Start();
324. IIC_Send_Byte((addr<<1)|0); //发送器件地址+写命令
325. if(IIC_Wait_Ack()) //等待应答
326. {
327. IIC_Stop();
328. return 1;
329. }
330. IIC_Send_Byte(reg); //写寄存器地址
331. IIC_Wait_Ack(); //等待应答
332. IIC_Send_Byte(data); //发送数据
333. if(IIC_Wait_Ack()) //等待ACK
334. {
335. IIC_Stop();
336. return 1;
337. }
338. IIC_Stop();
339. return 0;
340. }
341.  
342. //IIC读一个字节
343. //reg:寄存器地址
344. //返回值:读到的数据
345. u8 MPU_Read_Byte(u8 addr,u8 reg)
346. {
347. u8 res;
348. IIC_Start();
349. IIC_Send_Byte((addr<<1)|0); //发送器件地址+写命令
350. IIC_Wait_Ack(); //等待应答
351. IIC_Send_Byte(reg); //写寄存器地址
352. IIC_Wait_Ack(); //等待应答
353. IIC_Start();
354. IIC_Send_Byte((addr<<1)|1); //发送器件地址+读命令
355. IIC_Wait_Ack(); //等待应答
356. res=IIC_Read_Byte(0); //读数据,发送nACK
357. IIC_Stop(); //产生一个停止条件
358. return res;
359. }

下面的代码是控制MPU9250的关键代码，是针对芯片本身的。也是主要代码。
首先初始化DMP

1. u8 mpu_dmp_init(void)
2. {
3. u8 res=0;
4. struct int_param_s int_param;
5. unsigned char accel_fsr;
6. unsigned short gyro_rate, gyro_fsr;
7. unsigned short compass_fsr;
8.  
9. IIC_Init(); //初始化IIC总线
10. if(mpu_init(&int_param)==0) //初始化MPU9250
11. {
12. res=inv_init_mpl(); //初始化MPL
13. if(res)return 1;
14. inv_enable_quaternion();
15. inv_enable_9x_sensor_fusion();
16. inv_enable_fast_nomot();
17. inv_enable_gyro_tc();
18. inv_enable_vector_compass_cal();
19. inv_enable_magnetic_disturbance();
20. inv_enable_eMPL_outputs();
21. res=inv_start_mpl(); //开启MPL
22. if(res)return 1;
23. res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL|INV_XYZ_COMPASS);//设置所需要的传感器
24. if(res)return 2;
25. res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); //设置FIFO
26. if(res)return 3;
27. res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率
28. if(res)return 4;
29. res=mpu_set_compass_sample_rate(1000/COMPASS_READ_MS); //设置磁力计采样率
30. if(res)return 5;
31. mpu_get_sample_rate(&gyro_rate);
32. mpu_get_gyro_fsr(&gyro_fsr);
33. mpu_get_accel_fsr(&accel_fsr);
34. mpu_get_compass_fsr(&compass_fsr);
35. inv_set_gyro_sample_rate(1000000L/gyro_rate);
36. inv_set_accel_sample_rate(1000000L/gyro_rate);
37. inv_set_compass_sample_rate(COMPASS_READ_MS*1000L);
38. inv_set_gyro_orientation_and_scale(
39. inv_orientation_matrix_to_scalar(gyro_orientation),(long)gyro_fsr<<15);
40. inv_set_accel_orientation_and_scale(
41. inv_orientation_matrix_to_scalar(gyro_orientation),(long)accel_fsr<<15);
42. inv_set_compass_orientation_and_scale(
43. inv_orientation_matrix_to_scalar(comp_orientation),(long)compass_fsr<<15);
44.  
45.  
46. res=dmp_load_motion_driver_firmware(); //加载dmp固件
47. if(res)return 6;
48. res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
49. if(res)return 7;
50. res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能
51. DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
52. DMP_FEATURE_GYRO_CAL);
53. if(res)return 8;
54. res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz)
55. if(res)return 9;
56. res=run_self_test(); //自检
57. if(res)return 10;
58. res=mpu_set_dmp_state(1); //使能DMP
59. if(res)return 11;
60. }
61. return 0;
62. }

获取mp1的数据

1. u8 mpu_mpl_get_data(float *pitch,float *roll,float *yaw)
2. {
3. unsigned long sensor_timestamp,timestamp;
4. short gyro[3], accel_short[3],compass_short[3],sensors;
5. unsigned char more;
6. long compass[3],accel[3],quat[4],temperature;
7. long data[9];
8. int8_t accuracy;
9.  
10. if(dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors,&more))return 1;
11.  
12. if(sensors&INV_XYZ_GYRO)
13. {
14. inv_build_gyro(gyro,sensor_timestamp); //把新数据发送给MPL
15. mpu_get_temperature(&temperature,&sensor_timestamp);
16. inv_build_temp(temperature,sensor_timestamp); //把温度值发给MPL，只有陀螺仪需要温度值
17. }
18.  
19. if(sensors&INV_XYZ_ACCEL)
20. {
21. accel[0] = (long)accel_short[0];
22. accel[1] = (long)accel_short[1];
23. accel[2] = (long)accel_short[2];
24. inv_build_accel(accel,0,sensor_timestamp); //把加速度值发给MPL
25. }
26.  
27. if (!mpu_get_compass_reg(compass_short, &sensor_timestamp))
28. {
29. compass[0]=(long)compass_short[0];
30. compass[1]=(long)compass_short[1];
31. compass[2]=(long)compass_short[2];
32. inv_build_compass(compass,0,sensor_timestamp); //把磁力计值发给MPL
33. }
34. inv_execute_on_data();
35. inv_get_sensor_type_euler(data,&accuracy,×tamp);
36.  
37. *roll = (data[0]/q16);
38. *pitch = -(data[1]/q16);
39. *yaw = -data[2] / q16;
40. return 0;
41. }

其中，数据从队列中读取代码如下

1. int dmp_read_fifo(short *gyro, short *accel, long *quat,
2. unsigned long *timestamp, short *sensors, unsigned char *more)
3. {
4. unsigned char fifo_data[MAX_PACKET_LENGTH];
5. unsigned char ii = 0;
6. sensors[0] = 0;
7. if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more))
8. return -1;
9. if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) {
10. #ifdef FIFO_CORRUPTION_CHECK
11. long quat_q14[4], quat_mag_sq;
12. #endif
13. quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) |
14. ((long)fifo_data[2] << 8) | fifo_data[3];
15. quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) |
16. ((long)fifo_data[6] << 8) | fifo_data[7];
17. quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) |
18. ((long)fifo_data[10] << 8) | fifo_data[11];
19. quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) |
20. ((long)fifo_data[14] << 8) | fifo_data[15];
21. ii += 16;
22. #ifdef FIFO_CORRUPTION_CHECK
23. quat_q14[0] = quat[0] >> 16;
24. quat_q14[1] = quat[1] >> 16;
25. quat_q14[2] = quat[2] >> 16;
26. quat_q14[3] = quat[3] >> 16;
27. quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] +
28. quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];
29. if ((quat_mag_sq < QUAT_MAG_SQ_MIN) ||
30. (quat_mag_sq > QUAT_MAG_SQ_MAX)) {
31. /@@* Quaternion is outside of the acceptable threshold. */
32. mpu_reset_fifo();
33. sensors[0] = 0;
34. return -1;
35. }
36. sensors[0] |= INV_WXYZ_QUAT;
37. #endif
38. }
39.  
40. if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) {
41. accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
42. accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
43. accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
44. ii += 6;
45. sensors[0] |= INV_XYZ_ACCEL;
46. }
47.  
48. if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) {
49. gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
50. gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
51. gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
52. ii += 6;
53. sensors[0] |= INV_XYZ_GYRO;
54. }
55. if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
56. decode_gesture(fifo_data + ii);
57.  
58. get_ms(timestamp);
59. return 0;
60. }

结果