资讯详情

MPU9250传感器

MPU9250 内部包括 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控制通信协议MPU我们需要实现9250IIC协议。 代码参考正点原子的源代码，包装函数更有效。
  1. void IIC_Init(void)
  2. {
  3. GPIO_InitTypeDef GPIO_Initure;
  5. __HAL_RCC_GPIOA_CLK_ENABLE(); //使能GPIOB时钟
  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);
  14. IIC_SDA=1;
  15. IIC_SCL=1;
  16. }
  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. // 成功接收应答
  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. }

结果

标签： ii5919传感器vetor温度传感器16加速传感器

锐单商城拥有海量元器件数据手册、 IC替代型号，打造 电子元器件IC百科大全！

锐单商城 - 一站式电子元器件采购平台