Quad-Rotor - Flight control with nRF and MPU6050

Users » Ucial » Code » Quad-Rotor

Songw Jiang / Mbed 2 deprecated Quad-Rotor

Flight control with nRF and MPU6050

mpu6050.cpp@1:23afaebeed15, 2018-08-31 (annotated)

Committer:: Ucial
Date:: Fri Aug 31 10:17:41 2018 +0000
Revision:: 1:23afaebeed15
Parent:: MPU6050.cpp@0:7365f8db1bac

mpu & nRF

Who changed what in which revision?

User	Revision	Line number	New contents of line
Ucial	0:7365f8db1bac	1	#include <stdio.h>
Ucial	0:7365f8db1bac	2	#include <stdint.h>
Ucial	0:7365f8db1bac	3	#include <stdlib.h>
Ucial	0:7365f8db1bac	4	#include <string.h>
Ucial	0:7365f8db1bac	5	#include <math.h>
Ucial	0:7365f8db1bac	6	#include "mpu6050.h"
Ucial	0:7365f8db1bac	7
Ucial	0:7365f8db1bac	8	//////////////////////////////////////////////////////////////////////////////////
Ucial	0:7365f8db1bac	9	//MPU6050驱动程序 C.H.
Ucial	0:7365f8db1bac	10	//////////////////////////////////////////////////////////////////////////////////
Ucial	0:7365f8db1bac	11
Ucial	0:7365f8db1bac	12
Ucial	0:7365f8db1bac	13
Ucial	0:7365f8db1bac	14	//初始化MPU6050
Ucial	0:7365f8db1bac	15	//返回值:0,成功
Ucial	0:7365f8db1bac	16	// 其他,错误代码
Ucial	0:7365f8db1bac	17	unsigned char MPU_Init(void)
Ucial	0:7365f8db1bac	18	{
Ucial	0:7365f8db1bac	19	unsigned char res;
Ucial	0:7365f8db1bac	20	MPU_IIC_Init();//初始化IIC总线
Ucial	0:7365f8db1bac	21	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
Ucial	0:7365f8db1bac	22	delay_ms(100);
Ucial	0:7365f8db1bac	23	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050
Ucial	0:7365f8db1bac	24	MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
Ucial	0:7365f8db1bac	25	MPU_Set_Accel_Fsr(0); //加速度传感器,±2g
Ucial	0:7365f8db1bac	26	MPU_Set_Rate(50); //设置采样率50Hz
Ucial	0:7365f8db1bac	27	MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断
Ucial	0:7365f8db1bac	28	MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭
Ucial	0:7365f8db1bac	29	MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO
Ucial	0:7365f8db1bac	30	MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效
Ucial	0:7365f8db1bac	31	res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
Ucial	0:7365f8db1bac	32	if(res==MPU_ADDR)//器件ID正确
Ucial	0:7365f8db1bac	33	{
Ucial	0:7365f8db1bac	34	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考
Ucial	0:7365f8db1bac	35	MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
Ucial	0:7365f8db1bac	36	MPU_Set_Rate(50); //设置采样率为50Hz
Ucial	0:7365f8db1bac	37	}else return 1;
Ucial	0:7365f8db1bac	38	return 0;
Ucial	0:7365f8db1bac	39	}
Ucial	0:7365f8db1bac	40
Ucial	0:7365f8db1bac	41
Ucial	0:7365f8db1bac	42
Ucial	0:7365f8db1bac	43	//设置MPU6050陀螺仪传感器满量程范围
Ucial	0:7365f8db1bac	44	//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
Ucial	0:7365f8db1bac	45	//返回值:0,设置成功
Ucial	0:7365f8db1bac	46	// 其他,设置失败
Ucial	0:7365f8db1bac	47	unsigned char MPU_Set_Gyro_Fsr(unsigned char fsr)
Ucial	0:7365f8db1bac	48	{
Ucial	0:7365f8db1bac	49	return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
Ucial	0:7365f8db1bac	50	}
Ucial	0:7365f8db1bac	51	//设置MPU6050加速度传感器满量程范围
Ucial	0:7365f8db1bac	52	//fsr:0,±2g;1,±4g;2,±8g;3,±16g
Ucial	0:7365f8db1bac	53	//返回值:0,设置成功
Ucial	0:7365f8db1bac	54	// 其他,设置失败
Ucial	0:7365f8db1bac	55	unsigned char MPU_Set_Accel_Fsr(unsigned char fsr)
Ucial	0:7365f8db1bac	56	{
Ucial	0:7365f8db1bac	57	return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
Ucial	0:7365f8db1bac	58	}
Ucial	0:7365f8db1bac	59	//设置MPU6050的数字低通滤波器
Ucial	0:7365f8db1bac	60	//lpf:数字低通滤波频率(Hz)
Ucial	0:7365f8db1bac	61	//返回值:0,设置成功
Ucial	0:7365f8db1bac	62	// 其他,设置失败
Ucial	0:7365f8db1bac	63	unsigned char MPU_Set_LPF(unsigned short lpf)
Ucial	0:7365f8db1bac	64	{
Ucial	0:7365f8db1bac	65	unsigned char data=0;
Ucial	0:7365f8db1bac	66	if(lpf>=188)data=1;
Ucial	0:7365f8db1bac	67	else if(lpf>=98)data=2;
Ucial	0:7365f8db1bac	68	else if(lpf>=42)data=3;
Ucial	0:7365f8db1bac	69	else if(lpf>=20)data=4;
Ucial	0:7365f8db1bac	70	else if(lpf>=10)data=5;
Ucial	0:7365f8db1bac	71	else data=6;
Ucial	0:7365f8db1bac	72	return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器
Ucial	0:7365f8db1bac	73	}
Ucial	0:7365f8db1bac	74	//设置MPU6050的采样率(假定Fs=1KHz)
Ucial	0:7365f8db1bac	75	//rate:4~1000(Hz)
Ucial	0:7365f8db1bac	76	//返回值:0,设置成功
Ucial	0:7365f8db1bac	77	// 其他,设置失败
Ucial	0:7365f8db1bac	78	unsigned char MPU_Set_Rate(unsigned short rate)
Ucial	0:7365f8db1bac	79	{
Ucial	0:7365f8db1bac	80	unsigned char data;
Ucial	0:7365f8db1bac	81	if(rate>1000)rate=1000;
Ucial	0:7365f8db1bac	82	if(rate<4)rate=4;
Ucial	0:7365f8db1bac	83	data=1000/rate-1;
Ucial	0:7365f8db1bac	84	data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data); //设置数字低通滤波器
Ucial	0:7365f8db1bac	85	return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
Ucial	0:7365f8db1bac	86	}
Ucial	0:7365f8db1bac	87
Ucial	0:7365f8db1bac	88	//得到温度值
Ucial	0:7365f8db1bac	89	//返回值:温度值(扩大了100倍)
Ucial	0:7365f8db1bac	90	short MPU_Get_Temperature(void)
Ucial	0:7365f8db1bac	91	{
Ucial	0:7365f8db1bac	92	unsigned char buf[2];
Ucial	0:7365f8db1bac	93	short raw;
Ucial	0:7365f8db1bac	94	float temp;
Ucial	0:7365f8db1bac	95	MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
Ucial	0:7365f8db1bac	96	raw=((unsigned short )buf[0]<<8)\|buf[1];
Ucial	0:7365f8db1bac	97	temp=36.53+((double)raw)/340;
Ucial	0:7365f8db1bac	98	return temp*100;;
Ucial	0:7365f8db1bac	99	}
Ucial	0:7365f8db1bac	100	//得到陀螺仪值(原始值)
Ucial	0:7365f8db1bac	101	//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
Ucial	0:7365f8db1bac	102	//返回值:0,成功
Ucial	0:7365f8db1bac	103	// 其他,错误代码
Ucial	0:7365f8db1bac	104	unsigned char MPU_Get_Gyroscope(short gx,short gy,short *gz)
Ucial	0:7365f8db1bac	105	{
Ucial	0:7365f8db1bac	106	unsigned char buf[6],res;
Ucial	0:7365f8db1bac	107	res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
Ucial	0:7365f8db1bac	108	if(res==0)
Ucial	0:7365f8db1bac	109	{
Ucial	0:7365f8db1bac	110	*gx=((unsigned short )buf[0]<<8)\|buf[1];
Ucial	0:7365f8db1bac	111	*gy=((unsigned short )buf[2]<<8)\|buf[3];
Ucial	0:7365f8db1bac	112	*gz=((unsigned short )buf[4]<<8)\|buf[5];
Ucial	0:7365f8db1bac	113	}
Ucial	0:7365f8db1bac	114	return res;;
Ucial	0:7365f8db1bac	115	}
Ucial	0:7365f8db1bac	116	//得到加速度值(原始值)
Ucial	0:7365f8db1bac	117	//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
Ucial	0:7365f8db1bac	118	//返回值:0,成功
Ucial	0:7365f8db1bac	119	// 其他,错误代码
Ucial	0:7365f8db1bac	120	unsigned char MPU_Get_Accelerometer(short ax,short ay,short *az)
Ucial	0:7365f8db1bac	121	{
Ucial	0:7365f8db1bac	122	unsigned char buf[6],res;
Ucial	0:7365f8db1bac	123	res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
Ucial	0:7365f8db1bac	124	if(res==0)
Ucial	0:7365f8db1bac	125	{
Ucial	0:7365f8db1bac	126	*ax=((unsigned short )buf[0]<<8)\|buf[1];
Ucial	0:7365f8db1bac	127	*ay=((unsigned short )buf[2]<<8)\|buf[3];
Ucial	0:7365f8db1bac	128	*az=((unsigned short )buf[4]<<8)\|buf[5];
Ucial	0:7365f8db1bac	129	}
Ucial	0:7365f8db1bac	130	return res;;
Ucial	0:7365f8db1bac	131	}
Ucial	0:7365f8db1bac	132	//IIC连续写
Ucial	0:7365f8db1bac	133	//addr:器件地址
Ucial	0:7365f8db1bac	134	//reg:寄存器地址
Ucial	0:7365f8db1bac	135	//len:写入长度
Ucial	0:7365f8db1bac	136	//buf:数据区
Ucial	0:7365f8db1bac	137	//返回值:0,正常
Ucial	0:7365f8db1bac	138	// 其他,错误代码
Ucial	0:7365f8db1bac	139	unsigned char MPU_Write_Len(unsigned char addr,unsigned char reg,unsigned char len,unsigned char *buf)
Ucial	0:7365f8db1bac	140	{
Ucial	0:7365f8db1bac	141	unsigned char i;
Ucial	0:7365f8db1bac	142	MPU_IIC_Start();
Ucial	0:7365f8db1bac	143	MPU_IIC_Send_Byte((addr<<1)\|0);//发送器件地址+写命令
Ucial	0:7365f8db1bac	144	if(MPU_IIC_Wait_Ack()) //等待应答
Ucial	0:7365f8db1bac	145	{
Ucial	0:7365f8db1bac	146	MPU_IIC_Stop();
Ucial	0:7365f8db1bac	147	return 1;
Ucial	0:7365f8db1bac	148	}
Ucial	0:7365f8db1bac	149	MPU_IIC_Send_Byte(reg); //写寄存器地址
Ucial	0:7365f8db1bac	150	MPU_IIC_Wait_Ack(); //等待应答
Ucial	0:7365f8db1bac	151	for(i=0;i<len;i++)
Ucial	0:7365f8db1bac	152	{
Ucial	0:7365f8db1bac	153	MPU_IIC_Send_Byte(buf[i]); //发送数据
Ucial	0:7365f8db1bac	154	if(MPU_IIC_Wait_Ack()) //等待ACK
Ucial	0:7365f8db1bac	155	{
Ucial	0:7365f8db1bac	156	MPU_IIC_Stop();
Ucial	0:7365f8db1bac	157	return 1;
Ucial	0:7365f8db1bac	158	}
Ucial	0:7365f8db1bac	159	}
Ucial	0:7365f8db1bac	160	MPU_IIC_Stop();
Ucial	0:7365f8db1bac	161	return 0;
Ucial	0:7365f8db1bac	162	}
Ucial	0:7365f8db1bac	163	//IIC连续读
Ucial	0:7365f8db1bac	164	//addr:器件地址
Ucial	0:7365f8db1bac	165	//reg:要读取的寄存器地址
Ucial	0:7365f8db1bac	166	//len:要读取的长度
Ucial	0:7365f8db1bac	167	//buf:读取到的数据存储区
Ucial	0:7365f8db1bac	168	//返回值:0,正常
Ucial	0:7365f8db1bac	169	// 其他,错误代码
Ucial	0:7365f8db1bac	170	unsigned char MPU_Read_Len(unsigned char addr,unsigned char reg,unsigned char len,unsigned char *buf)
Ucial	0:7365f8db1bac	171	{
Ucial	0:7365f8db1bac	172	MPU_IIC_Start();
Ucial	0:7365f8db1bac	173	MPU_IIC_Send_Byte((addr<<1)\|0);//发送器件地址+写命令
Ucial	0:7365f8db1bac	174	if(MPU_IIC_Wait_Ack()) //等待应答
Ucial	0:7365f8db1bac	175	{
Ucial	0:7365f8db1bac	176	MPU_IIC_Stop();
Ucial	0:7365f8db1bac	177	return 1;
Ucial	0:7365f8db1bac	178	}
Ucial	0:7365f8db1bac	179	MPU_IIC_Send_Byte(reg); //写寄存器地址
Ucial	0:7365f8db1bac	180	MPU_IIC_Wait_Ack(); //等待应答
Ucial	0:7365f8db1bac	181	MPU_IIC_Start();
Ucial	0:7365f8db1bac	182	MPU_IIC_Send_Byte((addr<<1)\|1);//发送器件地址+读命令
Ucial	0:7365f8db1bac	183	MPU_IIC_Wait_Ack(); //等待应答
Ucial	0:7365f8db1bac	184	while(len)
Ucial	0:7365f8db1bac	185	{
Ucial	0:7365f8db1bac	186	if(len==1)*buf=MPU_IIC_Read_Byte(0);//读数据,发送nACK
Ucial	0:7365f8db1bac	187	else *buf=MPU_IIC_Read_Byte(1); //读数据,发送ACK
Ucial	0:7365f8db1bac	188	len--;
Ucial	0:7365f8db1bac	189	buf++;
Ucial	0:7365f8db1bac	190	}
Ucial	0:7365f8db1bac	191	MPU_IIC_Stop(); //产生一个停止条件
Ucial	0:7365f8db1bac	192	return 0;
Ucial	0:7365f8db1bac	193	}
Ucial	0:7365f8db1bac	194	//IIC写一个字节
Ucial	0:7365f8db1bac	195	//reg:寄存器地址
Ucial	0:7365f8db1bac	196	//data:数据
Ucial	0:7365f8db1bac	197	//返回值:0,正常
Ucial	0:7365f8db1bac	198	// 其他,错误代码
Ucial	0:7365f8db1bac	199	unsigned char MPU_Write_Byte(unsigned char reg,unsigned char data)
Ucial	0:7365f8db1bac	200	{
Ucial	0:7365f8db1bac	201	MPU_IIC_Start();
Ucial	0:7365f8db1bac	202	MPU_IIC_Send_Byte((MPU_ADDR<<1)\|0);//发送器件地址+写命令
Ucial	0:7365f8db1bac	203	if(MPU_IIC_Wait_Ack()) //等待应答
Ucial	0:7365f8db1bac	204	{
Ucial	0:7365f8db1bac	205	MPU_IIC_Stop();
Ucial	0:7365f8db1bac	206	return 1;
Ucial	0:7365f8db1bac	207	}
Ucial	0:7365f8db1bac	208	MPU_IIC_Send_Byte(reg); //写寄存器地址
Ucial	0:7365f8db1bac	209	MPU_IIC_Wait_Ack(); //等待应答
Ucial	0:7365f8db1bac	210	MPU_IIC_Send_Byte(data);//发送数据
Ucial	0:7365f8db1bac	211	if(MPU_IIC_Wait_Ack()) //等待ACK
Ucial	0:7365f8db1bac	212	{
Ucial	0:7365f8db1bac	213	MPU_IIC_Stop();
Ucial	0:7365f8db1bac	214	return 1;
Ucial	0:7365f8db1bac	215	}
Ucial	0:7365f8db1bac	216	MPU_IIC_Stop();
Ucial	0:7365f8db1bac	217	return 0;
Ucial	0:7365f8db1bac	218	}
Ucial	0:7365f8db1bac	219	//IIC读一个字节
Ucial	0:7365f8db1bac	220	//reg:寄存器地址
Ucial	0:7365f8db1bac	221	//返回值:读到的数据
Ucial	0:7365f8db1bac	222	unsigned char MPU_Read_Byte(unsigned char reg)
Ucial	0:7365f8db1bac	223	{
Ucial	0:7365f8db1bac	224	unsigned char res;
Ucial	0:7365f8db1bac	225	MPU_IIC_Start();
Ucial	0:7365f8db1bac	226	MPU_IIC_Send_Byte((MPU_ADDR<<1)\|0);//发送器件地址+写命令
Ucial	0:7365f8db1bac	227	MPU_IIC_Wait_Ack(); //等待应答
Ucial	0:7365f8db1bac	228	MPU_IIC_Send_Byte(reg); //写寄存器地址
Ucial	0:7365f8db1bac	229	MPU_IIC_Wait_Ack(); //等待应答
Ucial	0:7365f8db1bac	230	MPU_IIC_Start();
Ucial	0:7365f8db1bac	231	MPU_IIC_Send_Byte((MPU_ADDR<<1)\|1);//发送器件地址+读命令
Ucial	0:7365f8db1bac	232	MPU_IIC_Wait_Ack(); //等待应答
Ucial	0:7365f8db1bac	233	res=MPU_IIC_Read_Byte(0);//读取数据,发送nACK
Ucial	0:7365f8db1bac	234	MPU_IIC_Stop(); //产生一个停止条件
Ucial	0:7365f8db1bac	235	return res;
Ucial	0:7365f8db1bac	236	}/**/
Ucial	0:7365f8db1bac	237
Ucial	0:7365f8db1bac	238
Ucial	0:7365f8db1bac	239	//MPU IIC 延时函数
Ucial	0:7365f8db1bac	240	void MPU_IIC_Delay(void)
Ucial	0:7365f8db1bac	241	{
Ucial	0:7365f8db1bac	242	//delay_us(2);
Ucial	0:7365f8db1bac	243	}
Ucial	0:7365f8db1bac	244	//初始化IIC
Ucial	0:7365f8db1bac	245	void MPU_IIC_Init(void)
Ucial	0:7365f8db1bac	246	{
Ucial	0:7365f8db1bac	247
Ucial	0:7365f8db1bac	248
Ucial	0:7365f8db1bac	249	#if defined DRIVER_MODE_BALANCE
Ucial	0:7365f8db1bac	250	RCC->APB2ENR\|=1<<3; //先使能外设IO PORTC时钟
Ucial	0:7365f8db1bac	251	GPIOB->CRL&=0X00FFFFFF; //PC11/12 推挽输出
Ucial	0:7365f8db1bac	252	GPIOB->CRL\|=0X33000000;
Ucial	0:7365f8db1bac	253	GPIOB->ODR\|=3<<6; //PC11,12 输出高
Ucial	0:7365f8db1bac	254	#elif defined DRIVER_MODE_ROTOR
Ucial	0:7365f8db1bac	255	RCC->APB2ENR\|=1<<3; //先使能外设IO PORTC时钟
Ucial	0:7365f8db1bac	256	GPIOB->CRH&=0XFFF00FFF; //PC11/12 推挽输出
Ucial	0:7365f8db1bac	257	GPIOB->CRH\|=0X00033000;
Ucial	0:7365f8db1bac	258	GPIOB->ODR\|=3<<11; //PC11,12 输出高
Ucial	0:7365f8db1bac	259	#else
Ucial	0:7365f8db1bac	260	#error Target Board is not specified.
Ucial	0:7365f8db1bac	261	#endif
Ucial	0:7365f8db1bac	262	}
Ucial	0:7365f8db1bac	263	//产生IIC起始信号
Ucial	0:7365f8db1bac	264	void MPU_IIC_Start(void)
Ucial	0:7365f8db1bac	265	{
Ucial	0:7365f8db1bac	266	MPU_SDA_OUT(); //sda线输出
Ucial	0:7365f8db1bac	267	MPU_IIC_SDA=1;
Ucial	0:7365f8db1bac	268	MPU_IIC_SCL=1;
Ucial	0:7365f8db1bac	269	MPU_IIC_Delay();
Ucial	0:7365f8db1bac	270	MPU_IIC_SDA=0;//START:when CLK is high,DATA change form high to low
Ucial	0:7365f8db1bac	271	MPU_IIC_Delay();
Ucial	0:7365f8db1bac	272	MPU_IIC_SCL=0;//钳住I2C总线，准备发送或接收数据
Ucial	0:7365f8db1bac	273	}
Ucial	0:7365f8db1bac	274	//产生IIC停止信号
Ucial	0:7365f8db1bac	275	void MPU_IIC_Stop(void)
Ucial	0:7365f8db1bac	276	{
Ucial	0:7365f8db1bac	277	MPU_SDA_OUT();//sda线输出
Ucial	0:7365f8db1bac	278	MPU_IIC_SCL=0;
Ucial	0:7365f8db1bac	279	MPU_IIC_SDA=0;//STOP:when CLK is high DATA change form low to high
Ucial	0:7365f8db1bac	280	MPU_IIC_Delay();
Ucial	0:7365f8db1bac	281	MPU_IIC_SCL=1;
Ucial	0:7365f8db1bac	282	MPU_IIC_SDA=1;//发送I2C总线结束信号
Ucial	0:7365f8db1bac	283	MPU_IIC_Delay();
Ucial	0:7365f8db1bac	284	}
Ucial	0:7365f8db1bac	285	//等待应答信号到来
Ucial	0:7365f8db1bac	286	//返回值：1，接收应答失败
Ucial	0:7365f8db1bac	287	// 0，接收应答成功
Ucial	0:7365f8db1bac	288	unsigned char MPU_IIC_Wait_Ack(void)
Ucial	0:7365f8db1bac	289	{
Ucial	0:7365f8db1bac	290	unsigned char ucErrTime=0;
Ucial	0:7365f8db1bac	291	MPU_SDA_IN(); //SDA设置为输入
Ucial	0:7365f8db1bac	292	MPU_IIC_SDA=1;MPU_IIC_Delay();
Ucial	0:7365f8db1bac	293	MPU_IIC_SCL=1;MPU_IIC_Delay();
Ucial	0:7365f8db1bac	294	while(MPU_READ_SDA)
Ucial	0:7365f8db1bac	295	{
Ucial	0:7365f8db1bac	296	ucErrTime++;
Ucial	0:7365f8db1bac	297	if(ucErrTime>250)
Ucial	0:7365f8db1bac	298	{
Ucial	0:7365f8db1bac	299	MPU_IIC_Stop();
Ucial	0:7365f8db1bac	300	return 1;
Ucial	0:7365f8db1bac	301	}
Ucial	0:7365f8db1bac	302	}
Ucial	0:7365f8db1bac	303	MPU_IIC_SCL=0;//时钟输出0
Ucial	0:7365f8db1bac	304	return 0;
Ucial	0:7365f8db1bac	305	}
Ucial	0:7365f8db1bac	306	//产生ACK应答
Ucial	0:7365f8db1bac	307	void MPU_IIC_Ack(void)
Ucial	0:7365f8db1bac	308	{
Ucial	0:7365f8db1bac	309	MPU_IIC_SCL=0;
Ucial	0:7365f8db1bac	310	MPU_SDA_OUT();
Ucial	0:7365f8db1bac	311	MPU_IIC_SDA=0;
Ucial	0:7365f8db1bac	312	MPU_IIC_Delay();
Ucial	0:7365f8db1bac	313	MPU_IIC_SCL=1;
Ucial	0:7365f8db1bac	314	MPU_IIC_Delay();
Ucial	0:7365f8db1bac	315	MPU_IIC_SCL=0;
Ucial	0:7365f8db1bac	316	}
Ucial	0:7365f8db1bac	317	//不产生ACK应答
Ucial	0:7365f8db1bac	318	void MPU_IIC_NAck(void)
Ucial	0:7365f8db1bac	319	{
Ucial	0:7365f8db1bac	320	MPU_IIC_SCL=0;
Ucial	0:7365f8db1bac	321	MPU_SDA_OUT();
Ucial	0:7365f8db1bac	322	MPU_IIC_SDA=1;
Ucial	0:7365f8db1bac	323	MPU_IIC_Delay();
Ucial	0:7365f8db1bac	324	MPU_IIC_SCL=1;
Ucial	0:7365f8db1bac	325	MPU_IIC_Delay();
Ucial	0:7365f8db1bac	326	MPU_IIC_SCL=0;
Ucial	0:7365f8db1bac	327	}
Ucial	0:7365f8db1bac	328	//IIC发送一个字节
Ucial	0:7365f8db1bac	329	//返回从机有无应答
Ucial	0:7365f8db1bac	330	//1，有应答
Ucial	0:7365f8db1bac	331	//0，无应答
Ucial	0:7365f8db1bac	332	void MPU_IIC_Send_Byte(unsigned char txd)
Ucial	0:7365f8db1bac	333	{
Ucial	0:7365f8db1bac	334	unsigned char t;
Ucial	0:7365f8db1bac	335	MPU_SDA_OUT();
Ucial	0:7365f8db1bac	336	MPU_IIC_SCL=0;//拉低时钟开始数据传输
Ucial	0:7365f8db1bac	337	for(t=0;t<8;t++)
Ucial	0:7365f8db1bac	338	{
Ucial	0:7365f8db1bac	339	MPU_IIC_SDA=(txd&0x80)>>7;
Ucial	0:7365f8db1bac	340	txd<<=1;
Ucial	0:7365f8db1bac	341	MPU_IIC_SCL=1;
Ucial	0:7365f8db1bac	342	MPU_IIC_Delay();
Ucial	0:7365f8db1bac	343	MPU_IIC_SCL=0;
Ucial	0:7365f8db1bac	344	MPU_IIC_Delay();
Ucial	0:7365f8db1bac	345	}
Ucial	0:7365f8db1bac	346	}
Ucial	0:7365f8db1bac	347	//读1个字节，ack=1时，发送ACK，ack=0，发送nACK
Ucial	0:7365f8db1bac	348	unsigned char MPU_IIC_Read_Byte(unsigned char ack)
Ucial	0:7365f8db1bac	349	{
Ucial	0:7365f8db1bac	350	unsigned char i,receive=0;
Ucial	0:7365f8db1bac	351	MPU_SDA_IN();//SDA设置为输入
Ucial	0:7365f8db1bac	352	for(i=0;i<8;i++ )
Ucial	0:7365f8db1bac	353	{
Ucial	0:7365f8db1bac	354	MPU_IIC_SCL=0;
Ucial	0:7365f8db1bac	355	MPU_IIC_Delay();
Ucial	0:7365f8db1bac	356	MPU_IIC_SCL=1;
Ucial	0:7365f8db1bac	357	receive<<=1;
Ucial	0:7365f8db1bac	358	if(MPU_READ_SDA)receive++;
Ucial	0:7365f8db1bac	359	MPU_IIC_Delay();
Ucial	0:7365f8db1bac	360	}
Ucial	0:7365f8db1bac	361	if (!ack)
Ucial	0:7365f8db1bac	362	MPU_IIC_NAck();//发送nACK
Ucial	0:7365f8db1bac	363	else
Ucial	0:7365f8db1bac	364	MPU_IIC_Ack(); //发送ACK
Ucial	0:7365f8db1bac	365	return receive;
Ucial	0:7365f8db1bac	366	}
Ucial	0:7365f8db1bac	367
Ucial	0:7365f8db1bac	368	#define MPU6050 //定义我们使用的传感器为MPU6050
Ucial	0:7365f8db1bac	369	#define MOTION_DRIVER_TARGET_MSP430 //定义驱动部分,采用MSP430的驱动(移植到STM32F1)
Ucial	0:7365f8db1bac	370
Ucial	0:7365f8db1bac	371	/* The following functions must be defined for this platform:
Ucial	0:7365f8db1bac	372	* i2c_write(unsigned char slave_addr, unsigned char reg_addr,
Ucial	0:7365f8db1bac	373	* unsigned char length, unsigned char const *data)
Ucial	0:7365f8db1bac	374	* i2c_read(unsigned char slave_addr, unsigned char reg_addr,
Ucial	0:7365f8db1bac	375	* unsigned char length, unsigned char *data)
Ucial	0:7365f8db1bac	376	* delay_ms(unsigned long num_ms)
Ucial	0:7365f8db1bac	377	* get_ms(unsigned long *count)
Ucial	0:7365f8db1bac	378	* reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin)
Ucial	0:7365f8db1bac	379	* labs(long x)
Ucial	0:7365f8db1bac	380	* fabsf(float x)
Ucial	0:7365f8db1bac	381	* min(int a, int b)
Ucial	0:7365f8db1bac	382	*/
Ucial	0:7365f8db1bac	383	#if defined MOTION_DRIVER_TARGET_MSP430
Ucial	0:7365f8db1bac	384	//#include "msp430.h"
Ucial	0:7365f8db1bac	385	//#include "msp430_i2c.h"
Ucial	0:7365f8db1bac	386	//#include "msp430_clock.h"
Ucial	0:7365f8db1bac	387	//#include "msp430_interrupt.h"
Ucial	0:7365f8db1bac	388
Ucial	0:7365f8db1bac	389	#define i2c_write MPU_Write_Len
Ucial	0:7365f8db1bac	390	#define i2c_read MPU_Read_Len
Ucial	0:7365f8db1bac	391	//#define delay_ms delay_ms
Ucial	0:7365f8db1bac	392	#define get_ms mget_ms
Ucial	0:7365f8db1bac	393	//static inline int reg_int_cb(struct int_param_s *int_param)
Ucial	0:7365f8db1bac	394	//{
Ucial	0:7365f8db1bac	395	// return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
Ucial	0:7365f8db1bac	396	// int_param->active_low);
Ucial	0:7365f8db1bac	397	//}
Ucial	0:7365f8db1bac	398	////#define // printf //打印信息
Ucial	0:7365f8db1bac	399	////#define // printf //打印信息
Ucial	0:7365f8db1bac	400	/* labs is already defined by TI's toolchain. */
Ucial	0:7365f8db1bac	401	/* fabs is for doubles. fabsf is for floats. */
Ucial	0:7365f8db1bac	402	#define fabs fabsf
Ucial	0:7365f8db1bac	403	#define min(a,b) ((a<b)?a:b)
Ucial	0:7365f8db1bac	404	#elif defined EMPL_TARGET_MSP430
Ucial	0:7365f8db1bac	405	#include "msp430.h"
Ucial	0:7365f8db1bac	406	#include "msp430_i2c.h"
Ucial	0:7365f8db1bac	407	#include "msp430_clock.h"
Ucial	0:7365f8db1bac	408	#include "msp430_interrupt.h"
Ucial	0:7365f8db1bac	409	#include "log.h"
Ucial	0:7365f8db1bac	410	#define i2c_write msp430_i2c_write
Ucial	0:7365f8db1bac	411	#define i2c_read msp430_i2c_read
Ucial	0:7365f8db1bac	412	#define delay_ms msp430_delay_ms
Ucial	0:7365f8db1bac	413	#define get_ms msp430_get_clock_ms
Ucial	0:7365f8db1bac	414	static inline int reg_int_cb(struct int_param_s *int_param)
Ucial	0:7365f8db1bac	415	{
Ucial	0:7365f8db1bac	416	return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
Ucial	0:7365f8db1bac	417	int_param->active_low);
Ucial	0:7365f8db1bac	418	}
Ucial	0:7365f8db1bac	419	//#define // MPL_LOGI
Ucial	0:7365f8db1bac	420	//#define // MPL_LOGE
Ucial	0:7365f8db1bac	421	/* labs is already defined by TI's toolchain. */
Ucial	0:7365f8db1bac	422	/* fabs is for doubles. fabsf is for floats. */
Ucial	0:7365f8db1bac	423	#define fabs fabsf
Ucial	0:7365f8db1bac	424	#define min(a,b) ((a<b)?a:b)
Ucial	0:7365f8db1bac	425	#elif defined EMPL_TARGET_UC3L0
Ucial	0:7365f8db1bac	426	/* Instead of using the standard TWI driver from the ASF library, we're using
Ucial	0:7365f8db1bac	427	* a TWI driver that follows the slave address + register address convention.
Ucial	0:7365f8db1bac	428	*/
Ucial	0:7365f8db1bac	429	#include "twi.h"
Ucial	0:7365f8db1bac	430	#include "delay.h"
Ucial	0:7365f8db1bac	431	#include "sysclk.h"
Ucial	0:7365f8db1bac	432	#include "log.h"
Ucial	0:7365f8db1bac	433	#include "sensors_xplained.h"
Ucial	0:7365f8db1bac	434	#include "uc3l0_clock.h"
Ucial	0:7365f8db1bac	435	#define i2c_write(a, b, c, d) twi_write(a, b, d, c)
Ucial	0:7365f8db1bac	436	#define i2c_read(a, b, c, d) twi_read(a, b, d, c)
Ucial	0:7365f8db1bac	437	/* delay_ms is a function already defined in ASF. */
Ucial	0:7365f8db1bac	438	#define get_ms uc3l0_get_clock_ms
Ucial	0:7365f8db1bac	439	static inline int reg_int_cb(struct int_param_s *int_param)
Ucial	0:7365f8db1bac	440	{
Ucial	0:7365f8db1bac	441	sensor_board_irq_connect(int_param->pin, int_param->cb, int_param->arg);
Ucial	0:7365f8db1bac	442	return 0;
Ucial	0:7365f8db1bac	443	}
Ucial	0:7365f8db1bac	444	//#define // MPL_LOGI
Ucial	0:7365f8db1bac	445	//#define // MPL_LOGE
Ucial	0:7365f8db1bac	446	/* UC3 is a 32-bit processor, so abs and labs are equivalent. */
Ucial	0:7365f8db1bac	447	#define labs abs
Ucial	0:7365f8db1bac	448	#define fabs(x) (((x)>0)?(x):-(x))
Ucial	0:7365f8db1bac	449	#else
Ucial	0:7365f8db1bac	450	#error Gyro driver is missing the system layer implementations.
Ucial	0:7365f8db1bac	451	#endif
Ucial	0:7365f8db1bac	452
Ucial	0:7365f8db1bac	453	#if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250
Ucial	0:7365f8db1bac	454	#error Which gyro are you using? Define MPUxxxx in your compiler options.
Ucial	0:7365f8db1bac	455	#endif
Ucial	0:7365f8db1bac	456
Ucial	0:7365f8db1bac	457	/* Time for some messy macro work. =]
Ucial	0:7365f8db1bac	458	* #define MPU9150
Ucial	0:7365f8db1bac	459	* is equivalent to..
Ucial	0:7365f8db1bac	460	* #define MPU6050
Ucial	0:7365f8db1bac	461	* #define AK8975_SECONDARY
Ucial	0:7365f8db1bac	462	*
Ucial	0:7365f8db1bac	463	* #define MPU9250
Ucial	0:7365f8db1bac	464	* is equivalent to..
Ucial	0:7365f8db1bac	465	* #define MPU6500
Ucial	0:7365f8db1bac	466	* #define AK8963_SECONDARY
Ucial	0:7365f8db1bac	467	*/
Ucial	0:7365f8db1bac	468	#if defined MPU9150
Ucial	0:7365f8db1bac	469	#ifndef MPU6050
Ucial	0:7365f8db1bac	470	#define MPU6050
Ucial	0:7365f8db1bac	471	#endif /* #ifndef MPU6050 */
Ucial	0:7365f8db1bac	472	#if defined AK8963_SECONDARY
Ucial	0:7365f8db1bac	473	#error "MPU9150 and AK8963_SECONDARY cannot both be defined."
Ucial	0:7365f8db1bac	474	#elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */
Ucial	0:7365f8db1bac	475	#define AK8975_SECONDARY
Ucial	0:7365f8db1bac	476	#endif /* #if defined AK8963_SECONDARY */
Ucial	0:7365f8db1bac	477	#elif defined MPU9250 /* #if defined MPU9150 */
Ucial	0:7365f8db1bac	478	#ifndef MPU6500
Ucial	0:7365f8db1bac	479	#define MPU6500
Ucial	0:7365f8db1bac	480	#endif /* #ifndef MPU6500 */
Ucial	0:7365f8db1bac	481	#if defined AK8975_SECONDARY
Ucial	0:7365f8db1bac	482	#error "MPU9250 and AK8975_SECONDARY cannot both be defined."
Ucial	0:7365f8db1bac	483	#elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */
Ucial	0:7365f8db1bac	484	#define AK8963_SECONDARY
Ucial	0:7365f8db1bac	485	#endif /* #if defined AK8975_SECONDARY */
Ucial	0:7365f8db1bac	486	#endif /* #if defined MPU9150 */
Ucial	0:7365f8db1bac	487
Ucial	0:7365f8db1bac	488	#if defined AK8975_SECONDARY \|\| defined AK8963_SECONDARY
Ucial	0:7365f8db1bac	489	#define AK89xx_SECONDARY
Ucial	0:7365f8db1bac	490	#else
Ucial	0:7365f8db1bac	491	/* #warning "No compass = less profit for Invensense. Lame." */
Ucial	0:7365f8db1bac	492	#endif
Ucial	0:7365f8db1bac	493
Ucial	0:7365f8db1bac	494	static int set_int_enable(unsigned char enable);
Ucial	0:7365f8db1bac	495
Ucial	0:7365f8db1bac	496	/* Hardware registers needed by driver. */
Ucial	0:7365f8db1bac	497	struct gyro_reg_s {
Ucial	0:7365f8db1bac	498	unsigned char who_am_i;
Ucial	0:7365f8db1bac	499	unsigned char rate_div;
Ucial	0:7365f8db1bac	500	unsigned char lpf;
Ucial	0:7365f8db1bac	501	unsigned char prod_id;
Ucial	0:7365f8db1bac	502	unsigned char user_ctrl;
Ucial	0:7365f8db1bac	503	unsigned char fifo_en;
Ucial	0:7365f8db1bac	504	unsigned char gyro_cfg;
Ucial	0:7365f8db1bac	505	unsigned char accel_cfg;
Ucial	0:7365f8db1bac	506	// unsigned char accel_cfg2;
Ucial	0:7365f8db1bac	507	// unsigned char lp_accel_odr;
Ucial	0:7365f8db1bac	508	unsigned char motion_thr;
Ucial	0:7365f8db1bac	509	unsigned char motion_dur;
Ucial	0:7365f8db1bac	510	unsigned char fifo_count_h;
Ucial	0:7365f8db1bac	511	unsigned char fifo_r_w;
Ucial	0:7365f8db1bac	512	unsigned char raw_gyro;
Ucial	0:7365f8db1bac	513	unsigned char raw_accel;
Ucial	0:7365f8db1bac	514	unsigned char temp;
Ucial	0:7365f8db1bac	515	unsigned char int_enable;
Ucial	0:7365f8db1bac	516	unsigned char dmp_int_status;
Ucial	0:7365f8db1bac	517	unsigned char int_status;
Ucial	0:7365f8db1bac	518	// unsigned char accel_intel;
Ucial	0:7365f8db1bac	519	unsigned char pwr_mgmt_1;
Ucial	0:7365f8db1bac	520	unsigned char pwr_mgmt_2;
Ucial	0:7365f8db1bac	521	unsigned char int_pin_cfg;
Ucial	0:7365f8db1bac	522	unsigned char mem_r_w;
Ucial	0:7365f8db1bac	523	unsigned char accel_offs;
Ucial	0:7365f8db1bac	524	unsigned char i2c_mst;
Ucial	0:7365f8db1bac	525	unsigned char bank_sel;
Ucial	0:7365f8db1bac	526	unsigned char mem_start_addr;
Ucial	0:7365f8db1bac	527	unsigned char prgm_start_h;
Ucial	0:7365f8db1bac	528	#if defined AK89xx_SECONDARY
Ucial	0:7365f8db1bac	529	unsigned char s0_addr;
Ucial	0:7365f8db1bac	530	unsigned char s0_reg;
Ucial	0:7365f8db1bac	531	unsigned char s0_ctrl;
Ucial	0:7365f8db1bac	532	unsigned char s1_addr;
Ucial	0:7365f8db1bac	533	unsigned char s1_reg;
Ucial	0:7365f8db1bac	534	unsigned char s1_ctrl;
Ucial	0:7365f8db1bac	535	unsigned char s4_ctrl;
Ucial	0:7365f8db1bac	536	unsigned char s0_do;
Ucial	0:7365f8db1bac	537	unsigned char s1_do;
Ucial	0:7365f8db1bac	538	unsigned char i2c_delay_ctrl;
Ucial	0:7365f8db1bac	539	unsigned char raw_compass;
Ucial	0:7365f8db1bac	540	/* The I2C_MST_VDDIO bit is in this register. */
Ucial	0:7365f8db1bac	541	unsigned char yg_offs_tc;
Ucial	0:7365f8db1bac	542	#endif
Ucial	0:7365f8db1bac	543	};
Ucial	0:7365f8db1bac	544
Ucial	0:7365f8db1bac	545	/* Information specific to a particular device. */
Ucial	0:7365f8db1bac	546	struct hw_s {
Ucial	0:7365f8db1bac	547	unsigned char addr;
Ucial	0:7365f8db1bac	548	unsigned short max_fifo;
Ucial	0:7365f8db1bac	549	unsigned char num_reg;
Ucial	0:7365f8db1bac	550	unsigned short temp_sens;
Ucial	0:7365f8db1bac	551	short temp_offset;
Ucial	0:7365f8db1bac	552	unsigned short bank_size;
Ucial	0:7365f8db1bac	553	#if defined AK89xx_SECONDARY
Ucial	0:7365f8db1bac	554	unsigned short compass_fsr;
Ucial	0:7365f8db1bac	555	#endif
Ucial	0:7365f8db1bac	556	};
Ucial	0:7365f8db1bac	557
Ucial	0:7365f8db1bac	558	/* When entering motion interrupt mode, the driver keeps track of the
Ucial	0:7365f8db1bac	559	* previous state so that it can be restored at a later time.
Ucial	0:7365f8db1bac	560	* TODO: This is tacky. Fix it.
Ucial	0:7365f8db1bac	561	*/
Ucial	0:7365f8db1bac	562	struct motion_int_cache_s {
Ucial	0:7365f8db1bac	563	unsigned short gyro_fsr;
Ucial	0:7365f8db1bac	564	unsigned char accel_fsr;
Ucial	0:7365f8db1bac	565	unsigned short lpf;
Ucial	0:7365f8db1bac	566	unsigned short sample_rate;
Ucial	0:7365f8db1bac	567	unsigned char sensors_on;
Ucial	0:7365f8db1bac	568	unsigned char fifo_sensors;
Ucial	0:7365f8db1bac	569	unsigned char dmp_on;
Ucial	0:7365f8db1bac	570	};
Ucial	0:7365f8db1bac	571
Ucial	0:7365f8db1bac	572	/* Cached chip configuration data.
Ucial	0:7365f8db1bac	573	* TODO: A lot of these can be handled with a bitmask.
Ucial	0:7365f8db1bac	574	*/
Ucial	0:7365f8db1bac	575	struct chip_cfg_s {
Ucial	0:7365f8db1bac	576	/* Matches gyro_cfg >> 3 & 0x03 */
Ucial	0:7365f8db1bac	577	unsigned char gyro_fsr;
Ucial	0:7365f8db1bac	578	/* Matches accel_cfg >> 3 & 0x03 */
Ucial	0:7365f8db1bac	579	unsigned char accel_fsr;
Ucial	0:7365f8db1bac	580	/* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */
Ucial	0:7365f8db1bac	581	unsigned char sensors;
Ucial	0:7365f8db1bac	582	/* Matches config register. */
Ucial	0:7365f8db1bac	583	unsigned char lpf;
Ucial	0:7365f8db1bac	584	unsigned char clk_src;
Ucial	0:7365f8db1bac	585	/* Sample rate, NOT rate divider. */
Ucial	0:7365f8db1bac	586	unsigned short sample_rate;
Ucial	0:7365f8db1bac	587	/* Matches fifo_en register. */
Ucial	0:7365f8db1bac	588	unsigned char fifo_enable;
Ucial	0:7365f8db1bac	589	/* Matches int enable register. */
Ucial	0:7365f8db1bac	590	unsigned char int_enable;
Ucial	0:7365f8db1bac	591	/* 1 if devices on auxiliary I2C bus appear on the primary. */
Ucial	0:7365f8db1bac	592	unsigned char bypass_mode;
Ucial	0:7365f8db1bac	593	/* 1 if half-sensitivity.
Ucial	0:7365f8db1bac	594	* NOTE: This doesn't belong here, but everything else in hw_s is const,
Ucial	0:7365f8db1bac	595	* and this allows us to save some precious RAM.
Ucial	0:7365f8db1bac	596	*/
Ucial	0:7365f8db1bac	597	unsigned char accel_half;
Ucial	0:7365f8db1bac	598	/* 1 if device in low-power accel-only mode. */
Ucial	0:7365f8db1bac	599	unsigned char lp_accel_mode;
Ucial	0:7365f8db1bac	600	/* 1 if interrupts are only triggered on motion events. */
Ucial	0:7365f8db1bac	601	unsigned char int_motion_only;
Ucial	0:7365f8db1bac	602	struct motion_int_cache_s cache;
Ucial	0:7365f8db1bac	603	/* 1 for active low interrupts. */
Ucial	0:7365f8db1bac	604	unsigned char active_low_int;
Ucial	0:7365f8db1bac	605	/* 1 for latched interrupts. */
Ucial	0:7365f8db1bac	606	unsigned char latched_int;
Ucial	0:7365f8db1bac	607	/* 1 if DMP is enabled. */
Ucial	0:7365f8db1bac	608	unsigned char dmp_on;
Ucial	0:7365f8db1bac	609	/* Ensures that DMP will only be loaded once. */
Ucial	0:7365f8db1bac	610	unsigned char dmp_loaded;
Ucial	0:7365f8db1bac	611	/* Sampling rate used when DMP is enabled. */
Ucial	0:7365f8db1bac	612	unsigned short dmp_sample_rate;
Ucial	0:7365f8db1bac	613	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	614	/* Compass sample rate. */
Ucial	0:7365f8db1bac	615	unsigned short compass_sample_rate;
Ucial	0:7365f8db1bac	616	unsigned char compass_addr;
Ucial	0:7365f8db1bac	617	short mag_sens_adj[3];
Ucial	0:7365f8db1bac	618	#endif
Ucial	0:7365f8db1bac	619	};
Ucial	0:7365f8db1bac	620
Ucial	0:7365f8db1bac	621	/* Information for self-test. */
Ucial	0:7365f8db1bac	622	struct test_s {
Ucial	0:7365f8db1bac	623	unsigned long gyro_sens;
Ucial	0:7365f8db1bac	624	unsigned long accel_sens;
Ucial	0:7365f8db1bac	625	unsigned char reg_rate_div;
Ucial	0:7365f8db1bac	626	unsigned char reg_lpf;
Ucial	0:7365f8db1bac	627	unsigned char reg_gyro_fsr;
Ucial	0:7365f8db1bac	628	unsigned char reg_accel_fsr;
Ucial	0:7365f8db1bac	629	unsigned short wait_ms;
Ucial	0:7365f8db1bac	630	unsigned char packet_thresh;
Ucial	0:7365f8db1bac	631	float min_dps;
Ucial	0:7365f8db1bac	632	float max_dps;
Ucial	0:7365f8db1bac	633	float max_gyro_var;
Ucial	0:7365f8db1bac	634	float min_g;
Ucial	0:7365f8db1bac	635	float max_g;
Ucial	0:7365f8db1bac	636	float max_accel_var;
Ucial	0:7365f8db1bac	637	};
Ucial	0:7365f8db1bac	638
Ucial	0:7365f8db1bac	639	/* Gyro driver state variables. */
Ucial	0:7365f8db1bac	640	struct gyro_state_s {
Ucial	0:7365f8db1bac	641	const struct gyro_reg_s *reg;
Ucial	0:7365f8db1bac	642	const struct hw_s *hw;
Ucial	0:7365f8db1bac	643	struct chip_cfg_s chip_cfg;
Ucial	0:7365f8db1bac	644	const struct test_s *test;
Ucial	0:7365f8db1bac	645	};
Ucial	0:7365f8db1bac	646
Ucial	0:7365f8db1bac	647	/* Filter configurations. */
Ucial	0:7365f8db1bac	648	enum lpf_e {
Ucial	0:7365f8db1bac	649	INV_FILTER_256HZ_NOLPF2 = 0,
Ucial	0:7365f8db1bac	650	INV_FILTER_188HZ,
Ucial	0:7365f8db1bac	651	INV_FILTER_98HZ,
Ucial	0:7365f8db1bac	652	INV_FILTER_42HZ,
Ucial	0:7365f8db1bac	653	INV_FILTER_20HZ,
Ucial	0:7365f8db1bac	654	INV_FILTER_10HZ,
Ucial	0:7365f8db1bac	655	INV_FILTER_5HZ,
Ucial	0:7365f8db1bac	656	INV_FILTER_2100HZ_NOLPF,
Ucial	0:7365f8db1bac	657	NUM_FILTER
Ucial	0:7365f8db1bac	658	};
Ucial	0:7365f8db1bac	659
Ucial	0:7365f8db1bac	660	/* Full scale ranges. */
Ucial	0:7365f8db1bac	661	enum gyro_fsr_e {
Ucial	0:7365f8db1bac	662	INV_FSR_250DPS = 0,
Ucial	0:7365f8db1bac	663	INV_FSR_500DPS,
Ucial	0:7365f8db1bac	664	INV_FSR_1000DPS,
Ucial	0:7365f8db1bac	665	INV_FSR_2000DPS,
Ucial	0:7365f8db1bac	666	NUM_GYRO_FSR
Ucial	0:7365f8db1bac	667	};
Ucial	0:7365f8db1bac	668
Ucial	0:7365f8db1bac	669	/* Full scale ranges. */
Ucial	0:7365f8db1bac	670	enum accel_fsr_e {
Ucial	0:7365f8db1bac	671	INV_FSR_2G = 0,
Ucial	0:7365f8db1bac	672	INV_FSR_4G,
Ucial	0:7365f8db1bac	673	INV_FSR_8G,
Ucial	0:7365f8db1bac	674	INV_FSR_16G,
Ucial	0:7365f8db1bac	675	NUM_ACCEL_FSR
Ucial	0:7365f8db1bac	676	};
Ucial	0:7365f8db1bac	677
Ucial	0:7365f8db1bac	678	/* Clock sources. */
Ucial	0:7365f8db1bac	679	enum clock_sel_e {
Ucial	0:7365f8db1bac	680	INV_CLK_INTERNAL = 0,
Ucial	0:7365f8db1bac	681	INV_CLK_PLL,
Ucial	0:7365f8db1bac	682	NUM_CLK
Ucial	0:7365f8db1bac	683	};
Ucial	0:7365f8db1bac	684
Ucial	0:7365f8db1bac	685	/* Low-power accel wakeup rates. */
Ucial	0:7365f8db1bac	686	enum lp_accel_rate_e {
Ucial	0:7365f8db1bac	687	#if defined MPU6050
Ucial	0:7365f8db1bac	688	INV_LPA_1_25HZ,
Ucial	0:7365f8db1bac	689	INV_LPA_5HZ,
Ucial	0:7365f8db1bac	690	INV_LPA_20HZ,
Ucial	0:7365f8db1bac	691	INV_LPA_40HZ
Ucial	0:7365f8db1bac	692	#elif defined MPU6500
Ucial	0:7365f8db1bac	693	INV_LPA_0_3125HZ,
Ucial	0:7365f8db1bac	694	INV_LPA_0_625HZ,
Ucial	0:7365f8db1bac	695	INV_LPA_1_25HZ,
Ucial	0:7365f8db1bac	696	INV_LPA_2_5HZ,
Ucial	0:7365f8db1bac	697	INV_LPA_5HZ,
Ucial	0:7365f8db1bac	698	INV_LPA_10HZ,
Ucial	0:7365f8db1bac	699	INV_LPA_20HZ,
Ucial	0:7365f8db1bac	700	INV_LPA_40HZ,
Ucial	0:7365f8db1bac	701	INV_LPA_80HZ,
Ucial	0:7365f8db1bac	702	INV_LPA_160HZ,
Ucial	0:7365f8db1bac	703	INV_LPA_320HZ,
Ucial	0:7365f8db1bac	704	INV_LPA_640HZ
Ucial	0:7365f8db1bac	705	#endif
Ucial	0:7365f8db1bac	706	};
Ucial	0:7365f8db1bac	707
Ucial	0:7365f8db1bac	708	#define BIT_I2C_MST_VDDIO (0x80)
Ucial	0:7365f8db1bac	709	#define BIT_FIFO_EN (0x40)
Ucial	0:7365f8db1bac	710	#define BIT_DMP_EN (0x80)
Ucial	0:7365f8db1bac	711	#define BIT_FIFO_RST (0x04)
Ucial	0:7365f8db1bac	712	#define BIT_DMP_RST (0x08)
Ucial	0:7365f8db1bac	713	#define BIT_FIFO_OVERFLOW (0x10)
Ucial	0:7365f8db1bac	714	#define BIT_DATA_RDY_EN (0x01)
Ucial	0:7365f8db1bac	715	#define BIT_DMP_INT_EN (0x02)
Ucial	0:7365f8db1bac	716	#define BIT_MOT_INT_EN (0x40)
Ucial	0:7365f8db1bac	717	#define BITS_FSR (0x18)
Ucial	0:7365f8db1bac	718	#define BITS_LPF (0x07)
Ucial	0:7365f8db1bac	719	#define BITS_HPF (0x07)
Ucial	0:7365f8db1bac	720	#define BITS_CLK (0x07)
Ucial	0:7365f8db1bac	721	#define BIT_FIFO_SIZE_1024 (0x40)
Ucial	0:7365f8db1bac	722	#define BIT_FIFO_SIZE_2048 (0x80)
Ucial	0:7365f8db1bac	723	#define BIT_FIFO_SIZE_4096 (0xC0)
Ucial	0:7365f8db1bac	724	#define BIT_RESET (0x80)
Ucial	0:7365f8db1bac	725	#define BIT_SLEEP (0x40)
Ucial	0:7365f8db1bac	726	#define BIT_S0_DELAY_EN (0x01)
Ucial	0:7365f8db1bac	727	#define BIT_S2_DELAY_EN (0x04)
Ucial	0:7365f8db1bac	728	#define BITS_SLAVE_LENGTH (0x0F)
Ucial	0:7365f8db1bac	729	#define BIT_SLAVE_BYTE_SW (0x40)
Ucial	0:7365f8db1bac	730	#define BIT_SLAVE_GROUP (0x10)
Ucial	0:7365f8db1bac	731	#define BIT_SLAVE_EN (0x80)
Ucial	0:7365f8db1bac	732	#define BIT_I2C_READ (0x80)
Ucial	0:7365f8db1bac	733	#define BITS_I2C_MASTER_DLY (0x1F)
Ucial	0:7365f8db1bac	734	#define BIT_AUX_IF_EN (0x20)
Ucial	0:7365f8db1bac	735	#define BIT_ACTL (0x80)
Ucial	0:7365f8db1bac	736	#define BIT_LATCH_EN (0x20)
Ucial	0:7365f8db1bac	737	#define BIT_ANY_RD_CLR (0x10)
Ucial	0:7365f8db1bac	738	#define BIT_BYPASS_EN (0x02)
Ucial	0:7365f8db1bac	739	#define BITS_WOM_EN (0xC0)
Ucial	0:7365f8db1bac	740	#define BIT_LPA_CYCLE (0x20)
Ucial	0:7365f8db1bac	741	#define BIT_STBY_XA (0x20)
Ucial	0:7365f8db1bac	742	#define BIT_STBY_YA (0x10)
Ucial	0:7365f8db1bac	743	#define BIT_STBY_ZA (0x08)
Ucial	0:7365f8db1bac	744	#define BIT_STBY_XG (0x04)
Ucial	0:7365f8db1bac	745	#define BIT_STBY_YG (0x02)
Ucial	0:7365f8db1bac	746	#define BIT_STBY_ZG (0x01)
Ucial	0:7365f8db1bac	747	#define BIT_STBY_XYZA (BIT_STBY_XA \| BIT_STBY_YA \| BIT_STBY_ZA)
Ucial	0:7365f8db1bac	748	#define BIT_STBY_XYZG (BIT_STBY_XG \| BIT_STBY_YG \| BIT_STBY_ZG)
Ucial	0:7365f8db1bac	749
Ucial	0:7365f8db1bac	750	#if defined AK8975_SECONDARY
Ucial	0:7365f8db1bac	751	#define SUPPORTS_AK89xx_HIGH_SENS (0x00)
Ucial	0:7365f8db1bac	752	#define AK89xx_FSR (9830)
Ucial	0:7365f8db1bac	753	#elif defined AK8963_SECONDARY
Ucial	0:7365f8db1bac	754	#define SUPPORTS_AK89xx_HIGH_SENS (0x10)
Ucial	0:7365f8db1bac	755	#define AK89xx_FSR (4915)
Ucial	0:7365f8db1bac	756	#endif
Ucial	0:7365f8db1bac	757
Ucial	0:7365f8db1bac	758	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	759	#define AKM_REG_WHOAMI (0x00)
Ucial	0:7365f8db1bac	760
Ucial	0:7365f8db1bac	761	#define AKM_REG_ST1 (0x02)
Ucial	0:7365f8db1bac	762	#define AKM_REG_HXL (0x03)
Ucial	0:7365f8db1bac	763	#define AKM_REG_ST2 (0x09)
Ucial	0:7365f8db1bac	764
Ucial	0:7365f8db1bac	765	#define AKM_REG_CNTL (0x0A)
Ucial	0:7365f8db1bac	766	#define AKM_REG_ASTC (0x0C)
Ucial	0:7365f8db1bac	767	#define AKM_REG_ASAX (0x10)
Ucial	0:7365f8db1bac	768	#define AKM_REG_ASAY (0x11)
Ucial	0:7365f8db1bac	769	#define AKM_REG_ASAZ (0x12)
Ucial	0:7365f8db1bac	770
Ucial	0:7365f8db1bac	771	#define AKM_DATA_READY (0x01)
Ucial	0:7365f8db1bac	772	#define AKM_DATA_OVERRUN (0x02)
Ucial	0:7365f8db1bac	773	#define AKM_OVERFLOW (0x80)
Ucial	0:7365f8db1bac	774	#define AKM_DATA_ERROR (0x40)
Ucial	0:7365f8db1bac	775
Ucial	0:7365f8db1bac	776	#define AKM_BIT_SELF_TEST (0x40)
Ucial	0:7365f8db1bac	777
Ucial	0:7365f8db1bac	778	#define AKM_POWER_DOWN (0x00 \| SUPPORTS_AK89xx_HIGH_SENS)
Ucial	0:7365f8db1bac	779	#define AKM_SINGLE_MEASUREMENT (0x01 \| SUPPORTS_AK89xx_HIGH_SENS)
Ucial	0:7365f8db1bac	780	#define AKM_FUSE_ROM_ACCESS (0x0F \| SUPPORTS_AK89xx_HIGH_SENS)
Ucial	0:7365f8db1bac	781	#define AKM_MODE_SELF_TEST (0x08 \| SUPPORTS_AK89xx_HIGH_SENS)
Ucial	0:7365f8db1bac	782
Ucial	0:7365f8db1bac	783	#define AKM_WHOAMI (0x48)
Ucial	0:7365f8db1bac	784	#endif
Ucial	0:7365f8db1bac	785
Ucial	0:7365f8db1bac	786	#if defined MPU6050
Ucial	0:7365f8db1bac	787	//const struct gyro_reg_s reg = {
Ucial	0:7365f8db1bac	788	// .who_am_i = 0x75,
Ucial	0:7365f8db1bac	789	// .rate_div = 0x19,
Ucial	0:7365f8db1bac	790	// .lpf = 0x1A,
Ucial	0:7365f8db1bac	791	// .prod_id = 0x0C,
Ucial	0:7365f8db1bac	792	// .user_ctrl = 0x6A,
Ucial	0:7365f8db1bac	793	// .fifo_en = 0x23,
Ucial	0:7365f8db1bac	794	// .gyro_cfg = 0x1B,
Ucial	0:7365f8db1bac	795	// .accel_cfg = 0x1C,
Ucial	0:7365f8db1bac	796	// .motion_thr = 0x1F,
Ucial	0:7365f8db1bac	797	// .motion_dur = 0x20,
Ucial	0:7365f8db1bac	798	// .fifo_count_h = 0x72,
Ucial	0:7365f8db1bac	799	// .fifo_r_w = 0x74,
Ucial	0:7365f8db1bac	800	// .raw_gyro = 0x43,
Ucial	0:7365f8db1bac	801	// .raw_accel = 0x3B,
Ucial	0:7365f8db1bac	802	// .temp = 0x41,
Ucial	0:7365f8db1bac	803	// .int_enable = 0x38,
Ucial	0:7365f8db1bac	804	// .dmp_int_status = 0x39,
Ucial	0:7365f8db1bac	805	// .int_status = 0x3A,
Ucial	0:7365f8db1bac	806	// .pwr_mgmt_1 = 0x6B,
Ucial	0:7365f8db1bac	807	// .pwr_mgmt_2 = 0x6C,
Ucial	0:7365f8db1bac	808	// .int_pin_cfg = 0x37,
Ucial	0:7365f8db1bac	809	// .mem_r_w = 0x6F,
Ucial	0:7365f8db1bac	810	// .accel_offs = 0x06,
Ucial	0:7365f8db1bac	811	// .i2c_mst = 0x24,
Ucial	0:7365f8db1bac	812	// .bank_sel = 0x6D,
Ucial	0:7365f8db1bac	813	// .mem_start_addr = 0x6E,
Ucial	0:7365f8db1bac	814	// .prgm_start_h = 0x70
Ucial	0:7365f8db1bac	815	//#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	816	// ,.raw_compass = 0x49,
Ucial	0:7365f8db1bac	817	// .yg_offs_tc = 0x01,
Ucial	0:7365f8db1bac	818	// .s0_addr = 0x25,
Ucial	0:7365f8db1bac	819	// .s0_reg = 0x26,
Ucial	0:7365f8db1bac	820	// .s0_ctrl = 0x27,
Ucial	0:7365f8db1bac	821	// .s1_addr = 0x28,
Ucial	0:7365f8db1bac	822	// .s1_reg = 0x29,
Ucial	0:7365f8db1bac	823	// .s1_ctrl = 0x2A,
Ucial	0:7365f8db1bac	824	// .s4_ctrl = 0x34,
Ucial	0:7365f8db1bac	825	// .s0_do = 0x63,
Ucial	0:7365f8db1bac	826	// .s1_do = 0x64,
Ucial	0:7365f8db1bac	827	// .i2c_delay_ctrl = 0x67
Ucial	0:7365f8db1bac	828	//#endif
Ucial	0:7365f8db1bac	829	//};
Ucial	0:7365f8db1bac	830	const struct gyro_reg_s reg = {
Ucial	0:7365f8db1bac	831	0x75, //who_am_i
Ucial	0:7365f8db1bac	832	0x19, //rate_div
Ucial	0:7365f8db1bac	833	0x1A, //lpf
Ucial	0:7365f8db1bac	834	0x0C, //prod_id
Ucial	0:7365f8db1bac	835	0x6A, //user_ctrl
Ucial	0:7365f8db1bac	836	0x23, //fifo_en
Ucial	0:7365f8db1bac	837	0x1B, //gyro_cfg
Ucial	0:7365f8db1bac	838	0x1C, //accel_cfg
Ucial	0:7365f8db1bac	839	0x1F, // motion_thr
Ucial	0:7365f8db1bac	840	0x20, // motion_dur
Ucial	0:7365f8db1bac	841	0x72, // fifo_count_h
Ucial	0:7365f8db1bac	842	0x74, // fifo_r_w
Ucial	0:7365f8db1bac	843	0x43, // raw_gyro
Ucial	0:7365f8db1bac	844	0x3B, // raw_accel
Ucial	0:7365f8db1bac	845	0x41, // temp
Ucial	0:7365f8db1bac	846	0x38, // int_enable
Ucial	0:7365f8db1bac	847	0x39, // dmp_int_status
Ucial	0:7365f8db1bac	848	0x3A, // int_status
Ucial	0:7365f8db1bac	849	0x6B, // pwr_mgmt_1
Ucial	0:7365f8db1bac	850	0x6C, // pwr_mgmt_2
Ucial	0:7365f8db1bac	851	0x37, // int_pin_cfg
Ucial	0:7365f8db1bac	852	0x6F, // mem_r_w
Ucial	0:7365f8db1bac	853	0x06, // accel_offs
Ucial	0:7365f8db1bac	854	0x24, // i2c_mst
Ucial	0:7365f8db1bac	855	0x6D, // bank_sel
Ucial	0:7365f8db1bac	856	0x6E, // mem_start_addr
Ucial	0:7365f8db1bac	857	0x70 // prgm_start_h
Ucial	0:7365f8db1bac	858	};
Ucial	0:7365f8db1bac	859
Ucial	0:7365f8db1bac	860	//const struct hw_s hw = {
Ucial	0:7365f8db1bac	861	// .addr = 0x68,
Ucial	0:7365f8db1bac	862	// .max_fifo = 1024,
Ucial	0:7365f8db1bac	863	// .num_reg = 118,
Ucial	0:7365f8db1bac	864	// .temp_sens = 340,
Ucial	0:7365f8db1bac	865	// .temp_offset = -521,
Ucial	0:7365f8db1bac	866	// .bank_size = 256
Ucial	0:7365f8db1bac	867	//#if defined AK89xx_SECONDARY
Ucial	0:7365f8db1bac	868	// ,.compass_fsr = AK89xx_FSR
Ucial	0:7365f8db1bac	869	//#endif
Ucial	0:7365f8db1bac	870	//};
Ucial	0:7365f8db1bac	871	const struct hw_s hw={
Ucial	0:7365f8db1bac	872	0x68, //addr
Ucial	0:7365f8db1bac	873	1024, //max_fifo
Ucial	0:7365f8db1bac	874	118, //num_reg
Ucial	0:7365f8db1bac	875	340, //temp_sens
Ucial	0:7365f8db1bac	876	-521, //temp_offset
Ucial	0:7365f8db1bac	877	256 //bank_size
Ucial	0:7365f8db1bac	878	};
Ucial	0:7365f8db1bac	879
Ucial	0:7365f8db1bac	880	//const struct test_s test = {
Ucial	0:7365f8db1bac	881	// .gyro_sens = 32768/250,
Ucial	0:7365f8db1bac	882	// .accel_sens = 32768/16,
Ucial	0:7365f8db1bac	883	// .reg_rate_div = 0, /* 1kHz. */
Ucial	0:7365f8db1bac	884	// .reg_lpf = 1, /* 188Hz. */
Ucial	0:7365f8db1bac	885	// .reg_gyro_fsr = 0, /* 250dps. */
Ucial	0:7365f8db1bac	886	// .reg_accel_fsr = 0x18, /* 16g. */
Ucial	0:7365f8db1bac	887	// .wait_ms = 50,
Ucial	0:7365f8db1bac	888	// .packet_thresh = 5, /* 5% */
Ucial	0:7365f8db1bac	889	// .min_dps = 10.f,
Ucial	0:7365f8db1bac	890	// .max_dps = 105.f,
Ucial	0:7365f8db1bac	891	// .max_gyro_var = 0.14f,
Ucial	0:7365f8db1bac	892	// .min_g = 0.3f,
Ucial	0:7365f8db1bac	893	// .max_g = 0.95f,
Ucial	0:7365f8db1bac	894	// .max_accel_var = 0.14f
Ucial	0:7365f8db1bac	895	//};
Ucial	0:7365f8db1bac	896	const struct test_s test={
Ucial	0:7365f8db1bac	897	32768/250, //gyro_sens
Ucial	0:7365f8db1bac	898	32768/16, // accel_sens
Ucial	0:7365f8db1bac	899	0, // reg_rate_div
Ucial	0:7365f8db1bac	900	1, // reg_lpf
Ucial	0:7365f8db1bac	901	0, // reg_gyro_fsr
Ucial	0:7365f8db1bac	902	0x18, // reg_accel_fsr
Ucial	0:7365f8db1bac	903	50, // wait_ms
Ucial	0:7365f8db1bac	904	5, // packet_thresh
Ucial	0:7365f8db1bac	905	10.0f, // min_dps
Ucial	0:7365f8db1bac	906	105.0f, // max_dps
Ucial	0:7365f8db1bac	907	0.14f, // max_gyro_var
Ucial	0:7365f8db1bac	908	0.3f, // min_g
Ucial	0:7365f8db1bac	909	0.95f, // max_g
Ucial	0:7365f8db1bac	910	0.14f // max_accel_var
Ucial	0:7365f8db1bac	911	};
Ucial	0:7365f8db1bac	912
Ucial	0:7365f8db1bac	913	//static struct gyro_state_s st = {
Ucial	0:7365f8db1bac	914	// .reg = &reg,
Ucial	0:7365f8db1bac	915	// .hw = &hw,
Ucial	0:7365f8db1bac	916	// .test = &test
Ucial	0:7365f8db1bac	917	//};
Ucial	0:7365f8db1bac	918	static struct gyro_state_s st={
Ucial	0:7365f8db1bac	919	&reg,
Ucial	0:7365f8db1bac	920	&hw,
Ucial	0:7365f8db1bac	921	{0},
Ucial	0:7365f8db1bac	922	&test
Ucial	0:7365f8db1bac	923	};
Ucial	0:7365f8db1bac	924
Ucial	0:7365f8db1bac	925
Ucial	0:7365f8db1bac	926	#elif defined MPU6500
Ucial	0:7365f8db1bac	927	const struct gyro_reg_s reg = {
Ucial	0:7365f8db1bac	928	.who_am_i = 0x75,
Ucial	0:7365f8db1bac	929	.rate_div = 0x19,
Ucial	0:7365f8db1bac	930	.lpf = 0x1A,
Ucial	0:7365f8db1bac	931	.prod_id = 0x0C,
Ucial	0:7365f8db1bac	932	.user_ctrl = 0x6A,
Ucial	0:7365f8db1bac	933	.fifo_en = 0x23,
Ucial	0:7365f8db1bac	934	.gyro_cfg = 0x1B,
Ucial	0:7365f8db1bac	935	.accel_cfg = 0x1C,
Ucial	0:7365f8db1bac	936	.accel_cfg2 = 0x1D,
Ucial	0:7365f8db1bac	937	.lp_accel_odr = 0x1E,
Ucial	0:7365f8db1bac	938	.motion_thr = 0x1F,
Ucial	0:7365f8db1bac	939	.motion_dur = 0x20,
Ucial	0:7365f8db1bac	940	.fifo_count_h = 0x72,
Ucial	0:7365f8db1bac	941	.fifo_r_w = 0x74,
Ucial	0:7365f8db1bac	942	.raw_gyro = 0x43,
Ucial	0:7365f8db1bac	943	.raw_accel = 0x3B,
Ucial	0:7365f8db1bac	944	.temp = 0x41,
Ucial	0:7365f8db1bac	945	.int_enable = 0x38,
Ucial	0:7365f8db1bac	946	.dmp_int_status = 0x39,
Ucial	0:7365f8db1bac	947	.int_status = 0x3A,
Ucial	0:7365f8db1bac	948	.accel_intel = 0x69,
Ucial	0:7365f8db1bac	949	.pwr_mgmt_1 = 0x6B,
Ucial	0:7365f8db1bac	950	.pwr_mgmt_2 = 0x6C,
Ucial	0:7365f8db1bac	951	.int_pin_cfg = 0x37,
Ucial	0:7365f8db1bac	952	.mem_r_w = 0x6F,
Ucial	0:7365f8db1bac	953	.accel_offs = 0x77,
Ucial	0:7365f8db1bac	954	.i2c_mst = 0x24,
Ucial	0:7365f8db1bac	955	.bank_sel = 0x6D,
Ucial	0:7365f8db1bac	956	.mem_start_addr = 0x6E,
Ucial	0:7365f8db1bac	957	.prgm_start_h = 0x70
Ucial	0:7365f8db1bac	958	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	959	,.raw_compass = 0x49,
Ucial	0:7365f8db1bac	960	.s0_addr = 0x25,
Ucial	0:7365f8db1bac	961	.s0_reg = 0x26,
Ucial	0:7365f8db1bac	962	.s0_ctrl = 0x27,
Ucial	0:7365f8db1bac	963	.s1_addr = 0x28,
Ucial	0:7365f8db1bac	964	.s1_reg = 0x29,
Ucial	0:7365f8db1bac	965	.s1_ctrl = 0x2A,
Ucial	0:7365f8db1bac	966	.s4_ctrl = 0x34,
Ucial	0:7365f8db1bac	967	.s0_do = 0x63,
Ucial	0:7365f8db1bac	968	.s1_do = 0x64,
Ucial	0:7365f8db1bac	969	.i2c_delay_ctrl = 0x67
Ucial	0:7365f8db1bac	970	#endif
Ucial	0:7365f8db1bac	971	};
Ucial	0:7365f8db1bac	972	const struct hw_s hw = {
Ucial	0:7365f8db1bac	973	.addr = 0x68,
Ucial	0:7365f8db1bac	974	.max_fifo = 1024,
Ucial	0:7365f8db1bac	975	.num_reg = 128,
Ucial	0:7365f8db1bac	976	.temp_sens = 321,
Ucial	0:7365f8db1bac	977	.temp_offset = 0,
Ucial	0:7365f8db1bac	978	.bank_size = 256
Ucial	0:7365f8db1bac	979	#if defined AK89xx_SECONDARY
Ucial	0:7365f8db1bac	980	,.compass_fsr = AK89xx_FSR
Ucial	0:7365f8db1bac	981	#endif
Ucial	0:7365f8db1bac	982	};
Ucial	0:7365f8db1bac	983
Ucial	0:7365f8db1bac	984	const struct test_s test = {
Ucial	0:7365f8db1bac	985	.gyro_sens = 32768/250,
Ucial	0:7365f8db1bac	986	.accel_sens = 32768/16,
Ucial	0:7365f8db1bac	987	.reg_rate_div = 0, /* 1kHz. */
Ucial	0:7365f8db1bac	988	.reg_lpf = 1, /* 188Hz. */
Ucial	0:7365f8db1bac	989	.reg_gyro_fsr = 0, /* 250dps. */
Ucial	0:7365f8db1bac	990	.reg_accel_fsr = 0x18, /* 16g. */
Ucial	0:7365f8db1bac	991	.wait_ms = 50,
Ucial	0:7365f8db1bac	992	.packet_thresh = 5, /* 5% */
Ucial	0:7365f8db1bac	993	.min_dps = 10.f,
Ucial	0:7365f8db1bac	994	.max_dps = 105.f,
Ucial	0:7365f8db1bac	995	.max_gyro_var = 0.14f,
Ucial	0:7365f8db1bac	996	.min_g = 0.3f,
Ucial	0:7365f8db1bac	997	.max_g = 0.95f,
Ucial	0:7365f8db1bac	998	.max_accel_var = 0.14f
Ucial	0:7365f8db1bac	999	};
Ucial	0:7365f8db1bac	1000
Ucial	0:7365f8db1bac	1001	static struct gyro_state_s st = {
Ucial	0:7365f8db1bac	1002	.reg = &reg,
Ucial	0:7365f8db1bac	1003	.hw = &hw,
Ucial	0:7365f8db1bac	1004	.test = &test
Ucial	0:7365f8db1bac	1005	};
Ucial	0:7365f8db1bac	1006	#endif
Ucial	0:7365f8db1bac	1007
Ucial	0:7365f8db1bac	1008	#define MAX_PACKET_LENGTH (12)
Ucial	0:7365f8db1bac	1009
Ucial	0:7365f8db1bac	1010	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	1011	static int setup_compass(void);
Ucial	0:7365f8db1bac	1012	#define MAX_COMPASS_SAMPLE_RATE (100)
Ucial	0:7365f8db1bac	1013	#endif
Ucial	0:7365f8db1bac	1014
Ucial	0:7365f8db1bac	1015	/**
Ucial	0:7365f8db1bac	1016	* @brief Enable/disable data ready interrupt.
Ucial	0:7365f8db1bac	1017	* If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready
Ucial	0:7365f8db1bac	1018	* interrupt is used.
Ucial	0:7365f8db1bac	1019	* @param[in] enable 1 to enable interrupt.
Ucial	0:7365f8db1bac	1020	* @return 0 if successful.
Ucial	0:7365f8db1bac	1021	*/
Ucial	0:7365f8db1bac	1022	static int set_int_enable(unsigned char enable)
Ucial	0:7365f8db1bac	1023	{
Ucial	0:7365f8db1bac	1024	unsigned char tmp;
Ucial	0:7365f8db1bac	1025
Ucial	0:7365f8db1bac	1026	if (st.chip_cfg.dmp_on) {
Ucial	0:7365f8db1bac	1027	if (enable)
Ucial	0:7365f8db1bac	1028	tmp = BIT_DMP_INT_EN;
Ucial	0:7365f8db1bac	1029	else
Ucial	0:7365f8db1bac	1030	tmp = 0x00;
Ucial	0:7365f8db1bac	1031	if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
Ucial	0:7365f8db1bac	1032	return -1;
Ucial	0:7365f8db1bac	1033	st.chip_cfg.int_enable = tmp;
Ucial	0:7365f8db1bac	1034	} else {
Ucial	0:7365f8db1bac	1035	if (!st.chip_cfg.sensors)
Ucial	0:7365f8db1bac	1036	return -1;
Ucial	0:7365f8db1bac	1037	if (enable && st.chip_cfg.int_enable)
Ucial	0:7365f8db1bac	1038	return 0;
Ucial	0:7365f8db1bac	1039	if (enable)
Ucial	0:7365f8db1bac	1040	tmp = BIT_DATA_RDY_EN;
Ucial	0:7365f8db1bac	1041	else
Ucial	0:7365f8db1bac	1042	tmp = 0x00;
Ucial	0:7365f8db1bac	1043	if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
Ucial	0:7365f8db1bac	1044	return -1;
Ucial	0:7365f8db1bac	1045	st.chip_cfg.int_enable = tmp;
Ucial	0:7365f8db1bac	1046	}
Ucial	0:7365f8db1bac	1047	return 0;
Ucial	0:7365f8db1bac	1048	}
Ucial	0:7365f8db1bac	1049
Ucial	0:7365f8db1bac	1050	/**
Ucial	0:7365f8db1bac	1051	* @brief Register dump for testing.
Ucial	0:7365f8db1bac	1052	* @return 0 if successful.
Ucial	0:7365f8db1bac	1053	*/
Ucial	0:7365f8db1bac	1054	int mpu_reg_dump(void)
Ucial	0:7365f8db1bac	1055	{
Ucial	0:7365f8db1bac	1056	unsigned char ii;
Ucial	0:7365f8db1bac	1057	unsigned char data;
Ucial	0:7365f8db1bac	1058
Ucial	0:7365f8db1bac	1059	for (ii = 0; ii < st.hw->num_reg; ii++) {
Ucial	0:7365f8db1bac	1060	if (ii == st.reg->fifo_r_w \|\| ii == st.reg->mem_r_w)
Ucial	0:7365f8db1bac	1061	continue;
Ucial	0:7365f8db1bac	1062	if (i2c_read(st.hw->addr, ii, 1, &data))
Ucial	0:7365f8db1bac	1063	return -1;
Ucial	0:7365f8db1bac	1064	//("%#5x: %#5x\r\n", ii, data);
Ucial	0:7365f8db1bac	1065	}
Ucial	0:7365f8db1bac	1066	return 0;
Ucial	0:7365f8db1bac	1067	}
Ucial	0:7365f8db1bac	1068
Ucial	0:7365f8db1bac	1069	/**
Ucial	0:7365f8db1bac	1070	* @brief Read from a single register.
Ucial	0:7365f8db1bac	1071	* NOTE: The memory and FIFO read/write registers cannot be accessed.
Ucial	0:7365f8db1bac	1072	* @param[in] reg Register address.
Ucial	0:7365f8db1bac	1073	* @param[out] data Register data.
Ucial	0:7365f8db1bac	1074	* @return 0 if successful.
Ucial	0:7365f8db1bac	1075	*/
Ucial	0:7365f8db1bac	1076	int mpu_read_reg(unsigned char reg, unsigned char *data)
Ucial	0:7365f8db1bac	1077	{
Ucial	0:7365f8db1bac	1078	if (reg == st.reg->fifo_r_w \|\| reg == st.reg->mem_r_w)
Ucial	0:7365f8db1bac	1079	return -1;
Ucial	0:7365f8db1bac	1080	if (reg >= st.hw->num_reg)
Ucial	0:7365f8db1bac	1081	return -1;
Ucial	0:7365f8db1bac	1082	return i2c_read(st.hw->addr, reg, 1, data);
Ucial	0:7365f8db1bac	1083	}
Ucial	0:7365f8db1bac	1084
Ucial	0:7365f8db1bac	1085	/**
Ucial	0:7365f8db1bac	1086	* @brief Initialize hardware.
Ucial	0:7365f8db1bac	1087	* Initial configuration:\n
Ucial	0:7365f8db1bac	1088	* Gyro FSR: +/- 2000DPS\n
Ucial	0:7365f8db1bac	1089	* Accel FSR +/- 2G\n
Ucial	0:7365f8db1bac	1090	* DLPF: 42Hz\n
Ucial	0:7365f8db1bac	1091	* FIFO rate: 50Hz\n
Ucial	0:7365f8db1bac	1092	* Clock source: Gyro PLL\n
Ucial	0:7365f8db1bac	1093	* FIFO: Disabled.\n
Ucial	0:7365f8db1bac	1094	* Data ready interrupt: Disabled, active low, unlatched.
Ucial	0:7365f8db1bac	1095	* @param[in] int_param Platform-specific parameters to interrupt API.
Ucial	0:7365f8db1bac	1096	* @return 0 if successful.
Ucial	0:7365f8db1bac	1097	*/
Ucial	0:7365f8db1bac	1098	int mpu_init(void)
Ucial	0:7365f8db1bac	1099	{
Ucial	0:7365f8db1bac	1100	unsigned char data[6], rev;
Ucial	0:7365f8db1bac	1101
Ucial	0:7365f8db1bac	1102	/* Reset device. */
Ucial	0:7365f8db1bac	1103	data[0] = BIT_RESET;
Ucial	0:7365f8db1bac	1104	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
Ucial	0:7365f8db1bac	1105	return -1;
Ucial	0:7365f8db1bac	1106	delay_ms(100);
Ucial	0:7365f8db1bac	1107
Ucial	0:7365f8db1bac	1108	/* Wake up chip. */
Ucial	0:7365f8db1bac	1109	data[0] = 0x00;
Ucial	0:7365f8db1bac	1110	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
Ucial	0:7365f8db1bac	1111	return -1;
Ucial	0:7365f8db1bac	1112
Ucial	0:7365f8db1bac	1113	#if defined MPU6050
Ucial	0:7365f8db1bac	1114	/* Check product revision. */
Ucial	0:7365f8db1bac	1115	if (i2c_read(st.hw->addr, st.reg->accel_offs, 6, data))
Ucial	0:7365f8db1bac	1116	return -1;
Ucial	0:7365f8db1bac	1117	rev = ((data[5] & 0x01) << 2) \| ((data[3] & 0x01) << 1) \|
Ucial	0:7365f8db1bac	1118	(data[1] & 0x01);
Ucial	0:7365f8db1bac	1119
Ucial	0:7365f8db1bac	1120	if (rev) {
Ucial	0:7365f8db1bac	1121	/* Congrats, these parts are better. */
Ucial	0:7365f8db1bac	1122	if (rev == 1)
Ucial	0:7365f8db1bac	1123	st.chip_cfg.accel_half = 1;
Ucial	0:7365f8db1bac	1124	else if (rev == 2)
Ucial	0:7365f8db1bac	1125	st.chip_cfg.accel_half = 0;
Ucial	0:7365f8db1bac	1126	else {
Ucial	0:7365f8db1bac	1127	//("Unsupported software product rev %d.\n", rev);
Ucial	0:7365f8db1bac	1128	return -1;
Ucial	0:7365f8db1bac	1129	}
Ucial	0:7365f8db1bac	1130	} else {
Ucial	0:7365f8db1bac	1131	if (i2c_read(st.hw->addr, st.reg->prod_id, 1, data))
Ucial	0:7365f8db1bac	1132	return -1;
Ucial	0:7365f8db1bac	1133	rev = data[0] & 0x0F;
Ucial	0:7365f8db1bac	1134	if (!rev) {
Ucial	0:7365f8db1bac	1135	//("Product ID read as 0 indicates device is either "
Ucial	0:7365f8db1bac	1136	//"incompatible or an MPU3050.\n");
Ucial	0:7365f8db1bac	1137	return -1;
Ucial	0:7365f8db1bac	1138	} else if (rev == 4) {
Ucial	0:7365f8db1bac	1139	//("Half sensitivity part found.\n");
Ucial	0:7365f8db1bac	1140	st.chip_cfg.accel_half = 1;
Ucial	0:7365f8db1bac	1141	} else
Ucial	0:7365f8db1bac	1142	st.chip_cfg.accel_half = 0;
Ucial	0:7365f8db1bac	1143	}
Ucial	0:7365f8db1bac	1144	#elif defined MPU6500
Ucial	0:7365f8db1bac	1145	#define MPU6500_MEM_REV_ADDR (0x17)
Ucial	0:7365f8db1bac	1146	if (mpu_read_mem(MPU6500_MEM_REV_ADDR, 1, &rev))
Ucial	0:7365f8db1bac	1147	return -1;
Ucial	0:7365f8db1bac	1148	if (rev == 0x1)
Ucial	0:7365f8db1bac	1149	st.chip_cfg.accel_half = 0;
Ucial	0:7365f8db1bac	1150	else {
Ucial	0:7365f8db1bac	1151	//("Unsupported software product rev %d.\n", rev);
Ucial	0:7365f8db1bac	1152	return -1;
Ucial	0:7365f8db1bac	1153	}
Ucial	0:7365f8db1bac	1154
Ucial	0:7365f8db1bac	1155	/* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the
Ucial	0:7365f8db1bac	1156	* first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO.
Ucial	0:7365f8db1bac	1157	*/
Ucial	0:7365f8db1bac	1158	data[0] = BIT_FIFO_SIZE_1024 \| 0x8;
Ucial	0:7365f8db1bac	1159	if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, data))
Ucial	0:7365f8db1bac	1160	return -1;
Ucial	0:7365f8db1bac	1161	#endif
Ucial	0:7365f8db1bac	1162
Ucial	0:7365f8db1bac	1163	/* Set to invalid values to ensure no I2C writes are skipped. */
Ucial	0:7365f8db1bac	1164	st.chip_cfg.sensors = 0xFF;
Ucial	0:7365f8db1bac	1165	st.chip_cfg.gyro_fsr = 0xFF;
Ucial	0:7365f8db1bac	1166	st.chip_cfg.accel_fsr = 0xFF;
Ucial	0:7365f8db1bac	1167	st.chip_cfg.lpf = 0xFF;
Ucial	0:7365f8db1bac	1168	st.chip_cfg.sample_rate = 0xFFFF;
Ucial	0:7365f8db1bac	1169	st.chip_cfg.fifo_enable = 0xFF;
Ucial	0:7365f8db1bac	1170	st.chip_cfg.bypass_mode = 0xFF;
Ucial	0:7365f8db1bac	1171	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	1172	st.chip_cfg.compass_sample_rate = 0xFFFF;
Ucial	0:7365f8db1bac	1173	#endif
Ucial	0:7365f8db1bac	1174	/* mpu_set_sensors always preserves this setting. */
Ucial	0:7365f8db1bac	1175	st.chip_cfg.clk_src = INV_CLK_PLL;
Ucial	0:7365f8db1bac	1176	/* Handled in next call to mpu_set_bypass. */
Ucial	0:7365f8db1bac	1177	st.chip_cfg.active_low_int = 1;
Ucial	0:7365f8db1bac	1178	st.chip_cfg.latched_int = 0;
Ucial	0:7365f8db1bac	1179	st.chip_cfg.int_motion_only = 0;
Ucial	0:7365f8db1bac	1180	st.chip_cfg.lp_accel_mode = 0;
Ucial	0:7365f8db1bac	1181	memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache));
Ucial	0:7365f8db1bac	1182	st.chip_cfg.dmp_on = 0;
Ucial	0:7365f8db1bac	1183	st.chip_cfg.dmp_loaded = 0;
Ucial	0:7365f8db1bac	1184	st.chip_cfg.dmp_sample_rate = 0;
Ucial	0:7365f8db1bac	1185
Ucial	0:7365f8db1bac	1186	if (mpu_set_gyro_fsr(2000))
Ucial	0:7365f8db1bac	1187	return -1;
Ucial	0:7365f8db1bac	1188	if (mpu_set_accel_fsr(2))
Ucial	0:7365f8db1bac	1189	return -1;
Ucial	0:7365f8db1bac	1190	if (mpu_set_lpf(42))
Ucial	0:7365f8db1bac	1191	return -1;
Ucial	0:7365f8db1bac	1192	if (mpu_set_sample_rate(50))
Ucial	0:7365f8db1bac	1193	return -1;
Ucial	0:7365f8db1bac	1194	if (mpu_configure_fifo(0))
Ucial	0:7365f8db1bac	1195	return -1;
Ucial	0:7365f8db1bac	1196
Ucial	0:7365f8db1bac	1197	// if (int_param)
Ucial	0:7365f8db1bac	1198	// reg_int_cb(int_param);
Ucial	0:7365f8db1bac	1199
Ucial	0:7365f8db1bac	1200	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	1201	setup_compass();
Ucial	0:7365f8db1bac	1202	if (mpu_set_compass_sample_rate(10))
Ucial	0:7365f8db1bac	1203	return -1;
Ucial	0:7365f8db1bac	1204	#else
Ucial	0:7365f8db1bac	1205	/* Already disabled by setup_compass. */
Ucial	0:7365f8db1bac	1206	if (mpu_set_bypass(0))
Ucial	0:7365f8db1bac	1207	return -1;
Ucial	0:7365f8db1bac	1208	#endif
Ucial	0:7365f8db1bac	1209
Ucial	0:7365f8db1bac	1210	mpu_set_sensors(0);
Ucial	0:7365f8db1bac	1211	return 0;
Ucial	0:7365f8db1bac	1212	}
Ucial	0:7365f8db1bac	1213
Ucial	0:7365f8db1bac	1214	/**
Ucial	0:7365f8db1bac	1215	* @brief Enter low-power accel-only mode.
Ucial	0:7365f8db1bac	1216	* In low-power accel mode, the chip goes to sleep and only wakes up to sample
Ucial	0:7365f8db1bac	1217	* the accelerometer at one of the following frequencies:
Ucial	0:7365f8db1bac	1218	* \n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz
Ucial	0:7365f8db1bac	1219	* \n MPU6500: 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
Ucial	0:7365f8db1bac	1220	* \n If the requested rate is not one listed above, the device will be set to
Ucial	0:7365f8db1bac	1221	* the next highest rate. Requesting a rate above the maximum supported
Ucial	0:7365f8db1bac	1222	* frequency will result in an error.
Ucial	0:7365f8db1bac	1223	* \n To select a fractional wake-up frequency, round down the value passed to
Ucial	0:7365f8db1bac	1224	* @e rate.
Ucial	0:7365f8db1bac	1225	* @param[in] rate Minimum sampling rate, or zero to disable LP
Ucial	0:7365f8db1bac	1226	* accel mode.
Ucial	0:7365f8db1bac	1227	* @return 0 if successful.
Ucial	0:7365f8db1bac	1228	*/
Ucial	0:7365f8db1bac	1229	int mpu_lp_accel_mode(unsigned char rate)
Ucial	0:7365f8db1bac	1230	{
Ucial	0:7365f8db1bac	1231	unsigned char tmp[2];
Ucial	0:7365f8db1bac	1232
Ucial	0:7365f8db1bac	1233	if (rate > 40)
Ucial	0:7365f8db1bac	1234	return -1;
Ucial	0:7365f8db1bac	1235
Ucial	0:7365f8db1bac	1236	if (!rate) {
Ucial	0:7365f8db1bac	1237	mpu_set_int_latched(0);
Ucial	0:7365f8db1bac	1238	tmp[0] = 0;
Ucial	0:7365f8db1bac	1239	tmp[1] = BIT_STBY_XYZG;
Ucial	0:7365f8db1bac	1240	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
Ucial	0:7365f8db1bac	1241	return -1;
Ucial	0:7365f8db1bac	1242	st.chip_cfg.lp_accel_mode = 0;
Ucial	0:7365f8db1bac	1243	return 0;
Ucial	0:7365f8db1bac	1244	}
Ucial	0:7365f8db1bac	1245	/* For LP accel, we automatically configure the hardware to produce latched
Ucial	0:7365f8db1bac	1246	* interrupts. In LP accel mode, the hardware cycles into sleep mode before
Ucial	0:7365f8db1bac	1247	* it gets a chance to deassert the interrupt pin; therefore, we shift this
Ucial	0:7365f8db1bac	1248	* responsibility over to the MCU.
Ucial	0:7365f8db1bac	1249	*
Ucial	0:7365f8db1bac	1250	* Any register read will clear the interrupt.
Ucial	0:7365f8db1bac	1251	*/
Ucial	0:7365f8db1bac	1252	mpu_set_int_latched(1);
Ucial	0:7365f8db1bac	1253	#if defined MPU6050
Ucial	0:7365f8db1bac	1254	tmp[0] = BIT_LPA_CYCLE;
Ucial	0:7365f8db1bac	1255	if (rate == 1) {
Ucial	0:7365f8db1bac	1256	tmp[1] = INV_LPA_1_25HZ;
Ucial	0:7365f8db1bac	1257	mpu_set_lpf(5);
Ucial	0:7365f8db1bac	1258	} else if (rate <= 5) {
Ucial	0:7365f8db1bac	1259	tmp[1] = INV_LPA_5HZ;
Ucial	0:7365f8db1bac	1260	mpu_set_lpf(5);
Ucial	0:7365f8db1bac	1261	} else if (rate <= 20) {
Ucial	0:7365f8db1bac	1262	tmp[1] = INV_LPA_20HZ;
Ucial	0:7365f8db1bac	1263	mpu_set_lpf(10);
Ucial	0:7365f8db1bac	1264	} else {
Ucial	0:7365f8db1bac	1265	tmp[1] = INV_LPA_40HZ;
Ucial	0:7365f8db1bac	1266	mpu_set_lpf(20);
Ucial	0:7365f8db1bac	1267	}
Ucial	0:7365f8db1bac	1268	tmp[1] = (tmp[1] << 6) \| BIT_STBY_XYZG;
Ucial	0:7365f8db1bac	1269	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
Ucial	0:7365f8db1bac	1270	return -1;
Ucial	0:7365f8db1bac	1271	#elif defined MPU6500
Ucial	0:7365f8db1bac	1272	/* Set wake frequency. */
Ucial	0:7365f8db1bac	1273	if (rate == 1)
Ucial	0:7365f8db1bac	1274	tmp[0] = INV_LPA_1_25HZ;
Ucial	0:7365f8db1bac	1275	else if (rate == 2)
Ucial	0:7365f8db1bac	1276	tmp[0] = INV_LPA_2_5HZ;
Ucial	0:7365f8db1bac	1277	else if (rate <= 5)
Ucial	0:7365f8db1bac	1278	tmp[0] = INV_LPA_5HZ;
Ucial	0:7365f8db1bac	1279	else if (rate <= 10)
Ucial	0:7365f8db1bac	1280	tmp[0] = INV_LPA_10HZ;
Ucial	0:7365f8db1bac	1281	else if (rate <= 20)
Ucial	0:7365f8db1bac	1282	tmp[0] = INV_LPA_20HZ;
Ucial	0:7365f8db1bac	1283	else if (rate <= 40)
Ucial	0:7365f8db1bac	1284	tmp[0] = INV_LPA_40HZ;
Ucial	0:7365f8db1bac	1285	else if (rate <= 80)
Ucial	0:7365f8db1bac	1286	tmp[0] = INV_LPA_80HZ;
Ucial	0:7365f8db1bac	1287	else if (rate <= 160)
Ucial	0:7365f8db1bac	1288	tmp[0] = INV_LPA_160HZ;
Ucial	0:7365f8db1bac	1289	else if (rate <= 320)
Ucial	0:7365f8db1bac	1290	tmp[0] = INV_LPA_320HZ;
Ucial	0:7365f8db1bac	1291	else
Ucial	0:7365f8db1bac	1292	tmp[0] = INV_LPA_640HZ;
Ucial	0:7365f8db1bac	1293	if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp))
Ucial	0:7365f8db1bac	1294	return -1;
Ucial	0:7365f8db1bac	1295	tmp[0] = BIT_LPA_CYCLE;
Ucial	0:7365f8db1bac	1296	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp))
Ucial	0:7365f8db1bac	1297	return -1;
Ucial	0:7365f8db1bac	1298	#endif
Ucial	0:7365f8db1bac	1299	st.chip_cfg.sensors = INV_XYZ_ACCEL;
Ucial	0:7365f8db1bac	1300	st.chip_cfg.clk_src = 0;
Ucial	0:7365f8db1bac	1301	st.chip_cfg.lp_accel_mode = 1;
Ucial	0:7365f8db1bac	1302	mpu_configure_fifo(0);
Ucial	0:7365f8db1bac	1303
Ucial	0:7365f8db1bac	1304	return 0;
Ucial	0:7365f8db1bac	1305	}
Ucial	0:7365f8db1bac	1306
Ucial	0:7365f8db1bac	1307	/**
Ucial	0:7365f8db1bac	1308	* @brief Read raw gyro data directly from the registers.
Ucial	0:7365f8db1bac	1309	* @param[out] data Raw data in hardware units.
Ucial	0:7365f8db1bac	1310	* @param[out] timestamp Timestamp in milliseconds. Null if not needed.
Ucial	0:7365f8db1bac	1311	* @return 0 if successful.
Ucial	0:7365f8db1bac	1312	*/
Ucial	0:7365f8db1bac	1313	int mpu_get_gyro_reg(short data, unsigned long timestamp)
Ucial	0:7365f8db1bac	1314	{
Ucial	0:7365f8db1bac	1315	unsigned char tmp[6];
Ucial	0:7365f8db1bac	1316
Ucial	0:7365f8db1bac	1317	if (!(st.chip_cfg.sensors & INV_XYZ_GYRO))
Ucial	0:7365f8db1bac	1318	return -1;
Ucial	0:7365f8db1bac	1319
Ucial	0:7365f8db1bac	1320	if (i2c_read(st.hw->addr, st.reg->raw_gyro, 6, tmp))
Ucial	0:7365f8db1bac	1321	return -1;
Ucial	0:7365f8db1bac	1322	data[0] = (tmp[0] << 8) \| tmp[1];
Ucial	0:7365f8db1bac	1323	data[1] = (tmp[2] << 8) \| tmp[3];
Ucial	0:7365f8db1bac	1324	data[2] = (tmp[4] << 8) \| tmp[5];
Ucial	0:7365f8db1bac	1325	if (timestamp)
Ucial	0:7365f8db1bac	1326	get_ms(timestamp);
Ucial	0:7365f8db1bac	1327	return 0;
Ucial	0:7365f8db1bac	1328	}
Ucial	0:7365f8db1bac	1329
Ucial	0:7365f8db1bac	1330	/**
Ucial	0:7365f8db1bac	1331	* @brief Read raw accel data directly from the registers.
Ucial	0:7365f8db1bac	1332	* @param[out] data Raw data in hardware units.
Ucial	0:7365f8db1bac	1333	* @param[out] timestamp Timestamp in milliseconds. Null if not needed.
Ucial	0:7365f8db1bac	1334	* @return 0 if successful.
Ucial	0:7365f8db1bac	1335	*/
Ucial	0:7365f8db1bac	1336	int mpu_get_accel_reg(short data, unsigned long timestamp)
Ucial	0:7365f8db1bac	1337	{
Ucial	0:7365f8db1bac	1338	unsigned char tmp[6];
Ucial	0:7365f8db1bac	1339
Ucial	0:7365f8db1bac	1340	if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL))
Ucial	0:7365f8db1bac	1341	return -1;
Ucial	0:7365f8db1bac	1342
Ucial	0:7365f8db1bac	1343	if (i2c_read(st.hw->addr, st.reg->raw_accel, 6, tmp))
Ucial	0:7365f8db1bac	1344	return -1;
Ucial	0:7365f8db1bac	1345	data[0] = (tmp[0] << 8) \| tmp[1];
Ucial	0:7365f8db1bac	1346	data[1] = (tmp[2] << 8) \| tmp[3];
Ucial	0:7365f8db1bac	1347	data[2] = (tmp[4] << 8) \| tmp[5];
Ucial	0:7365f8db1bac	1348	if (timestamp)
Ucial	0:7365f8db1bac	1349	get_ms(timestamp);
Ucial	0:7365f8db1bac	1350	return 0;
Ucial	0:7365f8db1bac	1351	}
Ucial	0:7365f8db1bac	1352
Ucial	0:7365f8db1bac	1353	/**
Ucial	0:7365f8db1bac	1354	* @brief Read temperature data directly from the registers.
Ucial	0:7365f8db1bac	1355	* @param[out] data Data in q16 format.
Ucial	0:7365f8db1bac	1356	* @param[out] timestamp Timestamp in milliseconds. Null if not needed.
Ucial	0:7365f8db1bac	1357	* @return 0 if successful.
Ucial	0:7365f8db1bac	1358	*/
Ucial	0:7365f8db1bac	1359	int mpu_get_temperature(long data, unsigned long timestamp)
Ucial	0:7365f8db1bac	1360	{
Ucial	0:7365f8db1bac	1361	unsigned char tmp[2];
Ucial	0:7365f8db1bac	1362	short raw;
Ucial	0:7365f8db1bac	1363
Ucial	0:7365f8db1bac	1364	if (!(st.chip_cfg.sensors))
Ucial	0:7365f8db1bac	1365	return -1;
Ucial	0:7365f8db1bac	1366
Ucial	0:7365f8db1bac	1367	if (i2c_read(st.hw->addr, st.reg->temp, 2, tmp))
Ucial	0:7365f8db1bac	1368	return -1;
Ucial	0:7365f8db1bac	1369	raw = (tmp[0] << 8) \| tmp[1];
Ucial	0:7365f8db1bac	1370	if (timestamp)
Ucial	0:7365f8db1bac	1371	get_ms(timestamp);
Ucial	0:7365f8db1bac	1372
Ucial	0:7365f8db1bac	1373	data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) / st.hw->temp_sens)) * 65536L);
Ucial	0:7365f8db1bac	1374	return 0;
Ucial	0:7365f8db1bac	1375	}
Ucial	0:7365f8db1bac	1376
Ucial	0:7365f8db1bac	1377	/**
Ucial	0:7365f8db1bac	1378	* @brief Push biases to the accel bias registers.
Ucial	0:7365f8db1bac	1379	* This function expects biases relative to the current sensor output, and
Ucial	0:7365f8db1bac	1380	* these biases will be added to the factory-supplied values.
Ucial	0:7365f8db1bac	1381	* @param[in] accel_bias New biases.
Ucial	0:7365f8db1bac	1382	* @return 0 if successful.
Ucial	0:7365f8db1bac	1383	*/
Ucial	0:7365f8db1bac	1384	int mpu_set_accel_bias(const long *accel_bias)
Ucial	0:7365f8db1bac	1385	{
Ucial	0:7365f8db1bac	1386	unsigned char data[6];
Ucial	0:7365f8db1bac	1387	short accel_hw[3];
Ucial	0:7365f8db1bac	1388	short got_accel[3];
Ucial	0:7365f8db1bac	1389	short fg[3];
Ucial	0:7365f8db1bac	1390
Ucial	0:7365f8db1bac	1391	if (!accel_bias)
Ucial	0:7365f8db1bac	1392	return -1;
Ucial	0:7365f8db1bac	1393	if (!accel_bias[0] && !accel_bias[1] && !accel_bias[2])
Ucial	0:7365f8db1bac	1394	return 0;
Ucial	0:7365f8db1bac	1395
Ucial	0:7365f8db1bac	1396	if (i2c_read(st.hw->addr, 3, 3, data))
Ucial	0:7365f8db1bac	1397	return -1;
Ucial	0:7365f8db1bac	1398	fg[0] = ((data[0] >> 4) + 8) & 0xf;
Ucial	0:7365f8db1bac	1399	fg[1] = ((data[1] >> 4) + 8) & 0xf;
Ucial	0:7365f8db1bac	1400	fg[2] = ((data[2] >> 4) + 8) & 0xf;
Ucial	0:7365f8db1bac	1401
Ucial	0:7365f8db1bac	1402	accel_hw[0] = (short)(accel_bias[0] * 2 / (64 + fg[0]));
Ucial	0:7365f8db1bac	1403	accel_hw[1] = (short)(accel_bias[1] * 2 / (64 + fg[1]));
Ucial	0:7365f8db1bac	1404	accel_hw[2] = (short)(accel_bias[2] * 2 / (64 + fg[2]));
Ucial	0:7365f8db1bac	1405
Ucial	0:7365f8db1bac	1406	if (i2c_read(st.hw->addr, 0x06, 6, data))
Ucial	0:7365f8db1bac	1407	return -1;
Ucial	0:7365f8db1bac	1408
Ucial	0:7365f8db1bac	1409	got_accel[0] = ((short)data[0] << 8) \| data[1];
Ucial	0:7365f8db1bac	1410	got_accel[1] = ((short)data[2] << 8) \| data[3];
Ucial	0:7365f8db1bac	1411	got_accel[2] = ((short)data[4] << 8) \| data[5];
Ucial	0:7365f8db1bac	1412
Ucial	0:7365f8db1bac	1413	accel_hw[0] += got_accel[0];
Ucial	0:7365f8db1bac	1414	accel_hw[1] += got_accel[1];
Ucial	0:7365f8db1bac	1415	accel_hw[2] += got_accel[2];
Ucial	0:7365f8db1bac	1416
Ucial	0:7365f8db1bac	1417	data[0] = (accel_hw[0] >> 8) & 0xff;
Ucial	0:7365f8db1bac	1418	data[1] = (accel_hw[0]) & 0xff;
Ucial	0:7365f8db1bac	1419	data[2] = (accel_hw[1] >> 8) & 0xff;
Ucial	0:7365f8db1bac	1420	data[3] = (accel_hw[1]) & 0xff;
Ucial	0:7365f8db1bac	1421	data[4] = (accel_hw[2] >> 8) & 0xff;
Ucial	0:7365f8db1bac	1422	data[5] = (accel_hw[2]) & 0xff;
Ucial	0:7365f8db1bac	1423
Ucial	0:7365f8db1bac	1424	if (i2c_write(st.hw->addr, 0x06, 6, data))
Ucial	0:7365f8db1bac	1425	return -1;
Ucial	0:7365f8db1bac	1426	return 0;
Ucial	0:7365f8db1bac	1427	}
Ucial	0:7365f8db1bac	1428
Ucial	0:7365f8db1bac	1429	/**
Ucial	0:7365f8db1bac	1430	* @brief Reset FIFO read/write pointers.
Ucial	0:7365f8db1bac	1431	* @return 0 if successful.
Ucial	0:7365f8db1bac	1432	*/
Ucial	0:7365f8db1bac	1433	int mpu_reset_fifo(void)
Ucial	0:7365f8db1bac	1434	{
Ucial	0:7365f8db1bac	1435	unsigned char data;
Ucial	0:7365f8db1bac	1436
Ucial	0:7365f8db1bac	1437	if (!(st.chip_cfg.sensors))
Ucial	0:7365f8db1bac	1438	return -1;
Ucial	0:7365f8db1bac	1439
Ucial	0:7365f8db1bac	1440	data = 0;
Ucial	0:7365f8db1bac	1441	if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
Ucial	0:7365f8db1bac	1442	return -1;
Ucial	0:7365f8db1bac	1443	if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))
Ucial	0:7365f8db1bac	1444	return -1;
Ucial	0:7365f8db1bac	1445	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
Ucial	0:7365f8db1bac	1446	return -1;
Ucial	0:7365f8db1bac	1447
Ucial	0:7365f8db1bac	1448	if (st.chip_cfg.dmp_on) {
Ucial	0:7365f8db1bac	1449	data = BIT_FIFO_RST \| BIT_DMP_RST;
Ucial	0:7365f8db1bac	1450	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
Ucial	0:7365f8db1bac	1451	return -1;
Ucial	0:7365f8db1bac	1452	delay_ms(50);
Ucial	0:7365f8db1bac	1453	data = BIT_DMP_EN \| BIT_FIFO_EN;
Ucial	0:7365f8db1bac	1454	if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
Ucial	0:7365f8db1bac	1455	data \|= BIT_AUX_IF_EN;
Ucial	0:7365f8db1bac	1456	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
Ucial	0:7365f8db1bac	1457	return -1;
Ucial	0:7365f8db1bac	1458	if (st.chip_cfg.int_enable)
Ucial	0:7365f8db1bac	1459	data = BIT_DMP_INT_EN;
Ucial	0:7365f8db1bac	1460	else
Ucial	0:7365f8db1bac	1461	data = 0;
Ucial	0:7365f8db1bac	1462	if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
Ucial	0:7365f8db1bac	1463	return -1;
Ucial	0:7365f8db1bac	1464	data = 0;
Ucial	0:7365f8db1bac	1465	if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))
Ucial	0:7365f8db1bac	1466	return -1;
Ucial	0:7365f8db1bac	1467	} else {
Ucial	0:7365f8db1bac	1468	data = BIT_FIFO_RST;
Ucial	0:7365f8db1bac	1469	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
Ucial	0:7365f8db1bac	1470	return -1;
Ucial	0:7365f8db1bac	1471	if (st.chip_cfg.bypass_mode \|\| !(st.chip_cfg.sensors & INV_XYZ_COMPASS))
Ucial	0:7365f8db1bac	1472	data = BIT_FIFO_EN;
Ucial	0:7365f8db1bac	1473	else
Ucial	0:7365f8db1bac	1474	data = BIT_FIFO_EN \| BIT_AUX_IF_EN;
Ucial	0:7365f8db1bac	1475	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
Ucial	0:7365f8db1bac	1476	return -1;
Ucial	0:7365f8db1bac	1477	delay_ms(50);
Ucial	0:7365f8db1bac	1478	if (st.chip_cfg.int_enable)
Ucial	0:7365f8db1bac	1479	data = BIT_DATA_RDY_EN;
Ucial	0:7365f8db1bac	1480	else
Ucial	0:7365f8db1bac	1481	data = 0;
Ucial	0:7365f8db1bac	1482	if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
Ucial	0:7365f8db1bac	1483	return -1;
Ucial	0:7365f8db1bac	1484	if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable))
Ucial	0:7365f8db1bac	1485	return -1;
Ucial	0:7365f8db1bac	1486	}
Ucial	0:7365f8db1bac	1487	return 0;
Ucial	0:7365f8db1bac	1488	}
Ucial	0:7365f8db1bac	1489
Ucial	0:7365f8db1bac	1490	/**
Ucial	0:7365f8db1bac	1491	* @brief Get the gyro full-scale range.
Ucial	0:7365f8db1bac	1492	* @param[out] fsr Current full-scale range.
Ucial	0:7365f8db1bac	1493	* @return 0 if successful.
Ucial	0:7365f8db1bac	1494	*/
Ucial	0:7365f8db1bac	1495	int mpu_get_gyro_fsr(unsigned short *fsr)
Ucial	0:7365f8db1bac	1496	{
Ucial	0:7365f8db1bac	1497	switch (st.chip_cfg.gyro_fsr) {
Ucial	0:7365f8db1bac	1498	case INV_FSR_250DPS:
Ucial	0:7365f8db1bac	1499	fsr[0] = 250;
Ucial	0:7365f8db1bac	1500	break;
Ucial	0:7365f8db1bac	1501	case INV_FSR_500DPS:
Ucial	0:7365f8db1bac	1502	fsr[0] = 500;
Ucial	0:7365f8db1bac	1503	break;
Ucial	0:7365f8db1bac	1504	case INV_FSR_1000DPS:
Ucial	0:7365f8db1bac	1505	fsr[0] = 1000;
Ucial	0:7365f8db1bac	1506	break;
Ucial	0:7365f8db1bac	1507	case INV_FSR_2000DPS:
Ucial	0:7365f8db1bac	1508	fsr[0] = 2000;
Ucial	0:7365f8db1bac	1509	break;
Ucial	0:7365f8db1bac	1510	default:
Ucial	0:7365f8db1bac	1511	fsr[0] = 0;
Ucial	0:7365f8db1bac	1512	break;
Ucial	0:7365f8db1bac	1513	}
Ucial	0:7365f8db1bac	1514	return 0;
Ucial	0:7365f8db1bac	1515	}
Ucial	0:7365f8db1bac	1516
Ucial	0:7365f8db1bac	1517	/**
Ucial	0:7365f8db1bac	1518	* @brief Set the gyro full-scale range.
Ucial	0:7365f8db1bac	1519	* @param[in] fsr Desired full-scale range.
Ucial	0:7365f8db1bac	1520	* @return 0 if successful.
Ucial	0:7365f8db1bac	1521	*/
Ucial	0:7365f8db1bac	1522	int mpu_set_gyro_fsr(unsigned short fsr)
Ucial	0:7365f8db1bac	1523	{
Ucial	0:7365f8db1bac	1524	unsigned char data;
Ucial	0:7365f8db1bac	1525
Ucial	0:7365f8db1bac	1526	if (!(st.chip_cfg.sensors))
Ucial	0:7365f8db1bac	1527	return -1;
Ucial	0:7365f8db1bac	1528
Ucial	0:7365f8db1bac	1529	switch (fsr) {
Ucial	0:7365f8db1bac	1530	case 250:
Ucial	0:7365f8db1bac	1531	data = INV_FSR_250DPS << 3;
Ucial	0:7365f8db1bac	1532	break;
Ucial	0:7365f8db1bac	1533	case 500:
Ucial	0:7365f8db1bac	1534	data = INV_FSR_500DPS << 3;
Ucial	0:7365f8db1bac	1535	break;
Ucial	0:7365f8db1bac	1536	case 1000:
Ucial	0:7365f8db1bac	1537	data = INV_FSR_1000DPS << 3;
Ucial	0:7365f8db1bac	1538	break;
Ucial	0:7365f8db1bac	1539	case 2000:
Ucial	0:7365f8db1bac	1540	data = INV_FSR_2000DPS << 3;
Ucial	0:7365f8db1bac	1541	break;
Ucial	0:7365f8db1bac	1542	default:
Ucial	0:7365f8db1bac	1543	return -1;
Ucial	0:7365f8db1bac	1544	}
Ucial	0:7365f8db1bac	1545
Ucial	0:7365f8db1bac	1546	if (st.chip_cfg.gyro_fsr == (data >> 3))
Ucial	0:7365f8db1bac	1547	return 0;
Ucial	0:7365f8db1bac	1548	if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, &data))
Ucial	0:7365f8db1bac	1549	return -1;
Ucial	0:7365f8db1bac	1550	st.chip_cfg.gyro_fsr = data >> 3;
Ucial	0:7365f8db1bac	1551	return 0;
Ucial	0:7365f8db1bac	1552	}
Ucial	0:7365f8db1bac	1553
Ucial	0:7365f8db1bac	1554	/**
Ucial	0:7365f8db1bac	1555	* @brief Get the accel full-scale range.
Ucial	0:7365f8db1bac	1556	* @param[out] fsr Current full-scale range.
Ucial	0:7365f8db1bac	1557	* @return 0 if successful.
Ucial	0:7365f8db1bac	1558	*/
Ucial	0:7365f8db1bac	1559	int mpu_get_accel_fsr(unsigned char *fsr)
Ucial	0:7365f8db1bac	1560	{
Ucial	0:7365f8db1bac	1561	switch (st.chip_cfg.accel_fsr) {
Ucial	0:7365f8db1bac	1562	case INV_FSR_2G:
Ucial	0:7365f8db1bac	1563	fsr[0] = 2;
Ucial	0:7365f8db1bac	1564	break;
Ucial	0:7365f8db1bac	1565	case INV_FSR_4G:
Ucial	0:7365f8db1bac	1566	fsr[0] = 4;
Ucial	0:7365f8db1bac	1567	break;
Ucial	0:7365f8db1bac	1568	case INV_FSR_8G:
Ucial	0:7365f8db1bac	1569	fsr[0] = 8;
Ucial	0:7365f8db1bac	1570	break;
Ucial	0:7365f8db1bac	1571	case INV_FSR_16G:
Ucial	0:7365f8db1bac	1572	fsr[0] = 16;
Ucial	0:7365f8db1bac	1573	break;
Ucial	0:7365f8db1bac	1574	default:
Ucial	0:7365f8db1bac	1575	return -1;
Ucial	0:7365f8db1bac	1576	}
Ucial	0:7365f8db1bac	1577	if (st.chip_cfg.accel_half)
Ucial	0:7365f8db1bac	1578	fsr[0] <<= 1;
Ucial	0:7365f8db1bac	1579	return 0;
Ucial	0:7365f8db1bac	1580	}
Ucial	0:7365f8db1bac	1581
Ucial	0:7365f8db1bac	1582	/**
Ucial	0:7365f8db1bac	1583	* @brief Set the accel full-scale range.
Ucial	0:7365f8db1bac	1584	* @param[in] fsr Desired full-scale range.
Ucial	0:7365f8db1bac	1585	* @return 0 if successful.
Ucial	0:7365f8db1bac	1586	*/
Ucial	0:7365f8db1bac	1587	int mpu_set_accel_fsr(unsigned char fsr)
Ucial	0:7365f8db1bac	1588	{
Ucial	0:7365f8db1bac	1589	unsigned char data;
Ucial	0:7365f8db1bac	1590
Ucial	0:7365f8db1bac	1591	if (!(st.chip_cfg.sensors))
Ucial	0:7365f8db1bac	1592	return -1;
Ucial	0:7365f8db1bac	1593
Ucial	0:7365f8db1bac	1594	switch (fsr) {
Ucial	0:7365f8db1bac	1595	case 2:
Ucial	0:7365f8db1bac	1596	data = INV_FSR_2G << 3;
Ucial	0:7365f8db1bac	1597	break;
Ucial	0:7365f8db1bac	1598	case 4:
Ucial	0:7365f8db1bac	1599	data = INV_FSR_4G << 3;
Ucial	0:7365f8db1bac	1600	break;
Ucial	0:7365f8db1bac	1601	case 8:
Ucial	0:7365f8db1bac	1602	data = INV_FSR_8G << 3;
Ucial	0:7365f8db1bac	1603	break;
Ucial	0:7365f8db1bac	1604	case 16:
Ucial	0:7365f8db1bac	1605	data = INV_FSR_16G << 3;
Ucial	0:7365f8db1bac	1606	break;
Ucial	0:7365f8db1bac	1607	default:
Ucial	0:7365f8db1bac	1608	return -1;
Ucial	0:7365f8db1bac	1609	}
Ucial	0:7365f8db1bac	1610
Ucial	0:7365f8db1bac	1611	if (st.chip_cfg.accel_fsr == (data >> 3))
Ucial	0:7365f8db1bac	1612	return 0;
Ucial	0:7365f8db1bac	1613	if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, &data))
Ucial	0:7365f8db1bac	1614	return -1;
Ucial	0:7365f8db1bac	1615	st.chip_cfg.accel_fsr = data >> 3;
Ucial	0:7365f8db1bac	1616	return 0;
Ucial	0:7365f8db1bac	1617	}
Ucial	0:7365f8db1bac	1618
Ucial	0:7365f8db1bac	1619	/**
Ucial	0:7365f8db1bac	1620	* @brief Get the current DLPF setting.
Ucial	0:7365f8db1bac	1621	* @param[out] lpf Current LPF setting.
Ucial	0:7365f8db1bac	1622	* 0 if successful.
Ucial	0:7365f8db1bac	1623	*/
Ucial	0:7365f8db1bac	1624	int mpu_get_lpf(unsigned short *lpf)
Ucial	0:7365f8db1bac	1625	{
Ucial	0:7365f8db1bac	1626	switch (st.chip_cfg.lpf) {
Ucial	0:7365f8db1bac	1627	case INV_FILTER_188HZ:
Ucial	0:7365f8db1bac	1628	lpf[0] = 188;
Ucial	0:7365f8db1bac	1629	break;
Ucial	0:7365f8db1bac	1630	case INV_FILTER_98HZ:
Ucial	0:7365f8db1bac	1631	lpf[0] = 98;
Ucial	0:7365f8db1bac	1632	break;
Ucial	0:7365f8db1bac	1633	case INV_FILTER_42HZ:
Ucial	0:7365f8db1bac	1634	lpf[0] = 42;
Ucial	0:7365f8db1bac	1635	break;
Ucial	0:7365f8db1bac	1636	case INV_FILTER_20HZ:
Ucial	0:7365f8db1bac	1637	lpf[0] = 20;
Ucial	0:7365f8db1bac	1638	break;
Ucial	0:7365f8db1bac	1639	case INV_FILTER_10HZ:
Ucial	0:7365f8db1bac	1640	lpf[0] = 10;
Ucial	0:7365f8db1bac	1641	break;
Ucial	0:7365f8db1bac	1642	case INV_FILTER_5HZ:
Ucial	0:7365f8db1bac	1643	lpf[0] = 5;
Ucial	0:7365f8db1bac	1644	break;
Ucial	0:7365f8db1bac	1645	case INV_FILTER_256HZ_NOLPF2:
Ucial	0:7365f8db1bac	1646	case INV_FILTER_2100HZ_NOLPF:
Ucial	0:7365f8db1bac	1647	default:
Ucial	0:7365f8db1bac	1648	lpf[0] = 0;
Ucial	0:7365f8db1bac	1649	break;
Ucial	0:7365f8db1bac	1650	}
Ucial	0:7365f8db1bac	1651	return 0;
Ucial	0:7365f8db1bac	1652	}
Ucial	0:7365f8db1bac	1653
Ucial	0:7365f8db1bac	1654	/**
Ucial	0:7365f8db1bac	1655	* @brief Set digital low pass filter.
Ucial	0:7365f8db1bac	1656	* The following LPF settings are supported: 188, 98, 42, 20, 10, 5.
Ucial	0:7365f8db1bac	1657	* @param[in] lpf Desired LPF setting.
Ucial	0:7365f8db1bac	1658	* @return 0 if successful.
Ucial	0:7365f8db1bac	1659	*/
Ucial	0:7365f8db1bac	1660	int mpu_set_lpf(unsigned short lpf)
Ucial	0:7365f8db1bac	1661	{
Ucial	0:7365f8db1bac	1662	unsigned char data;
Ucial	0:7365f8db1bac	1663
Ucial	0:7365f8db1bac	1664	if (!(st.chip_cfg.sensors))
Ucial	0:7365f8db1bac	1665	return -1;
Ucial	0:7365f8db1bac	1666
Ucial	0:7365f8db1bac	1667	if (lpf >= 188)
Ucial	0:7365f8db1bac	1668	data = INV_FILTER_188HZ;
Ucial	0:7365f8db1bac	1669	else if (lpf >= 98)
Ucial	0:7365f8db1bac	1670	data = INV_FILTER_98HZ;
Ucial	0:7365f8db1bac	1671	else if (lpf >= 42)
Ucial	0:7365f8db1bac	1672	data = INV_FILTER_42HZ;
Ucial	0:7365f8db1bac	1673	else if (lpf >= 20)
Ucial	0:7365f8db1bac	1674	data = INV_FILTER_20HZ;
Ucial	0:7365f8db1bac	1675	else if (lpf >= 10)
Ucial	0:7365f8db1bac	1676	data = INV_FILTER_10HZ;
Ucial	0:7365f8db1bac	1677	else
Ucial	0:7365f8db1bac	1678	data = INV_FILTER_5HZ;
Ucial	0:7365f8db1bac	1679
Ucial	0:7365f8db1bac	1680	if (st.chip_cfg.lpf == data)
Ucial	0:7365f8db1bac	1681	return 0;
Ucial	0:7365f8db1bac	1682	if (i2c_write(st.hw->addr, st.reg->lpf, 1, &data))
Ucial	0:7365f8db1bac	1683	return -1;
Ucial	0:7365f8db1bac	1684	st.chip_cfg.lpf = data;
Ucial	0:7365f8db1bac	1685	return 0;
Ucial	0:7365f8db1bac	1686	}
Ucial	0:7365f8db1bac	1687
Ucial	0:7365f8db1bac	1688	/**
Ucial	0:7365f8db1bac	1689	* @brief Get sampling rate.
Ucial	0:7365f8db1bac	1690	* @param[out] rate Current sampling rate (Hz).
Ucial	0:7365f8db1bac	1691	* @return 0 if successful.
Ucial	0:7365f8db1bac	1692	*/
Ucial	0:7365f8db1bac	1693	int mpu_get_sample_rate(unsigned short *rate)
Ucial	0:7365f8db1bac	1694	{
Ucial	0:7365f8db1bac	1695	if (st.chip_cfg.dmp_on)
Ucial	0:7365f8db1bac	1696	return -1;
Ucial	0:7365f8db1bac	1697	else
Ucial	0:7365f8db1bac	1698	rate[0] = st.chip_cfg.sample_rate;
Ucial	0:7365f8db1bac	1699	return 0;
Ucial	0:7365f8db1bac	1700	}
Ucial	0:7365f8db1bac	1701
Ucial	0:7365f8db1bac	1702	/**
Ucial	0:7365f8db1bac	1703	* @brief Set sampling rate.
Ucial	0:7365f8db1bac	1704	* Sampling rate must be between 4Hz and 1kHz.
Ucial	0:7365f8db1bac	1705	* @param[in] rate Desired sampling rate (Hz).
Ucial	0:7365f8db1bac	1706	* @return 0 if successful.
Ucial	0:7365f8db1bac	1707	*/
Ucial	0:7365f8db1bac	1708	int mpu_set_sample_rate(unsigned short rate)
Ucial	0:7365f8db1bac	1709	{
Ucial	0:7365f8db1bac	1710	unsigned char data;
Ucial	0:7365f8db1bac	1711
Ucial	0:7365f8db1bac	1712	if (!(st.chip_cfg.sensors))
Ucial	0:7365f8db1bac	1713	return -1;
Ucial	0:7365f8db1bac	1714
Ucial	0:7365f8db1bac	1715	if (st.chip_cfg.dmp_on)
Ucial	0:7365f8db1bac	1716	return -1;
Ucial	0:7365f8db1bac	1717	else {
Ucial	0:7365f8db1bac	1718	if (st.chip_cfg.lp_accel_mode) {
Ucial	0:7365f8db1bac	1719	if (rate && (rate <= 40)) {
Ucial	0:7365f8db1bac	1720	/* Just stay in low-power accel mode. */
Ucial	0:7365f8db1bac	1721	mpu_lp_accel_mode(rate);
Ucial	0:7365f8db1bac	1722	return 0;
Ucial	0:7365f8db1bac	1723	}
Ucial	0:7365f8db1bac	1724	/* Requested rate exceeds the allowed frequencies in LP accel mode,
Ucial	0:7365f8db1bac	1725	* switch back to full-power mode.
Ucial	0:7365f8db1bac	1726	*/
Ucial	0:7365f8db1bac	1727	mpu_lp_accel_mode(0);
Ucial	0:7365f8db1bac	1728	}
Ucial	0:7365f8db1bac	1729	if (rate < 4)
Ucial	0:7365f8db1bac	1730	rate = 4;
Ucial	0:7365f8db1bac	1731	else if (rate > 1000)
Ucial	0:7365f8db1bac	1732	rate = 1000;
Ucial	0:7365f8db1bac	1733
Ucial	0:7365f8db1bac	1734	data = 1000 / rate - 1;
Ucial	0:7365f8db1bac	1735	if (i2c_write(st.hw->addr, st.reg->rate_div, 1, &data))
Ucial	0:7365f8db1bac	1736	return -1;
Ucial	0:7365f8db1bac	1737
Ucial	0:7365f8db1bac	1738	st.chip_cfg.sample_rate = 1000 / (1 + data);
Ucial	0:7365f8db1bac	1739
Ucial	0:7365f8db1bac	1740	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	1741	mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE));
Ucial	0:7365f8db1bac	1742	#endif
Ucial	0:7365f8db1bac	1743
Ucial	0:7365f8db1bac	1744	/* Automatically set LPF to 1/2 sampling rate. */
Ucial	0:7365f8db1bac	1745	mpu_set_lpf(st.chip_cfg.sample_rate >> 1);
Ucial	0:7365f8db1bac	1746	return 0;
Ucial	0:7365f8db1bac	1747	}
Ucial	0:7365f8db1bac	1748	}
Ucial	0:7365f8db1bac	1749
Ucial	0:7365f8db1bac	1750	/**
Ucial	0:7365f8db1bac	1751	* @brief Get compass sampling rate.
Ucial	0:7365f8db1bac	1752	* @param[out] rate Current compass sampling rate (Hz).
Ucial	0:7365f8db1bac	1753	* @return 0 if successful.
Ucial	0:7365f8db1bac	1754	*/
Ucial	0:7365f8db1bac	1755	int mpu_get_compass_sample_rate(unsigned short *rate)
Ucial	0:7365f8db1bac	1756	{
Ucial	0:7365f8db1bac	1757	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	1758	rate[0] = st.chip_cfg.compass_sample_rate;
Ucial	0:7365f8db1bac	1759	return 0;
Ucial	0:7365f8db1bac	1760	#else
Ucial	0:7365f8db1bac	1761	rate[0] = 0;
Ucial	0:7365f8db1bac	1762	return -1;
Ucial	0:7365f8db1bac	1763	#endif
Ucial	0:7365f8db1bac	1764	}
Ucial	0:7365f8db1bac	1765
Ucial	0:7365f8db1bac	1766	/**
Ucial	0:7365f8db1bac	1767	* @brief Set compass sampling rate.
Ucial	0:7365f8db1bac	1768	* The compass on the auxiliary I2C bus is read by the MPU hardware at a
Ucial	0:7365f8db1bac	1769	* maximum of 100Hz. The actual rate can be set to a fraction of the gyro
Ucial	0:7365f8db1bac	1770	* sampling rate.
Ucial	0:7365f8db1bac	1771	*
Ucial	0:7365f8db1bac	1772	* \n WARNING: The new rate may be different than what was requested. Call
Ucial	0:7365f8db1bac	1773	* mpu_get_compass_sample_rate to check the actual setting.
Ucial	0:7365f8db1bac	1774	* @param[in] rate Desired compass sampling rate (Hz).
Ucial	0:7365f8db1bac	1775	* @return 0 if successful.
Ucial	0:7365f8db1bac	1776	*/
Ucial	0:7365f8db1bac	1777	int mpu_set_compass_sample_rate(unsigned short rate)
Ucial	0:7365f8db1bac	1778	{
Ucial	0:7365f8db1bac	1779	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	1780	unsigned char div;
Ucial	0:7365f8db1bac	1781	if (!rate \|\| rate > st.chip_cfg.sample_rate \|\| rate > MAX_COMPASS_SAMPLE_RATE)
Ucial	0:7365f8db1bac	1782	return -1;
Ucial	0:7365f8db1bac	1783
Ucial	0:7365f8db1bac	1784	div = st.chip_cfg.sample_rate / rate - 1;
Ucial	0:7365f8db1bac	1785	if (i2c_write(st.hw->addr, st.reg->s4_ctrl, 1, &div))
Ucial	0:7365f8db1bac	1786	return -1;
Ucial	0:7365f8db1bac	1787	st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate / (div + 1);
Ucial	0:7365f8db1bac	1788	return 0;
Ucial	0:7365f8db1bac	1789	#else
Ucial	0:7365f8db1bac	1790	return -1;
Ucial	0:7365f8db1bac	1791	#endif
Ucial	0:7365f8db1bac	1792	}
Ucial	0:7365f8db1bac	1793
Ucial	0:7365f8db1bac	1794	/**
Ucial	0:7365f8db1bac	1795	* @brief Get gyro sensitivity scale factor.
Ucial	0:7365f8db1bac	1796	* @param[out] sens Conversion from hardware units to dps.
Ucial	0:7365f8db1bac	1797	* @return 0 if successful.
Ucial	0:7365f8db1bac	1798	*/
Ucial	0:7365f8db1bac	1799	int mpu_get_gyro_sens(float *sens)
Ucial	0:7365f8db1bac	1800	{
Ucial	0:7365f8db1bac	1801	switch (st.chip_cfg.gyro_fsr) {
Ucial	0:7365f8db1bac	1802	case INV_FSR_250DPS:
Ucial	0:7365f8db1bac	1803	sens[0] = 131.f;
Ucial	0:7365f8db1bac	1804	break;
Ucial	0:7365f8db1bac	1805	case INV_FSR_500DPS:
Ucial	0:7365f8db1bac	1806	sens[0] = 65.5f;
Ucial	0:7365f8db1bac	1807	break;
Ucial	0:7365f8db1bac	1808	case INV_FSR_1000DPS:
Ucial	0:7365f8db1bac	1809	sens[0] = 32.8f;
Ucial	0:7365f8db1bac	1810	break;
Ucial	0:7365f8db1bac	1811	case INV_FSR_2000DPS:
Ucial	0:7365f8db1bac	1812	sens[0] = 16.4f;
Ucial	0:7365f8db1bac	1813	break;
Ucial	0:7365f8db1bac	1814	default:
Ucial	0:7365f8db1bac	1815	return -1;
Ucial	0:7365f8db1bac	1816	}
Ucial	0:7365f8db1bac	1817	return 0;
Ucial	0:7365f8db1bac	1818	}
Ucial	0:7365f8db1bac	1819
Ucial	0:7365f8db1bac	1820	/**
Ucial	0:7365f8db1bac	1821	* @brief Get accel sensitivity scale factor.
Ucial	0:7365f8db1bac	1822	* @param[out] sens Conversion from hardware units to g's.
Ucial	0:7365f8db1bac	1823	* @return 0 if successful.
Ucial	0:7365f8db1bac	1824	*/
Ucial	0:7365f8db1bac	1825	int mpu_get_accel_sens(unsigned short *sens)
Ucial	0:7365f8db1bac	1826	{
Ucial	0:7365f8db1bac	1827	switch (st.chip_cfg.accel_fsr) {
Ucial	0:7365f8db1bac	1828	case INV_FSR_2G:
Ucial	0:7365f8db1bac	1829	sens[0] = 16384;
Ucial	0:7365f8db1bac	1830	break;
Ucial	0:7365f8db1bac	1831	case INV_FSR_4G:
Ucial	0:7365f8db1bac	1832	sens[0] = 8092;
Ucial	0:7365f8db1bac	1833	break;
Ucial	0:7365f8db1bac	1834	case INV_FSR_8G:
Ucial	0:7365f8db1bac	1835	sens[0] = 4096;
Ucial	0:7365f8db1bac	1836	break;
Ucial	0:7365f8db1bac	1837	case INV_FSR_16G:
Ucial	0:7365f8db1bac	1838	sens[0] = 2048;
Ucial	0:7365f8db1bac	1839	break;
Ucial	0:7365f8db1bac	1840	default:
Ucial	0:7365f8db1bac	1841	return -1;
Ucial	0:7365f8db1bac	1842	}
Ucial	0:7365f8db1bac	1843	if (st.chip_cfg.accel_half)
Ucial	0:7365f8db1bac	1844	sens[0] >>= 1;
Ucial	0:7365f8db1bac	1845	return 0;
Ucial	0:7365f8db1bac	1846	}
Ucial	0:7365f8db1bac	1847
Ucial	0:7365f8db1bac	1848	/**
Ucial	0:7365f8db1bac	1849	* @brief Get current FIFO configuration.
Ucial	0:7365f8db1bac	1850	* @e sensors can contain a combination of the following flags:
Ucial	0:7365f8db1bac	1851	* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
Ucial	0:7365f8db1bac	1852	* \n INV_XYZ_GYRO
Ucial	0:7365f8db1bac	1853	* \n INV_XYZ_ACCEL
Ucial	0:7365f8db1bac	1854	* @param[out] sensors Mask of sensors in FIFO.
Ucial	0:7365f8db1bac	1855	* @return 0 if successful.
Ucial	0:7365f8db1bac	1856	*/
Ucial	0:7365f8db1bac	1857	int mpu_get_fifo_config(unsigned char *sensors)
Ucial	0:7365f8db1bac	1858	{
Ucial	0:7365f8db1bac	1859	sensors[0] = st.chip_cfg.fifo_enable;
Ucial	0:7365f8db1bac	1860	return 0;
Ucial	0:7365f8db1bac	1861	}
Ucial	0:7365f8db1bac	1862
Ucial	0:7365f8db1bac	1863	/**
Ucial	0:7365f8db1bac	1864	* @brief Select which sensors are pushed to FIFO.
Ucial	0:7365f8db1bac	1865	* @e sensors can contain a combination of the following flags:
Ucial	0:7365f8db1bac	1866	* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
Ucial	0:7365f8db1bac	1867	* \n INV_XYZ_GYRO
Ucial	0:7365f8db1bac	1868	* \n INV_XYZ_ACCEL
Ucial	0:7365f8db1bac	1869	* @param[in] sensors Mask of sensors to push to FIFO.
Ucial	0:7365f8db1bac	1870	* @return 0 if successful.
Ucial	0:7365f8db1bac	1871	*/
Ucial	0:7365f8db1bac	1872	int mpu_configure_fifo(unsigned char sensors)
Ucial	0:7365f8db1bac	1873	{
Ucial	0:7365f8db1bac	1874	unsigned char prev;
Ucial	0:7365f8db1bac	1875	int result = 0;
Ucial	0:7365f8db1bac	1876
Ucial	0:7365f8db1bac	1877	/* Compass data isn't going into the FIFO. Stop trying. */
Ucial	0:7365f8db1bac	1878	sensors &= ~INV_XYZ_COMPASS;
Ucial	0:7365f8db1bac	1879
Ucial	0:7365f8db1bac	1880	if (st.chip_cfg.dmp_on)
Ucial	0:7365f8db1bac	1881	return 0;
Ucial	0:7365f8db1bac	1882	else {
Ucial	0:7365f8db1bac	1883	if (!(st.chip_cfg.sensors))
Ucial	0:7365f8db1bac	1884	return -1;
Ucial	0:7365f8db1bac	1885	prev = st.chip_cfg.fifo_enable;
Ucial	0:7365f8db1bac	1886	st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors;
Ucial	0:7365f8db1bac	1887	if (st.chip_cfg.fifo_enable != sensors)
Ucial	0:7365f8db1bac	1888	/* You're not getting what you asked for. Some sensors are
Ucial	0:7365f8db1bac	1889	* asleep.
Ucial	0:7365f8db1bac	1890	*/
Ucial	0:7365f8db1bac	1891	result = -1;
Ucial	0:7365f8db1bac	1892	else
Ucial	0:7365f8db1bac	1893	result = 0;
Ucial	0:7365f8db1bac	1894	if (sensors \|\| st.chip_cfg.lp_accel_mode)
Ucial	0:7365f8db1bac	1895	set_int_enable(1);
Ucial	0:7365f8db1bac	1896	else
Ucial	0:7365f8db1bac	1897	set_int_enable(0);
Ucial	0:7365f8db1bac	1898	if (sensors) {
Ucial	0:7365f8db1bac	1899	if (mpu_reset_fifo()) {
Ucial	0:7365f8db1bac	1900	st.chip_cfg.fifo_enable = prev;
Ucial	0:7365f8db1bac	1901	return -1;
Ucial	0:7365f8db1bac	1902	}
Ucial	0:7365f8db1bac	1903	}
Ucial	0:7365f8db1bac	1904	}
Ucial	0:7365f8db1bac	1905
Ucial	0:7365f8db1bac	1906	return result;
Ucial	0:7365f8db1bac	1907	}
Ucial	0:7365f8db1bac	1908
Ucial	0:7365f8db1bac	1909	/**
Ucial	0:7365f8db1bac	1910	* @brief Get current power state.
Ucial	0:7365f8db1bac	1911	* @param[in] power_on 1 if turned on, 0 if suspended.
Ucial	0:7365f8db1bac	1912	* @return 0 if successful.
Ucial	0:7365f8db1bac	1913	*/
Ucial	0:7365f8db1bac	1914	int mpu_get_power_state(unsigned char *power_on)
Ucial	0:7365f8db1bac	1915	{
Ucial	0:7365f8db1bac	1916	if (st.chip_cfg.sensors)
Ucial	0:7365f8db1bac	1917	power_on[0] = 1;
Ucial	0:7365f8db1bac	1918	else
Ucial	0:7365f8db1bac	1919	power_on[0] = 0;
Ucial	0:7365f8db1bac	1920	return 0;
Ucial	0:7365f8db1bac	1921	}
Ucial	0:7365f8db1bac	1922
Ucial	0:7365f8db1bac	1923	/**
Ucial	0:7365f8db1bac	1924	* @brief Turn specific sensors on/off.
Ucial	0:7365f8db1bac	1925	* @e sensors can contain a combination of the following flags:
Ucial	0:7365f8db1bac	1926	* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
Ucial	0:7365f8db1bac	1927	* \n INV_XYZ_GYRO
Ucial	0:7365f8db1bac	1928	* \n INV_XYZ_ACCEL
Ucial	0:7365f8db1bac	1929	* \n INV_XYZ_COMPASS
Ucial	0:7365f8db1bac	1930	* @param[in] sensors Mask of sensors to wake.
Ucial	0:7365f8db1bac	1931	* @return 0 if successful.
Ucial	0:7365f8db1bac	1932	*/
Ucial	0:7365f8db1bac	1933	int mpu_set_sensors(unsigned char sensors)
Ucial	0:7365f8db1bac	1934	{
Ucial	0:7365f8db1bac	1935	unsigned char data;
Ucial	0:7365f8db1bac	1936	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	1937	unsigned char user_ctrl;
Ucial	0:7365f8db1bac	1938	#endif
Ucial	0:7365f8db1bac	1939
Ucial	0:7365f8db1bac	1940	if (sensors & INV_XYZ_GYRO)
Ucial	0:7365f8db1bac	1941	data = INV_CLK_PLL;
Ucial	0:7365f8db1bac	1942	else if (sensors)
Ucial	0:7365f8db1bac	1943	data = 0;
Ucial	0:7365f8db1bac	1944	else
Ucial	0:7365f8db1bac	1945	data = BIT_SLEEP;
Ucial	0:7365f8db1bac	1946	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) {
Ucial	0:7365f8db1bac	1947	st.chip_cfg.sensors = 0;
Ucial	0:7365f8db1bac	1948	return -1;
Ucial	0:7365f8db1bac	1949	}
Ucial	0:7365f8db1bac	1950	st.chip_cfg.clk_src = data & ~BIT_SLEEP;
Ucial	0:7365f8db1bac	1951
Ucial	0:7365f8db1bac	1952	data = 0;
Ucial	0:7365f8db1bac	1953	if (!(sensors & INV_X_GYRO))
Ucial	0:7365f8db1bac	1954	data \|= BIT_STBY_XG;
Ucial	0:7365f8db1bac	1955	if (!(sensors & INV_Y_GYRO))
Ucial	0:7365f8db1bac	1956	data \|= BIT_STBY_YG;
Ucial	0:7365f8db1bac	1957	if (!(sensors & INV_Z_GYRO))
Ucial	0:7365f8db1bac	1958	data \|= BIT_STBY_ZG;
Ucial	0:7365f8db1bac	1959	if (!(sensors & INV_XYZ_ACCEL))
Ucial	0:7365f8db1bac	1960	data \|= BIT_STBY_XYZA;
Ucial	0:7365f8db1bac	1961	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) {
Ucial	0:7365f8db1bac	1962	st.chip_cfg.sensors = 0;
Ucial	0:7365f8db1bac	1963	return -1;
Ucial	0:7365f8db1bac	1964	}
Ucial	0:7365f8db1bac	1965
Ucial	0:7365f8db1bac	1966	if (sensors && (sensors != INV_XYZ_ACCEL))
Ucial	0:7365f8db1bac	1967	/* Latched interrupts only used in LP accel mode. */
Ucial	0:7365f8db1bac	1968	mpu_set_int_latched(0);
Ucial	0:7365f8db1bac	1969
Ucial	0:7365f8db1bac	1970	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	1971	#ifdef AK89xx_BYPASS
Ucial	0:7365f8db1bac	1972	if (sensors & INV_XYZ_COMPASS)
Ucial	0:7365f8db1bac	1973	mpu_set_bypass(1);
Ucial	0:7365f8db1bac	1974	else
Ucial	0:7365f8db1bac	1975	mpu_set_bypass(0);
Ucial	0:7365f8db1bac	1976	#else
Ucial	0:7365f8db1bac	1977	if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
Ucial	0:7365f8db1bac	1978	return -1;
Ucial	0:7365f8db1bac	1979	/* Handle AKM power management. */
Ucial	0:7365f8db1bac	1980	if (sensors & INV_XYZ_COMPASS) {
Ucial	0:7365f8db1bac	1981	data = AKM_SINGLE_MEASUREMENT;
Ucial	0:7365f8db1bac	1982	user_ctrl \|= BIT_AUX_IF_EN;
Ucial	0:7365f8db1bac	1983	} else {
Ucial	0:7365f8db1bac	1984	data = AKM_POWER_DOWN;
Ucial	0:7365f8db1bac	1985	user_ctrl &= ~BIT_AUX_IF_EN;
Ucial	0:7365f8db1bac	1986	}
Ucial	0:7365f8db1bac	1987	if (st.chip_cfg.dmp_on)
Ucial	0:7365f8db1bac	1988	user_ctrl \|= BIT_DMP_EN;
Ucial	0:7365f8db1bac	1989	else
Ucial	0:7365f8db1bac	1990	user_ctrl &= ~BIT_DMP_EN;
Ucial	0:7365f8db1bac	1991	if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data))
Ucial	0:7365f8db1bac	1992	return -1;
Ucial	0:7365f8db1bac	1993	/* Enable/disable I2C master mode. */
Ucial	0:7365f8db1bac	1994	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
Ucial	0:7365f8db1bac	1995	return -1;
Ucial	0:7365f8db1bac	1996	#endif
Ucial	0:7365f8db1bac	1997	#endif
Ucial	0:7365f8db1bac	1998
Ucial	0:7365f8db1bac	1999	st.chip_cfg.sensors = sensors;
Ucial	0:7365f8db1bac	2000	st.chip_cfg.lp_accel_mode = 0;
Ucial	0:7365f8db1bac	2001	delay_ms(50);
Ucial	0:7365f8db1bac	2002	return 0;
Ucial	0:7365f8db1bac	2003	}
Ucial	0:7365f8db1bac	2004
Ucial	0:7365f8db1bac	2005	/**
Ucial	0:7365f8db1bac	2006	* @brief Read the MPU interrupt status registers.
Ucial	0:7365f8db1bac	2007	* @param[out] status Mask of interrupt bits.
Ucial	0:7365f8db1bac	2008	* @return 0 if successful.
Ucial	0:7365f8db1bac	2009	*/
Ucial	0:7365f8db1bac	2010	int mpu_get_int_status(short *status)
Ucial	0:7365f8db1bac	2011	{
Ucial	0:7365f8db1bac	2012	unsigned char tmp[2];
Ucial	0:7365f8db1bac	2013	if (!st.chip_cfg.sensors)
Ucial	0:7365f8db1bac	2014	return -1;
Ucial	0:7365f8db1bac	2015	if (i2c_read(st.hw->addr, st.reg->dmp_int_status, 2, tmp))
Ucial	0:7365f8db1bac	2016	return -1;
Ucial	0:7365f8db1bac	2017	status[0] = (tmp[0] << 8) \| tmp[1];
Ucial	0:7365f8db1bac	2018	return 0;
Ucial	0:7365f8db1bac	2019	}
Ucial	0:7365f8db1bac	2020
Ucial	0:7365f8db1bac	2021	/**
Ucial	0:7365f8db1bac	2022	* @brief Get one packet from the FIFO.
Ucial	0:7365f8db1bac	2023	* If @e sensors does not contain a particular sensor, disregard the data
Ucial	0:7365f8db1bac	2024	* returned to that pointer.
Ucial	0:7365f8db1bac	2025	* \n @e sensors can contain a combination of the following flags:
Ucial	0:7365f8db1bac	2026	* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
Ucial	0:7365f8db1bac	2027	* \n INV_XYZ_GYRO
Ucial	0:7365f8db1bac	2028	* \n INV_XYZ_ACCEL
Ucial	0:7365f8db1bac	2029	* \n If the FIFO has no new data, @e sensors will be zero.
Ucial	0:7365f8db1bac	2030	* \n If the FIFO is disabled, @e sensors will be zero and this function will
Ucial	0:7365f8db1bac	2031	* return a non-zero error code.
Ucial	0:7365f8db1bac	2032	* @param[out] gyro Gyro data in hardware units.
Ucial	0:7365f8db1bac	2033	* @param[out] accel Accel data in hardware units.
Ucial	0:7365f8db1bac	2034	* @param[out] timestamp Timestamp in milliseconds.
Ucial	0:7365f8db1bac	2035	* @param[out] sensors Mask of sensors read from FIFO.
Ucial	0:7365f8db1bac	2036	* @param[out] more Number of remaining packets.
Ucial	0:7365f8db1bac	2037	* @return 0 if successful.
Ucial	0:7365f8db1bac	2038	*/
Ucial	0:7365f8db1bac	2039	int mpu_read_fifo(short gyro, short accel, unsigned long *timestamp,
Ucial	0:7365f8db1bac	2040	unsigned char sensors, unsigned char more)
Ucial	0:7365f8db1bac	2041	{
Ucial	0:7365f8db1bac	2042	/* Assumes maximum packet size is gyro (6) + accel (6). */
Ucial	0:7365f8db1bac	2043	unsigned char data[MAX_PACKET_LENGTH];
Ucial	0:7365f8db1bac	2044	unsigned char packet_size = 0;
Ucial	0:7365f8db1bac	2045	unsigned short fifo_count, index = 0;
Ucial	0:7365f8db1bac	2046
Ucial	0:7365f8db1bac	2047	if (st.chip_cfg.dmp_on)
Ucial	0:7365f8db1bac	2048	return -1;
Ucial	0:7365f8db1bac	2049
Ucial	0:7365f8db1bac	2050	sensors[0] = 0;
Ucial	0:7365f8db1bac	2051	if (!st.chip_cfg.sensors)
Ucial	0:7365f8db1bac	2052	return -1;
Ucial	0:7365f8db1bac	2053	if (!st.chip_cfg.fifo_enable)
Ucial	0:7365f8db1bac	2054	return -1;
Ucial	0:7365f8db1bac	2055
Ucial	0:7365f8db1bac	2056	if (st.chip_cfg.fifo_enable & INV_X_GYRO)
Ucial	0:7365f8db1bac	2057	packet_size += 2;
Ucial	0:7365f8db1bac	2058	if (st.chip_cfg.fifo_enable & INV_Y_GYRO)
Ucial	0:7365f8db1bac	2059	packet_size += 2;
Ucial	0:7365f8db1bac	2060	if (st.chip_cfg.fifo_enable & INV_Z_GYRO)
Ucial	0:7365f8db1bac	2061	packet_size += 2;
Ucial	0:7365f8db1bac	2062	if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL)
Ucial	0:7365f8db1bac	2063	packet_size += 6;
Ucial	0:7365f8db1bac	2064
Ucial	0:7365f8db1bac	2065	if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
Ucial	0:7365f8db1bac	2066	return -1;
Ucial	0:7365f8db1bac	2067	fifo_count = (data[0] << 8) \| data[1];
Ucial	0:7365f8db1bac	2068	if (fifo_count < packet_size)
Ucial	0:7365f8db1bac	2069	return 0;
Ucial	0:7365f8db1bac	2070	// //("FIFO count: %hd\n", fifo_count);
Ucial	0:7365f8db1bac	2071	if (fifo_count > (st.hw->max_fifo >> 1)) {
Ucial	0:7365f8db1bac	2072	/* FIFO is 50% full, better check overflow bit. */
Ucial	0:7365f8db1bac	2073	if (i2c_read(st.hw->addr, st.reg->int_status, 1, data))
Ucial	0:7365f8db1bac	2074	return -1;
Ucial	0:7365f8db1bac	2075	if (data[0] & BIT_FIFO_OVERFLOW) {
Ucial	0:7365f8db1bac	2076	mpu_reset_fifo();
Ucial	0:7365f8db1bac	2077	return -2;
Ucial	0:7365f8db1bac	2078	}
Ucial	0:7365f8db1bac	2079	}
Ucial	0:7365f8db1bac	2080	get_ms((unsigned long*)timestamp);
Ucial	0:7365f8db1bac	2081
Ucial	0:7365f8db1bac	2082	if (i2c_read(st.hw->addr, st.reg->fifo_r_w, packet_size, data))
Ucial	0:7365f8db1bac	2083	return -1;
Ucial	0:7365f8db1bac	2084	more[0] = fifo_count / packet_size - 1;
Ucial	0:7365f8db1bac	2085	sensors[0] = 0;
Ucial	0:7365f8db1bac	2086
Ucial	0:7365f8db1bac	2087	if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) {
Ucial	0:7365f8db1bac	2088	accel[0] = (data[index+0] << 8) \| data[index+1];
Ucial	0:7365f8db1bac	2089	accel[1] = (data[index+2] << 8) \| data[index+3];
Ucial	0:7365f8db1bac	2090	accel[2] = (data[index+4] << 8) \| data[index+5];
Ucial	0:7365f8db1bac	2091	sensors[0] \|= INV_XYZ_ACCEL;
Ucial	0:7365f8db1bac	2092	index += 6;
Ucial	0:7365f8db1bac	2093	}
Ucial	0:7365f8db1bac	2094	if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) {
Ucial	0:7365f8db1bac	2095	gyro[0] = (data[index+0] << 8) \| data[index+1];
Ucial	0:7365f8db1bac	2096	sensors[0] \|= INV_X_GYRO;
Ucial	0:7365f8db1bac	2097	index += 2;
Ucial	0:7365f8db1bac	2098	}
Ucial	0:7365f8db1bac	2099	if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) {
Ucial	0:7365f8db1bac	2100	gyro[1] = (data[index+0] << 8) \| data[index+1];
Ucial	0:7365f8db1bac	2101	sensors[0] \|= INV_Y_GYRO;
Ucial	0:7365f8db1bac	2102	index += 2;
Ucial	0:7365f8db1bac	2103	}
Ucial	0:7365f8db1bac	2104	if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) {
Ucial	0:7365f8db1bac	2105	gyro[2] = (data[index+0] << 8) \| data[index+1];
Ucial	0:7365f8db1bac	2106	sensors[0] \|= INV_Z_GYRO;
Ucial	0:7365f8db1bac	2107	index += 2;
Ucial	0:7365f8db1bac	2108	}
Ucial	0:7365f8db1bac	2109
Ucial	0:7365f8db1bac	2110	return 0;
Ucial	0:7365f8db1bac	2111	}
Ucial	0:7365f8db1bac	2112
Ucial	0:7365f8db1bac	2113	/**
Ucial	0:7365f8db1bac	2114	* @brief Get one unparsed packet from the FIFO.
Ucial	0:7365f8db1bac	2115	* This function should be used if the packet is to be parsed elsewhere.
Ucial	0:7365f8db1bac	2116	* @param[in] length Length of one FIFO packet.
Ucial	0:7365f8db1bac	2117	* @param[in] data FIFO packet.
Ucial	0:7365f8db1bac	2118	* @param[in] more Number of remaining packets.
Ucial	0:7365f8db1bac	2119	*/
Ucial	0:7365f8db1bac	2120	int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
Ucial	0:7365f8db1bac	2121	unsigned char *more)
Ucial	0:7365f8db1bac	2122	{
Ucial	0:7365f8db1bac	2123	unsigned char tmp[2];
Ucial	0:7365f8db1bac	2124	unsigned short fifo_count;
Ucial	0:7365f8db1bac	2125	if (!st.chip_cfg.dmp_on)
Ucial	0:7365f8db1bac	2126	return -1;
Ucial	0:7365f8db1bac	2127	if (!st.chip_cfg.sensors)
Ucial	0:7365f8db1bac	2128	return -1;
Ucial	0:7365f8db1bac	2129
Ucial	0:7365f8db1bac	2130	if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp))
Ucial	0:7365f8db1bac	2131	return -1;
Ucial	0:7365f8db1bac	2132	fifo_count = (tmp[0] << 8) \| tmp[1];
Ucial	0:7365f8db1bac	2133	if (fifo_count < length) {
Ucial	0:7365f8db1bac	2134	more[0] = 0;
Ucial	0:7365f8db1bac	2135	return -1;
Ucial	0:7365f8db1bac	2136	}
Ucial	0:7365f8db1bac	2137	if (fifo_count > (st.hw->max_fifo >> 1)) {
Ucial	0:7365f8db1bac	2138	/* FIFO is 50% full, better check overflow bit. */
Ucial	0:7365f8db1bac	2139	if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp))
Ucial	0:7365f8db1bac	2140	return -1;
Ucial	0:7365f8db1bac	2141	if (tmp[0] & BIT_FIFO_OVERFLOW) {
Ucial	0:7365f8db1bac	2142	mpu_reset_fifo();
Ucial	0:7365f8db1bac	2143	return -2;
Ucial	0:7365f8db1bac	2144	}
Ucial	0:7365f8db1bac	2145	}
Ucial	0:7365f8db1bac	2146
Ucial	0:7365f8db1bac	2147	if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data))
Ucial	0:7365f8db1bac	2148	return -1;
Ucial	0:7365f8db1bac	2149	more[0] = fifo_count / length - 1;
Ucial	0:7365f8db1bac	2150	return 0;
Ucial	0:7365f8db1bac	2151	}
Ucial	0:7365f8db1bac	2152
Ucial	0:7365f8db1bac	2153	/**
Ucial	0:7365f8db1bac	2154	* @brief Set device to bypass mode.
Ucial	0:7365f8db1bac	2155	* @param[in] bypass_on 1 to enable bypass mode.
Ucial	0:7365f8db1bac	2156	* @return 0 if successful.
Ucial	0:7365f8db1bac	2157	*/
Ucial	0:7365f8db1bac	2158	int mpu_set_bypass(unsigned char bypass_on)
Ucial	0:7365f8db1bac	2159	{
Ucial	0:7365f8db1bac	2160	unsigned char tmp;
Ucial	0:7365f8db1bac	2161
Ucial	0:7365f8db1bac	2162	if (st.chip_cfg.bypass_mode == bypass_on)
Ucial	0:7365f8db1bac	2163	return 0;
Ucial	0:7365f8db1bac	2164
Ucial	0:7365f8db1bac	2165	if (bypass_on) {
Ucial	0:7365f8db1bac	2166	if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
Ucial	0:7365f8db1bac	2167	return -1;
Ucial	0:7365f8db1bac	2168	tmp &= ~BIT_AUX_IF_EN;
Ucial	0:7365f8db1bac	2169	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
Ucial	0:7365f8db1bac	2170	return -1;
Ucial	0:7365f8db1bac	2171	delay_ms(3);
Ucial	0:7365f8db1bac	2172	tmp = BIT_BYPASS_EN;
Ucial	0:7365f8db1bac	2173	if (st.chip_cfg.active_low_int)
Ucial	0:7365f8db1bac	2174	tmp \|= BIT_ACTL;
Ucial	0:7365f8db1bac	2175	if (st.chip_cfg.latched_int)
Ucial	0:7365f8db1bac	2176	tmp \|= BIT_LATCH_EN \| BIT_ANY_RD_CLR;
Ucial	0:7365f8db1bac	2177	if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
Ucial	0:7365f8db1bac	2178	return -1;
Ucial	0:7365f8db1bac	2179	} else {
Ucial	0:7365f8db1bac	2180	/* Enable I2C master mode if compass is being used. */
Ucial	0:7365f8db1bac	2181	if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
Ucial	0:7365f8db1bac	2182	return -1;
Ucial	0:7365f8db1bac	2183	if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
Ucial	0:7365f8db1bac	2184	tmp \|= BIT_AUX_IF_EN;
Ucial	0:7365f8db1bac	2185	else
Ucial	0:7365f8db1bac	2186	tmp &= ~BIT_AUX_IF_EN;
Ucial	0:7365f8db1bac	2187	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
Ucial	0:7365f8db1bac	2188	return -1;
Ucial	0:7365f8db1bac	2189	delay_ms(3);
Ucial	0:7365f8db1bac	2190	if (st.chip_cfg.active_low_int)
Ucial	0:7365f8db1bac	2191	tmp = BIT_ACTL;
Ucial	0:7365f8db1bac	2192	else
Ucial	0:7365f8db1bac	2193	tmp = 0;
Ucial	0:7365f8db1bac	2194	if (st.chip_cfg.latched_int)
Ucial	0:7365f8db1bac	2195	tmp \|= BIT_LATCH_EN \| BIT_ANY_RD_CLR;
Ucial	0:7365f8db1bac	2196	if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
Ucial	0:7365f8db1bac	2197	return -1;
Ucial	0:7365f8db1bac	2198	}
Ucial	0:7365f8db1bac	2199	st.chip_cfg.bypass_mode = bypass_on;
Ucial	0:7365f8db1bac	2200	return 0;
Ucial	0:7365f8db1bac	2201	}
Ucial	0:7365f8db1bac	2202
Ucial	0:7365f8db1bac	2203	/**
Ucial	0:7365f8db1bac	2204	* @brief Set interrupt level.
Ucial	0:7365f8db1bac	2205	* @param[in] active_low 1 for active low, 0 for active high.
Ucial	0:7365f8db1bac	2206	* @return 0 if successful.
Ucial	0:7365f8db1bac	2207	*/
Ucial	0:7365f8db1bac	2208	int mpu_set_int_level(unsigned char active_low)
Ucial	0:7365f8db1bac	2209	{
Ucial	0:7365f8db1bac	2210	st.chip_cfg.active_low_int = active_low;
Ucial	0:7365f8db1bac	2211	return 0;
Ucial	0:7365f8db1bac	2212	}
Ucial	0:7365f8db1bac	2213
Ucial	0:7365f8db1bac	2214	/**
Ucial	0:7365f8db1bac	2215	* @brief Enable latched interrupts.
Ucial	0:7365f8db1bac	2216	* Any MPU register will clear the interrupt.
Ucial	0:7365f8db1bac	2217	* @param[in] enable 1 to enable, 0 to disable.
Ucial	0:7365f8db1bac	2218	* @return 0 if successful.
Ucial	0:7365f8db1bac	2219	*/
Ucial	0:7365f8db1bac	2220	int mpu_set_int_latched(unsigned char enable)
Ucial	0:7365f8db1bac	2221	{
Ucial	0:7365f8db1bac	2222	unsigned char tmp;
Ucial	0:7365f8db1bac	2223	if (st.chip_cfg.latched_int == enable)
Ucial	0:7365f8db1bac	2224	return 0;
Ucial	0:7365f8db1bac	2225
Ucial	0:7365f8db1bac	2226	if (enable)
Ucial	0:7365f8db1bac	2227	tmp = BIT_LATCH_EN \| BIT_ANY_RD_CLR;
Ucial	0:7365f8db1bac	2228	else
Ucial	0:7365f8db1bac	2229	tmp = 0;
Ucial	0:7365f8db1bac	2230	if (st.chip_cfg.bypass_mode)
Ucial	0:7365f8db1bac	2231	tmp \|= BIT_BYPASS_EN;
Ucial	0:7365f8db1bac	2232	if (st.chip_cfg.active_low_int)
Ucial	0:7365f8db1bac	2233	tmp \|= BIT_ACTL;
Ucial	0:7365f8db1bac	2234	if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
Ucial	0:7365f8db1bac	2235	return -1;
Ucial	0:7365f8db1bac	2236	st.chip_cfg.latched_int = enable;
Ucial	0:7365f8db1bac	2237	return 0;
Ucial	0:7365f8db1bac	2238	}
Ucial	0:7365f8db1bac	2239
Ucial	0:7365f8db1bac	2240	#ifdef MPU6050
Ucial	0:7365f8db1bac	2241	static int get_accel_prod_shift(float *st_shift)
Ucial	0:7365f8db1bac	2242	{
Ucial	0:7365f8db1bac	2243	unsigned char tmp[4], shift_code[3], ii;
Ucial	0:7365f8db1bac	2244
Ucial	0:7365f8db1bac	2245	if (i2c_read(st.hw->addr, 0x0D, 4, tmp))
Ucial	0:7365f8db1bac	2246	return 0x07;
Ucial	0:7365f8db1bac	2247
Ucial	0:7365f8db1bac	2248	shift_code[0] = ((tmp[0] & 0xE0) >> 3) \| ((tmp[3] & 0x30) >> 4);
Ucial	0:7365f8db1bac	2249	shift_code[1] = ((tmp[1] & 0xE0) >> 3) \| ((tmp[3] & 0x0C) >> 2);
Ucial	0:7365f8db1bac	2250	shift_code[2] = ((tmp[2] & 0xE0) >> 3) \| (tmp[3] & 0x03);
Ucial	0:7365f8db1bac	2251	for (ii = 0; ii < 3; ii++) {
Ucial	0:7365f8db1bac	2252	if (!shift_code[ii]) {
Ucial	0:7365f8db1bac	2253	st_shift[ii] = 0.f;
Ucial	0:7365f8db1bac	2254	continue;
Ucial	0:7365f8db1bac	2255	}
Ucial	0:7365f8db1bac	2256	/* Equivalent to..
Ucial	0:7365f8db1bac	2257	* st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f)
Ucial	0:7365f8db1bac	2258	*/
Ucial	0:7365f8db1bac	2259	st_shift[ii] = 0.34f;
Ucial	0:7365f8db1bac	2260	while (--shift_code[ii])
Ucial	0:7365f8db1bac	2261	st_shift[ii] *= 1.034f;
Ucial	0:7365f8db1bac	2262	}
Ucial	0:7365f8db1bac	2263	return 0;
Ucial	0:7365f8db1bac	2264	}
Ucial	0:7365f8db1bac	2265
Ucial	0:7365f8db1bac	2266	static int accel_self_test(long bias_regular, long bias_st)
Ucial	0:7365f8db1bac	2267	{
Ucial	0:7365f8db1bac	2268	int jj, result = 0;
Ucial	0:7365f8db1bac	2269	float st_shift[3], st_shift_cust, st_shift_var;
Ucial	0:7365f8db1bac	2270
Ucial	0:7365f8db1bac	2271	get_accel_prod_shift(st_shift);
Ucial	0:7365f8db1bac	2272	for(jj = 0; jj < 3; jj++) {
Ucial	0:7365f8db1bac	2273	st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
Ucial	0:7365f8db1bac	2274	if (st_shift[jj]) {
Ucial	0:7365f8db1bac	2275	st_shift_var = st_shift_cust / st_shift[jj] - 1.f;
Ucial	0:7365f8db1bac	2276	if (fabs(st_shift_var) > test.max_accel_var)
Ucial	0:7365f8db1bac	2277	result \|= 1 << jj;
Ucial	0:7365f8db1bac	2278	} else if ((st_shift_cust < test.min_g) \|\|
Ucial	0:7365f8db1bac	2279	(st_shift_cust > test.max_g))
Ucial	0:7365f8db1bac	2280	result \|= 1 << jj;
Ucial	0:7365f8db1bac	2281	}
Ucial	0:7365f8db1bac	2282
Ucial	0:7365f8db1bac	2283	return result;
Ucial	0:7365f8db1bac	2284	}
Ucial	0:7365f8db1bac	2285
Ucial	0:7365f8db1bac	2286	static int gyro_self_test(long bias_regular, long bias_st)
Ucial	0:7365f8db1bac	2287	{
Ucial	0:7365f8db1bac	2288	int jj, result = 0;
Ucial	0:7365f8db1bac	2289	unsigned char tmp[3];
Ucial	0:7365f8db1bac	2290	float st_shift, st_shift_cust, st_shift_var;
Ucial	0:7365f8db1bac	2291
Ucial	0:7365f8db1bac	2292	if (i2c_read(st.hw->addr, 0x0D, 3, tmp))
Ucial	0:7365f8db1bac	2293	return 0x07;
Ucial	0:7365f8db1bac	2294
Ucial	0:7365f8db1bac	2295	tmp[0] &= 0x1F;
Ucial	0:7365f8db1bac	2296	tmp[1] &= 0x1F;
Ucial	0:7365f8db1bac	2297	tmp[2] &= 0x1F;
Ucial	0:7365f8db1bac	2298
Ucial	0:7365f8db1bac	2299	for (jj = 0; jj < 3; jj++) {
Ucial	0:7365f8db1bac	2300	st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
Ucial	0:7365f8db1bac	2301	if (tmp[jj]) {
Ucial	0:7365f8db1bac	2302	st_shift = 3275.f / test.gyro_sens;
Ucial	0:7365f8db1bac	2303	while (--tmp[jj])
Ucial	0:7365f8db1bac	2304	st_shift *= 1.046f;
Ucial	0:7365f8db1bac	2305	st_shift_var = st_shift_cust / st_shift - 1.f;
Ucial	0:7365f8db1bac	2306	if (fabs(st_shift_var) > test.max_gyro_var)
Ucial	0:7365f8db1bac	2307	result \|= 1 << jj;
Ucial	0:7365f8db1bac	2308	} else if ((st_shift_cust < test.min_dps) \|\|
Ucial	0:7365f8db1bac	2309	(st_shift_cust > test.max_dps))
Ucial	0:7365f8db1bac	2310	result \|= 1 << jj;
Ucial	0:7365f8db1bac	2311	}
Ucial	0:7365f8db1bac	2312	return result;
Ucial	0:7365f8db1bac	2313	}
Ucial	0:7365f8db1bac	2314
Ucial	0:7365f8db1bac	2315	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	2316	static int compass_self_test(void)
Ucial	0:7365f8db1bac	2317	{
Ucial	0:7365f8db1bac	2318	unsigned char tmp[6];
Ucial	0:7365f8db1bac	2319	unsigned char tries = 10;
Ucial	0:7365f8db1bac	2320	int result = 0x07;
Ucial	0:7365f8db1bac	2321	short data;
Ucial	0:7365f8db1bac	2322
Ucial	0:7365f8db1bac	2323	mpu_set_bypass(1);
Ucial	0:7365f8db1bac	2324
Ucial	0:7365f8db1bac	2325	tmp[0] = AKM_POWER_DOWN;
Ucial	0:7365f8db1bac	2326	if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
Ucial	0:7365f8db1bac	2327	return 0x07;
Ucial	0:7365f8db1bac	2328	tmp[0] = AKM_BIT_SELF_TEST;
Ucial	0:7365f8db1bac	2329	if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp))
Ucial	0:7365f8db1bac	2330	goto AKM_restore;
Ucial	0:7365f8db1bac	2331	tmp[0] = AKM_MODE_SELF_TEST;
Ucial	0:7365f8db1bac	2332	if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp))
Ucial	0:7365f8db1bac	2333	goto AKM_restore;
Ucial	0:7365f8db1bac	2334
Ucial	0:7365f8db1bac	2335	do {
Ucial	0:7365f8db1bac	2336	delay_ms(10);
Ucial	0:7365f8db1bac	2337	if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 1, tmp))
Ucial	0:7365f8db1bac	2338	goto AKM_restore;
Ucial	0:7365f8db1bac	2339	if (tmp[0] & AKM_DATA_READY)
Ucial	0:7365f8db1bac	2340	break;
Ucial	0:7365f8db1bac	2341	} while (tries--);
Ucial	0:7365f8db1bac	2342	if (!(tmp[0] & AKM_DATA_READY))
Ucial	0:7365f8db1bac	2343	goto AKM_restore;
Ucial	0:7365f8db1bac	2344
Ucial	0:7365f8db1bac	2345	if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_HXL, 6, tmp))
Ucial	0:7365f8db1bac	2346	goto AKM_restore;
Ucial	0:7365f8db1bac	2347
Ucial	0:7365f8db1bac	2348	result = 0;
Ucial	0:7365f8db1bac	2349	data = (short)(tmp[1] << 8) \| tmp[0];
Ucial	0:7365f8db1bac	2350	if ((data > 100) \|\| (data < -100))
Ucial	0:7365f8db1bac	2351	result \|= 0x01;
Ucial	0:7365f8db1bac	2352	data = (short)(tmp[3] << 8) \| tmp[2];
Ucial	0:7365f8db1bac	2353	if ((data > 100) \|\| (data < -100))
Ucial	0:7365f8db1bac	2354	result \|= 0x02;
Ucial	0:7365f8db1bac	2355	data = (short)(tmp[5] << 8) \| tmp[4];
Ucial	0:7365f8db1bac	2356	if ((data > -300) \|\| (data < -1000))
Ucial	0:7365f8db1bac	2357	result \|= 0x04;
Ucial	0:7365f8db1bac	2358
Ucial	0:7365f8db1bac	2359	AKM_restore:
Ucial	0:7365f8db1bac	2360	tmp[0] = 0 \| SUPPORTS_AK89xx_HIGH_SENS;
Ucial	0:7365f8db1bac	2361	i2c_write(st.chip_cfg.compass_addr, AKM_REG_ASTC, 1, tmp);
Ucial	0:7365f8db1bac	2362	tmp[0] = SUPPORTS_AK89xx_HIGH_SENS;
Ucial	0:7365f8db1bac	2363	i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp);
Ucial	0:7365f8db1bac	2364	mpu_set_bypass(0);
Ucial	0:7365f8db1bac	2365	return result;
Ucial	0:7365f8db1bac	2366	}
Ucial	0:7365f8db1bac	2367	#endif
Ucial	0:7365f8db1bac	2368	#endif
Ucial	0:7365f8db1bac	2369
Ucial	0:7365f8db1bac	2370	static int get_st_biases(long gyro, long accel, unsigned char hw_test)
Ucial	0:7365f8db1bac	2371	{
Ucial	0:7365f8db1bac	2372	unsigned char data[MAX_PACKET_LENGTH];
Ucial	0:7365f8db1bac	2373	unsigned char packet_count, ii;
Ucial	0:7365f8db1bac	2374	unsigned short fifo_count;
Ucial	0:7365f8db1bac	2375
Ucial	0:7365f8db1bac	2376	data[0] = 0x01;
Ucial	0:7365f8db1bac	2377	data[1] = 0;
Ucial	0:7365f8db1bac	2378	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
Ucial	0:7365f8db1bac	2379	return -1;
Ucial	0:7365f8db1bac	2380	delay_ms(200);
Ucial	0:7365f8db1bac	2381	data[0] = 0;
Ucial	0:7365f8db1bac	2382	if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
Ucial	0:7365f8db1bac	2383	return -1;
Ucial	0:7365f8db1bac	2384	if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
Ucial	0:7365f8db1bac	2385	return -1;
Ucial	0:7365f8db1bac	2386	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
Ucial	0:7365f8db1bac	2387	return -1;
Ucial	0:7365f8db1bac	2388	if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
Ucial	0:7365f8db1bac	2389	return -1;
Ucial	0:7365f8db1bac	2390	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
Ucial	0:7365f8db1bac	2391	return -1;
Ucial	0:7365f8db1bac	2392	data[0] = BIT_FIFO_RST \| BIT_DMP_RST;
Ucial	0:7365f8db1bac	2393	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
Ucial	0:7365f8db1bac	2394	return -1;
Ucial	0:7365f8db1bac	2395	delay_ms(15);
Ucial	0:7365f8db1bac	2396	data[0] = st.test->reg_lpf;
Ucial	0:7365f8db1bac	2397	if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))
Ucial	0:7365f8db1bac	2398	return -1;
Ucial	0:7365f8db1bac	2399	data[0] = st.test->reg_rate_div;
Ucial	0:7365f8db1bac	2400	if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data))
Ucial	0:7365f8db1bac	2401	return -1;
Ucial	0:7365f8db1bac	2402	if (hw_test)
Ucial	0:7365f8db1bac	2403	data[0] = st.test->reg_gyro_fsr \| 0xE0;
Ucial	0:7365f8db1bac	2404	else
Ucial	0:7365f8db1bac	2405	data[0] = st.test->reg_gyro_fsr;
Ucial	0:7365f8db1bac	2406	if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data))
Ucial	0:7365f8db1bac	2407	return -1;
Ucial	0:7365f8db1bac	2408
Ucial	0:7365f8db1bac	2409	if (hw_test)
Ucial	0:7365f8db1bac	2410	data[0] = st.test->reg_accel_fsr \| 0xE0;
Ucial	0:7365f8db1bac	2411	else
Ucial	0:7365f8db1bac	2412	data[0] = test.reg_accel_fsr;
Ucial	0:7365f8db1bac	2413	if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
Ucial	0:7365f8db1bac	2414	return -1;
Ucial	0:7365f8db1bac	2415	if (hw_test)
Ucial	0:7365f8db1bac	2416	delay_ms(200);
Ucial	0:7365f8db1bac	2417
Ucial	0:7365f8db1bac	2418	/* Fill FIFO for test.wait_ms milliseconds. */
Ucial	0:7365f8db1bac	2419	data[0] = BIT_FIFO_EN;
Ucial	0:7365f8db1bac	2420	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
Ucial	0:7365f8db1bac	2421	return -1;
Ucial	0:7365f8db1bac	2422
Ucial	0:7365f8db1bac	2423	data[0] = INV_XYZ_GYRO \| INV_XYZ_ACCEL;
Ucial	0:7365f8db1bac	2424	if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
Ucial	0:7365f8db1bac	2425	return -1;
Ucial	0:7365f8db1bac	2426	delay_ms(test.wait_ms);
Ucial	0:7365f8db1bac	2427	data[0] = 0;
Ucial	0:7365f8db1bac	2428	if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
Ucial	0:7365f8db1bac	2429	return -1;
Ucial	0:7365f8db1bac	2430
Ucial	0:7365f8db1bac	2431	if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
Ucial	0:7365f8db1bac	2432	return -1;
Ucial	0:7365f8db1bac	2433
Ucial	0:7365f8db1bac	2434	fifo_count = (data[0] << 8) \| data[1];
Ucial	0:7365f8db1bac	2435	packet_count = fifo_count / MAX_PACKET_LENGTH;
Ucial	0:7365f8db1bac	2436	gyro[0] = gyro[1] = gyro[2] = 0;
Ucial	0:7365f8db1bac	2437	accel[0] = accel[1] = accel[2] = 0;
Ucial	0:7365f8db1bac	2438
Ucial	0:7365f8db1bac	2439	for (ii = 0; ii < packet_count; ii++) {
Ucial	0:7365f8db1bac	2440	short accel_cur[3], gyro_cur[3];
Ucial	0:7365f8db1bac	2441	if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data))
Ucial	0:7365f8db1bac	2442	return -1;
Ucial	0:7365f8db1bac	2443	accel_cur[0] = ((short)data[0] << 8) \| data[1];
Ucial	0:7365f8db1bac	2444	accel_cur[1] = ((short)data[2] << 8) \| data[3];
Ucial	0:7365f8db1bac	2445	accel_cur[2] = ((short)data[4] << 8) \| data[5];
Ucial	0:7365f8db1bac	2446	accel[0] += (long)accel_cur[0];
Ucial	0:7365f8db1bac	2447	accel[1] += (long)accel_cur[1];
Ucial	0:7365f8db1bac	2448	accel[2] += (long)accel_cur[2];
Ucial	0:7365f8db1bac	2449	gyro_cur[0] = (((short)data[6] << 8) \| data[7]);
Ucial	0:7365f8db1bac	2450	gyro_cur[1] = (((short)data[8] << 8) \| data[9]);
Ucial	0:7365f8db1bac	2451	gyro_cur[2] = (((short)data[10] << 8) \| data[11]);
Ucial	0:7365f8db1bac	2452	gyro[0] += (long)gyro_cur[0];
Ucial	0:7365f8db1bac	2453	gyro[1] += (long)gyro_cur[1];
Ucial	0:7365f8db1bac	2454	gyro[2] += (long)gyro_cur[2];
Ucial	0:7365f8db1bac	2455	}
Ucial	0:7365f8db1bac	2456	#ifdef EMPL_NO_64BIT
Ucial	0:7365f8db1bac	2457	gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count);
Ucial	0:7365f8db1bac	2458	gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count);
Ucial	0:7365f8db1bac	2459	gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count);
Ucial	0:7365f8db1bac	2460	if (has_accel) {
Ucial	0:7365f8db1bac	2461	accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens /
Ucial	0:7365f8db1bac	2462	packet_count);
Ucial	0:7365f8db1bac	2463	accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens /
Ucial	0:7365f8db1bac	2464	packet_count);
Ucial	0:7365f8db1bac	2465	accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens /
Ucial	0:7365f8db1bac	2466	packet_count);
Ucial	0:7365f8db1bac	2467	/* Don't remove gravity! */
Ucial	0:7365f8db1bac	2468	accel[2] -= 65536L;
Ucial	0:7365f8db1bac	2469	}
Ucial	0:7365f8db1bac	2470	#else
Ucial	0:7365f8db1bac	2471	gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count);
Ucial	0:7365f8db1bac	2472	gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count);
Ucial	0:7365f8db1bac	2473	gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count);
Ucial	0:7365f8db1bac	2474	accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens /
Ucial	0:7365f8db1bac	2475	packet_count);
Ucial	0:7365f8db1bac	2476	accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens /
Ucial	0:7365f8db1bac	2477	packet_count);
Ucial	0:7365f8db1bac	2478	accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens /
Ucial	0:7365f8db1bac	2479	packet_count);
Ucial	0:7365f8db1bac	2480	/* Don't remove gravity! */
Ucial	0:7365f8db1bac	2481	if (accel[2] > 0L)
Ucial	0:7365f8db1bac	2482	accel[2] -= 65536L;
Ucial	0:7365f8db1bac	2483	else
Ucial	0:7365f8db1bac	2484	accel[2] += 65536L;
Ucial	0:7365f8db1bac	2485	#endif
Ucial	0:7365f8db1bac	2486
Ucial	0:7365f8db1bac	2487	return 0;
Ucial	0:7365f8db1bac	2488	}
Ucial	0:7365f8db1bac	2489
Ucial	0:7365f8db1bac	2490	/**
Ucial	0:7365f8db1bac	2491	* @brief Trigger gyro/accel/compass self-test.
Ucial	0:7365f8db1bac	2492	* On success/error, the self-test returns a mask representing the sensor(s)
Ucial	0:7365f8db1bac	2493	* that failed. For each bit, a one (1) represents a "pass" case; conversely,
Ucial	0:7365f8db1bac	2494	* a zero (0) indicates a failure.
Ucial	0:7365f8db1bac	2495	*
Ucial	0:7365f8db1bac	2496	* \n The mask is defined as follows:
Ucial	0:7365f8db1bac	2497	* \n Bit 0: Gyro.
Ucial	0:7365f8db1bac	2498	* \n Bit 1: Accel.
Ucial	0:7365f8db1bac	2499	* \n Bit 2: Compass.
Ucial	0:7365f8db1bac	2500	*
Ucial	0:7365f8db1bac	2501	* \n Currently, the hardware self-test is unsupported for MPU6500. However,
Ucial	0:7365f8db1bac	2502	* this function can still be used to obtain the accel and gyro biases.
Ucial	0:7365f8db1bac	2503	*
Ucial	0:7365f8db1bac	2504	* \n This function must be called with the device either face-up or face-down
Ucial	0:7365f8db1bac	2505	* (z-axis is parallel to gravity).
Ucial	0:7365f8db1bac	2506	* @param[out] gyro Gyro biases in q16 format.
Ucial	0:7365f8db1bac	2507	* @param[out] accel Accel biases (if applicable) in q16 format.
Ucial	0:7365f8db1bac	2508	* @return Result mask (see above).
Ucial	0:7365f8db1bac	2509	*/
Ucial	0:7365f8db1bac	2510	int mpu_run_self_test(long gyro, long accel)
Ucial	0:7365f8db1bac	2511	{
Ucial	0:7365f8db1bac	2512	#ifdef MPU6050
Ucial	0:7365f8db1bac	2513	const unsigned char tries = 2;
Ucial	0:7365f8db1bac	2514	long gyro_st[3], accel_st[3];
Ucial	0:7365f8db1bac	2515	unsigned char accel_result, gyro_result;
Ucial	0:7365f8db1bac	2516	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	2517	unsigned char compass_result;
Ucial	0:7365f8db1bac	2518	#endif
Ucial	0:7365f8db1bac	2519	int ii;
Ucial	0:7365f8db1bac	2520	#endif
Ucial	0:7365f8db1bac	2521	int result;
Ucial	0:7365f8db1bac	2522	unsigned char accel_fsr, fifo_sensors, sensors_on;
Ucial	0:7365f8db1bac	2523	unsigned short gyro_fsr, sample_rate, lpf;
Ucial	0:7365f8db1bac	2524	unsigned char dmp_was_on;
Ucial	0:7365f8db1bac	2525
Ucial	0:7365f8db1bac	2526	if (st.chip_cfg.dmp_on) {
Ucial	0:7365f8db1bac	2527	mpu_set_dmp_state(0);
Ucial	0:7365f8db1bac	2528	dmp_was_on = 1;
Ucial	0:7365f8db1bac	2529	} else
Ucial	0:7365f8db1bac	2530	dmp_was_on = 0;
Ucial	0:7365f8db1bac	2531
Ucial	0:7365f8db1bac	2532	/* Get initial settings. */
Ucial	0:7365f8db1bac	2533	mpu_get_gyro_fsr(&gyro_fsr);
Ucial	0:7365f8db1bac	2534	mpu_get_accel_fsr(&accel_fsr);
Ucial	0:7365f8db1bac	2535	mpu_get_lpf(&lpf);
Ucial	0:7365f8db1bac	2536	mpu_get_sample_rate(&sample_rate);
Ucial	0:7365f8db1bac	2537	sensors_on = st.chip_cfg.sensors;
Ucial	0:7365f8db1bac	2538	mpu_get_fifo_config(&fifo_sensors);
Ucial	0:7365f8db1bac	2539
Ucial	0:7365f8db1bac	2540	/* For older chips, the self-test will be different. */
Ucial	0:7365f8db1bac	2541	#if defined MPU6050
Ucial	0:7365f8db1bac	2542	for (ii = 0; ii < tries; ii++)
Ucial	0:7365f8db1bac	2543	if (!get_st_biases(gyro, accel, 0))
Ucial	0:7365f8db1bac	2544	break;
Ucial	0:7365f8db1bac	2545	if (ii == tries) {
Ucial	0:7365f8db1bac	2546	/* If we reach this point, we most likely encountered an I2C error.
Ucial	0:7365f8db1bac	2547	* We'll just report an error for all three sensors.
Ucial	0:7365f8db1bac	2548	*/
Ucial	0:7365f8db1bac	2549	result = 0;
Ucial	0:7365f8db1bac	2550	goto restore;
Ucial	0:7365f8db1bac	2551	}
Ucial	0:7365f8db1bac	2552	for (ii = 0; ii < tries; ii++)
Ucial	0:7365f8db1bac	2553	if (!get_st_biases(gyro_st, accel_st, 1))
Ucial	0:7365f8db1bac	2554	break;
Ucial	0:7365f8db1bac	2555	if (ii == tries) {
Ucial	0:7365f8db1bac	2556	/* Again, probably an I2C error. */
Ucial	0:7365f8db1bac	2557	result = 0;
Ucial	0:7365f8db1bac	2558	goto restore;
Ucial	0:7365f8db1bac	2559	}
Ucial	0:7365f8db1bac	2560	accel_result = accel_self_test(accel, accel_st);
Ucial	0:7365f8db1bac	2561	gyro_result = gyro_self_test(gyro, gyro_st);
Ucial	0:7365f8db1bac	2562
Ucial	0:7365f8db1bac	2563	result = 0;
Ucial	0:7365f8db1bac	2564	if (!gyro_result)
Ucial	0:7365f8db1bac	2565	result \|= 0x01;
Ucial	0:7365f8db1bac	2566	if (!accel_result)
Ucial	0:7365f8db1bac	2567	result \|= 0x02;
Ucial	0:7365f8db1bac	2568
Ucial	0:7365f8db1bac	2569	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	2570	compass_result = compass_self_test();
Ucial	0:7365f8db1bac	2571	if (!compass_result)
Ucial	0:7365f8db1bac	2572	result \|= 0x04;
Ucial	0:7365f8db1bac	2573	#endif
Ucial	0:7365f8db1bac	2574	restore:
Ucial	0:7365f8db1bac	2575	#elif defined MPU6500
Ucial	0:7365f8db1bac	2576	/* For now, this function will return a "pass" result for all three sensors
Ucial	0:7365f8db1bac	2577	* for compatibility with current test applications.
Ucial	0:7365f8db1bac	2578	*/
Ucial	0:7365f8db1bac	2579	get_st_biases(gyro, accel, 0);
Ucial	0:7365f8db1bac	2580	result = 0x7;
Ucial	0:7365f8db1bac	2581	#endif
Ucial	0:7365f8db1bac	2582	/* Set to invalid values to ensure no I2C writes are skipped. */
Ucial	0:7365f8db1bac	2583	st.chip_cfg.gyro_fsr = 0xFF;
Ucial	0:7365f8db1bac	2584	st.chip_cfg.accel_fsr = 0xFF;
Ucial	0:7365f8db1bac	2585	st.chip_cfg.lpf = 0xFF;
Ucial	0:7365f8db1bac	2586	st.chip_cfg.sample_rate = 0xFFFF;
Ucial	0:7365f8db1bac	2587	st.chip_cfg.sensors = 0xFF;
Ucial	0:7365f8db1bac	2588	st.chip_cfg.fifo_enable = 0xFF;
Ucial	0:7365f8db1bac	2589	st.chip_cfg.clk_src = INV_CLK_PLL;
Ucial	0:7365f8db1bac	2590	mpu_set_gyro_fsr(gyro_fsr);
Ucial	0:7365f8db1bac	2591	mpu_set_accel_fsr(accel_fsr);
Ucial	0:7365f8db1bac	2592	mpu_set_lpf(lpf);
Ucial	0:7365f8db1bac	2593	mpu_set_sample_rate(sample_rate);
Ucial	0:7365f8db1bac	2594	mpu_set_sensors(sensors_on);
Ucial	0:7365f8db1bac	2595	mpu_configure_fifo(fifo_sensors);
Ucial	0:7365f8db1bac	2596
Ucial	0:7365f8db1bac	2597	if (dmp_was_on)
Ucial	0:7365f8db1bac	2598	mpu_set_dmp_state(1);
Ucial	0:7365f8db1bac	2599
Ucial	0:7365f8db1bac	2600	return result;
Ucial	0:7365f8db1bac	2601	}
Ucial	0:7365f8db1bac	2602
Ucial	0:7365f8db1bac	2603	/**
Ucial	0:7365f8db1bac	2604	* @brief Write to the DMP memory.
Ucial	0:7365f8db1bac	2605	* This function prevents I2C writes past the bank boundaries. The DMP memory
Ucial	0:7365f8db1bac	2606	* is only accessible when the chip is awake.
Ucial	0:7365f8db1bac	2607	* @param[in] mem_addr Memory location (bank << 8 \| start address)
Ucial	0:7365f8db1bac	2608	* @param[in] length Number of bytes to write.
Ucial	0:7365f8db1bac	2609	* @param[in] data Bytes to write to memory.
Ucial	0:7365f8db1bac	2610	* @return 0 if successful.
Ucial	0:7365f8db1bac	2611	*/
Ucial	0:7365f8db1bac	2612	int mpu_write_mem(unsigned short mem_addr, unsigned short length,
Ucial	0:7365f8db1bac	2613	unsigned char *data)
Ucial	0:7365f8db1bac	2614	{
Ucial	0:7365f8db1bac	2615	unsigned char tmp[2];
Ucial	0:7365f8db1bac	2616
Ucial	0:7365f8db1bac	2617	if (!data)
Ucial	0:7365f8db1bac	2618	return -1;
Ucial	0:7365f8db1bac	2619	if (!st.chip_cfg.sensors)
Ucial	0:7365f8db1bac	2620	return -1;
Ucial	0:7365f8db1bac	2621
Ucial	0:7365f8db1bac	2622	tmp[0] = (unsigned char)(mem_addr >> 8);
Ucial	0:7365f8db1bac	2623	tmp[1] = (unsigned char)(mem_addr & 0xFF);
Ucial	0:7365f8db1bac	2624
Ucial	0:7365f8db1bac	2625	/* Check bank boundaries. */
Ucial	0:7365f8db1bac	2626	if (tmp[1] + length > st.hw->bank_size)
Ucial	0:7365f8db1bac	2627	return -1;
Ucial	0:7365f8db1bac	2628
Ucial	0:7365f8db1bac	2629	if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
Ucial	0:7365f8db1bac	2630	return -1;
Ucial	0:7365f8db1bac	2631	if (i2c_write(st.hw->addr, st.reg->mem_r_w, length, data))
Ucial	0:7365f8db1bac	2632	return -1;
Ucial	0:7365f8db1bac	2633	return 0;
Ucial	0:7365f8db1bac	2634	}
Ucial	0:7365f8db1bac	2635
Ucial	0:7365f8db1bac	2636	/**
Ucial	0:7365f8db1bac	2637	* @brief Read from the DMP memory.
Ucial	0:7365f8db1bac	2638	* This function prevents I2C reads past the bank boundaries. The DMP memory
Ucial	0:7365f8db1bac	2639	* is only accessible when the chip is awake.
Ucial	0:7365f8db1bac	2640	* @param[in] mem_addr Memory location (bank << 8 \| start address)
Ucial	0:7365f8db1bac	2641	* @param[in] length Number of bytes to read.
Ucial	0:7365f8db1bac	2642	* @param[out] data Bytes read from memory.
Ucial	0:7365f8db1bac	2643	* @return 0 if successful.
Ucial	0:7365f8db1bac	2644	*/
Ucial	0:7365f8db1bac	2645	int mpu_read_mem(unsigned short mem_addr, unsigned short length,
Ucial	0:7365f8db1bac	2646	unsigned char *data)
Ucial	0:7365f8db1bac	2647	{
Ucial	0:7365f8db1bac	2648	unsigned char tmp[2];
Ucial	0:7365f8db1bac	2649
Ucial	0:7365f8db1bac	2650	if (!data)
Ucial	0:7365f8db1bac	2651	return -1;
Ucial	0:7365f8db1bac	2652	if (!st.chip_cfg.sensors)
Ucial	0:7365f8db1bac	2653	return -1;
Ucial	0:7365f8db1bac	2654
Ucial	0:7365f8db1bac	2655	tmp[0] = (unsigned char)(mem_addr >> 8);
Ucial	0:7365f8db1bac	2656	tmp[1] = (unsigned char)(mem_addr & 0xFF);
Ucial	0:7365f8db1bac	2657
Ucial	0:7365f8db1bac	2658	/* Check bank boundaries. */
Ucial	0:7365f8db1bac	2659	if (tmp[1] + length > st.hw->bank_size)
Ucial	0:7365f8db1bac	2660	return -1;
Ucial	0:7365f8db1bac	2661
Ucial	0:7365f8db1bac	2662	if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
Ucial	0:7365f8db1bac	2663	return -1;
Ucial	0:7365f8db1bac	2664	if (i2c_read(st.hw->addr, st.reg->mem_r_w, length, data))
Ucial	0:7365f8db1bac	2665	return -1;
Ucial	0:7365f8db1bac	2666	return 0;
Ucial	0:7365f8db1bac	2667	}
Ucial	0:7365f8db1bac	2668
Ucial	0:7365f8db1bac	2669	/**
Ucial	0:7365f8db1bac	2670	* @brief Load and verify DMP image.
Ucial	0:7365f8db1bac	2671	* @param[in] length Length of DMP image.
Ucial	0:7365f8db1bac	2672	* @param[in] firmware DMP code.
Ucial	0:7365f8db1bac	2673	* @param[in] start_addr Starting address of DMP code memory.
Ucial	0:7365f8db1bac	2674	* @param[in] sample_rate Fixed sampling rate used when DMP is enabled.
Ucial	0:7365f8db1bac	2675	* @return 0 if successful.
Ucial	0:7365f8db1bac	2676	*/
Ucial	0:7365f8db1bac	2677	int mpu_load_firmware(unsigned short length, const unsigned char *firmware,
Ucial	0:7365f8db1bac	2678	unsigned short start_addr, unsigned short sample_rate)
Ucial	0:7365f8db1bac	2679	{
Ucial	0:7365f8db1bac	2680	unsigned short ii;
Ucial	0:7365f8db1bac	2681	unsigned short this_write;
Ucial	0:7365f8db1bac	2682	/* Must divide evenly into st.hw->bank_size to avoid bank crossings. */
Ucial	0:7365f8db1bac	2683	#define LOAD_CHUNK (16)
Ucial	0:7365f8db1bac	2684	unsigned char cur[LOAD_CHUNK], tmp[2];
Ucial	0:7365f8db1bac	2685
Ucial	0:7365f8db1bac	2686	if (st.chip_cfg.dmp_loaded)
Ucial	0:7365f8db1bac	2687	/* DMP should only be loaded once. */
Ucial	0:7365f8db1bac	2688	return -1;
Ucial	0:7365f8db1bac	2689
Ucial	0:7365f8db1bac	2690	if (!firmware)
Ucial	0:7365f8db1bac	2691	return -1;
Ucial	0:7365f8db1bac	2692	for (ii = 0; ii < length; ii += this_write) {
Ucial	0:7365f8db1bac	2693	this_write = min(LOAD_CHUNK, length - ii);
Ucial	0:7365f8db1bac	2694	if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii]))
Ucial	0:7365f8db1bac	2695	return -1;
Ucial	0:7365f8db1bac	2696	if (mpu_read_mem(ii, this_write, cur))
Ucial	0:7365f8db1bac	2697	return -1;
Ucial	0:7365f8db1bac	2698	if (memcmp(firmware+ii, cur, this_write))
Ucial	0:7365f8db1bac	2699	return -2;
Ucial	0:7365f8db1bac	2700	}
Ucial	0:7365f8db1bac	2701
Ucial	0:7365f8db1bac	2702	/* Set program start address. */
Ucial	0:7365f8db1bac	2703	tmp[0] = start_addr >> 8;
Ucial	0:7365f8db1bac	2704	tmp[1] = start_addr & 0xFF;
Ucial	0:7365f8db1bac	2705	if (i2c_write(st.hw->addr, st.reg->prgm_start_h, 2, tmp))
Ucial	0:7365f8db1bac	2706	return -1;
Ucial	0:7365f8db1bac	2707
Ucial	0:7365f8db1bac	2708	st.chip_cfg.dmp_loaded = 1;
Ucial	0:7365f8db1bac	2709	st.chip_cfg.dmp_sample_rate = sample_rate;
Ucial	0:7365f8db1bac	2710	return 0;
Ucial	0:7365f8db1bac	2711	}
Ucial	0:7365f8db1bac	2712
Ucial	0:7365f8db1bac	2713	/**
Ucial	0:7365f8db1bac	2714	* @brief Enable/disable DMP support.
Ucial	0:7365f8db1bac	2715	* @param[in] enable 1 to turn on the DMP.
Ucial	0:7365f8db1bac	2716	* @return 0 if successful.
Ucial	0:7365f8db1bac	2717	*/
Ucial	0:7365f8db1bac	2718	int mpu_set_dmp_state(unsigned char enable)
Ucial	0:7365f8db1bac	2719	{
Ucial	0:7365f8db1bac	2720	unsigned char tmp;
Ucial	0:7365f8db1bac	2721	if (st.chip_cfg.dmp_on == enable)
Ucial	0:7365f8db1bac	2722	return 0;
Ucial	0:7365f8db1bac	2723
Ucial	0:7365f8db1bac	2724	if (enable) {
Ucial	0:7365f8db1bac	2725	if (!st.chip_cfg.dmp_loaded)
Ucial	0:7365f8db1bac	2726	return -1;
Ucial	0:7365f8db1bac	2727	/* Disable data ready interrupt. */
Ucial	0:7365f8db1bac	2728	set_int_enable(0);
Ucial	0:7365f8db1bac	2729	/* Disable bypass mode. */
Ucial	0:7365f8db1bac	2730	mpu_set_bypass(0);
Ucial	0:7365f8db1bac	2731	/* Keep constant sample rate, FIFO rate controlled by DMP. */
Ucial	0:7365f8db1bac	2732	mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate);
Ucial	0:7365f8db1bac	2733	/* Remove FIFO elements. */
Ucial	0:7365f8db1bac	2734	tmp = 0;
Ucial	0:7365f8db1bac	2735	i2c_write(st.hw->addr, 0x23, 1, &tmp);
Ucial	0:7365f8db1bac	2736	st.chip_cfg.dmp_on = 1;
Ucial	0:7365f8db1bac	2737	/* Enable DMP interrupt. */
Ucial	0:7365f8db1bac	2738	set_int_enable(1);
Ucial	0:7365f8db1bac	2739	mpu_reset_fifo();
Ucial	0:7365f8db1bac	2740	} else {
Ucial	0:7365f8db1bac	2741	/* Disable DMP interrupt. */
Ucial	0:7365f8db1bac	2742	set_int_enable(0);
Ucial	0:7365f8db1bac	2743	/* Restore FIFO settings. */
Ucial	0:7365f8db1bac	2744	tmp = st.chip_cfg.fifo_enable;
Ucial	0:7365f8db1bac	2745	i2c_write(st.hw->addr, 0x23, 1, &tmp);
Ucial	0:7365f8db1bac	2746	st.chip_cfg.dmp_on = 0;
Ucial	0:7365f8db1bac	2747	mpu_reset_fifo();
Ucial	0:7365f8db1bac	2748	}
Ucial	0:7365f8db1bac	2749	return 0;
Ucial	0:7365f8db1bac	2750	}
Ucial	0:7365f8db1bac	2751
Ucial	0:7365f8db1bac	2752	/**
Ucial	0:7365f8db1bac	2753	* @brief Get DMP state.
Ucial	0:7365f8db1bac	2754	* @param[out] enabled 1 if enabled.
Ucial	0:7365f8db1bac	2755	* @return 0 if successful.
Ucial	0:7365f8db1bac	2756	*/
Ucial	0:7365f8db1bac	2757	int mpu_get_dmp_state(unsigned char *enabled)
Ucial	0:7365f8db1bac	2758	{
Ucial	0:7365f8db1bac	2759	enabled[0] = st.chip_cfg.dmp_on;
Ucial	0:7365f8db1bac	2760	return 0;
Ucial	0:7365f8db1bac	2761	}
Ucial	0:7365f8db1bac	2762
Ucial	0:7365f8db1bac	2763
Ucial	0:7365f8db1bac	2764	/* This initialization is similar to the one in ak8975.c. */
Ucial	0:7365f8db1bac	2765	int setup_compass(void)
Ucial	0:7365f8db1bac	2766	{
Ucial	0:7365f8db1bac	2767	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	2768	unsigned char data[4], akm_addr;
Ucial	0:7365f8db1bac	2769
Ucial	0:7365f8db1bac	2770	mpu_set_bypass(1);
Ucial	0:7365f8db1bac	2771
Ucial	0:7365f8db1bac	2772	/* Find compass. Possible addresses range from 0x0C to 0x0F. */
Ucial	0:7365f8db1bac	2773	for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {
Ucial	0:7365f8db1bac	2774	int result;
Ucial	0:7365f8db1bac	2775	result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data);
Ucial	0:7365f8db1bac	2776	if (!result && (data[0] == AKM_WHOAMI))
Ucial	0:7365f8db1bac	2777	break;
Ucial	0:7365f8db1bac	2778	}
Ucial	0:7365f8db1bac	2779
Ucial	0:7365f8db1bac	2780	if (akm_addr > 0x0F) {
Ucial	0:7365f8db1bac	2781	/* TODO: Handle this case in all compass-related functions. */
Ucial	0:7365f8db1bac	2782	//("Compass not found.\n");
Ucial	0:7365f8db1bac	2783	return -1;
Ucial	0:7365f8db1bac	2784	}
Ucial	0:7365f8db1bac	2785
Ucial	0:7365f8db1bac	2786	st.chip_cfg.compass_addr = akm_addr;
Ucial	0:7365f8db1bac	2787
Ucial	0:7365f8db1bac	2788	data[0] = AKM_POWER_DOWN;
Ucial	0:7365f8db1bac	2789	if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
Ucial	0:7365f8db1bac	2790	return -1;
Ucial	0:7365f8db1bac	2791	delay_ms(1);
Ucial	0:7365f8db1bac	2792
Ucial	0:7365f8db1bac	2793	data[0] = AKM_FUSE_ROM_ACCESS;
Ucial	0:7365f8db1bac	2794	if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
Ucial	0:7365f8db1bac	2795	return -1;
Ucial	0:7365f8db1bac	2796	delay_ms(1);
Ucial	0:7365f8db1bac	2797
Ucial	0:7365f8db1bac	2798	/* Get sensitivity adjustment data from fuse ROM. */
Ucial	0:7365f8db1bac	2799	if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data))
Ucial	0:7365f8db1bac	2800	return -1;
Ucial	0:7365f8db1bac	2801	st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128;
Ucial	0:7365f8db1bac	2802	st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128;
Ucial	0:7365f8db1bac	2803	st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128;
Ucial	0:7365f8db1bac	2804
Ucial	0:7365f8db1bac	2805	data[0] = AKM_POWER_DOWN;
Ucial	0:7365f8db1bac	2806	if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
Ucial	0:7365f8db1bac	2807	return -1;
Ucial	0:7365f8db1bac	2808	delay_ms(1);
Ucial	0:7365f8db1bac	2809
Ucial	0:7365f8db1bac	2810	mpu_set_bypass(0);
Ucial	0:7365f8db1bac	2811
Ucial	0:7365f8db1bac	2812	/* Set up master mode, master clock, and ES bit. */
Ucial	0:7365f8db1bac	2813	data[0] = 0x40;
Ucial	0:7365f8db1bac	2814	if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
Ucial	0:7365f8db1bac	2815	return -1;
Ucial	0:7365f8db1bac	2816
Ucial	0:7365f8db1bac	2817	/* Slave 0 reads from AKM data registers. */
Ucial	0:7365f8db1bac	2818	data[0] = BIT_I2C_READ \| st.chip_cfg.compass_addr;
Ucial	0:7365f8db1bac	2819	if (i2c_write(st.hw->addr, st.reg->s0_addr, 1, data))
Ucial	0:7365f8db1bac	2820	return -1;
Ucial	0:7365f8db1bac	2821
Ucial	0:7365f8db1bac	2822	/* Compass reads start at this register. */
Ucial	0:7365f8db1bac	2823	data[0] = AKM_REG_ST1;
Ucial	0:7365f8db1bac	2824	if (i2c_write(st.hw->addr, st.reg->s0_reg, 1, data))
Ucial	0:7365f8db1bac	2825	return -1;
Ucial	0:7365f8db1bac	2826
Ucial	0:7365f8db1bac	2827	/* Enable slave 0, 8-byte reads. */
Ucial	0:7365f8db1bac	2828	data[0] = BIT_SLAVE_EN \| 8;
Ucial	0:7365f8db1bac	2829	if (i2c_write(st.hw->addr, st.reg->s0_ctrl, 1, data))
Ucial	0:7365f8db1bac	2830	return -1;
Ucial	0:7365f8db1bac	2831
Ucial	0:7365f8db1bac	2832	/* Slave 1 changes AKM measurement mode. */
Ucial	0:7365f8db1bac	2833	data[0] = st.chip_cfg.compass_addr;
Ucial	0:7365f8db1bac	2834	if (i2c_write(st.hw->addr, st.reg->s1_addr, 1, data))
Ucial	0:7365f8db1bac	2835	return -1;
Ucial	0:7365f8db1bac	2836
Ucial	0:7365f8db1bac	2837	/* AKM measurement mode register. */
Ucial	0:7365f8db1bac	2838	data[0] = AKM_REG_CNTL;
Ucial	0:7365f8db1bac	2839	if (i2c_write(st.hw->addr, st.reg->s1_reg, 1, data))
Ucial	0:7365f8db1bac	2840	return -1;
Ucial	0:7365f8db1bac	2841
Ucial	0:7365f8db1bac	2842	/* Enable slave 1, 1-byte writes. */
Ucial	0:7365f8db1bac	2843	data[0] = BIT_SLAVE_EN \| 1;
Ucial	0:7365f8db1bac	2844	if (i2c_write(st.hw->addr, st.reg->s1_ctrl, 1, data))
Ucial	0:7365f8db1bac	2845	return -1;
Ucial	0:7365f8db1bac	2846
Ucial	0:7365f8db1bac	2847	/* Set slave 1 data. */
Ucial	0:7365f8db1bac	2848	data[0] = AKM_SINGLE_MEASUREMENT;
Ucial	0:7365f8db1bac	2849	if (i2c_write(st.hw->addr, st.reg->s1_do, 1, data))
Ucial	0:7365f8db1bac	2850	return -1;
Ucial	0:7365f8db1bac	2851
Ucial	0:7365f8db1bac	2852	/* Trigger slave 0 and slave 1 actions at each sample. */
Ucial	0:7365f8db1bac	2853	data[0] = 0x03;
Ucial	0:7365f8db1bac	2854	if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data))
Ucial	0:7365f8db1bac	2855	return -1;
Ucial	0:7365f8db1bac	2856
Ucial	0:7365f8db1bac	2857	#ifdef MPU9150
Ucial	0:7365f8db1bac	2858	/* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */
Ucial	0:7365f8db1bac	2859	data[0] = BIT_I2C_MST_VDDIO;
Ucial	0:7365f8db1bac	2860	if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data))
Ucial	0:7365f8db1bac	2861	return -1;
Ucial	0:7365f8db1bac	2862	#endif
Ucial	0:7365f8db1bac	2863
Ucial	0:7365f8db1bac	2864	return 0;
Ucial	0:7365f8db1bac	2865	#else
Ucial	0:7365f8db1bac	2866	return -1;
Ucial	0:7365f8db1bac	2867	#endif
Ucial	0:7365f8db1bac	2868	}
Ucial	0:7365f8db1bac	2869
Ucial	0:7365f8db1bac	2870	/**
Ucial	0:7365f8db1bac	2871	* @brief Read raw compass data.
Ucial	0:7365f8db1bac	2872	* @param[out] data Raw data in hardware units.
Ucial	0:7365f8db1bac	2873	* @param[out] timestamp Timestamp in milliseconds. Null if not needed.
Ucial	0:7365f8db1bac	2874	* @return 0 if successful.
Ucial	0:7365f8db1bac	2875	*/
Ucial	0:7365f8db1bac	2876	int mpu_get_compass_reg(short data, unsigned long timestamp)
Ucial	0:7365f8db1bac	2877	{
Ucial	0:7365f8db1bac	2878	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	2879	unsigned char tmp[9];
Ucial	0:7365f8db1bac	2880
Ucial	0:7365f8db1bac	2881	if (!(st.chip_cfg.sensors & INV_XYZ_COMPASS))
Ucial	0:7365f8db1bac	2882	return -1;
Ucial	0:7365f8db1bac	2883
Ucial	0:7365f8db1bac	2884	#ifdef AK89xx_BYPASS
Ucial	0:7365f8db1bac	2885	if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp))
Ucial	0:7365f8db1bac	2886	return -1;
Ucial	0:7365f8db1bac	2887	tmp[8] = AKM_SINGLE_MEASUREMENT;
Ucial	0:7365f8db1bac	2888	if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8))
Ucial	0:7365f8db1bac	2889	return -1;
Ucial	0:7365f8db1bac	2890	#else
Ucial	0:7365f8db1bac	2891	if (i2c_read(st.hw->addr, st.reg->raw_compass, 8, tmp))
Ucial	0:7365f8db1bac	2892	return -1;
Ucial	0:7365f8db1bac	2893	#endif
Ucial	0:7365f8db1bac	2894
Ucial	0:7365f8db1bac	2895	#if defined AK8975_SECONDARY
Ucial	0:7365f8db1bac	2896	/* AK8975 doesn't have the overrun error bit. */
Ucial	0:7365f8db1bac	2897	if (!(tmp[0] & AKM_DATA_READY))
Ucial	0:7365f8db1bac	2898	return -2;
Ucial	0:7365f8db1bac	2899	if ((tmp[7] & AKM_OVERFLOW) \|\| (tmp[7] & AKM_DATA_ERROR))
Ucial	0:7365f8db1bac	2900	return -3;
Ucial	0:7365f8db1bac	2901	#elif defined AK8963_SECONDARY
Ucial	0:7365f8db1bac	2902	/* AK8963 doesn't have the data read error bit. */
Ucial	0:7365f8db1bac	2903	if (!(tmp[0] & AKM_DATA_READY) \|\| (tmp[0] & AKM_DATA_OVERRUN))
Ucial	0:7365f8db1bac	2904	return -2;
Ucial	0:7365f8db1bac	2905	if (tmp[7] & AKM_OVERFLOW)
Ucial	0:7365f8db1bac	2906	return -3;
Ucial	0:7365f8db1bac	2907	#endif
Ucial	0:7365f8db1bac	2908	data[0] = (tmp[2] << 8) \| tmp[1];
Ucial	0:7365f8db1bac	2909	data[1] = (tmp[4] << 8) \| tmp[3];
Ucial	0:7365f8db1bac	2910	data[2] = (tmp[6] << 8) \| tmp[5];
Ucial	0:7365f8db1bac	2911
Ucial	0:7365f8db1bac	2912	data[0] = ((long)data[0] * st.chip_cfg.mag_sens_adj[0]) >> 8;
Ucial	0:7365f8db1bac	2913	data[1] = ((long)data[1] * st.chip_cfg.mag_sens_adj[1]) >> 8;
Ucial	0:7365f8db1bac	2914	data[2] = ((long)data[2] * st.chip_cfg.mag_sens_adj[2]) >> 8;
Ucial	0:7365f8db1bac	2915
Ucial	0:7365f8db1bac	2916	if (timestamp)
Ucial	0:7365f8db1bac	2917	get_ms(timestamp);
Ucial	0:7365f8db1bac	2918	return 0;
Ucial	0:7365f8db1bac	2919	#else
Ucial	0:7365f8db1bac	2920	return -1;
Ucial	0:7365f8db1bac	2921	#endif
Ucial	0:7365f8db1bac	2922	}
Ucial	0:7365f8db1bac	2923
Ucial	0:7365f8db1bac	2924	/**
Ucial	0:7365f8db1bac	2925	* @brief Get the compass full-scale range.
Ucial	0:7365f8db1bac	2926	* @param[out] fsr Current full-scale range.
Ucial	0:7365f8db1bac	2927	* @return 0 if successful.
Ucial	0:7365f8db1bac	2928	*/
Ucial	0:7365f8db1bac	2929	int mpu_get_compass_fsr(unsigned short *fsr)
Ucial	0:7365f8db1bac	2930	{
Ucial	0:7365f8db1bac	2931	#ifdef AK89xx_SECONDARY
Ucial	0:7365f8db1bac	2932	fsr[0] = st.hw->compass_fsr;
Ucial	0:7365f8db1bac	2933	return 0;
Ucial	0:7365f8db1bac	2934	#else
Ucial	0:7365f8db1bac	2935	return -1;
Ucial	0:7365f8db1bac	2936	#endif
Ucial	0:7365f8db1bac	2937	}
Ucial	0:7365f8db1bac	2938
Ucial	0:7365f8db1bac	2939	/**
Ucial	0:7365f8db1bac	2940	* @brief Enters LP accel motion interrupt mode.
Ucial	0:7365f8db1bac	2941	* The behavior of this feature is very different between the MPU6050 and the
Ucial	0:7365f8db1bac	2942	* MPU6500. Each chip's version of this feature is explained below.
Ucial	0:7365f8db1bac	2943	*
Ucial	0:7365f8db1bac	2944	* \n MPU6050:
Ucial	0:7365f8db1bac	2945	* \n When this mode is first enabled, the hardware captures a single accel
Ucial	0:7365f8db1bac	2946	* sample, and subsequent samples are compared with this one to determine if
Ucial	0:7365f8db1bac	2947	* the device is in motion. Therefore, whenever this "locked" sample needs to
Ucial	0:7365f8db1bac	2948	* be changed, this function must be called again.
Ucial	0:7365f8db1bac	2949	*
Ucial	0:7365f8db1bac	2950	* \n The hardware motion threshold can be between 32mg and 8160mg in 32mg
Ucial	0:7365f8db1bac	2951	* increments.
Ucial	0:7365f8db1bac	2952	*
Ucial	0:7365f8db1bac	2953	* \n Low-power accel mode supports the following frequencies:
Ucial	0:7365f8db1bac	2954	* \n 1.25Hz, 5Hz, 20Hz, 40Hz
Ucial	0:7365f8db1bac	2955	*
Ucial	0:7365f8db1bac	2956	* \n MPU6500:
Ucial	0:7365f8db1bac	2957	* \n Unlike the MPU6050 version, the hardware does not "lock in" a reference
Ucial	0:7365f8db1bac	2958	* sample. The hardware monitors the accel data and detects any large change
Ucial	0:7365f8db1bac	2959	* over a short period of time.
Ucial	0:7365f8db1bac	2960	*
Ucial	0:7365f8db1bac	2961	* \n The hardware motion threshold can be between 4mg and 1020mg in 4mg
Ucial	0:7365f8db1bac	2962	* increments.
Ucial	0:7365f8db1bac	2963	*
Ucial	0:7365f8db1bac	2964	* \n MPU6500 Low-power accel mode supports the following frequencies:
Ucial	0:7365f8db1bac	2965	* \n 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
Ucial	0:7365f8db1bac	2966	*
Ucial	0:7365f8db1bac	2967	* \n\n NOTES:
Ucial	0:7365f8db1bac	2968	* \n The driver will round down @e thresh to the nearest supported value if
Ucial	0:7365f8db1bac	2969	* an unsupported threshold is selected.
Ucial	0:7365f8db1bac	2970	* \n To select a fractional wake-up frequency, round down the value passed to
Ucial	0:7365f8db1bac	2971	* @e lpa_freq.
Ucial	0:7365f8db1bac	2972	* \n The MPU6500 does not support a delay parameter. If this function is used
Ucial	0:7365f8db1bac	2973	* for the MPU6500, the value passed to @e time will be ignored.
Ucial	0:7365f8db1bac	2974	* \n To disable this mode, set @e lpa_freq to zero. The driver will restore
Ucial	0:7365f8db1bac	2975	* the previous configuration.
Ucial	0:7365f8db1bac	2976	*
Ucial	0:7365f8db1bac	2977	* @param[in] thresh Motion threshold in mg.
Ucial	0:7365f8db1bac	2978	* @param[in] time Duration in milliseconds that the accel data must
Ucial	0:7365f8db1bac	2979	* exceed @e thresh before motion is reported.
Ucial	0:7365f8db1bac	2980	* @param[in] lpa_freq Minimum sampling rate, or zero to disable.
Ucial	0:7365f8db1bac	2981	* @return 0 if successful.
Ucial	0:7365f8db1bac	2982	*/
Ucial	0:7365f8db1bac	2983	int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
Ucial	0:7365f8db1bac	2984	unsigned char lpa_freq)
Ucial	0:7365f8db1bac	2985	{
Ucial	0:7365f8db1bac	2986	unsigned char data[3];
Ucial	0:7365f8db1bac	2987
Ucial	0:7365f8db1bac	2988	if (lpa_freq) {
Ucial	0:7365f8db1bac	2989	unsigned char thresh_hw;
Ucial	0:7365f8db1bac	2990
Ucial	0:7365f8db1bac	2991	#if defined MPU6050
Ucial	0:7365f8db1bac	2992	/* TODO: Make these const/#defines. */
Ucial	0:7365f8db1bac	2993	/* 1LSb = 32mg. */
Ucial	0:7365f8db1bac	2994	if (thresh > 8160)
Ucial	0:7365f8db1bac	2995	thresh_hw = 255;
Ucial	0:7365f8db1bac	2996	else if (thresh < 32)
Ucial	0:7365f8db1bac	2997	thresh_hw = 1;
Ucial	0:7365f8db1bac	2998	else
Ucial	0:7365f8db1bac	2999	thresh_hw = thresh >> 5;
Ucial	0:7365f8db1bac	3000	#elif defined MPU6500
Ucial	0:7365f8db1bac	3001	/* 1LSb = 4mg. */
Ucial	0:7365f8db1bac	3002	if (thresh > 1020)
Ucial	0:7365f8db1bac	3003	thresh_hw = 255;
Ucial	0:7365f8db1bac	3004	else if (thresh < 4)
Ucial	0:7365f8db1bac	3005	thresh_hw = 1;
Ucial	0:7365f8db1bac	3006	else
Ucial	0:7365f8db1bac	3007	thresh_hw = thresh >> 2;
Ucial	0:7365f8db1bac	3008	#endif
Ucial	0:7365f8db1bac	3009
Ucial	0:7365f8db1bac	3010	if (!time)
Ucial	0:7365f8db1bac	3011	/* Minimum duration must be 1ms. */
Ucial	0:7365f8db1bac	3012	time = 1;
Ucial	0:7365f8db1bac	3013
Ucial	0:7365f8db1bac	3014	#if defined MPU6050
Ucial	0:7365f8db1bac	3015	if (lpa_freq > 40)
Ucial	0:7365f8db1bac	3016	#elif defined MPU6500
Ucial	0:7365f8db1bac	3017	if (lpa_freq > 640)
Ucial	0:7365f8db1bac	3018	#endif
Ucial	0:7365f8db1bac	3019	/* At this point, the chip has not been re-configured, so the
Ucial	0:7365f8db1bac	3020	* function can safely exit.
Ucial	0:7365f8db1bac	3021	*/
Ucial	0:7365f8db1bac	3022	return -1;
Ucial	0:7365f8db1bac	3023
Ucial	0:7365f8db1bac	3024	if (!st.chip_cfg.int_motion_only) {
Ucial	0:7365f8db1bac	3025	/* Store current settings for later. */
Ucial	0:7365f8db1bac	3026	if (st.chip_cfg.dmp_on) {
Ucial	0:7365f8db1bac	3027	mpu_set_dmp_state(0);
Ucial	0:7365f8db1bac	3028	st.chip_cfg.cache.dmp_on = 1;
Ucial	0:7365f8db1bac	3029	} else
Ucial	0:7365f8db1bac	3030	st.chip_cfg.cache.dmp_on = 0;
Ucial	0:7365f8db1bac	3031	mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr);
Ucial	0:7365f8db1bac	3032	mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr);
Ucial	0:7365f8db1bac	3033	mpu_get_lpf(&st.chip_cfg.cache.lpf);
Ucial	0:7365f8db1bac	3034	mpu_get_sample_rate(&st.chip_cfg.cache.sample_rate);
Ucial	0:7365f8db1bac	3035	st.chip_cfg.cache.sensors_on = st.chip_cfg.sensors;
Ucial	0:7365f8db1bac	3036	mpu_get_fifo_config(&st.chip_cfg.cache.fifo_sensors);
Ucial	0:7365f8db1bac	3037	}
Ucial	0:7365f8db1bac	3038
Ucial	0:7365f8db1bac	3039	#ifdef MPU6050
Ucial	0:7365f8db1bac	3040	/* Disable hardware interrupts for now. */
Ucial	0:7365f8db1bac	3041	set_int_enable(0);
Ucial	0:7365f8db1bac	3042
Ucial	0:7365f8db1bac	3043	/* Enter full-power accel-only mode. */
Ucial	0:7365f8db1bac	3044	mpu_lp_accel_mode(0);
Ucial	0:7365f8db1bac	3045
Ucial	0:7365f8db1bac	3046	/* Override current LPF (and HPF) settings to obtain a valid accel
Ucial	0:7365f8db1bac	3047	* reading.
Ucial	0:7365f8db1bac	3048	*/
Ucial	0:7365f8db1bac	3049	data[0] = INV_FILTER_256HZ_NOLPF2;
Ucial	0:7365f8db1bac	3050	if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))
Ucial	0:7365f8db1bac	3051	return -1;
Ucial	0:7365f8db1bac	3052
Ucial	0:7365f8db1bac	3053	/* NOTE: Digital high pass filter should be configured here. Since this
Ucial	0:7365f8db1bac	3054	* driver doesn't modify those bits anywhere, they should already be
Ucial	0:7365f8db1bac	3055	* cleared by default.
Ucial	0:7365f8db1bac	3056	*/
Ucial	0:7365f8db1bac	3057
Ucial	0:7365f8db1bac	3058	/* Configure the device to send motion interrupts. */
Ucial	0:7365f8db1bac	3059	/* Enable motion interrupt. */
Ucial	0:7365f8db1bac	3060	data[0] = BIT_MOT_INT_EN;
Ucial	0:7365f8db1bac	3061	if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
Ucial	0:7365f8db1bac	3062	goto lp_int_restore;
Ucial	0:7365f8db1bac	3063
Ucial	0:7365f8db1bac	3064	/* Set motion interrupt parameters. */
Ucial	0:7365f8db1bac	3065	data[0] = thresh_hw;
Ucial	0:7365f8db1bac	3066	data[1] = time;
Ucial	0:7365f8db1bac	3067	if (i2c_write(st.hw->addr, st.reg->motion_thr, 2, data))
Ucial	0:7365f8db1bac	3068	goto lp_int_restore;
Ucial	0:7365f8db1bac	3069
Ucial	0:7365f8db1bac	3070	/* Force hardware to "lock" current accel sample. */
Ucial	0:7365f8db1bac	3071	delay_ms(5);
Ucial	0:7365f8db1bac	3072	data[0] = (st.chip_cfg.accel_fsr << 3) \| BITS_HPF;
Ucial	0:7365f8db1bac	3073	if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
Ucial	0:7365f8db1bac	3074	goto lp_int_restore;
Ucial	0:7365f8db1bac	3075
Ucial	0:7365f8db1bac	3076	/* Set up LP accel mode. */
Ucial	0:7365f8db1bac	3077	data[0] = BIT_LPA_CYCLE;
Ucial	0:7365f8db1bac	3078	if (lpa_freq == 1)
Ucial	0:7365f8db1bac	3079	data[1] = INV_LPA_1_25HZ;
Ucial	0:7365f8db1bac	3080	else if (lpa_freq <= 5)
Ucial	0:7365f8db1bac	3081	data[1] = INV_LPA_5HZ;
Ucial	0:7365f8db1bac	3082	else if (lpa_freq <= 20)
Ucial	0:7365f8db1bac	3083	data[1] = INV_LPA_20HZ;
Ucial	0:7365f8db1bac	3084	else
Ucial	0:7365f8db1bac	3085	data[1] = INV_LPA_40HZ;
Ucial	0:7365f8db1bac	3086	data[1] = (data[1] << 6) \| BIT_STBY_XYZG;
Ucial	0:7365f8db1bac	3087	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
Ucial	0:7365f8db1bac	3088	goto lp_int_restore;
Ucial	0:7365f8db1bac	3089
Ucial	0:7365f8db1bac	3090	st.chip_cfg.int_motion_only = 1;
Ucial	0:7365f8db1bac	3091	return 0;
Ucial	0:7365f8db1bac	3092	#elif defined MPU6500
Ucial	0:7365f8db1bac	3093	/* Disable hardware interrupts. */
Ucial	0:7365f8db1bac	3094	set_int_enable(0);
Ucial	0:7365f8db1bac	3095
Ucial	0:7365f8db1bac	3096	/* Enter full-power accel-only mode, no FIFO/DMP. */
Ucial	0:7365f8db1bac	3097	data[0] = 0;
Ucial	0:7365f8db1bac	3098	data[1] = 0;
Ucial	0:7365f8db1bac	3099	data[2] = BIT_STBY_XYZG;
Ucial	0:7365f8db1bac	3100	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 3, data))
Ucial	0:7365f8db1bac	3101	goto lp_int_restore;
Ucial	0:7365f8db1bac	3102
Ucial	0:7365f8db1bac	3103	/* Set motion threshold. */
Ucial	0:7365f8db1bac	3104	data[0] = thresh_hw;
Ucial	0:7365f8db1bac	3105	if (i2c_write(st.hw->addr, st.reg->motion_thr, 1, data))
Ucial	0:7365f8db1bac	3106	goto lp_int_restore;
Ucial	0:7365f8db1bac	3107
Ucial	0:7365f8db1bac	3108	/* Set wake frequency. */
Ucial	0:7365f8db1bac	3109	if (lpa_freq == 1)
Ucial	0:7365f8db1bac	3110	data[0] = INV_LPA_1_25HZ;
Ucial	0:7365f8db1bac	3111	else if (lpa_freq == 2)
Ucial	0:7365f8db1bac	3112	data[0] = INV_LPA_2_5HZ;
Ucial	0:7365f8db1bac	3113	else if (lpa_freq <= 5)
Ucial	0:7365f8db1bac	3114	data[0] = INV_LPA_5HZ;
Ucial	0:7365f8db1bac	3115	else if (lpa_freq <= 10)
Ucial	0:7365f8db1bac	3116	data[0] = INV_LPA_10HZ;
Ucial	0:7365f8db1bac	3117	else if (lpa_freq <= 20)
Ucial	0:7365f8db1bac	3118	data[0] = INV_LPA_20HZ;
Ucial	0:7365f8db1bac	3119	else if (lpa_freq <= 40)
Ucial	0:7365f8db1bac	3120	data[0] = INV_LPA_40HZ;
Ucial	0:7365f8db1bac	3121	else if (lpa_freq <= 80)
Ucial	0:7365f8db1bac	3122	data[0] = INV_LPA_80HZ;
Ucial	0:7365f8db1bac	3123	else if (lpa_freq <= 160)
Ucial	0:7365f8db1bac	3124	data[0] = INV_LPA_160HZ;
Ucial	0:7365f8db1bac	3125	else if (lpa_freq <= 320)
Ucial	0:7365f8db1bac	3126	data[0] = INV_LPA_320HZ;
Ucial	0:7365f8db1bac	3127	else
Ucial	0:7365f8db1bac	3128	data[0] = INV_LPA_640HZ;
Ucial	0:7365f8db1bac	3129	if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, data))
Ucial	0:7365f8db1bac	3130	goto lp_int_restore;
Ucial	0:7365f8db1bac	3131
Ucial	0:7365f8db1bac	3132	/* Enable motion interrupt (MPU6500 version). */
Ucial	0:7365f8db1bac	3133	data[0] = BITS_WOM_EN;
Ucial	0:7365f8db1bac	3134	if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data))
Ucial	0:7365f8db1bac	3135	goto lp_int_restore;
Ucial	0:7365f8db1bac	3136
Ucial	0:7365f8db1bac	3137	/* Enable cycle mode. */
Ucial	0:7365f8db1bac	3138	data[0] = BIT_LPA_CYCLE;
Ucial	0:7365f8db1bac	3139	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
Ucial	0:7365f8db1bac	3140	goto lp_int_restore;
Ucial	0:7365f8db1bac	3141
Ucial	0:7365f8db1bac	3142	/* Enable interrupt. */
Ucial	0:7365f8db1bac	3143	data[0] = BIT_MOT_INT_EN;
Ucial	0:7365f8db1bac	3144	if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
Ucial	0:7365f8db1bac	3145	goto lp_int_restore;
Ucial	0:7365f8db1bac	3146
Ucial	0:7365f8db1bac	3147	st.chip_cfg.int_motion_only = 1;
Ucial	0:7365f8db1bac	3148	return 0;
Ucial	0:7365f8db1bac	3149	#endif
Ucial	0:7365f8db1bac	3150	} else {
Ucial	0:7365f8db1bac	3151	/* Don't "restore" the previous state if no state has been saved. */
Ucial	0:7365f8db1bac	3152	int ii;
Ucial	0:7365f8db1bac	3153	char cache_ptr = (char)&st.chip_cfg.cache;
Ucial	0:7365f8db1bac	3154	for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++) {
Ucial	0:7365f8db1bac	3155	if (cache_ptr[ii] != 0)
Ucial	0:7365f8db1bac	3156	goto lp_int_restore;
Ucial	0:7365f8db1bac	3157	}
Ucial	0:7365f8db1bac	3158	/* If we reach this point, motion interrupt mode hasn't been used yet. */
Ucial	0:7365f8db1bac	3159	return -1;
Ucial	0:7365f8db1bac	3160	}
Ucial	0:7365f8db1bac	3161	lp_int_restore:
Ucial	0:7365f8db1bac	3162	/* Set to invalid values to ensure no I2C writes are skipped. */
Ucial	0:7365f8db1bac	3163	st.chip_cfg.gyro_fsr = 0xFF;
Ucial	0:7365f8db1bac	3164	st.chip_cfg.accel_fsr = 0xFF;
Ucial	0:7365f8db1bac	3165	st.chip_cfg.lpf = 0xFF;
Ucial	0:7365f8db1bac	3166	st.chip_cfg.sample_rate = 0xFFFF;
Ucial	0:7365f8db1bac	3167	st.chip_cfg.sensors = 0xFF;
Ucial	0:7365f8db1bac	3168	st.chip_cfg.fifo_enable = 0xFF;
Ucial	0:7365f8db1bac	3169	st.chip_cfg.clk_src = INV_CLK_PLL;
Ucial	0:7365f8db1bac	3170	mpu_set_sensors(st.chip_cfg.cache.sensors_on);
Ucial	0:7365f8db1bac	3171	mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr);
Ucial	0:7365f8db1bac	3172	mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr);
Ucial	0:7365f8db1bac	3173	mpu_set_lpf(st.chip_cfg.cache.lpf);
Ucial	0:7365f8db1bac	3174	mpu_set_sample_rate(st.chip_cfg.cache.sample_rate);
Ucial	0:7365f8db1bac	3175	mpu_configure_fifo(st.chip_cfg.cache.fifo_sensors);
Ucial	0:7365f8db1bac	3176
Ucial	0:7365f8db1bac	3177	if (st.chip_cfg.cache.dmp_on)
Ucial	0:7365f8db1bac	3178	mpu_set_dmp_state(1);
Ucial	0:7365f8db1bac	3179
Ucial	0:7365f8db1bac	3180	#ifdef MPU6500
Ucial	0:7365f8db1bac	3181	/* Disable motion interrupt (MPU6500 version). */
Ucial	0:7365f8db1bac	3182	data[0] = 0;
Ucial	0:7365f8db1bac	3183	if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data))
Ucial	0:7365f8db1bac	3184	goto lp_int_restore;
Ucial	0:7365f8db1bac	3185	#endif
Ucial	0:7365f8db1bac	3186
Ucial	0:7365f8db1bac	3187	st.chip_cfg.int_motion_only = 0;
Ucial	0:7365f8db1bac	3188	return 0;
Ucial	0:7365f8db1bac	3189	}
Ucial	0:7365f8db1bac	3190	//////////////////////////////////////////////////////////////////////////////////
Ucial	0:7365f8db1bac	3191	//添加的代码部分
Ucial	0:7365f8db1bac	3192	//////////////////////////////////////////////////////////////////////////////////
Ucial	0:7365f8db1bac	3193	//本程序只供学习使用，未经作者许可，不得用于其它任何用途
Ucial	0:7365f8db1bac	3194	//ALIENTEK STM32开发板
Ucial	0:7365f8db1bac	3195	//MPU6050 DMP 驱动代码
Ucial	0:7365f8db1bac	3196	//正点原子@ALIENTEK
Ucial	0:7365f8db1bac	3197	//技术论坛:www.openedv.com
Ucial	0:7365f8db1bac	3198	//创建日期:2015/1/17
Ucial	0:7365f8db1bac	3199	//版本：V1.0
Ucial	0:7365f8db1bac	3200	//版权所有，盗版必究。
Ucial	0:7365f8db1bac	3201	//Copyright(C) 广州市星翼电子科技有限公司 2009-2019
Ucial	0:7365f8db1bac	3202	//All rights reserved
Ucial	0:7365f8db1bac	3203	//////////////////////////////////////////////////////////////////////////////////
Ucial	0:7365f8db1bac	3204
Ucial	0:7365f8db1bac	3205	//q30格式,long转float时的除数.
Ucial	0:7365f8db1bac	3206	#define q30 1073741824.0f
Ucial	0:7365f8db1bac	3207
Ucial	0:7365f8db1bac	3208	//陀螺仪方向设置
Ucial	0:7365f8db1bac	3209	static signed char gyro_orientation[9] = { 1, 0, 0,
Ucial	0:7365f8db1bac	3210	0, 1, 0,
Ucial	0:7365f8db1bac	3211	0, 0, 1};
Ucial	0:7365f8db1bac	3212	//MPU6050自测试
Ucial	0:7365f8db1bac	3213	//返回值:0,正常
Ucial	0:7365f8db1bac	3214	// 其他,失败
Ucial	0:7365f8db1bac	3215	unsigned char run_self_test(void)
Ucial	0:7365f8db1bac	3216	{
Ucial	0:7365f8db1bac	3217	int result;
Ucial	0:7365f8db1bac	3218	//char test_packet[4] = {0};
Ucial	0:7365f8db1bac	3219	long gyro[3], accel[3];
Ucial	0:7365f8db1bac	3220	result = mpu_run_self_test(gyro, accel);
Ucial	0:7365f8db1bac	3221	if (result == 0x3)
Ucial	0:7365f8db1bac	3222	{
Ucial	0:7365f8db1bac	3223	/* Test passed. We can trust the gyro data here, so let's push it down
Ucial	0:7365f8db1bac	3224	* to the DMP.
Ucial	0:7365f8db1bac	3225	*/
Ucial	0:7365f8db1bac	3226	float sens;
Ucial	0:7365f8db1bac	3227	unsigned short accel_sens;
Ucial	0:7365f8db1bac	3228	mpu_get_gyro_sens(&sens);
Ucial	0:7365f8db1bac	3229	gyro[0] = (long)(gyro[0] * sens);
Ucial	0:7365f8db1bac	3230	gyro[1] = (long)(gyro[1] * sens);
Ucial	0:7365f8db1bac	3231	gyro[2] = (long)(gyro[2] * sens);
Ucial	0:7365f8db1bac	3232	dmp_set_gyro_bias(gyro);
Ucial	0:7365f8db1bac	3233	mpu_get_accel_sens(&accel_sens);
Ucial	0:7365f8db1bac	3234	accel[0] *= accel_sens;
Ucial	0:7365f8db1bac	3235	accel[1] *= accel_sens;
Ucial	0:7365f8db1bac	3236	accel[2] *= accel_sens;
Ucial	0:7365f8db1bac	3237	dmp_set_accel_bias(accel);
Ucial	0:7365f8db1bac	3238	return 0;
Ucial	0:7365f8db1bac	3239	}else return 1;
Ucial	0:7365f8db1bac	3240	}
Ucial	0:7365f8db1bac	3241	//陀螺仪方向控制
Ucial	0:7365f8db1bac	3242	unsigned short inv_orientation_matrix_to_scalar(
Ucial	0:7365f8db1bac	3243	const signed char *mtx)
Ucial	0:7365f8db1bac	3244	{
Ucial	0:7365f8db1bac	3245	unsigned short scalar;
Ucial	0:7365f8db1bac	3246	/*
Ucial	0:7365f8db1bac	3247	XYZ 010_001_000 Identity Matrix
Ucial	0:7365f8db1bac	3248	XZY 001_010_000
Ucial	0:7365f8db1bac	3249	YXZ 010_000_001
Ucial	0:7365f8db1bac	3250	YZX 000_010_001
Ucial	0:7365f8db1bac	3251	ZXY 001_000_010
Ucial	0:7365f8db1bac	3252	ZYX 000_001_010
Ucial	0:7365f8db1bac	3253	*/
Ucial	0:7365f8db1bac	3254
Ucial	0:7365f8db1bac	3255	scalar = inv_row_2_scale(mtx);
Ucial	0:7365f8db1bac	3256	scalar \|= inv_row_2_scale(mtx + 3) << 3;
Ucial	0:7365f8db1bac	3257	scalar \|= inv_row_2_scale(mtx + 6) << 6;
Ucial	0:7365f8db1bac	3258
Ucial	0:7365f8db1bac	3259
Ucial	0:7365f8db1bac	3260	return scalar;
Ucial	0:7365f8db1bac	3261	}
Ucial	0:7365f8db1bac	3262	//方向转换
Ucial	0:7365f8db1bac	3263	unsigned short inv_row_2_scale(const signed char *row)
Ucial	0:7365f8db1bac	3264	{
Ucial	0:7365f8db1bac	3265	unsigned short b;
Ucial	0:7365f8db1bac	3266
Ucial	0:7365f8db1bac	3267	if (row[0] > 0)
Ucial	0:7365f8db1bac	3268	b = 0;
Ucial	0:7365f8db1bac	3269	else if (row[0] < 0)
Ucial	0:7365f8db1bac	3270	b = 4;
Ucial	0:7365f8db1bac	3271	else if (row[1] > 0)
Ucial	0:7365f8db1bac	3272	b = 1;
Ucial	0:7365f8db1bac	3273	else if (row[1] < 0)
Ucial	0:7365f8db1bac	3274	b = 5;
Ucial	0:7365f8db1bac	3275	else if (row[2] > 0)
Ucial	0:7365f8db1bac	3276	b = 2;
Ucial	0:7365f8db1bac	3277	else if (row[2] < 0)
Ucial	0:7365f8db1bac	3278	b = 6;
Ucial	0:7365f8db1bac	3279	else
Ucial	0:7365f8db1bac	3280	b = 7; // error
Ucial	0:7365f8db1bac	3281	return b;
Ucial	0:7365f8db1bac	3282	}
Ucial	0:7365f8db1bac	3283	//空函数,未用到.
Ucial	0:7365f8db1bac	3284	void mget_ms(unsigned long *time)
Ucial	0:7365f8db1bac	3285	{
Ucial	0:7365f8db1bac	3286
Ucial	0:7365f8db1bac	3287	}
Ucial	0:7365f8db1bac	3288	//mpu6050,dmp初始化
Ucial	0:7365f8db1bac	3289	//返回值:0,正常
Ucial	0:7365f8db1bac	3290	// 其他,失败
Ucial	0:7365f8db1bac	3291	unsigned char mpu_dmp_init(void)
Ucial	0:7365f8db1bac	3292	{
Ucial	0:7365f8db1bac	3293	unsigned char res=0;
Ucial	0:7365f8db1bac	3294	MPU_IIC_Init(); //初始化IIC总线
Ucial	0:7365f8db1bac	3295	if(mpu_init()==0) //初始化MPU6050
Ucial	0:7365f8db1bac	3296	{
Ucial	0:7365f8db1bac	3297	res=mpu_set_sensors(INV_XYZ_GYRO\|INV_XYZ_ACCEL);//设置所需要的传感器
Ucial	0:7365f8db1bac	3298	if(res)return 1;
Ucial	0:7365f8db1bac	3299	res=mpu_configure_fifo(INV_XYZ_GYRO\|INV_XYZ_ACCEL);//设置FIFO
Ucial	0:7365f8db1bac	3300	if(res)return 2;
Ucial	0:7365f8db1bac	3301	res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率
Ucial	0:7365f8db1bac	3302	if(res)return 3;
Ucial	0:7365f8db1bac	3303	res=dmp_load_motion_driver_firmware(); //加载dmp固件
Ucial	0:7365f8db1bac	3304	if(res)return 4;
Ucial	0:7365f8db1bac	3305	res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
Ucial	0:7365f8db1bac	3306	if(res)return 5;
Ucial	0:7365f8db1bac	3307	res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT\|DMP_FEATURE_TAP\| //设置dmp功能
Ucial	0:7365f8db1bac	3308	DMP_FEATURE_ANDROID_ORIENT\|DMP_FEATURE_SEND_RAW_ACCEL\|DMP_FEATURE_SEND_CAL_GYRO\|
Ucial	0:7365f8db1bac	3309	DMP_FEATURE_GYRO_CAL);
Ucial	0:7365f8db1bac	3310	if(res)return 6;
Ucial	0:7365f8db1bac	3311	res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz)
Ucial	0:7365f8db1bac	3312	if(res)return 7;
Ucial	0:7365f8db1bac	3313	res=run_self_test(); //自检
Ucial	0:7365f8db1bac	3314	if(res)return 8;
Ucial	0:7365f8db1bac	3315	res=mpu_set_dmp_state(1); //使能DMP
Ucial	0:7365f8db1bac	3316	if(res)return 9;
Ucial	0:7365f8db1bac	3317	}else return 10;
Ucial	0:7365f8db1bac	3318	return 0;
Ucial	0:7365f8db1bac	3319	}
Ucial	0:7365f8db1bac	3320	//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
Ucial	0:7365f8db1bac	3321	//pitch:俯仰角精度:0.1° 范围:-90.0° <---> +90.0°
Ucial	0:7365f8db1bac	3322	//roll:横滚角精度:0.1° 范围:-180.0°<---> +180.0°
Ucial	0:7365f8db1bac	3323	//yaw:航向角精度:0.1° 范围:-180.0°<---> +180.0°
Ucial	0:7365f8db1bac	3324	//返回值:0,正常
Ucial	0:7365f8db1bac	3325	// 其他,失败
Ucial	0:7365f8db1bac	3326	unsigned char mpu_dmp_get_data(float pitch,float roll,float *yaw)
Ucial	0:7365f8db1bac	3327	{
Ucial	0:7365f8db1bac	3328	float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
Ucial	0:7365f8db1bac	3329	unsigned long sensor_timestamp;
Ucial	0:7365f8db1bac	3330	short gyro[3], accel[3], sensors;
Ucial	0:7365f8db1bac	3331	unsigned char more;
Ucial	0:7365f8db1bac	3332	long quat[4];
Ucial	0:7365f8db1bac	3333	if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;
Ucial	0:7365f8db1bac	3334	/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
Ucial	0:7365f8db1bac	3335	* This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
Ucial	0:7365f8db1bac	3336	**/
Ucial	0:7365f8db1bac	3337	/*if (sensors & INV_XYZ_GYRO )
Ucial	0:7365f8db1bac	3338	send_packet(PACKET_TYPE_GYRO, gyro);
Ucial	0:7365f8db1bac	3339	if (sensors & INV_XYZ_ACCEL)
Ucial	0:7365f8db1bac	3340	send_packet(PACKET_TYPE_ACCEL, accel); */
Ucial	0:7365f8db1bac	3341	/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
Ucial	0:7365f8db1bac	3342	* The orientation is set by the scalar passed to dmp_set_orientation during initialization.
Ucial	0:7365f8db1bac	3343	**/
Ucial	0:7365f8db1bac	3344	if(sensors&INV_WXYZ_QUAT)
Ucial	0:7365f8db1bac	3345	{
Ucial	0:7365f8db1bac	3346	q0 = quat[0] / q30; //q30格式转换为浮点数
Ucial	0:7365f8db1bac	3347	q1 = quat[1] / q30;
Ucial	0:7365f8db1bac	3348	q2 = quat[2] / q30;
Ucial	0:7365f8db1bac	3349	q3 = quat[3] / q30;
Ucial	0:7365f8db1bac	3350	//计算得到俯仰角/横滚角/航向角
Ucial	0:7365f8db1bac	3351	pitch = asin(-2 q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Ucial	0:7365f8db1bac	3352	roll = atan2(2 q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Ucial	0:7365f8db1bac	3353	yaw = atan2(2(q1q2 + q0q3),q0q0+q1q1-q2q2-q3q3) * 57.3; //yaw
Ucial	0:7365f8db1bac	3354	}else return 2;
Ucial	0:7365f8db1bac	3355	return 0;
Ucial	0:7365f8db1bac	3356	}
Ucial	0:7365f8db1bac	3357
Ucial	0:7365f8db1bac	3358	//定义目标板采用MSP430
Ucial	0:7365f8db1bac	3359	#define MOTION_DRIVER_TARGET_MSP430
Ucial	0:7365f8db1bac	3360
Ucial	0:7365f8db1bac	3361	/* The following functions must be defined for this platform:
Ucial	0:7365f8db1bac	3362	* i2c_write(unsigned char slave_addr, unsigned char reg_addr,
Ucial	0:7365f8db1bac	3363	* unsigned char length, unsigned char const *data)
Ucial	0:7365f8db1bac	3364	* i2c_read(unsigned char slave_addr, unsigned char reg_addr,
Ucial	0:7365f8db1bac	3365	* unsigned char length, unsigned char *data)
Ucial	0:7365f8db1bac	3366	* delay_ms(unsigned long num_ms)
Ucial	0:7365f8db1bac	3367	* get_ms(unsigned long *count)
Ucial	0:7365f8db1bac	3368	*/
Ucial	0:7365f8db1bac	3369	#if defined MOTION_DRIVER_TARGET_MSP430
Ucial	0:7365f8db1bac	3370	//#include "msp430.h"
Ucial	0:7365f8db1bac	3371	//#include "msp430_clock.h"
Ucial	0:7365f8db1bac	3372	//#define delay_ms delay_ms
Ucial	0:7365f8db1bac	3373	#define get_ms mget_ms
Ucial	0:7365f8db1bac	3374	//#define // printf
Ucial	0:7365f8db1bac	3375	//#define // printf
Ucial	0:7365f8db1bac	3376
Ucial	0:7365f8db1bac	3377	#elif defined EMPL_TARGET_MSP430
Ucial	0:7365f8db1bac	3378	#include "msp430.h"
Ucial	0:7365f8db1bac	3379	#include "msp430_clock.h"
Ucial	0:7365f8db1bac	3380	#include "log.h"
Ucial	0:7365f8db1bac	3381	#define delay_ms msp430_delay_ms
Ucial	0:7365f8db1bac	3382	#define get_ms msp430_get_clock_ms
Ucial	0:7365f8db1bac	3383	//#define // MPL_LOGI
Ucial	0:7365f8db1bac	3384	//#define // MPL_LOGE
Ucial	0:7365f8db1bac	3385
Ucial	0:7365f8db1bac	3386	#elif defined EMPL_TARGET_UC3L0
Ucial	0:7365f8db1bac	3387	/* Instead of using the standard TWI driver from the ASF library, we're using
Ucial	0:7365f8db1bac	3388	* a TWI driver that follows the slave address + register address convention.
Ucial	0:7365f8db1bac	3389	*/
Ucial	0:7365f8db1bac	3390	#include "delay.h"
Ucial	0:7365f8db1bac	3391	#include "sysclk.h"
Ucial	0:7365f8db1bac	3392	#include "log.h"
Ucial	0:7365f8db1bac	3393	#include "uc3l0_clock.h"
Ucial	0:7365f8db1bac	3394	/* delay_ms is a function already defined in ASF. */
Ucial	0:7365f8db1bac	3395	#define get_ms uc3l0_get_clock_ms
Ucial	0:7365f8db1bac	3396	//#define // MPL_LOGI
Ucial	0:7365f8db1bac	3397	//#define // MPL_LOGE
Ucial	0:7365f8db1bac	3398
Ucial	0:7365f8db1bac	3399	#else
Ucial	0:7365f8db1bac	3400	#error Gyro driver is missing the system layer implementations.
Ucial	0:7365f8db1bac	3401	#endif
Ucial	0:7365f8db1bac	3402
Ucial	0:7365f8db1bac	3403	/* These defines are copied from dmpDefaultMPU6050.c in the general MPL
Ucial	0:7365f8db1bac	3404	* releases. These defines may change for each DMP image, so be sure to modify
Ucial	0:7365f8db1bac	3405	* these values when switching to a new image.
Ucial	0:7365f8db1bac	3406	*/
Ucial	0:7365f8db1bac	3407	#define CFG_LP_QUAT (2712)
Ucial	0:7365f8db1bac	3408	#define END_ORIENT_TEMP (1866)
Ucial	0:7365f8db1bac	3409	#define CFG_27 (2742)
Ucial	0:7365f8db1bac	3410	#define CFG_20 (2224)
Ucial	0:7365f8db1bac	3411	#define CFG_23 (2745)
Ucial	0:7365f8db1bac	3412	#define CFG_FIFO_ON_EVENT (2690)
Ucial	0:7365f8db1bac	3413	#define END_PREDICTION_UPDATE (1761)
Ucial	0:7365f8db1bac	3414	#define CGNOTICE_INTR (2620)
Ucial	0:7365f8db1bac	3415	#define X_GRT_Y_TMP (1358)
Ucial	0:7365f8db1bac	3416	#define CFG_DR_INT (1029)
Ucial	0:7365f8db1bac	3417	#define CFG_AUTH (1035)
Ucial	0:7365f8db1bac	3418	#define UPDATE_PROP_ROT (1835)
Ucial	0:7365f8db1bac	3419	#define END_COMPARE_Y_X_TMP2 (1455)
Ucial	0:7365f8db1bac	3420	#define SKIP_X_GRT_Y_TMP (1359)
Ucial	0:7365f8db1bac	3421	#define SKIP_END_COMPARE (1435)
Ucial	0:7365f8db1bac	3422	#define FCFG_3 (1088)
Ucial	0:7365f8db1bac	3423	#define FCFG_2 (1066)
Ucial	0:7365f8db1bac	3424	#define FCFG_1 (1062)
Ucial	0:7365f8db1bac	3425	#define END_COMPARE_Y_X_TMP3 (1434)
Ucial	0:7365f8db1bac	3426	#define FCFG_7 (1073)
Ucial	0:7365f8db1bac	3427	#define FCFG_6 (1106)
Ucial	0:7365f8db1bac	3428	#define FLAT_STATE_END (1713)
Ucial	0:7365f8db1bac	3429	#define SWING_END_4 (1616)
Ucial	0:7365f8db1bac	3430	#define SWING_END_2 (1565)
Ucial	0:7365f8db1bac	3431	#define SWING_END_3 (1587)
Ucial	0:7365f8db1bac	3432	#define SWING_END_1 (1550)
Ucial	0:7365f8db1bac	3433	#define CFG_8 (2718)
Ucial	0:7365f8db1bac	3434	#define CFG_15 (2727)
Ucial	0:7365f8db1bac	3435	#define CFG_16 (2746)
Ucial	0:7365f8db1bac	3436	#define CFG_EXT_GYRO_BIAS (1189)
Ucial	0:7365f8db1bac	3437	#define END_COMPARE_Y_X_TMP (1407)
Ucial	0:7365f8db1bac	3438	#define DO_NOT_UPDATE_PROP_ROT (1839)
Ucial	0:7365f8db1bac	3439	#define CFG_7 (1205)
Ucial	0:7365f8db1bac	3440	#define FLAT_STATE_END_TEMP (1683)
Ucial	0:7365f8db1bac	3441	#define END_COMPARE_Y_X (1484)
Ucial	0:7365f8db1bac	3442	#define SKIP_SWING_END_1 (1551)
Ucial	0:7365f8db1bac	3443	#define SKIP_SWING_END_3 (1588)
Ucial	0:7365f8db1bac	3444	#define SKIP_SWING_END_2 (1566)
Ucial	0:7365f8db1bac	3445	#define TILTG75_START (1672)
Ucial	0:7365f8db1bac	3446	#define CFG_6 (2753)
Ucial	0:7365f8db1bac	3447	#define TILTL75_END (1669)
Ucial	0:7365f8db1bac	3448	#define END_ORIENT (1884)
Ucial	0:7365f8db1bac	3449	#define CFG_FLICK_IN (2573)
Ucial	0:7365f8db1bac	3450	#define TILTL75_START (1643)
Ucial	0:7365f8db1bac	3451	#define CFG_MOTION_BIAS (1208)
Ucial	0:7365f8db1bac	3452	#define X_GRT_Y (1408)
Ucial	0:7365f8db1bac	3453	#define TEMPLABEL (2324)
Ucial	0:7365f8db1bac	3454	#define CFG_ANDROID_ORIENT_INT (1853)
Ucial	0:7365f8db1bac	3455	#define CFG_GYRO_RAW_DATA (2722)
Ucial	0:7365f8db1bac	3456	#define X_GRT_Y_TMP2 (1379)
Ucial	0:7365f8db1bac	3457
Ucial	0:7365f8db1bac	3458	#define D_0_22 (22+512)
Ucial	0:7365f8db1bac	3459	#define D_0_24 (24+512)
Ucial	0:7365f8db1bac	3460
Ucial	0:7365f8db1bac	3461	#define D_0_36 (36)
Ucial	0:7365f8db1bac	3462	#define D_0_52 (52)
Ucial	0:7365f8db1bac	3463	#define D_0_96 (96)
Ucial	0:7365f8db1bac	3464	#define D_0_104 (104)
Ucial	0:7365f8db1bac	3465	#define D_0_108 (108)
Ucial	0:7365f8db1bac	3466	#define D_0_163 (163)
Ucial	0:7365f8db1bac	3467	#define D_0_188 (188)
Ucial	0:7365f8db1bac	3468	#define D_0_192 (192)
Ucial	0:7365f8db1bac	3469	#define D_0_224 (224)
Ucial	0:7365f8db1bac	3470	#define D_0_228 (228)
Ucial	0:7365f8db1bac	3471	#define D_0_232 (232)
Ucial	0:7365f8db1bac	3472	#define D_0_236 (236)
Ucial	0:7365f8db1bac	3473
Ucial	0:7365f8db1bac	3474	#define D_1_2 (256 + 2)
Ucial	0:7365f8db1bac	3475	#define D_1_4 (256 + 4)
Ucial	0:7365f8db1bac	3476	#define D_1_8 (256 + 8)
Ucial	0:7365f8db1bac	3477	#define D_1_10 (256 + 10)
Ucial	0:7365f8db1bac	3478	#define D_1_24 (256 + 24)
Ucial	0:7365f8db1bac	3479	#define D_1_28 (256 + 28)
Ucial	0:7365f8db1bac	3480	#define D_1_36 (256 + 36)
Ucial	0:7365f8db1bac	3481	#define D_1_40 (256 + 40)
Ucial	0:7365f8db1bac	3482	#define D_1_44 (256 + 44)
Ucial	0:7365f8db1bac	3483	#define D_1_72 (256 + 72)
Ucial	0:7365f8db1bac	3484	#define D_1_74 (256 + 74)
Ucial	0:7365f8db1bac	3485	#define D_1_79 (256 + 79)
Ucial	0:7365f8db1bac	3486	#define D_1_88 (256 + 88)
Ucial	0:7365f8db1bac	3487	#define D_1_90 (256 + 90)
Ucial	0:7365f8db1bac	3488	#define D_1_92 (256 + 92)
Ucial	0:7365f8db1bac	3489	#define D_1_96 (256 + 96)
Ucial	0:7365f8db1bac	3490	#define D_1_98 (256 + 98)
Ucial	0:7365f8db1bac	3491	#define D_1_106 (256 + 106)
Ucial	0:7365f8db1bac	3492	#define D_1_108 (256 + 108)
Ucial	0:7365f8db1bac	3493	#define D_1_112 (256 + 112)
Ucial	0:7365f8db1bac	3494	#define D_1_128 (256 + 144)
Ucial	0:7365f8db1bac	3495	#define D_1_152 (256 + 12)
Ucial	0:7365f8db1bac	3496	#define D_1_160 (256 + 160)
Ucial	0:7365f8db1bac	3497	#define D_1_176 (256 + 176)
Ucial	0:7365f8db1bac	3498	#define D_1_178 (256 + 178)
Ucial	0:7365f8db1bac	3499	#define D_1_218 (256 + 218)
Ucial	0:7365f8db1bac	3500	#define D_1_232 (256 + 232)
Ucial	0:7365f8db1bac	3501	#define D_1_236 (256 + 236)
Ucial	0:7365f8db1bac	3502	#define D_1_240 (256 + 240)
Ucial	0:7365f8db1bac	3503	#define D_1_244 (256 + 244)
Ucial	0:7365f8db1bac	3504	#define D_1_250 (256 + 250)
Ucial	0:7365f8db1bac	3505	#define D_1_252 (256 + 252)
Ucial	0:7365f8db1bac	3506	#define D_2_12 (512 + 12)
Ucial	0:7365f8db1bac	3507	#define D_2_96 (512 + 96)
Ucial	0:7365f8db1bac	3508	#define D_2_108 (512 + 108)
Ucial	0:7365f8db1bac	3509	#define D_2_208 (512 + 208)
Ucial	0:7365f8db1bac	3510	#define D_2_224 (512 + 224)
Ucial	0:7365f8db1bac	3511	#define D_2_236 (512 + 236)
Ucial	0:7365f8db1bac	3512	#define D_2_244 (512 + 244)
Ucial	0:7365f8db1bac	3513	#define D_2_248 (512 + 248)
Ucial	0:7365f8db1bac	3514	#define D_2_252 (512 + 252)
Ucial	0:7365f8db1bac	3515
Ucial	0:7365f8db1bac	3516	#define CPASS_BIAS_X (35 * 16 + 4)
Ucial	0:7365f8db1bac	3517	#define CPASS_BIAS_Y (35 * 16 + 8)
Ucial	0:7365f8db1bac	3518	#define CPASS_BIAS_Z (35 * 16 + 12)
Ucial	0:7365f8db1bac	3519	#define CPASS_MTX_00 (36 * 16)
Ucial	0:7365f8db1bac	3520	#define CPASS_MTX_01 (36 * 16 + 4)
Ucial	0:7365f8db1bac	3521	#define CPASS_MTX_02 (36 * 16 + 8)
Ucial	0:7365f8db1bac	3522	#define CPASS_MTX_10 (36 * 16 + 12)
Ucial	0:7365f8db1bac	3523	#define CPASS_MTX_11 (37 * 16)
Ucial	0:7365f8db1bac	3524	#define CPASS_MTX_12 (37 * 16 + 4)
Ucial	0:7365f8db1bac	3525	#define CPASS_MTX_20 (37 * 16 + 8)
Ucial	0:7365f8db1bac	3526	#define CPASS_MTX_21 (37 * 16 + 12)
Ucial	0:7365f8db1bac	3527	#define CPASS_MTX_22 (43 * 16 + 12)
Ucial	0:7365f8db1bac	3528	#define D_EXT_GYRO_BIAS_X (61 * 16)
Ucial	0:7365f8db1bac	3529	#define D_EXT_GYRO_BIAS_Y (61 * 16) + 4
Ucial	0:7365f8db1bac	3530	#define D_EXT_GYRO_BIAS_Z (61 * 16) + 8
Ucial	0:7365f8db1bac	3531	#define D_ACT0 (40 * 16)
Ucial	0:7365f8db1bac	3532	#define D_ACSX (40 * 16 + 4)
Ucial	0:7365f8db1bac	3533	#define D_ACSY (40 * 16 + 8)
Ucial	0:7365f8db1bac	3534	#define D_ACSZ (40 * 16 + 12)
Ucial	0:7365f8db1bac	3535
Ucial	0:7365f8db1bac	3536	#define FLICK_MSG (45 * 16 + 4)
Ucial	0:7365f8db1bac	3537	#define FLICK_COUNTER (45 * 16 + 8)
Ucial	0:7365f8db1bac	3538	#define FLICK_LOWER (45 * 16 + 12)
Ucial	0:7365f8db1bac	3539	#define FLICK_UPPER (46 * 16 + 12)
Ucial	0:7365f8db1bac	3540
Ucial	0:7365f8db1bac	3541	#define D_AUTH_OUT (992)
Ucial	0:7365f8db1bac	3542	#define D_AUTH_IN (996)
Ucial	0:7365f8db1bac	3543	#define D_AUTH_A (1000)
Ucial	0:7365f8db1bac	3544	#define D_AUTH_B (1004)
Ucial	0:7365f8db1bac	3545
Ucial	0:7365f8db1bac	3546	#define D_PEDSTD_BP_B (768 + 0x1C)
Ucial	0:7365f8db1bac	3547	#define D_PEDSTD_HP_A (768 + 0x78)
Ucial	0:7365f8db1bac	3548	#define D_PEDSTD_HP_B (768 + 0x7C)
Ucial	0:7365f8db1bac	3549	#define D_PEDSTD_BP_A4 (768 + 0x40)
Ucial	0:7365f8db1bac	3550	#define D_PEDSTD_BP_A3 (768 + 0x44)
Ucial	0:7365f8db1bac	3551	#define D_PEDSTD_BP_A2 (768 + 0x48)
Ucial	0:7365f8db1bac	3552	#define D_PEDSTD_BP_A1 (768 + 0x4C)
Ucial	0:7365f8db1bac	3553	#define D_PEDSTD_INT_THRSH (768 + 0x68)
Ucial	0:7365f8db1bac	3554	#define D_PEDSTD_CLIP (768 + 0x6C)
Ucial	0:7365f8db1bac	3555	#define D_PEDSTD_SB (768 + 0x28)
Ucial	0:7365f8db1bac	3556	#define D_PEDSTD_SB_TIME (768 + 0x2C)
Ucial	0:7365f8db1bac	3557	#define D_PEDSTD_PEAKTHRSH (768 + 0x98)
Ucial	0:7365f8db1bac	3558	#define D_PEDSTD_TIML (768 + 0x2A)
Ucial	0:7365f8db1bac	3559	#define D_PEDSTD_TIMH (768 + 0x2E)
Ucial	0:7365f8db1bac	3560	#define D_PEDSTD_PEAK (768 + 0X94)
Ucial	0:7365f8db1bac	3561	#define D_PEDSTD_STEPCTR (768 + 0x60)
Ucial	0:7365f8db1bac	3562	#define D_PEDSTD_TIMECTR (964)
Ucial	0:7365f8db1bac	3563	#define D_PEDSTD_DECI (768 + 0xA0)
Ucial	0:7365f8db1bac	3564
Ucial	0:7365f8db1bac	3565	#define D_HOST_NO_MOT (976)
Ucial	0:7365f8db1bac	3566	#define D_ACCEL_BIAS (660)
Ucial	0:7365f8db1bac	3567
Ucial	0:7365f8db1bac	3568	#define D_ORIENT_GAP (76)
Ucial	0:7365f8db1bac	3569
Ucial	0:7365f8db1bac	3570	#define D_TILT0_H (48)
Ucial	0:7365f8db1bac	3571	#define D_TILT0_L (50)
Ucial	0:7365f8db1bac	3572	#define D_TILT1_H (52)
Ucial	0:7365f8db1bac	3573	#define D_TILT1_L (54)
Ucial	0:7365f8db1bac	3574	#define D_TILT2_H (56)
Ucial	0:7365f8db1bac	3575	#define D_TILT2_L (58)
Ucial	0:7365f8db1bac	3576	#define D_TILT3_H (60)
Ucial	0:7365f8db1bac	3577	#define D_TILT3_L (62)
Ucial	0:7365f8db1bac	3578
Ucial	0:7365f8db1bac	3579	#define DMP_CODE_SIZE (3062)
Ucial	0:7365f8db1bac	3580
Ucial	0:7365f8db1bac	3581	static const unsigned char dmp_memory[DMP_CODE_SIZE] = {
Ucial	0:7365f8db1bac	3582	/* bank # 0 */
Ucial	0:7365f8db1bac	3583	0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
Ucial	0:7365f8db1bac	3584	0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01,
Ucial	0:7365f8db1bac	3585	0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e,
Ucial	0:7365f8db1bac	3586	0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83,
Ucial	0:7365f8db1bac	3587	0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1,
Ucial	0:7365f8db1bac	3588	0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2,
Ucial	0:7365f8db1bac	3589	0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00,
Ucial	0:7365f8db1bac	3590	0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3591	0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf,
Ucial	0:7365f8db1bac	3592	0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa,
Ucial	0:7365f8db1bac	3593	0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59,
Ucial	0:7365f8db1bac	3594	0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3595	0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c,
Ucial	0:7365f8db1bac	3596	0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65,
Ucial	0:7365f8db1bac	3597	0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3598	0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3599	/* bank # 1 */
Ucial	0:7365f8db1bac	3600	0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f,
Ucial	0:7365f8db1bac	3601	0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00,
Ucial	0:7365f8db1bac	3602	0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00,
Ucial	0:7365f8db1bac	3603	0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3604	0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00,
Ucial	0:7365f8db1bac	3605	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00,
Ucial	0:7365f8db1bac	3606	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00,
Ucial	0:7365f8db1bac	3607	0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00,
Ucial	0:7365f8db1bac	3608	0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3609	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3610	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
Ucial	0:7365f8db1bac	3611	0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00,
Ucial	0:7365f8db1bac	3612	0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83,
Ucial	0:7365f8db1bac	3613	0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28,
Ucial	0:7365f8db1bac	3614	0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
Ucial	0:7365f8db1bac	3615	0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00,
Ucial	0:7365f8db1bac	3616	/* bank # 2 */
Ucial	0:7365f8db1bac	3617	0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3618	0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2,
Ucial	0:7365f8db1bac	3619	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
Ucial	0:7365f8db1bac	3620	0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0,
Ucial	0:7365f8db1bac	3621	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3622	0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3623	0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3624	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3625	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3626	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3627	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3628	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3629	0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e,
Ucial	0:7365f8db1bac	3630	0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c,
Ucial	0:7365f8db1bac	3631	0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64,
Ucial	0:7365f8db1bac	3632	0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3633	/* bank # 3 */
Ucial	0:7365f8db1bac	3634	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3635	0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3,
Ucial	0:7365f8db1bac	3636	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c,
Ucial	0:7365f8db1bac	3637	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3638	0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82,
Ucial	0:7365f8db1bac	3639	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3640	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3641	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19,
Ucial	0:7365f8db1bac	3642	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3643	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00,
Ucial	0:7365f8db1bac	3644	0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3645	0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3646	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3647	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3648	0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88,
Ucial	0:7365f8db1bac	3649	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
Ucial	0:7365f8db1bac	3650
Ucial	0:7365f8db1bac	3651	/* bank # 4 */
Ucial	0:7365f8db1bac	3652	0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e,
Ucial	0:7365f8db1bac	3653	0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf,
Ucial	0:7365f8db1bac	3654	0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9,
Ucial	0:7365f8db1bac	3655	0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96,
Ucial	0:7365f8db1bac	3656	0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83,
Ucial	0:7365f8db1bac	3657	0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1,
Ucial	0:7365f8db1bac	3658	0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2,
Ucial	0:7365f8db1bac	3659	0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda,
Ucial	0:7365f8db1bac	3660	0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf,
Ucial	0:7365f8db1bac	3661	0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8,
Ucial	0:7365f8db1bac	3662	0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf,
Ucial	0:7365f8db1bac	3663	0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35,
Ucial	0:7365f8db1bac	3664	0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a,
Ucial	0:7365f8db1bac	3665	0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2,
Ucial	0:7365f8db1bac	3666	0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55,
Ucial	0:7365f8db1bac	3667	0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78,
Ucial	0:7365f8db1bac	3668	/* bank # 5 */
Ucial	0:7365f8db1bac	3669	0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba,
Ucial	0:7365f8db1bac	3670	0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8,
Ucial	0:7365f8db1bac	3671	0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88,
Ucial	0:7365f8db1bac	3672	0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7,
Ucial	0:7365f8db1bac	3673	0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1,
Ucial	0:7365f8db1bac	3674	0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1,
Ucial	0:7365f8db1bac	3675	0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29,
Ucial	0:7365f8db1bac	3676	0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a,
Ucial	0:7365f8db1bac	3677	0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7,
Ucial	0:7365f8db1bac	3678	0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e,
Ucial	0:7365f8db1bac	3679	0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c,
Ucial	0:7365f8db1bac	3680	0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8,
Ucial	0:7365f8db1bac	3681	0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88,
Ucial	0:7365f8db1bac	3682	0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94,
Ucial	0:7365f8db1bac	3683	0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2,
Ucial	0:7365f8db1bac	3684	0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82,
Ucial	0:7365f8db1bac	3685	/* bank # 6 */
Ucial	0:7365f8db1bac	3686	0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1,
Ucial	0:7365f8db1bac	3687	0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98,
Ucial	0:7365f8db1bac	3688	0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88,
Ucial	0:7365f8db1bac	3689	0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02,
Ucial	0:7365f8db1bac	3690	0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf,
Ucial	0:7365f8db1bac	3691	0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1,
Ucial	0:7365f8db1bac	3692	0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39,
Ucial	0:7365f8db1bac	3693	0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9,
Ucial	0:7365f8db1bac	3694	0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9,
Ucial	0:7365f8db1bac	3695	0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde,
Ucial	0:7365f8db1bac	3696	0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0,
Ucial	0:7365f8db1bac	3697	0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8,
Ucial	0:7365f8db1bac	3698	0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2,
Ucial	0:7365f8db1bac	3699	0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3,
Ucial	0:7365f8db1bac	3700	0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb,
Ucial	0:7365f8db1bac	3701	0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8,
Ucial	0:7365f8db1bac	3702	/* bank # 7 */
Ucial	0:7365f8db1bac	3703	0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb,
Ucial	0:7365f8db1bac	3704	0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28,
Ucial	0:7365f8db1bac	3705	0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2,
Ucial	0:7365f8db1bac	3706	0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21, 0xd8, 0xf4, 0x0a,
Ucial	0:7365f8db1bac	3707	0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb,
Ucial	0:7365f8db1bac	3708	0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6,
Ucial	0:7365f8db1bac	3709	0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8,
Ucial	0:7365f8db1bac	3710	0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1,
Ucial	0:7365f8db1bac	3711	0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99,
Ucial	0:7365f8db1bac	3712	0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb,
Ucial	0:7365f8db1bac	3713	0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9,
Ucial	0:7365f8db1bac	3714	0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9,
Ucial	0:7365f8db1bac	3715	0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac,
Ucial	0:7365f8db1bac	3716	0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6,
Ucial	0:7365f8db1bac	3717	0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0,
Ucial	0:7365f8db1bac	3718	0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf,
Ucial	0:7365f8db1bac	3719	/* bank # 8 */
Ucial	0:7365f8db1bac	3720	0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30,
Ucial	0:7365f8db1bac	3721	0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9,
Ucial	0:7365f8db1bac	3722	0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0,
Ucial	0:7365f8db1bac	3723	0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1,
Ucial	0:7365f8db1bac	3724	0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6,
Ucial	0:7365f8db1bac	3725	0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8,
Ucial	0:7365f8db1bac	3726	0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda,
Ucial	0:7365f8db1bac	3727	0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde,
Ucial	0:7365f8db1bac	3728	0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde,
Ucial	0:7365f8db1bac	3729	0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88,
Ucial	0:7365f8db1bac	3730	0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf,
Ucial	0:7365f8db1bac	3731	0xd8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8,
Ucial	0:7365f8db1bac	3732	0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5,
Ucial	0:7365f8db1bac	3733	0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8,
Ucial	0:7365f8db1bac	3734	0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7,
Ucial	0:7365f8db1bac	3735	0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8,
Ucial	0:7365f8db1bac	3736	/* bank # 9 */
Ucial	0:7365f8db1bac	3737	0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0,
Ucial	0:7365f8db1bac	3738	0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b,
Ucial	0:7365f8db1bac	3739	0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab,
Ucial	0:7365f8db1bac	3740	0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0,
Ucial	0:7365f8db1bac	3741	0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0,
Ucial	0:7365f8db1bac	3742	0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59,
Ucial	0:7365f8db1bac	3743	0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31,
Ucial	0:7365f8db1bac	3744	0x8b, 0x30, 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28,
Ucial	0:7365f8db1bac	3745	0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0,
Ucial	0:7365f8db1bac	3746	0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9,
Ucial	0:7365f8db1bac	3747	0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c,
Ucial	0:7365f8db1bac	3748	0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e,
Ucial	0:7365f8db1bac	3749	0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0,
Ucial	0:7365f8db1bac	3750	0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf,
Ucial	0:7365f8db1bac	3751	0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2,
Ucial	0:7365f8db1bac	3752	0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83,
Ucial	0:7365f8db1bac	3753	/* bank # 10 */
Ucial	0:7365f8db1bac	3754	0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6,
Ucial	0:7365f8db1bac	3755	0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad,
Ucial	0:7365f8db1bac	3756	0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb,
Ucial	0:7365f8db1bac	3757	0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c,
Ucial	0:7365f8db1bac	3758	0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8,
Ucial	0:7365f8db1bac	3759	0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9,
Ucial	0:7365f8db1bac	3760	0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2,
Ucial	0:7365f8db1bac	3761	0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84,
Ucial	0:7365f8db1bac	3762	0xc1, 0xc3, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0,
Ucial	0:7365f8db1bac	3763	0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3,
Ucial	0:7365f8db1bac	3764	0xa3, 0xa3, 0xb2, 0x8b, 0xb6, 0x9b, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
Ucial	0:7365f8db1bac	3765	0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9,
Ucial	0:7365f8db1bac	3766	0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
Ucial	0:7365f8db1bac	3767	0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28,
Ucial	0:7365f8db1bac	3768	0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90,
Ucial	0:7365f8db1bac	3769	0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29,
Ucial	0:7365f8db1bac	3770	/* bank # 11 */
Ucial	0:7365f8db1bac	3771	0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83,
Ucial	0:7365f8db1bac	3772	0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19,
Ucial	0:7365f8db1bac	3773	0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39,
Ucial	0:7365f8db1bac	3774	0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7,
Ucial	0:7365f8db1bac	3775	0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80,
Ucial	0:7365f8db1bac	3776	0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9,
Ucial	0:7365f8db1bac	3777	0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26,
Ucial	0:7365f8db1bac	3778	0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89,
Ucial	0:7365f8db1bac	3779	0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8,
Ucial	0:7365f8db1bac	3780	0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8,
Ucial	0:7365f8db1bac	3781	0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22,
Ucial	0:7365f8db1bac	3782	0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2,
Ucial	0:7365f8db1bac	3783	0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00,
Ucial	0:7365f8db1bac	3784	0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10,
Ucial	0:7365f8db1bac	3785	0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88,
Ucial	0:7365f8db1bac	3786	0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff
Ucial	0:7365f8db1bac	3787	};
Ucial	0:7365f8db1bac	3788
Ucial	0:7365f8db1bac	3789	static const unsigned short sStartAddress = 0x0400;
Ucial	0:7365f8db1bac	3790
Ucial	0:7365f8db1bac	3791	/* END OF SECTION COPIED FROM dmpDefaultMPU6050.c */
Ucial	0:7365f8db1bac	3792
Ucial	0:7365f8db1bac	3793	#define INT_SRC_TAP (0x01)
Ucial	0:7365f8db1bac	3794	#define INT_SRC_ANDROID_ORIENT (0x08)
Ucial	0:7365f8db1bac	3795
Ucial	0:7365f8db1bac	3796	#define DMP_FEATURE_SEND_ANY_GYRO (DMP_FEATURE_SEND_RAW_GYRO \| \
Ucial	0:7365f8db1bac	3797	DMP_FEATURE_SEND_CAL_GYRO)
Ucial	0:7365f8db1bac	3798
Ucial	0:7365f8db1bac	3799	#define MAX_PACKET_LENGTH_2 (32) //前面已经有一个定义了你，避免冲突改成2
Ucial	0:7365f8db1bac	3800
Ucial	0:7365f8db1bac	3801	#define DMP_SAMPLE_RATE (200)
Ucial	0:7365f8db1bac	3802	#define GYRO_SF (46850825LL * 200 / DMP_SAMPLE_RATE)
Ucial	0:7365f8db1bac	3803
Ucial	0:7365f8db1bac	3804	#define FIFO_CORRUPTION_CHECK
Ucial	0:7365f8db1bac	3805	#ifdef FIFO_CORRUPTION_CHECK
Ucial	0:7365f8db1bac	3806	#define QUAT_ERROR_THRESH (1L<<24)
Ucial	0:7365f8db1bac	3807	#define QUAT_MAG_SQ_NORMALIZED (1L<<28)
Ucial	0:7365f8db1bac	3808	#define QUAT_MAG_SQ_MIN (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH)
Ucial	0:7365f8db1bac	3809	#define QUAT_MAG_SQ_MAX (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH)
Ucial	0:7365f8db1bac	3810	#endif
Ucial	0:7365f8db1bac	3811
Ucial	0:7365f8db1bac	3812	struct dmp_s {
Ucial	0:7365f8db1bac	3813	void (*tap_cb)(unsigned char count, unsigned char direction);
Ucial	0:7365f8db1bac	3814	void (*android_orient_cb)(unsigned char orientation);
Ucial	0:7365f8db1bac	3815	unsigned short orient;
Ucial	0:7365f8db1bac	3816	unsigned short feature_mask;
Ucial	0:7365f8db1bac	3817	unsigned short fifo_rate;
Ucial	0:7365f8db1bac	3818	unsigned char packet_length;
Ucial	0:7365f8db1bac	3819	};
Ucial	0:7365f8db1bac	3820
Ucial	0:7365f8db1bac	3821	//static struct dmp_s dmp = {
Ucial	0:7365f8db1bac	3822	// .tap_cb = NULL,
Ucial	0:7365f8db1bac	3823	// .android_orient_cb = NULL,
Ucial	0:7365f8db1bac	3824	// .orient = 0,
Ucial	0:7365f8db1bac	3825	// .feature_mask = 0,
Ucial	0:7365f8db1bac	3826	// .fifo_rate = 0,
Ucial	0:7365f8db1bac	3827	// .packet_length = 0
Ucial	0:7365f8db1bac	3828	//};
Ucial	0:7365f8db1bac	3829
Ucial	0:7365f8db1bac	3830	static struct dmp_s dmp={
Ucial	0:7365f8db1bac	3831	NULL,
Ucial	0:7365f8db1bac	3832	NULL,
Ucial	0:7365f8db1bac	3833	0,
Ucial	0:7365f8db1bac	3834	0,
Ucial	0:7365f8db1bac	3835	0,
Ucial	0:7365f8db1bac	3836	0
Ucial	0:7365f8db1bac	3837	};
Ucial	0:7365f8db1bac	3838
Ucial	0:7365f8db1bac	3839	/**
Ucial	0:7365f8db1bac	3840	* @brief Load the DMP with this image.
Ucial	0:7365f8db1bac	3841	* @return 0 if successful.
Ucial	0:7365f8db1bac	3842	*/
Ucial	0:7365f8db1bac	3843	int dmp_load_motion_driver_firmware(void)
Ucial	0:7365f8db1bac	3844	{
Ucial	0:7365f8db1bac	3845	return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress,
Ucial	0:7365f8db1bac	3846	DMP_SAMPLE_RATE);
Ucial	0:7365f8db1bac	3847	}
Ucial	0:7365f8db1bac	3848
Ucial	0:7365f8db1bac	3849	/**
Ucial	0:7365f8db1bac	3850	* @brief Push gyro and accel orientation to the DMP.
Ucial	0:7365f8db1bac	3851	* The orientation is represented here as the output of
Ucial	0:7365f8db1bac	3852	* @e inv_orientation_matrix_to_scalar.
Ucial	0:7365f8db1bac	3853	* @param[in] orient Gyro and accel orientation in body frame.
Ucial	0:7365f8db1bac	3854	* @return 0 if successful.
Ucial	0:7365f8db1bac	3855	*/
Ucial	0:7365f8db1bac	3856	int dmp_set_orientation(unsigned short orient)
Ucial	0:7365f8db1bac	3857	{
Ucial	0:7365f8db1bac	3858	unsigned char gyro_regs[3], accel_regs[3];
Ucial	0:7365f8db1bac	3859	const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C};
Ucial	0:7365f8db1bac	3860	const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C};
Ucial	0:7365f8db1bac	3861	const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76};
Ucial	0:7365f8db1bac	3862	const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66};
Ucial	0:7365f8db1bac	3863
Ucial	0:7365f8db1bac	3864	gyro_regs[0] = gyro_axes[orient & 3];
Ucial	0:7365f8db1bac	3865	gyro_regs[1] = gyro_axes[(orient >> 3) & 3];
Ucial	0:7365f8db1bac	3866	gyro_regs[2] = gyro_axes[(orient >> 6) & 3];
Ucial	0:7365f8db1bac	3867	accel_regs[0] = accel_axes[orient & 3];
Ucial	0:7365f8db1bac	3868	accel_regs[1] = accel_axes[(orient >> 3) & 3];
Ucial	0:7365f8db1bac	3869	accel_regs[2] = accel_axes[(orient >> 6) & 3];
Ucial	0:7365f8db1bac	3870
Ucial	0:7365f8db1bac	3871	/* Chip-to-body, axes only. */
Ucial	0:7365f8db1bac	3872	if (mpu_write_mem(FCFG_1, 3, gyro_regs))
Ucial	0:7365f8db1bac	3873	return -1;
Ucial	0:7365f8db1bac	3874	if (mpu_write_mem(FCFG_2, 3, accel_regs))
Ucial	0:7365f8db1bac	3875	return -1;
Ucial	0:7365f8db1bac	3876
Ucial	0:7365f8db1bac	3877	memcpy(gyro_regs, gyro_sign, 3);
Ucial	0:7365f8db1bac	3878	memcpy(accel_regs, accel_sign, 3);
Ucial	0:7365f8db1bac	3879	if (orient & 4) {
Ucial	0:7365f8db1bac	3880	gyro_regs[0] \|= 1;
Ucial	0:7365f8db1bac	3881	accel_regs[0] \|= 1;
Ucial	0:7365f8db1bac	3882	}
Ucial	0:7365f8db1bac	3883	if (orient & 0x20) {
Ucial	0:7365f8db1bac	3884	gyro_regs[1] \|= 1;
Ucial	0:7365f8db1bac	3885	accel_regs[1] \|= 1;
Ucial	0:7365f8db1bac	3886	}
Ucial	0:7365f8db1bac	3887	if (orient & 0x100) {
Ucial	0:7365f8db1bac	3888	gyro_regs[2] \|= 1;
Ucial	0:7365f8db1bac	3889	accel_regs[2] \|= 1;
Ucial	0:7365f8db1bac	3890	}
Ucial	0:7365f8db1bac	3891
Ucial	0:7365f8db1bac	3892	/* Chip-to-body, sign only. */
Ucial	0:7365f8db1bac	3893	if (mpu_write_mem(FCFG_3, 3, gyro_regs))
Ucial	0:7365f8db1bac	3894	return -1;
Ucial	0:7365f8db1bac	3895	if (mpu_write_mem(FCFG_7, 3, accel_regs))
Ucial	0:7365f8db1bac	3896	return -1;
Ucial	0:7365f8db1bac	3897	dmp.orient = orient;
Ucial	0:7365f8db1bac	3898	return 0;
Ucial	0:7365f8db1bac	3899	}
Ucial	0:7365f8db1bac	3900
Ucial	0:7365f8db1bac	3901	/**
Ucial	0:7365f8db1bac	3902	* @brief Push gyro biases to the DMP.
Ucial	0:7365f8db1bac	3903	* Because the gyro integration is handled in the DMP, any gyro biases
Ucial	0:7365f8db1bac	3904	* calculated by the MPL should be pushed down to DMP memory to remove
Ucial	0:7365f8db1bac	3905	* 3-axis quaternion drift.
Ucial	0:7365f8db1bac	3906	* \n NOTE: If the DMP-based gyro calibration is enabled, the DMP will
Ucial	0:7365f8db1bac	3907	* overwrite the biases written to this location once a new one is computed.
Ucial	0:7365f8db1bac	3908	* @param[in] bias Gyro biases in q16.
Ucial	0:7365f8db1bac	3909	* @return 0 if successful.
Ucial	0:7365f8db1bac	3910	*/
Ucial	0:7365f8db1bac	3911	int dmp_set_gyro_bias(long *bias)
Ucial	0:7365f8db1bac	3912	{
Ucial	0:7365f8db1bac	3913	long gyro_bias_body[3];
Ucial	0:7365f8db1bac	3914	unsigned char regs[4];
Ucial	0:7365f8db1bac	3915
Ucial	0:7365f8db1bac	3916	gyro_bias_body[0] = bias[dmp.orient & 3];
Ucial	0:7365f8db1bac	3917	if (dmp.orient & 4)
Ucial	0:7365f8db1bac	3918	gyro_bias_body[0] *= -1;
Ucial	0:7365f8db1bac	3919	gyro_bias_body[1] = bias[(dmp.orient >> 3) & 3];
Ucial	0:7365f8db1bac	3920	if (dmp.orient & 0x20)
Ucial	0:7365f8db1bac	3921	gyro_bias_body[1] *= -1;
Ucial	0:7365f8db1bac	3922	gyro_bias_body[2] = bias[(dmp.orient >> 6) & 3];
Ucial	0:7365f8db1bac	3923	if (dmp.orient & 0x100)
Ucial	0:7365f8db1bac	3924	gyro_bias_body[2] *= -1;
Ucial	0:7365f8db1bac	3925
Ucial	0:7365f8db1bac	3926	#ifdef EMPL_NO_64BIT
Ucial	0:7365f8db1bac	3927	gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) / 1073741824.f);
Ucial	0:7365f8db1bac	3928	gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) / 1073741824.f);
Ucial	0:7365f8db1bac	3929	gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) / 1073741824.f);
Ucial	0:7365f8db1bac	3930	#else
Ucial	0:7365f8db1bac	3931	gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30);
Ucial	0:7365f8db1bac	3932	gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30);
Ucial	0:7365f8db1bac	3933	gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30);
Ucial	0:7365f8db1bac	3934	#endif
Ucial	0:7365f8db1bac	3935
Ucial	0:7365f8db1bac	3936	regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF);
Ucial	0:7365f8db1bac	3937	regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF);
Ucial	0:7365f8db1bac	3938	regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF);
Ucial	0:7365f8db1bac	3939	regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF);
Ucial	0:7365f8db1bac	3940	if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs))
Ucial	0:7365f8db1bac	3941	return -1;
Ucial	0:7365f8db1bac	3942
Ucial	0:7365f8db1bac	3943	regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF);
Ucial	0:7365f8db1bac	3944	regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF);
Ucial	0:7365f8db1bac	3945	regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF);
Ucial	0:7365f8db1bac	3946	regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF);
Ucial	0:7365f8db1bac	3947	if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs))
Ucial	0:7365f8db1bac	3948	return -1;
Ucial	0:7365f8db1bac	3949
Ucial	0:7365f8db1bac	3950	regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF);
Ucial	0:7365f8db1bac	3951	regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF);
Ucial	0:7365f8db1bac	3952	regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF);
Ucial	0:7365f8db1bac	3953	regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF);
Ucial	0:7365f8db1bac	3954	return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs);
Ucial	0:7365f8db1bac	3955	}
Ucial	0:7365f8db1bac	3956
Ucial	0:7365f8db1bac	3957	/**
Ucial	0:7365f8db1bac	3958	* @brief Push accel biases to the DMP.
Ucial	0:7365f8db1bac	3959	* These biases will be removed from the DMP 6-axis quaternion.
Ucial	0:7365f8db1bac	3960	* @param[in] bias Accel biases in q16.
Ucial	0:7365f8db1bac	3961	* @return 0 if successful.
Ucial	0:7365f8db1bac	3962	*/
Ucial	0:7365f8db1bac	3963	int dmp_set_accel_bias(long *bias)
Ucial	0:7365f8db1bac	3964	{
Ucial	0:7365f8db1bac	3965	long accel_bias_body[3];
Ucial	0:7365f8db1bac	3966	unsigned char regs[12];
Ucial	0:7365f8db1bac	3967	long long accel_sf;
Ucial	0:7365f8db1bac	3968	unsigned short accel_sens;
Ucial	0:7365f8db1bac	3969
Ucial	0:7365f8db1bac	3970	mpu_get_accel_sens(&accel_sens);
Ucial	0:7365f8db1bac	3971	accel_sf = (long long)accel_sens << 15;
Ucial	0:7365f8db1bac	3972	//__no_operation();
Ucial	0:7365f8db1bac	3973
Ucial	0:7365f8db1bac	3974	accel_bias_body[0] = bias[dmp.orient & 3];
Ucial	0:7365f8db1bac	3975	if (dmp.orient & 4)
Ucial	0:7365f8db1bac	3976	accel_bias_body[0] *= -1;
Ucial	0:7365f8db1bac	3977	accel_bias_body[1] = bias[(dmp.orient >> 3) & 3];
Ucial	0:7365f8db1bac	3978	if (dmp.orient & 0x20)
Ucial	0:7365f8db1bac	3979	accel_bias_body[1] *= -1;
Ucial	0:7365f8db1bac	3980	accel_bias_body[2] = bias[(dmp.orient >> 6) & 3];
Ucial	0:7365f8db1bac	3981	if (dmp.orient & 0x100)
Ucial	0:7365f8db1bac	3982	accel_bias_body[2] *= -1;
Ucial	0:7365f8db1bac	3983
Ucial	0:7365f8db1bac	3984	#ifdef EMPL_NO_64BIT
Ucial	0:7365f8db1bac	3985	accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) / 1073741824.f);
Ucial	0:7365f8db1bac	3986	accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) / 1073741824.f);
Ucial	0:7365f8db1bac	3987	accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) / 1073741824.f);
Ucial	0:7365f8db1bac	3988	#else
Ucial	0:7365f8db1bac	3989	accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30);
Ucial	0:7365f8db1bac	3990	accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30);
Ucial	0:7365f8db1bac	3991	accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30);
Ucial	0:7365f8db1bac	3992	#endif
Ucial	0:7365f8db1bac	3993
Ucial	0:7365f8db1bac	3994	regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF);
Ucial	0:7365f8db1bac	3995	regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF);
Ucial	0:7365f8db1bac	3996	regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF);
Ucial	0:7365f8db1bac	3997	regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF);
Ucial	0:7365f8db1bac	3998	regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF);
Ucial	0:7365f8db1bac	3999	regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF);
Ucial	0:7365f8db1bac	4000	regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF);
Ucial	0:7365f8db1bac	4001	regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF);
Ucial	0:7365f8db1bac	4002	regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF);
Ucial	0:7365f8db1bac	4003	regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF);
Ucial	0:7365f8db1bac	4004	regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF);
Ucial	0:7365f8db1bac	4005	regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF);
Ucial	0:7365f8db1bac	4006	return mpu_write_mem(D_ACCEL_BIAS, 12, regs);
Ucial	0:7365f8db1bac	4007	}
Ucial	0:7365f8db1bac	4008
Ucial	0:7365f8db1bac	4009	/**
Ucial	0:7365f8db1bac	4010	* @brief Set DMP output rate.
Ucial	0:7365f8db1bac	4011	* Only used when DMP is on.
Ucial	0:7365f8db1bac	4012	* @param[in] rate Desired fifo rate (Hz).
Ucial	0:7365f8db1bac	4013	* @return 0 if successful.
Ucial	0:7365f8db1bac	4014	*/
Ucial	0:7365f8db1bac	4015	int dmp_set_fifo_rate(unsigned short rate)
Ucial	0:7365f8db1bac	4016	{
Ucial	0:7365f8db1bac	4017	const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB,
Ucial	0:7365f8db1bac	4018	0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF};
Ucial	0:7365f8db1bac	4019	unsigned short div;
Ucial	0:7365f8db1bac	4020	unsigned char tmp[8];
Ucial	0:7365f8db1bac	4021
Ucial	0:7365f8db1bac	4022	if (rate > DMP_SAMPLE_RATE)
Ucial	0:7365f8db1bac	4023	return -1;
Ucial	0:7365f8db1bac	4024	div = DMP_SAMPLE_RATE / rate - 1;
Ucial	0:7365f8db1bac	4025	tmp[0] = (unsigned char)((div >> 8) & 0xFF);
Ucial	0:7365f8db1bac	4026	tmp[1] = (unsigned char)(div & 0xFF);
Ucial	0:7365f8db1bac	4027	if (mpu_write_mem(D_0_22, 2, tmp))
Ucial	0:7365f8db1bac	4028	return -1;
Ucial	0:7365f8db1bac	4029	if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end))
Ucial	0:7365f8db1bac	4030	return -1;
Ucial	0:7365f8db1bac	4031
Ucial	0:7365f8db1bac	4032	dmp.fifo_rate = rate;
Ucial	0:7365f8db1bac	4033	return 0;
Ucial	0:7365f8db1bac	4034	}
Ucial	0:7365f8db1bac	4035
Ucial	0:7365f8db1bac	4036	/**
Ucial	0:7365f8db1bac	4037	* @brief Get DMP output rate.
Ucial	0:7365f8db1bac	4038	* @param[out] rate Current fifo rate (Hz).
Ucial	0:7365f8db1bac	4039	* @return 0 if successful.
Ucial	0:7365f8db1bac	4040	*/
Ucial	0:7365f8db1bac	4041	int dmp_get_fifo_rate(unsigned short *rate)
Ucial	0:7365f8db1bac	4042	{
Ucial	0:7365f8db1bac	4043	rate[0] = dmp.fifo_rate;
Ucial	0:7365f8db1bac	4044	return 0;
Ucial	0:7365f8db1bac	4045	}
Ucial	0:7365f8db1bac	4046
Ucial	0:7365f8db1bac	4047	/**
Ucial	0:7365f8db1bac	4048	* @brief Set tap threshold for a specific axis.
Ucial	0:7365f8db1bac	4049	* @param[in] axis 1, 2, and 4 for XYZ accel, respectively.
Ucial	0:7365f8db1bac	4050	* @param[in] thresh Tap threshold, in mg/ms.
Ucial	0:7365f8db1bac	4051	* @return 0 if successful.
Ucial	0:7365f8db1bac	4052	*/
Ucial	0:7365f8db1bac	4053	int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh)
Ucial	0:7365f8db1bac	4054	{
Ucial	0:7365f8db1bac	4055	unsigned char tmp[4], accel_fsr;
Ucial	0:7365f8db1bac	4056	float scaled_thresh;
Ucial	0:7365f8db1bac	4057	unsigned short dmp_thresh, dmp_thresh_2;
Ucial	0:7365f8db1bac	4058	if (!(axis & TAP_XYZ) \|\| thresh > 1600)
Ucial	0:7365f8db1bac	4059	return -1;
Ucial	0:7365f8db1bac	4060
Ucial	0:7365f8db1bac	4061	scaled_thresh = (float)thresh / DMP_SAMPLE_RATE;
Ucial	0:7365f8db1bac	4062
Ucial	0:7365f8db1bac	4063	mpu_get_accel_fsr(&accel_fsr);
Ucial	0:7365f8db1bac	4064	switch (accel_fsr) {
Ucial	0:7365f8db1bac	4065	case 2:
Ucial	0:7365f8db1bac	4066	dmp_thresh = (unsigned short)(scaled_thresh * 16384);
Ucial	0:7365f8db1bac	4067	/* dmp_thresh * 0.75 */
Ucial	0:7365f8db1bac	4068	dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288);
Ucial	0:7365f8db1bac	4069	break;
Ucial	0:7365f8db1bac	4070	case 4:
Ucial	0:7365f8db1bac	4071	dmp_thresh = (unsigned short)(scaled_thresh * 8192);
Ucial	0:7365f8db1bac	4072	/* dmp_thresh * 0.75 */
Ucial	0:7365f8db1bac	4073	dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144);
Ucial	0:7365f8db1bac	4074	break;
Ucial	0:7365f8db1bac	4075	case 8:
Ucial	0:7365f8db1bac	4076	dmp_thresh = (unsigned short)(scaled_thresh * 4096);
Ucial	0:7365f8db1bac	4077	/* dmp_thresh * 0.75 */
Ucial	0:7365f8db1bac	4078	dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072);
Ucial	0:7365f8db1bac	4079	break;
Ucial	0:7365f8db1bac	4080	case 16:
Ucial	0:7365f8db1bac	4081	dmp_thresh = (unsigned short)(scaled_thresh * 2048);
Ucial	0:7365f8db1bac	4082	/* dmp_thresh * 0.75 */
Ucial	0:7365f8db1bac	4083	dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536);
Ucial	0:7365f8db1bac	4084	break;
Ucial	0:7365f8db1bac	4085	default:
Ucial	0:7365f8db1bac	4086	return -1;
Ucial	0:7365f8db1bac	4087	}
Ucial	0:7365f8db1bac	4088	tmp[0] = (unsigned char)(dmp_thresh >> 8);
Ucial	0:7365f8db1bac	4089	tmp[1] = (unsigned char)(dmp_thresh & 0xFF);
Ucial	0:7365f8db1bac	4090	tmp[2] = (unsigned char)(dmp_thresh_2 >> 8);
Ucial	0:7365f8db1bac	4091	tmp[3] = (unsigned char)(dmp_thresh_2 & 0xFF);
Ucial	0:7365f8db1bac	4092
Ucial	0:7365f8db1bac	4093	if (axis & TAP_X) {
Ucial	0:7365f8db1bac	4094	if (mpu_write_mem(DMP_TAP_THX, 2, tmp))
Ucial	0:7365f8db1bac	4095	return -1;
Ucial	0:7365f8db1bac	4096	if (mpu_write_mem(D_1_36, 2, tmp+2))
Ucial	0:7365f8db1bac	4097	return -1;
Ucial	0:7365f8db1bac	4098	}
Ucial	0:7365f8db1bac	4099	if (axis & TAP_Y) {
Ucial	0:7365f8db1bac	4100	if (mpu_write_mem(DMP_TAP_THY, 2, tmp))
Ucial	0:7365f8db1bac	4101	return -1;
Ucial	0:7365f8db1bac	4102	if (mpu_write_mem(D_1_40, 2, tmp+2))
Ucial	0:7365f8db1bac	4103	return -1;
Ucial	0:7365f8db1bac	4104	}
Ucial	0:7365f8db1bac	4105	if (axis & TAP_Z) {
Ucial	0:7365f8db1bac	4106	if (mpu_write_mem(DMP_TAP_THZ, 2, tmp))
Ucial	0:7365f8db1bac	4107	return -1;
Ucial	0:7365f8db1bac	4108	if (mpu_write_mem(D_1_44, 2, tmp+2))
Ucial	0:7365f8db1bac	4109	return -1;
Ucial	0:7365f8db1bac	4110	}
Ucial	0:7365f8db1bac	4111	return 0;
Ucial	0:7365f8db1bac	4112	}
Ucial	0:7365f8db1bac	4113
Ucial	0:7365f8db1bac	4114	/**
Ucial	0:7365f8db1bac	4115	* @brief Set which axes will register a tap.
Ucial	0:7365f8db1bac	4116	* @param[in] axis 1, 2, and 4 for XYZ, respectively.
Ucial	0:7365f8db1bac	4117	* @return 0 if successful.
Ucial	0:7365f8db1bac	4118	*/
Ucial	0:7365f8db1bac	4119	int dmp_set_tap_axes(unsigned char axis)
Ucial	0:7365f8db1bac	4120	{
Ucial	0:7365f8db1bac	4121	unsigned char tmp = 0;
Ucial	0:7365f8db1bac	4122
Ucial	0:7365f8db1bac	4123	if (axis & TAP_X)
Ucial	0:7365f8db1bac	4124	tmp \|= 0x30;
Ucial	0:7365f8db1bac	4125	if (axis & TAP_Y)
Ucial	0:7365f8db1bac	4126	tmp \|= 0x0C;
Ucial	0:7365f8db1bac	4127	if (axis & TAP_Z)
Ucial	0:7365f8db1bac	4128	tmp \|= 0x03;
Ucial	0:7365f8db1bac	4129	return mpu_write_mem(D_1_72, 1, &tmp);
Ucial	0:7365f8db1bac	4130	}
Ucial	0:7365f8db1bac	4131
Ucial	0:7365f8db1bac	4132	/**
Ucial	0:7365f8db1bac	4133	* @brief Set minimum number of taps needed for an interrupt.
Ucial	0:7365f8db1bac	4134	* @param[in] min_taps Minimum consecutive taps (1-4).
Ucial	0:7365f8db1bac	4135	* @return 0 if successful.
Ucial	0:7365f8db1bac	4136	*/
Ucial	0:7365f8db1bac	4137	int dmp_set_tap_count(unsigned char min_taps)
Ucial	0:7365f8db1bac	4138	{
Ucial	0:7365f8db1bac	4139	unsigned char tmp;
Ucial	0:7365f8db1bac	4140
Ucial	0:7365f8db1bac	4141	if (min_taps < 1)
Ucial	0:7365f8db1bac	4142	min_taps = 1;
Ucial	0:7365f8db1bac	4143	else if (min_taps > 4)
Ucial	0:7365f8db1bac	4144	min_taps = 4;
Ucial	0:7365f8db1bac	4145
Ucial	0:7365f8db1bac	4146	tmp = min_taps - 1;
Ucial	0:7365f8db1bac	4147	return mpu_write_mem(D_1_79, 1, &tmp);
Ucial	0:7365f8db1bac	4148	}
Ucial	0:7365f8db1bac	4149
Ucial	0:7365f8db1bac	4150	/**
Ucial	0:7365f8db1bac	4151	* @brief Set length between valid taps.
Ucial	0:7365f8db1bac	4152	* @param[in] time Milliseconds between taps.
Ucial	0:7365f8db1bac	4153	* @return 0 if successful.
Ucial	0:7365f8db1bac	4154	*/
Ucial	0:7365f8db1bac	4155	int dmp_set_tap_time(unsigned short time)
Ucial	0:7365f8db1bac	4156	{
Ucial	0:7365f8db1bac	4157	unsigned short dmp_time;
Ucial	0:7365f8db1bac	4158	unsigned char tmp[2];
Ucial	0:7365f8db1bac	4159
Ucial	0:7365f8db1bac	4160	dmp_time = time / (1000 / DMP_SAMPLE_RATE);
Ucial	0:7365f8db1bac	4161	tmp[0] = (unsigned char)(dmp_time >> 8);
Ucial	0:7365f8db1bac	4162	tmp[1] = (unsigned char)(dmp_time & 0xFF);
Ucial	0:7365f8db1bac	4163	return mpu_write_mem(DMP_TAPW_MIN, 2, tmp);
Ucial	0:7365f8db1bac	4164	}
Ucial	0:7365f8db1bac	4165
Ucial	0:7365f8db1bac	4166	/**
Ucial	0:7365f8db1bac	4167	* @brief Set max time between taps to register as a multi-tap.
Ucial	0:7365f8db1bac	4168	* @param[in] time Max milliseconds between taps.
Ucial	0:7365f8db1bac	4169	* @return 0 if successful.
Ucial	0:7365f8db1bac	4170	*/
Ucial	0:7365f8db1bac	4171	int dmp_set_tap_time_multi(unsigned short time)
Ucial	0:7365f8db1bac	4172	{
Ucial	0:7365f8db1bac	4173	unsigned short dmp_time;
Ucial	0:7365f8db1bac	4174	unsigned char tmp[2];
Ucial	0:7365f8db1bac	4175
Ucial	0:7365f8db1bac	4176	dmp_time = time / (1000 / DMP_SAMPLE_RATE);
Ucial	0:7365f8db1bac	4177	tmp[0] = (unsigned char)(dmp_time >> 8);
Ucial	0:7365f8db1bac	4178	tmp[1] = (unsigned char)(dmp_time & 0xFF);
Ucial	0:7365f8db1bac	4179	return mpu_write_mem(D_1_218, 2, tmp);
Ucial	0:7365f8db1bac	4180	}
Ucial	0:7365f8db1bac	4181
Ucial	0:7365f8db1bac	4182	/**
Ucial	0:7365f8db1bac	4183	* @brief Set shake rejection threshold.
Ucial	0:7365f8db1bac	4184	* If the DMP detects a gyro sample larger than @e thresh, taps are rejected.
Ucial	0:7365f8db1bac	4185	* @param[in] sf Gyro scale factor.
Ucial	0:7365f8db1bac	4186	* @param[in] thresh Gyro threshold in dps.
Ucial	0:7365f8db1bac	4187	* @return 0 if successful.
Ucial	0:7365f8db1bac	4188	*/
Ucial	0:7365f8db1bac	4189	int dmp_set_shake_reject_thresh(long sf, unsigned short thresh)
Ucial	0:7365f8db1bac	4190	{
Ucial	0:7365f8db1bac	4191	unsigned char tmp[4];
Ucial	0:7365f8db1bac	4192	long thresh_scaled = sf / 1000 * thresh;
Ucial	0:7365f8db1bac	4193	tmp[0] = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF);
Ucial	0:7365f8db1bac	4194	tmp[1] = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF);
Ucial	0:7365f8db1bac	4195	tmp[2] = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF);
Ucial	0:7365f8db1bac	4196	tmp[3] = (unsigned char)((long)thresh_scaled & 0xFF);
Ucial	0:7365f8db1bac	4197	return mpu_write_mem(D_1_92, 4, tmp);
Ucial	0:7365f8db1bac	4198	}
Ucial	0:7365f8db1bac	4199
Ucial	0:7365f8db1bac	4200	/**
Ucial	0:7365f8db1bac	4201	* @brief Set shake rejection time.
Ucial	0:7365f8db1bac	4202	* Sets the length of time that the gyro must be outside of the threshold set
Ucial	0:7365f8db1bac	4203	* by @e gyro_set_shake_reject_thresh before taps are rejected. A mandatory
Ucial	0:7365f8db1bac	4204	* 60 ms is added to this parameter.
Ucial	0:7365f8db1bac	4205	* @param[in] time Time in milliseconds.
Ucial	0:7365f8db1bac	4206	* @return 0 if successful.
Ucial	0:7365f8db1bac	4207	*/
Ucial	0:7365f8db1bac	4208	int dmp_set_shake_reject_time(unsigned short time)
Ucial	0:7365f8db1bac	4209	{
Ucial	0:7365f8db1bac	4210	unsigned char tmp[2];
Ucial	0:7365f8db1bac	4211
Ucial	0:7365f8db1bac	4212	time /= (1000 / DMP_SAMPLE_RATE);
Ucial	0:7365f8db1bac	4213	tmp[0] = time >> 8;
Ucial	0:7365f8db1bac	4214	tmp[1] = time & 0xFF;
Ucial	0:7365f8db1bac	4215	return mpu_write_mem(D_1_90,2,tmp);
Ucial	0:7365f8db1bac	4216	}
Ucial	0:7365f8db1bac	4217
Ucial	0:7365f8db1bac	4218	/**
Ucial	0:7365f8db1bac	4219	* @brief Set shake rejection timeout.
Ucial	0:7365f8db1bac	4220	* Sets the length of time after a shake rejection that the gyro must stay
Ucial	0:7365f8db1bac	4221	* inside of the threshold before taps can be detected again. A mandatory
Ucial	0:7365f8db1bac	4222	* 60 ms is added to this parameter.
Ucial	0:7365f8db1bac	4223	* @param[in] time Time in milliseconds.
Ucial	0:7365f8db1bac	4224	* @return 0 if successful.
Ucial	0:7365f8db1bac	4225	*/
Ucial	0:7365f8db1bac	4226	int dmp_set_shake_reject_timeout(unsigned short time)
Ucial	0:7365f8db1bac	4227	{
Ucial	0:7365f8db1bac	4228	unsigned char tmp[2];
Ucial	0:7365f8db1bac	4229
Ucial	0:7365f8db1bac	4230	time /= (1000 / DMP_SAMPLE_RATE);
Ucial	0:7365f8db1bac	4231	tmp[0] = time >> 8;
Ucial	0:7365f8db1bac	4232	tmp[1] = time & 0xFF;
Ucial	0:7365f8db1bac	4233	return mpu_write_mem(D_1_88,2,tmp);
Ucial	0:7365f8db1bac	4234	}
Ucial	0:7365f8db1bac	4235
Ucial	0:7365f8db1bac	4236	/**
Ucial	0:7365f8db1bac	4237	* @brief Get current step count.
Ucial	0:7365f8db1bac	4238	* @param[out] count Number of steps detected.
Ucial	0:7365f8db1bac	4239	* @return 0 if successful.
Ucial	0:7365f8db1bac	4240	*/
Ucial	0:7365f8db1bac	4241	int dmp_get_pedometer_step_count(unsigned long *count)
Ucial	0:7365f8db1bac	4242	{
Ucial	0:7365f8db1bac	4243	unsigned char tmp[4];
Ucial	0:7365f8db1bac	4244	if (!count)
Ucial	0:7365f8db1bac	4245	return -1;
Ucial	0:7365f8db1bac	4246
Ucial	0:7365f8db1bac	4247	if (mpu_read_mem(D_PEDSTD_STEPCTR, 4, tmp))
Ucial	0:7365f8db1bac	4248	return -1;
Ucial	0:7365f8db1bac	4249
Ucial	0:7365f8db1bac	4250	count[0] = ((unsigned long)tmp[0] << 24) \| ((unsigned long)tmp[1] << 16) \|
Ucial	0:7365f8db1bac	4251	((unsigned long)tmp[2] << 8) \| tmp[3];
Ucial	0:7365f8db1bac	4252	return 0;
Ucial	0:7365f8db1bac	4253	}
Ucial	0:7365f8db1bac	4254
Ucial	0:7365f8db1bac	4255	/**
Ucial	0:7365f8db1bac	4256	* @brief Overwrite current step count.
Ucial	0:7365f8db1bac	4257	* WARNING: This function writes to DMP memory and could potentially encounter
Ucial	0:7365f8db1bac	4258	* a race condition if called while the pedometer is enabled.
Ucial	0:7365f8db1bac	4259	* @param[in] count New step count.
Ucial	0:7365f8db1bac	4260	* @return 0 if successful.
Ucial	0:7365f8db1bac	4261	*/
Ucial	0:7365f8db1bac	4262	int dmp_set_pedometer_step_count(unsigned long count)
Ucial	0:7365f8db1bac	4263	{
Ucial	0:7365f8db1bac	4264	unsigned char tmp[4];
Ucial	0:7365f8db1bac	4265
Ucial	0:7365f8db1bac	4266	tmp[0] = (unsigned char)((count >> 24) & 0xFF);
Ucial	0:7365f8db1bac	4267	tmp[1] = (unsigned char)((count >> 16) & 0xFF);
Ucial	0:7365f8db1bac	4268	tmp[2] = (unsigned char)((count >> 8) & 0xFF);
Ucial	0:7365f8db1bac	4269	tmp[3] = (unsigned char)(count & 0xFF);
Ucial	0:7365f8db1bac	4270	return mpu_write_mem(D_PEDSTD_STEPCTR, 4, tmp);
Ucial	0:7365f8db1bac	4271	}
Ucial	0:7365f8db1bac	4272
Ucial	0:7365f8db1bac	4273	/**
Ucial	0:7365f8db1bac	4274	* @brief Get duration of walking time.
Ucial	0:7365f8db1bac	4275	* @param[in] time Walk time in milliseconds.
Ucial	0:7365f8db1bac	4276	* @return 0 if successful.
Ucial	0:7365f8db1bac	4277	*/
Ucial	0:7365f8db1bac	4278	int dmp_get_pedometer_walk_time(unsigned long *time)
Ucial	0:7365f8db1bac	4279	{
Ucial	0:7365f8db1bac	4280	unsigned char tmp[4];
Ucial	0:7365f8db1bac	4281	if (!time)
Ucial	0:7365f8db1bac	4282	return -1;
Ucial	0:7365f8db1bac	4283
Ucial	0:7365f8db1bac	4284	if (mpu_read_mem(D_PEDSTD_TIMECTR, 4, tmp))
Ucial	0:7365f8db1bac	4285	return -1;
Ucial	0:7365f8db1bac	4286
Ucial	0:7365f8db1bac	4287	time[0] = (((unsigned long)tmp[0] << 24) \| ((unsigned long)tmp[1] << 16) \|
Ucial	0:7365f8db1bac	4288	((unsigned long)tmp[2] << 8) \| tmp[3]) * 20;
Ucial	0:7365f8db1bac	4289	return 0;
Ucial	0:7365f8db1bac	4290	}
Ucial	0:7365f8db1bac	4291
Ucial	0:7365f8db1bac	4292	/**
Ucial	0:7365f8db1bac	4293	* @brief Overwrite current walk time.
Ucial	0:7365f8db1bac	4294	* WARNING: This function writes to DMP memory and could potentially encounter
Ucial	0:7365f8db1bac	4295	* a race condition if called while the pedometer is enabled.
Ucial	0:7365f8db1bac	4296	* @param[in] time New walk time in milliseconds.
Ucial	0:7365f8db1bac	4297	*/
Ucial	0:7365f8db1bac	4298	int dmp_set_pedometer_walk_time(unsigned long time)
Ucial	0:7365f8db1bac	4299	{
Ucial	0:7365f8db1bac	4300	unsigned char tmp[4];
Ucial	0:7365f8db1bac	4301
Ucial	0:7365f8db1bac	4302	time /= 20;
Ucial	0:7365f8db1bac	4303
Ucial	0:7365f8db1bac	4304	tmp[0] = (unsigned char)((time >> 24) & 0xFF);
Ucial	0:7365f8db1bac	4305	tmp[1] = (unsigned char)((time >> 16) & 0xFF);
Ucial	0:7365f8db1bac	4306	tmp[2] = (unsigned char)((time >> 8) & 0xFF);
Ucial	0:7365f8db1bac	4307	tmp[3] = (unsigned char)(time & 0xFF);
Ucial	0:7365f8db1bac	4308	return mpu_write_mem(D_PEDSTD_TIMECTR, 4, tmp);
Ucial	0:7365f8db1bac	4309	}
Ucial	0:7365f8db1bac	4310
Ucial	0:7365f8db1bac	4311	/**
Ucial	0:7365f8db1bac	4312	* @brief Enable DMP features.
Ucial	0:7365f8db1bac	4313	* The following \#define's are used in the input mask:
Ucial	0:7365f8db1bac	4314	* \n DMP_FEATURE_TAP
Ucial	0:7365f8db1bac	4315	* \n DMP_FEATURE_ANDROID_ORIENT
Ucial	0:7365f8db1bac	4316	* \n DMP_FEATURE_LP_QUAT
Ucial	0:7365f8db1bac	4317	* \n DMP_FEATURE_6X_LP_QUAT
Ucial	0:7365f8db1bac	4318	* \n DMP_FEATURE_GYRO_CAL
Ucial	0:7365f8db1bac	4319	* \n DMP_FEATURE_SEND_RAW_ACCEL
Ucial	0:7365f8db1bac	4320	* \n DMP_FEATURE_SEND_RAW_GYRO
Ucial	0:7365f8db1bac	4321	* \n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually
Ucial	0:7365f8db1bac	4322	* exclusive.
Ucial	0:7365f8db1bac	4323	* \n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also
Ucial	0:7365f8db1bac	4324	* mutually exclusive.
Ucial	0:7365f8db1bac	4325	* @param[in] mask Mask of features to enable.
Ucial	0:7365f8db1bac	4326	* @return 0 if successful.
Ucial	0:7365f8db1bac	4327	*/
Ucial	0:7365f8db1bac	4328	int dmp_enable_feature(unsigned short mask)
Ucial	0:7365f8db1bac	4329	{
Ucial	0:7365f8db1bac	4330	unsigned char tmp[10];
Ucial	0:7365f8db1bac	4331
Ucial	0:7365f8db1bac	4332	/* TODO: All of these settings can probably be integrated into the default
Ucial	0:7365f8db1bac	4333	* DMP image.
Ucial	0:7365f8db1bac	4334	*/
Ucial	0:7365f8db1bac	4335	/* Set integration scale factor. */
Ucial	0:7365f8db1bac	4336	tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF);
Ucial	0:7365f8db1bac	4337	tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF);
Ucial	0:7365f8db1bac	4338	tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF);
Ucial	0:7365f8db1bac	4339	tmp[3] = (unsigned char)(GYRO_SF & 0xFF);
Ucial	0:7365f8db1bac	4340	mpu_write_mem(D_0_104, 4, tmp);
Ucial	0:7365f8db1bac	4341
Ucial	0:7365f8db1bac	4342	/* Send sensor data to the FIFO. */
Ucial	0:7365f8db1bac	4343	tmp[0] = 0xA3;
Ucial	0:7365f8db1bac	4344	if (mask & DMP_FEATURE_SEND_RAW_ACCEL) {
Ucial	0:7365f8db1bac	4345	tmp[1] = 0xC0;
Ucial	0:7365f8db1bac	4346	tmp[2] = 0xC8;
Ucial	0:7365f8db1bac	4347	tmp[3] = 0xC2;
Ucial	0:7365f8db1bac	4348	} else {
Ucial	0:7365f8db1bac	4349	tmp[1] = 0xA3;
Ucial	0:7365f8db1bac	4350	tmp[2] = 0xA3;
Ucial	0:7365f8db1bac	4351	tmp[3] = 0xA3;
Ucial	0:7365f8db1bac	4352	}
Ucial	0:7365f8db1bac	4353	if (mask & DMP_FEATURE_SEND_ANY_GYRO) {
Ucial	0:7365f8db1bac	4354	tmp[4] = 0xC4;
Ucial	0:7365f8db1bac	4355	tmp[5] = 0xCC;
Ucial	0:7365f8db1bac	4356	tmp[6] = 0xC6;
Ucial	0:7365f8db1bac	4357	} else {
Ucial	0:7365f8db1bac	4358	tmp[4] = 0xA3;
Ucial	0:7365f8db1bac	4359	tmp[5] = 0xA3;
Ucial	0:7365f8db1bac	4360	tmp[6] = 0xA3;
Ucial	0:7365f8db1bac	4361	}
Ucial	0:7365f8db1bac	4362	tmp[7] = 0xA3;
Ucial	0:7365f8db1bac	4363	tmp[8] = 0xA3;
Ucial	0:7365f8db1bac	4364	tmp[9] = 0xA3;
Ucial	0:7365f8db1bac	4365	mpu_write_mem(CFG_15,10,tmp);
Ucial	0:7365f8db1bac	4366
Ucial	0:7365f8db1bac	4367	/* Send gesture data to the FIFO. */
Ucial	0:7365f8db1bac	4368	if (mask & (DMP_FEATURE_TAP \| DMP_FEATURE_ANDROID_ORIENT))
Ucial	0:7365f8db1bac	4369	tmp[0] = DINA20;
Ucial	0:7365f8db1bac	4370	else
Ucial	0:7365f8db1bac	4371	tmp[0] = 0xD8;
Ucial	0:7365f8db1bac	4372	mpu_write_mem(CFG_27,1,tmp);
Ucial	0:7365f8db1bac	4373
Ucial	0:7365f8db1bac	4374	if (mask & DMP_FEATURE_GYRO_CAL)
Ucial	0:7365f8db1bac	4375	dmp_enable_gyro_cal(1);
Ucial	0:7365f8db1bac	4376	else
Ucial	0:7365f8db1bac	4377	dmp_enable_gyro_cal(0);
Ucial	0:7365f8db1bac	4378
Ucial	0:7365f8db1bac	4379	if (mask & DMP_FEATURE_SEND_ANY_GYRO) {
Ucial	0:7365f8db1bac	4380	if (mask & DMP_FEATURE_SEND_CAL_GYRO) {
Ucial	0:7365f8db1bac	4381	tmp[0] = 0xB2;
Ucial	0:7365f8db1bac	4382	tmp[1] = 0x8B;
Ucial	0:7365f8db1bac	4383	tmp[2] = 0xB6;
Ucial	0:7365f8db1bac	4384	tmp[3] = 0x9B;
Ucial	0:7365f8db1bac	4385	} else {
Ucial	0:7365f8db1bac	4386	tmp[0] = DINAC0;
Ucial	0:7365f8db1bac	4387	tmp[1] = DINA80;
Ucial	0:7365f8db1bac	4388	tmp[2] = DINAC2;
Ucial	0:7365f8db1bac	4389	tmp[3] = DINA90;
Ucial	0:7365f8db1bac	4390	}
Ucial	0:7365f8db1bac	4391	mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp);
Ucial	0:7365f8db1bac	4392	}
Ucial	0:7365f8db1bac	4393
Ucial	0:7365f8db1bac	4394	if (mask & DMP_FEATURE_TAP) {
Ucial	0:7365f8db1bac	4395	/* Enable tap. */
Ucial	0:7365f8db1bac	4396	tmp[0] = 0xF8;
Ucial	0:7365f8db1bac	4397	mpu_write_mem(CFG_20, 1, tmp);
Ucial	0:7365f8db1bac	4398	dmp_set_tap_thresh(TAP_XYZ, 250);
Ucial	0:7365f8db1bac	4399	dmp_set_tap_axes(TAP_XYZ);
Ucial	0:7365f8db1bac	4400	dmp_set_tap_count(1);
Ucial	0:7365f8db1bac	4401	dmp_set_tap_time(100);
Ucial	0:7365f8db1bac	4402	dmp_set_tap_time_multi(500);
Ucial	0:7365f8db1bac	4403
Ucial	0:7365f8db1bac	4404	dmp_set_shake_reject_thresh(GYRO_SF, 200);
Ucial	0:7365f8db1bac	4405	dmp_set_shake_reject_time(40);
Ucial	0:7365f8db1bac	4406	dmp_set_shake_reject_timeout(10);
Ucial	0:7365f8db1bac	4407	} else {
Ucial	0:7365f8db1bac	4408	tmp[0] = 0xD8;
Ucial	0:7365f8db1bac	4409	mpu_write_mem(CFG_20, 1, tmp);
Ucial	0:7365f8db1bac	4410	}
Ucial	0:7365f8db1bac	4411
Ucial	0:7365f8db1bac	4412	if (mask & DMP_FEATURE_ANDROID_ORIENT) {
Ucial	0:7365f8db1bac	4413	tmp[0] = 0xD9;
Ucial	0:7365f8db1bac	4414	} else
Ucial	0:7365f8db1bac	4415	tmp[0] = 0xD8;
Ucial	0:7365f8db1bac	4416	mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp);
Ucial	0:7365f8db1bac	4417
Ucial	0:7365f8db1bac	4418	if (mask & DMP_FEATURE_LP_QUAT)
Ucial	0:7365f8db1bac	4419	dmp_enable_lp_quat(1);
Ucial	0:7365f8db1bac	4420	else
Ucial	0:7365f8db1bac	4421	dmp_enable_lp_quat(0);
Ucial	0:7365f8db1bac	4422
Ucial	0:7365f8db1bac	4423	if (mask & DMP_FEATURE_6X_LP_QUAT)
Ucial	0:7365f8db1bac	4424	dmp_enable_6x_lp_quat(1);
Ucial	0:7365f8db1bac	4425	else
Ucial	0:7365f8db1bac	4426	dmp_enable_6x_lp_quat(0);
Ucial	0:7365f8db1bac	4427
Ucial	0:7365f8db1bac	4428	/* Pedometer is always enabled. */
Ucial	0:7365f8db1bac	4429	dmp.feature_mask = mask \| DMP_FEATURE_PEDOMETER;
Ucial	0:7365f8db1bac	4430	mpu_reset_fifo();
Ucial	0:7365f8db1bac	4431
Ucial	0:7365f8db1bac	4432	dmp.packet_length = 0;
Ucial	0:7365f8db1bac	4433	if (mask & DMP_FEATURE_SEND_RAW_ACCEL)
Ucial	0:7365f8db1bac	4434	dmp.packet_length += 6;
Ucial	0:7365f8db1bac	4435	if (mask & DMP_FEATURE_SEND_ANY_GYRO)
Ucial	0:7365f8db1bac	4436	dmp.packet_length += 6;
Ucial	0:7365f8db1bac	4437	if (mask & (DMP_FEATURE_LP_QUAT \| DMP_FEATURE_6X_LP_QUAT))
Ucial	0:7365f8db1bac	4438	dmp.packet_length += 16;
Ucial	0:7365f8db1bac	4439	if (mask & (DMP_FEATURE_TAP \| DMP_FEATURE_ANDROID_ORIENT))
Ucial	0:7365f8db1bac	4440	dmp.packet_length += 4;
Ucial	0:7365f8db1bac	4441
Ucial	0:7365f8db1bac	4442	return 0;
Ucial	0:7365f8db1bac	4443	}
Ucial	0:7365f8db1bac	4444
Ucial	0:7365f8db1bac	4445	/**
Ucial	0:7365f8db1bac	4446	* @brief Get list of currently enabled DMP features.
Ucial	0:7365f8db1bac	4447	* @param[out] Mask of enabled features.
Ucial	0:7365f8db1bac	4448	* @return 0 if successful.
Ucial	0:7365f8db1bac	4449	*/
Ucial	0:7365f8db1bac	4450	int dmp_get_enabled_features(unsigned short *mask)
Ucial	0:7365f8db1bac	4451	{
Ucial	0:7365f8db1bac	4452	mask[0] = dmp.feature_mask;
Ucial	0:7365f8db1bac	4453	return 0;
Ucial	0:7365f8db1bac	4454	}
Ucial	0:7365f8db1bac	4455
Ucial	0:7365f8db1bac	4456	/**
Ucial	0:7365f8db1bac	4457	* @brief Calibrate the gyro data in the DMP.
Ucial	0:7365f8db1bac	4458	* After eight seconds of no motion, the DMP will compute gyro biases and
Ucial	0:7365f8db1bac	4459	* subtract them from the quaternion output. If @e dmp_enable_feature is
Ucial	0:7365f8db1bac	4460	* called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be
Ucial	0:7365f8db1bac	4461	* subtracted from the gyro output.
Ucial	0:7365f8db1bac	4462	* @param[in] enable 1 to enable gyro calibration.
Ucial	0:7365f8db1bac	4463	* @return 0 if successful.
Ucial	0:7365f8db1bac	4464	*/
Ucial	0:7365f8db1bac	4465	int dmp_enable_gyro_cal(unsigned char enable)
Ucial	0:7365f8db1bac	4466	{
Ucial	0:7365f8db1bac	4467	if (enable) {
Ucial	0:7365f8db1bac	4468	unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d};
Ucial	0:7365f8db1bac	4469	return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
Ucial	0:7365f8db1bac	4470	} else {
Ucial	0:7365f8db1bac	4471	unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7};
Ucial	0:7365f8db1bac	4472	return mpu_write_mem(CFG_MOTION_BIAS, 9, regs);
Ucial	0:7365f8db1bac	4473	}
Ucial	0:7365f8db1bac	4474	}
Ucial	0:7365f8db1bac	4475
Ucial	0:7365f8db1bac	4476	/**
Ucial	0:7365f8db1bac	4477	* @brief Generate 3-axis quaternions from the DMP.
Ucial	0:7365f8db1bac	4478	* In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
Ucial	0:7365f8db1bac	4479	* exclusive.
Ucial	0:7365f8db1bac	4480	* @param[in] enable 1 to enable 3-axis quaternion.
Ucial	0:7365f8db1bac	4481	* @return 0 if successful.
Ucial	0:7365f8db1bac	4482	*/
Ucial	0:7365f8db1bac	4483	int dmp_enable_lp_quat(unsigned char enable)
Ucial	0:7365f8db1bac	4484	{
Ucial	0:7365f8db1bac	4485	unsigned char regs[4];
Ucial	0:7365f8db1bac	4486	if (enable) {
Ucial	0:7365f8db1bac	4487	regs[0] = DINBC0;
Ucial	0:7365f8db1bac	4488	regs[1] = DINBC2;
Ucial	0:7365f8db1bac	4489	regs[2] = DINBC4;
Ucial	0:7365f8db1bac	4490	regs[3] = DINBC6;
Ucial	0:7365f8db1bac	4491	}
Ucial	0:7365f8db1bac	4492	else
Ucial	0:7365f8db1bac	4493	memset(regs, 0x8B, 4);
Ucial	0:7365f8db1bac	4494
Ucial	0:7365f8db1bac	4495	mpu_write_mem(CFG_LP_QUAT, 4, regs);
Ucial	0:7365f8db1bac	4496
Ucial	0:7365f8db1bac	4497	return mpu_reset_fifo();
Ucial	0:7365f8db1bac	4498	}
Ucial	0:7365f8db1bac	4499
Ucial	0:7365f8db1bac	4500	/**
Ucial	0:7365f8db1bac	4501	* @brief Generate 6-axis quaternions from the DMP.
Ucial	0:7365f8db1bac	4502	* In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
Ucial	0:7365f8db1bac	4503	* exclusive.
Ucial	0:7365f8db1bac	4504	* @param[in] enable 1 to enable 6-axis quaternion.
Ucial	0:7365f8db1bac	4505	* @return 0 if successful.
Ucial	0:7365f8db1bac	4506	*/
Ucial	0:7365f8db1bac	4507	int dmp_enable_6x_lp_quat(unsigned char enable)
Ucial	0:7365f8db1bac	4508	{
Ucial	0:7365f8db1bac	4509	unsigned char regs[4];
Ucial	0:7365f8db1bac	4510	if (enable) {
Ucial	0:7365f8db1bac	4511	regs[0] = DINA20;
Ucial	0:7365f8db1bac	4512	regs[1] = DINA28;
Ucial	0:7365f8db1bac	4513	regs[2] = DINA30;
Ucial	0:7365f8db1bac	4514	regs[3] = DINA38;
Ucial	0:7365f8db1bac	4515	} else
Ucial	0:7365f8db1bac	4516	memset(regs, 0xA3, 4);
Ucial	0:7365f8db1bac	4517
Ucial	0:7365f8db1bac	4518	mpu_write_mem(CFG_8, 4, regs);
Ucial	0:7365f8db1bac	4519
Ucial	0:7365f8db1bac	4520	return mpu_reset_fifo();
Ucial	0:7365f8db1bac	4521	}
Ucial	0:7365f8db1bac	4522
Ucial	0:7365f8db1bac	4523	/**
Ucial	0:7365f8db1bac	4524	* @brief Decode the four-byte gesture data and execute any callbacks.
Ucial	0:7365f8db1bac	4525	* @param[in] gesture Gesture data from DMP packet.
Ucial	0:7365f8db1bac	4526	* @return 0 if successful.
Ucial	0:7365f8db1bac	4527	*/
Ucial	0:7365f8db1bac	4528	static int decode_gesture(unsigned char *gesture)
Ucial	0:7365f8db1bac	4529	{
Ucial	0:7365f8db1bac	4530	unsigned char tap, android_orient;
Ucial	0:7365f8db1bac	4531
Ucial	0:7365f8db1bac	4532	android_orient = gesture[3] & 0xC0;
Ucial	0:7365f8db1bac	4533	tap = 0x3F & gesture[3];
Ucial	0:7365f8db1bac	4534
Ucial	0:7365f8db1bac	4535	if (gesture[1] & INT_SRC_TAP) {
Ucial	0:7365f8db1bac	4536	unsigned char direction, count;
Ucial	0:7365f8db1bac	4537	direction = tap >> 3;
Ucial	0:7365f8db1bac	4538	count = (tap % 8) + 1;
Ucial	0:7365f8db1bac	4539	if (dmp.tap_cb)
Ucial	0:7365f8db1bac	4540	dmp.tap_cb(direction, count);
Ucial	0:7365f8db1bac	4541	}
Ucial	0:7365f8db1bac	4542
Ucial	0:7365f8db1bac	4543	if (gesture[1] & INT_SRC_ANDROID_ORIENT) {
Ucial	0:7365f8db1bac	4544	if (dmp.android_orient_cb)
Ucial	0:7365f8db1bac	4545	dmp.android_orient_cb(android_orient >> 6);
Ucial	0:7365f8db1bac	4546	}
Ucial	0:7365f8db1bac	4547
Ucial	0:7365f8db1bac	4548	return 0;
Ucial	0:7365f8db1bac	4549	}
Ucial	0:7365f8db1bac	4550
Ucial	0:7365f8db1bac	4551	/**
Ucial	0:7365f8db1bac	4552	* @brief Specify when a DMP interrupt should occur.
Ucial	0:7365f8db1bac	4553	* A DMP interrupt can be configured to trigger on either of the two
Ucial	0:7365f8db1bac	4554	* conditions below:
Ucial	0:7365f8db1bac	4555	* \n a. One FIFO period has elapsed (set by @e mpu_set_sample_rate).
Ucial	0:7365f8db1bac	4556	* \n b. A tap event has been detected.
Ucial	0:7365f8db1bac	4557	* @param[in] mode DMP_INT_GESTURE or DMP_INT_CONTINUOUS.
Ucial	0:7365f8db1bac	4558	* @return 0 if successful.
Ucial	0:7365f8db1bac	4559	*/
Ucial	0:7365f8db1bac	4560	int dmp_set_interrupt_mode(unsigned char mode)
Ucial	0:7365f8db1bac	4561	{
Ucial	0:7365f8db1bac	4562	const unsigned char regs_continuous[11] =
Ucial	0:7365f8db1bac	4563	{0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9};
Ucial	0:7365f8db1bac	4564	const unsigned char regs_gesture[11] =
Ucial	0:7365f8db1bac	4565	{0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda};
Ucial	0:7365f8db1bac	4566
Ucial	0:7365f8db1bac	4567	switch (mode) {
Ucial	0:7365f8db1bac	4568	case DMP_INT_CONTINUOUS:
Ucial	0:7365f8db1bac	4569	return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
Ucial	0:7365f8db1bac	4570	(unsigned char*)regs_continuous);
Ucial	0:7365f8db1bac	4571	case DMP_INT_GESTURE:
Ucial	0:7365f8db1bac	4572	return mpu_write_mem(CFG_FIFO_ON_EVENT, 11,
Ucial	0:7365f8db1bac	4573	(unsigned char*)regs_gesture);
Ucial	0:7365f8db1bac	4574	default:
Ucial	0:7365f8db1bac	4575	return -1;
Ucial	0:7365f8db1bac	4576	}
Ucial	0:7365f8db1bac	4577	}
Ucial	0:7365f8db1bac	4578
Ucial	0:7365f8db1bac	4579	/**
Ucial	0:7365f8db1bac	4580	* @brief Get one packet from the FIFO.
Ucial	0:7365f8db1bac	4581	* If @e sensors does not contain a particular sensor, disregard the data
Ucial	0:7365f8db1bac	4582	* returned to that pointer.
Ucial	0:7365f8db1bac	4583	* \n @e sensors can contain a combination of the following flags:
Ucial	0:7365f8db1bac	4584	* \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
Ucial	0:7365f8db1bac	4585	* \n INV_XYZ_GYRO
Ucial	0:7365f8db1bac	4586	* \n INV_XYZ_ACCEL
Ucial	0:7365f8db1bac	4587	* \n INV_WXYZ_QUAT
Ucial	0:7365f8db1bac	4588	* \n If the FIFO has no new data, @e sensors will be zero.
Ucial	0:7365f8db1bac	4589	* \n If the FIFO is disabled, @e sensors will be zero and this function will
Ucial	0:7365f8db1bac	4590	* return a non-zero error code.
Ucial	0:7365f8db1bac	4591	* @param[out] gyro Gyro data in hardware units.
Ucial	0:7365f8db1bac	4592	* @param[out] accel Accel data in hardware units.
Ucial	0:7365f8db1bac	4593	* @param[out] quat 3-axis quaternion data in hardware units.
Ucial	0:7365f8db1bac	4594	* @param[out] timestamp Timestamp in milliseconds.
Ucial	0:7365f8db1bac	4595	* @param[out] sensors Mask of sensors read from FIFO.
Ucial	0:7365f8db1bac	4596	* @param[out] more Number of remaining packets.
Ucial	0:7365f8db1bac	4597	* @return 0 if successful.
Ucial	0:7365f8db1bac	4598	*/
Ucial	0:7365f8db1bac	4599	int dmp_read_fifo(short gyro, short accel, long *quat,
Ucial	0:7365f8db1bac	4600	unsigned long timestamp, short sensors, unsigned char *more)
Ucial	0:7365f8db1bac	4601	{
Ucial	0:7365f8db1bac	4602	unsigned char fifo_data[MAX_PACKET_LENGTH_2];
Ucial	0:7365f8db1bac	4603	unsigned char ii = 0;
Ucial	0:7365f8db1bac	4604
Ucial	0:7365f8db1bac	4605	/* TODO: sensors[0] only changes when dmp_enable_feature is called. We can
Ucial	0:7365f8db1bac	4606	* cache this value and save some cycles.
Ucial	0:7365f8db1bac	4607	*/
Ucial	0:7365f8db1bac	4608	sensors[0] = 0;
Ucial	0:7365f8db1bac	4609
Ucial	0:7365f8db1bac	4610	/* Get a packet. */
Ucial	0:7365f8db1bac	4611	if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more))
Ucial	0:7365f8db1bac	4612	return -1;
Ucial	0:7365f8db1bac	4613
Ucial	0:7365f8db1bac	4614	/* Parse DMP packet. */
Ucial	0:7365f8db1bac	4615	if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT \| DMP_FEATURE_6X_LP_QUAT)) {
Ucial	0:7365f8db1bac	4616	#ifdef FIFO_CORRUPTION_CHECK
Ucial	0:7365f8db1bac	4617	long quat_q14[4], quat_mag_sq;
Ucial	0:7365f8db1bac	4618	#endif
Ucial	0:7365f8db1bac	4619	quat[0] = ((long)fifo_data[0] << 24) \| ((long)fifo_data[1] << 16) \|
Ucial	0:7365f8db1bac	4620	((long)fifo_data[2] << 8) \| fifo_data[3];
Ucial	0:7365f8db1bac	4621	quat[1] = ((long)fifo_data[4] << 24) \| ((long)fifo_data[5] << 16) \|
Ucial	0:7365f8db1bac	4622	((long)fifo_data[6] << 8) \| fifo_data[7];
Ucial	0:7365f8db1bac	4623	quat[2] = ((long)fifo_data[8] << 24) \| ((long)fifo_data[9] << 16) \|
Ucial	0:7365f8db1bac	4624	((long)fifo_data[10] << 8) \| fifo_data[11];
Ucial	0:7365f8db1bac	4625	quat[3] = ((long)fifo_data[12] << 24) \| ((long)fifo_data[13] << 16) \|
Ucial	0:7365f8db1bac	4626	((long)fifo_data[14] << 8) \| fifo_data[15];
Ucial	0:7365f8db1bac	4627	ii += 16;
Ucial	0:7365f8db1bac	4628	#ifdef FIFO_CORRUPTION_CHECK
Ucial	0:7365f8db1bac	4629	/* We can detect a corrupted FIFO by monitoring the quaternion data and
Ucial	0:7365f8db1bac	4630	* ensuring that the magnitude is always normalized to one. This
Ucial	0:7365f8db1bac	4631	* shouldn't happen in normal operation, but if an I2C error occurs,
Ucial	0:7365f8db1bac	4632	* the FIFO reads might become misaligned.
Ucial	0:7365f8db1bac	4633	*
Ucial	0:7365f8db1bac	4634	* Let's start by scaling down the quaternion data to avoid long long
Ucial	0:7365f8db1bac	4635	* math.
Ucial	0:7365f8db1bac	4636	*/
Ucial	0:7365f8db1bac	4637	quat_q14[0] = quat[0] >> 16;
Ucial	0:7365f8db1bac	4638	quat_q14[1] = quat[1] >> 16;
Ucial	0:7365f8db1bac	4639	quat_q14[2] = quat[2] >> 16;
Ucial	0:7365f8db1bac	4640	quat_q14[3] = quat[3] >> 16;
Ucial	0:7365f8db1bac	4641	quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] +
Ucial	0:7365f8db1bac	4642	quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];
Ucial	0:7365f8db1bac	4643	if ((quat_mag_sq < QUAT_MAG_SQ_MIN) \|\|
Ucial	0:7365f8db1bac	4644	(quat_mag_sq > QUAT_MAG_SQ_MAX)) {
Ucial	0:7365f8db1bac	4645	/* Quaternion is outside of the acceptable threshold. */
Ucial	0:7365f8db1bac	4646	mpu_reset_fifo();
Ucial	0:7365f8db1bac	4647	sensors[0] = 0;
Ucial	0:7365f8db1bac	4648	return -1;
Ucial	0:7365f8db1bac	4649	}
Ucial	0:7365f8db1bac	4650	sensors[0] \|= INV_WXYZ_QUAT;
Ucial	0:7365f8db1bac	4651	#endif
Ucial	0:7365f8db1bac	4652	}
Ucial	0:7365f8db1bac	4653
Ucial	0:7365f8db1bac	4654	if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) {
Ucial	0:7365f8db1bac	4655	accel[0] = ((short)fifo_data[ii+0] << 8) \| fifo_data[ii+1];
Ucial	0:7365f8db1bac	4656	accel[1] = ((short)fifo_data[ii+2] << 8) \| fifo_data[ii+3];
Ucial	0:7365f8db1bac	4657	accel[2] = ((short)fifo_data[ii+4] << 8) \| fifo_data[ii+5];
Ucial	0:7365f8db1bac	4658	ii += 6;
Ucial	0:7365f8db1bac	4659	sensors[0] \|= INV_XYZ_ACCEL;
Ucial	0:7365f8db1bac	4660	}
Ucial	0:7365f8db1bac	4661
Ucial	0:7365f8db1bac	4662	if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) {
Ucial	0:7365f8db1bac	4663	gyro[0] = ((short)fifo_data[ii+0] << 8) \| fifo_data[ii+1];
Ucial	0:7365f8db1bac	4664	gyro[1] = ((short)fifo_data[ii+2] << 8) \| fifo_data[ii+3];
Ucial	0:7365f8db1bac	4665	gyro[2] = ((short)fifo_data[ii+4] << 8) \| fifo_data[ii+5];
Ucial	0:7365f8db1bac	4666	ii += 6;
Ucial	0:7365f8db1bac	4667	sensors[0] \|= INV_XYZ_GYRO;
Ucial	0:7365f8db1bac	4668	}
Ucial	0:7365f8db1bac	4669
Ucial	0:7365f8db1bac	4670	/* Gesture data is at the end of the DMP packet. Parse it and call
Ucial	0:7365f8db1bac	4671	* the gesture callbacks (if registered).
Ucial	0:7365f8db1bac	4672	*/
Ucial	0:7365f8db1bac	4673	if (dmp.feature_mask & (DMP_FEATURE_TAP \| DMP_FEATURE_ANDROID_ORIENT))
Ucial	0:7365f8db1bac	4674	decode_gesture(fifo_data + ii);
Ucial	0:7365f8db1bac	4675
Ucial	0:7365f8db1bac	4676	get_ms(timestamp);
Ucial	0:7365f8db1bac	4677	return 0;
Ucial	0:7365f8db1bac	4678	}
Ucial	0:7365f8db1bac	4679
Ucial	0:7365f8db1bac	4680	/**
Ucial	0:7365f8db1bac	4681	* @brief Register a function to be executed on a tap event.
Ucial	0:7365f8db1bac	4682	* The tap direction is represented by one of the following:
Ucial	0:7365f8db1bac	4683	* \n TAP_X_UP
Ucial	0:7365f8db1bac	4684	* \n TAP_X_DOWN
Ucial	0:7365f8db1bac	4685	* \n TAP_Y_UP
Ucial	0:7365f8db1bac	4686	* \n TAP_Y_DOWN
Ucial	0:7365f8db1bac	4687	* \n TAP_Z_UP
Ucial	0:7365f8db1bac	4688	* \n TAP_Z_DOWN
Ucial	0:7365f8db1bac	4689	* @param[in] func Callback function.
Ucial	0:7365f8db1bac	4690	* @return 0 if successful.
Ucial	0:7365f8db1bac	4691	*/
Ucial	0:7365f8db1bac	4692	int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char))
Ucial	0:7365f8db1bac	4693	{
Ucial	0:7365f8db1bac	4694	dmp.tap_cb = func;
Ucial	0:7365f8db1bac	4695	return 0;
Ucial	0:7365f8db1bac	4696	}
Ucial	0:7365f8db1bac	4697
Ucial	0:7365f8db1bac	4698	/**
Ucial	0:7365f8db1bac	4699	* @brief Register a function to be executed on a android orientation event.
Ucial	0:7365f8db1bac	4700	* @param[in] func Callback function.
Ucial	0:7365f8db1bac	4701	* @return 0 if successful.
Ucial	0:7365f8db1bac	4702	*/
Ucial	0:7365f8db1bac	4703	int dmp_register_android_orient_cb(void (*func)(unsigned char))
Ucial	0:7365f8db1bac	4704	{
Ucial	0:7365f8db1bac	4705	dmp.android_orient_cb = func;
Ucial	0:7365f8db1bac	4706	return 0;
Ucial	0:7365f8db1bac	4707	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	26 Sep 2018
Imports:	9
Forks:	0
Commits:	3
Dependents:	0
Dependencies:	1
Followers:	1

mpu6050.cpp@1:23afaebeed15, 2018-08-31 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning