Flight control with nRF and MPU6050

Dependencies:   mbed

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

Who changed what in which revision?

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