Balance_Car——code - 最终代码

Users » glintligo » Code » Balance_Car——code

Keegan Hu / Mbed 2 deprecated Balance_Car——code

最终代码

Fork of MPU6050_Driver_Balance by Chen Huan

mpu6050.cpp@0:badebd32bd8b, 2018-04-09 (annotated)

Committer:: heroistired
Date:: Mon Apr 09 14:34:45 2018 +0000
Revision:: 0:badebd32bd8b

????MPU6050?? ; C.H.

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	20 May 2018
Imports:	4
Forks:	0
Commits:	2
Dependents:	0
Dependencies:	1
Followers:	1

mpu6050.cpp@0:badebd32bd8b, 2018-04-09 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning