1035 lines
27 KiB
C
1035 lines
27 KiB
C
#include "qmi8658.h"
|
||
|
||
//#define QMI8658_UINT_MG_DPS
|
||
#define M_PI (3.14159265358979323846f)
|
||
#define ONE_G (9.807f)
|
||
#define QFABS(x) (((x)<0.0f)?(-1.0f*(x)):(x))
|
||
|
||
|
||
static qmi8658_state g_imu;
|
||
#if defined(QMI8658_USE_CALI)
|
||
static qmi8658_cali g_cali;
|
||
#endif
|
||
|
||
unsigned char qmi8658_write_reg(unsigned char reg, unsigned char value)
|
||
{
|
||
unsigned char ret=0;
|
||
unsigned int retry = 0;
|
||
|
||
while((!ret) && (retry++ < 5))
|
||
{
|
||
#if defined(QMI8658_USE_SPI)
|
||
ret = qst_imu_spi_write(reg,value);
|
||
#elif defined(QST_USE_SW_I2C)
|
||
ret = qst_sw_writereg(g_imu.slave<<1, reg, value);
|
||
#else
|
||
ret = mx_i2c1_write(g_imu.slave<<1, reg, value);
|
||
#endif
|
||
}
|
||
return ret;
|
||
}
|
||
|
||
unsigned char qmi8658_write_regs(unsigned char reg, unsigned char *value, unsigned char len)
|
||
{
|
||
int i, ret;
|
||
|
||
for(i=0; i<len; i++)
|
||
{
|
||
#if defined(QMI8658_USE_SPI)
|
||
ret = qst_imu_spi_write_bytes(reg, value, len);
|
||
#elif defined(QST_USE_SW_I2C)
|
||
ret = qst_sw_writeregs(g_imu.slave<<1, reg, value, len);
|
||
#else
|
||
ret = I2C_BufferRead(g_imu.slave<<1, reg, value, len);
|
||
#endif
|
||
}
|
||
|
||
return ret;
|
||
}
|
||
|
||
unsigned char qmi8658_read_reg(unsigned char reg, unsigned char* buf, unsigned short len)
|
||
{
|
||
unsigned char ret=0;
|
||
unsigned int retry = 0;
|
||
|
||
while((!ret) && (retry++ < 5))
|
||
{
|
||
#if defined(QMI8658_USE_SPI)
|
||
ret = qst_8658_spi_read(reg, buf, len);
|
||
#elif defined(QST_USE_SW_I2C)
|
||
ret = qst_sw_readreg(g_imu.slave<<1, reg, buf, len);
|
||
#else
|
||
ret = mx_i2c1_read(g_imu.slave<<1, reg, buf, len);
|
||
#endif
|
||
}
|
||
return ret;
|
||
}
|
||
|
||
void qmi8658_delay(unsigned int ms)
|
||
{
|
||
extern void delay_ms(u32 ms);
|
||
|
||
delay_ms(ms);
|
||
}
|
||
|
||
void qmi8658_delay_us(unsigned int us)
|
||
{
|
||
extern void qst_delay_us(unsigned int delay);
|
||
|
||
qst_delay_us(us);
|
||
}
|
||
|
||
|
||
void qmi8658_axis_convert(float data_a[3], float data_g[3], int layout)
|
||
{
|
||
float raw[3],raw_g[3];
|
||
|
||
raw[0] = data_a[0];
|
||
raw[1] = data_a[1];
|
||
//raw[2] = data[2];
|
||
raw_g[0] = data_g[0];
|
||
raw_g[1] = data_g[1];
|
||
//raw_g[2] = data_g[2];
|
||
|
||
if(layout >=4 && layout <= 7)
|
||
{
|
||
data_a[2] = -data_a[2];
|
||
data_g[2] = -data_g[2];
|
||
}
|
||
|
||
if(layout%2)
|
||
{
|
||
data_a[0] = raw[1];
|
||
data_a[1] = raw[0];
|
||
|
||
data_g[0] = raw_g[1];
|
||
data_g[1] = raw_g[0];
|
||
}
|
||
else
|
||
{
|
||
data_a[0] = raw[0];
|
||
data_a[1] = raw[1];
|
||
|
||
data_g[0] = raw_g[0];
|
||
data_g[1] = raw_g[1];
|
||
}
|
||
|
||
if((layout==1)||(layout==2)||(layout==4)||(layout==7))
|
||
{
|
||
data_a[0] = -data_a[0];
|
||
data_g[0] = -data_g[0];
|
||
}
|
||
if((layout==2)||(layout==3)||(layout==6)||(layout==7))
|
||
{
|
||
data_a[1] = -data_a[1];
|
||
data_g[1] = -data_g[1];
|
||
}
|
||
}
|
||
|
||
#if defined(QMI8658_USE_CALI)
|
||
void qmi8658_data_cali(unsigned char sensor, float data[3])
|
||
{
|
||
float data_diff[3];
|
||
|
||
if(sensor == 1) // accel
|
||
{
|
||
data_diff[0] = QFABS((data[0]-g_cali.acc_last[0]));
|
||
data_diff[1] = QFABS((data[1]-g_cali.acc_last[1]));
|
||
data_diff[2] = QFABS((data[2]-g_cali.acc_last[2]));
|
||
g_cali.acc_last[0] = data[0];
|
||
g_cali.acc_last[1] = data[1];
|
||
g_cali.acc_last[2] = data[2];
|
||
|
||
// qmi8658_log("acc diff : %f ", (data_diff[0]+data_diff[1]+data_diff[2]));
|
||
if((data_diff[0]+data_diff[1]+data_diff[2]) < 0.5f)
|
||
{
|
||
if(g_cali.acc_cali_num == 0)
|
||
{
|
||
g_cali.acc_sum[0] = 0.0f;
|
||
g_cali.acc_sum[1] = 0.0f;
|
||
g_cali.acc_sum[2] = 0.0f;
|
||
}
|
||
if(g_cali.acc_cali_num < QMI8658_CALI_DATA_NUM)
|
||
{
|
||
g_cali.acc_cali_num++;
|
||
g_cali.acc_sum[0] += data[0];
|
||
g_cali.acc_sum[1] += data[1];
|
||
g_cali.acc_sum[2] += data[2];
|
||
if(g_cali.acc_cali_num == QMI8658_CALI_DATA_NUM)
|
||
{
|
||
if((g_cali.acc_cali_flag == 0)&&(data[2]<11.8f)&&(data[2]>7.8f))
|
||
{
|
||
g_cali.acc_sum[0] = g_cali.acc_sum[0]/QMI8658_CALI_DATA_NUM;
|
||
g_cali.acc_sum[1] = g_cali.acc_sum[1]/QMI8658_CALI_DATA_NUM;
|
||
g_cali.acc_sum[2] = g_cali.acc_sum[2]/QMI8658_CALI_DATA_NUM;
|
||
|
||
g_cali.acc_bias[0] = 0.0f - g_cali.acc_sum[0];
|
||
g_cali.acc_bias[1] = 0.0f - g_cali.acc_sum[1];
|
||
g_cali.acc_bias[2] = 9.807f - g_cali.acc_sum[2];
|
||
g_cali.acc_cali_flag = 1;
|
||
}
|
||
g_cali.imu_static_flag = 1;
|
||
qmi8658_log("qmi8658 acc static!!!\n");
|
||
}
|
||
}
|
||
|
||
if(g_cali.imu_static_flag)
|
||
{
|
||
if(g_cali.acc_fix_flag == 0)
|
||
{
|
||
g_cali.acc_fix_flag = 1;
|
||
g_cali.acc_fix[0] = data[0];
|
||
g_cali.acc_fix[1] = data[1];
|
||
g_cali.acc_fix[2] = data[2];
|
||
}
|
||
}
|
||
else
|
||
{
|
||
g_cali.acc_fix_flag = 0;
|
||
g_cali.gyr_fix_flag = 0;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
g_cali.acc_cali_num = 0;
|
||
g_cali.acc_sum[0] = 0.0f;
|
||
g_cali.acc_sum[1] = 0.0f;
|
||
g_cali.acc_sum[2] = 0.0f;
|
||
|
||
g_cali.imu_static_flag = 0;
|
||
g_cali.acc_fix_flag = 0;
|
||
g_cali.gyr_fix_flag = 0;
|
||
}
|
||
|
||
if(g_cali.acc_fix_flag)
|
||
{
|
||
if(g_cali.acc_fix_index != 0)
|
||
g_cali.acc_fix_index = 0;
|
||
else
|
||
g_cali.acc_fix_index = 1;
|
||
|
||
data[0] = g_cali.acc_fix[0] + g_cali.acc_fix_index*0.01f;
|
||
data[1] = g_cali.acc_fix[1] + g_cali.acc_fix_index*0.01f;
|
||
data[2] = g_cali.acc_fix[2] + g_cali.acc_fix_index*0.01f;
|
||
}
|
||
if(g_cali.acc_cali_flag)
|
||
{
|
||
g_cali.acc[0] = data[0] + g_cali.acc_bias[0];
|
||
g_cali.acc[1] = data[1] + g_cali.acc_bias[1];
|
||
g_cali.acc[2] = data[2] + g_cali.acc_bias[2];
|
||
data[0] = g_cali.acc[0];
|
||
data[1] = g_cali.acc[1];
|
||
data[2] = g_cali.acc[2];
|
||
}
|
||
else
|
||
{
|
||
g_cali.acc[0] = data[0];
|
||
g_cali.acc[1] = data[1];
|
||
g_cali.acc[2] = data[2];
|
||
}
|
||
}
|
||
else if(sensor == 2) // gyroscope
|
||
{
|
||
data_diff[0] = QFABS((data[0]-g_cali.gyr_last[0]));
|
||
data_diff[1] = QFABS((data[1]-g_cali.gyr_last[1]));
|
||
data_diff[2] = QFABS((data[2]-g_cali.gyr_last[2]));
|
||
g_cali.gyr_last[0] = data[0];
|
||
g_cali.gyr_last[1] = data[1];
|
||
g_cali.gyr_last[2] = data[2];
|
||
|
||
// qmi8658_log("gyr diff : %f \n", (data_diff[0]+data_diff[1]+data_diff[2]));
|
||
if(((data_diff[0]+data_diff[1]+data_diff[2]) < 0.03f)
|
||
&& ((data[0]>-1.0f)&&(data[0]<1.0f))
|
||
&& ((data[1]>-1.0f)&&(data[1]<1.0f))
|
||
&& ((data[2]>-1.0f)&&(data[2]<1.0f))
|
||
)
|
||
{
|
||
if(g_cali.gyr_cali_num == 0)
|
||
{
|
||
g_cali.gyr_sum[0] = 0.0f;
|
||
g_cali.gyr_sum[1] = 0.0f;
|
||
g_cali.gyr_sum[2] = 0.0f;
|
||
}
|
||
if(g_cali.gyr_cali_num < QMI8658_CALI_DATA_NUM)
|
||
{
|
||
g_cali.gyr_cali_num++;
|
||
g_cali.gyr_sum[0] += data[0];
|
||
g_cali.gyr_sum[1] += data[1];
|
||
g_cali.gyr_sum[2] += data[2];
|
||
if(g_cali.gyr_cali_num == QMI8658_CALI_DATA_NUM)
|
||
{
|
||
if(g_cali.gyr_cali_flag == 0)
|
||
{
|
||
g_cali.gyr_sum[0] = g_cali.gyr_sum[0]/QMI8658_CALI_DATA_NUM;
|
||
g_cali.gyr_sum[1] = g_cali.gyr_sum[1]/QMI8658_CALI_DATA_NUM;
|
||
g_cali.gyr_sum[2] = g_cali.gyr_sum[2]/QMI8658_CALI_DATA_NUM;
|
||
|
||
g_cali.gyr_bias[0] = 0.0f - g_cali.gyr_sum[0];
|
||
g_cali.gyr_bias[1] = 0.0f - g_cali.gyr_sum[1];
|
||
g_cali.gyr_bias[2] = 0.0f - g_cali.gyr_sum[2];
|
||
g_cali.gyr_cali_flag = 1;
|
||
}
|
||
g_cali.imu_static_flag = 1;
|
||
qmi8658_log("qmi8658 gyro static!!!\n");
|
||
}
|
||
}
|
||
|
||
if(g_cali.imu_static_flag)
|
||
{
|
||
if(g_cali.gyr_fix_flag == 0)
|
||
{
|
||
g_cali.gyr_fix_flag = 1;
|
||
g_cali.gyr_fix[0] = data[0];
|
||
g_cali.gyr_fix[1] = data[1];
|
||
g_cali.gyr_fix[2] = data[2];
|
||
}
|
||
}
|
||
else
|
||
{
|
||
g_cali.gyr_fix_flag = 0;
|
||
g_cali.acc_fix_flag = 0;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
g_cali.gyr_cali_num = 0;
|
||
g_cali.gyr_sum[0] = 0.0f;
|
||
g_cali.gyr_sum[1] = 0.0f;
|
||
g_cali.gyr_sum[2] = 0.0f;
|
||
|
||
g_cali.imu_static_flag = 0;
|
||
g_cali.gyr_fix_flag = 0;
|
||
g_cali.acc_fix_flag = 0;
|
||
}
|
||
|
||
if(g_cali.gyr_fix_flag)
|
||
{
|
||
if(g_cali.gyr_fix_index != 0)
|
||
g_cali.gyr_fix_index = 0;
|
||
else
|
||
g_cali.gyr_fix_index = 1;
|
||
|
||
data[0] = g_cali.gyr_fix[0] + g_cali.gyr_fix_index*0.00005f;
|
||
data[1] = g_cali.gyr_fix[1] + g_cali.gyr_fix_index*0.00005f;
|
||
data[2] = g_cali.gyr_fix[2] + g_cali.gyr_fix_index*0.00005f;
|
||
}
|
||
|
||
if(g_cali.gyr_cali_flag)
|
||
{
|
||
g_cali.gyr[0] = data[0] + g_cali.gyr_bias[0];
|
||
g_cali.gyr[1] = data[1] + g_cali.gyr_bias[1];
|
||
g_cali.gyr[2] = data[2] + g_cali.gyr_bias[2];
|
||
data[0] = g_cali.gyr[0];
|
||
data[1] = g_cali.gyr[1];
|
||
data[2] = g_cali.gyr[2];
|
||
}
|
||
else
|
||
{
|
||
g_cali.gyr[0] = data[0];
|
||
g_cali.gyr[1] = data[1];
|
||
g_cali.gyr[2] = data[2];
|
||
}
|
||
}
|
||
}
|
||
|
||
#endif
|
||
|
||
void qmi8658_config_acc(enum qmi8658_AccRange range, enum qmi8658_AccOdr odr, enum qmi8658_LpfConfig lpfEnable, enum qmi8658_StConfig stEnable)
|
||
{
|
||
unsigned char ctl_dada;
|
||
|
||
switch(range)
|
||
{
|
||
case Qmi8658AccRange_2g:
|
||
g_imu.ssvt_a = (1<<14);
|
||
break;
|
||
case Qmi8658AccRange_4g:
|
||
g_imu.ssvt_a = (1<<13);
|
||
break;
|
||
case Qmi8658AccRange_8g:
|
||
g_imu.ssvt_a = (1<<12);
|
||
break;
|
||
case Qmi8658AccRange_16g:
|
||
g_imu.ssvt_a = (1<<11);
|
||
break;
|
||
default:
|
||
range = Qmi8658AccRange_8g;
|
||
g_imu.ssvt_a = (1<<12);
|
||
}
|
||
if(stEnable == Qmi8658St_Enable)
|
||
ctl_dada = (unsigned char)range|(unsigned char)odr|0x80;
|
||
else
|
||
ctl_dada = (unsigned char)range|(unsigned char)odr;
|
||
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl2, ctl_dada);
|
||
// set LPF & HPF
|
||
qmi8658_read_reg(Qmi8658Register_Ctrl5, &ctl_dada, 1);
|
||
ctl_dada &= 0xf0;
|
||
if(lpfEnable == Qmi8658Lpf_Enable)
|
||
{
|
||
ctl_dada |= A_LSP_MODE_3;
|
||
ctl_dada |= 0x01;
|
||
}
|
||
else
|
||
{
|
||
ctl_dada &= ~0x01;
|
||
}
|
||
//ctl_dada = 0x00;
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl5,ctl_dada);
|
||
// set LPF & HPF
|
||
}
|
||
|
||
void qmi8658_config_gyro(enum qmi8658_GyrRange range, enum qmi8658_GyrOdr odr, enum qmi8658_LpfConfig lpfEnable, enum qmi8658_StConfig stEnable)
|
||
{
|
||
// Set the CTRL3 register to configure dynamic range and ODR
|
||
unsigned char ctl_dada;
|
||
|
||
// Store the scale factor for use when processing raw data
|
||
switch (range)
|
||
{
|
||
case Qmi8658GyrRange_16dps:
|
||
g_imu.ssvt_g = 2048;
|
||
break;
|
||
case Qmi8658GyrRange_32dps:
|
||
g_imu.ssvt_g = 1024;
|
||
break;
|
||
case Qmi8658GyrRange_64dps:
|
||
g_imu.ssvt_g = 512;
|
||
break;
|
||
case Qmi8658GyrRange_128dps:
|
||
g_imu.ssvt_g = 256;
|
||
break;
|
||
case Qmi8658GyrRange_256dps:
|
||
g_imu.ssvt_g = 128;
|
||
break;
|
||
case Qmi8658GyrRange_512dps:
|
||
g_imu.ssvt_g = 64;
|
||
break;
|
||
case Qmi8658GyrRange_1024dps:
|
||
g_imu.ssvt_g = 32;
|
||
break;
|
||
case Qmi8658GyrRange_2048dps:
|
||
g_imu.ssvt_g = 16;
|
||
break;
|
||
// case Qmi8658GyrRange_4096dps:
|
||
// g_imu.ssvt_g = 8;
|
||
// break;
|
||
default:
|
||
range = Qmi8658GyrRange_512dps;
|
||
g_imu.ssvt_g = 64;
|
||
break;
|
||
}
|
||
|
||
if(stEnable == Qmi8658St_Enable)
|
||
ctl_dada = (unsigned char)range|(unsigned char)odr|0x80;
|
||
else
|
||
ctl_dada = (unsigned char)range | (unsigned char)odr;
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl3, ctl_dada);
|
||
|
||
// Conversion from degrees/s to rad/s if necessary
|
||
// set LPF & HPF
|
||
qmi8658_read_reg(Qmi8658Register_Ctrl5, &ctl_dada,1);
|
||
ctl_dada &= 0x0f;
|
||
if(lpfEnable == Qmi8658Lpf_Enable)
|
||
{
|
||
ctl_dada |= G_LSP_MODE_3;
|
||
ctl_dada |= 0x10;
|
||
}
|
||
else
|
||
{
|
||
ctl_dada &= ~0x10;
|
||
}
|
||
//ctl_dada = 0x00;
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl5,ctl_dada);
|
||
// set LPF & HPF
|
||
}
|
||
|
||
|
||
void qmi8658_send_ctl9cmd(enum qmi8658_Ctrl9Command cmd)
|
||
{
|
||
unsigned char status1 = 0x00;
|
||
unsigned short count=0;
|
||
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl9, (unsigned char)cmd); // write commond to ctrl9
|
||
#if 1 //defined(QMI8658_NEW_FIRMWARE)
|
||
unsigned char status_reg = Qmi8658Register_StatusInt;
|
||
unsigned char cmd_done = 0x80;
|
||
//unsigned char status_reg = Qmi8658Register_Status1;
|
||
//unsigned char cmd_done = 0x01;
|
||
|
||
qmi8658_read_reg(status_reg, &status1, 1);
|
||
while(((status1&cmd_done)!=cmd_done)&&(count++<100)) // read statusINT until bit7 is 1
|
||
{
|
||
qmi8658_delay(1);
|
||
qmi8658_read_reg(status_reg, &status1, 1);
|
||
}
|
||
//qmi8658_log("ctrl9 cmd done1 count=%d\n",count);
|
||
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl9, qmi8658_Ctrl9_Cmd_NOP); // write commond 0x00 to ctrl9
|
||
count = 0;
|
||
qmi8658_read_reg(status_reg, &status1, 1);
|
||
while(((status1&cmd_done)==cmd_done)&&(count++<100)) // read statusINT until bit7 is 0
|
||
{
|
||
qmi8658_delay(1); // 1 ms
|
||
qmi8658_read_reg(status_reg, &status1, 1);
|
||
}
|
||
//qmi8658_log("ctrl9 cmd done2 count=%d\n",count);
|
||
#else
|
||
while(((status1&QMI8658_STATUS1_CMD_DONE)==0)&&(count++<100))
|
||
{
|
||
qmi8658_delay(1);
|
||
qmi8658_read_reg(Qmi8658Register_Status1, &status1, sizeof(status1));
|
||
}
|
||
#endif
|
||
|
||
}
|
||
|
||
unsigned char qmi8658_readStatusInt(void)
|
||
{
|
||
unsigned char status_int;
|
||
|
||
qmi8658_read_reg(Qmi8658Register_StatusInt, &status_int, 1);
|
||
|
||
return status_int;
|
||
}
|
||
|
||
unsigned char qmi8658_readStatus0(void)
|
||
{
|
||
unsigned char status0;
|
||
|
||
qmi8658_read_reg(Qmi8658Register_Status0, &status0, 1);
|
||
|
||
return status0;
|
||
}
|
||
|
||
unsigned char qmi8658_readStatus1(void)
|
||
{
|
||
unsigned char status1;
|
||
|
||
qmi8658_read_reg(Qmi8658Register_Status1, &status1, 1);
|
||
|
||
return status1;
|
||
}
|
||
|
||
float qmi8658_readTemp(void)
|
||
{
|
||
unsigned char buf[2];
|
||
short temp = 0;
|
||
float temp_f = 0;
|
||
|
||
qmi8658_read_reg(Qmi8658Register_Tempearture_L, buf, 2);
|
||
temp = ((short)buf[1]<<8)|buf[0];
|
||
temp_f = (float)temp/256.0f;
|
||
|
||
return temp_f;
|
||
}
|
||
|
||
void qmi8658_read_timestamp(unsigned int *tim_count)
|
||
{
|
||
unsigned char buf[3];
|
||
unsigned int timestamp;
|
||
|
||
if(tim_count)
|
||
{
|
||
qmi8658_read_reg(Qmi8658Register_Timestamp_L, buf, 3);
|
||
timestamp = (unsigned int)(((unsigned int)buf[2]<<16)|((unsigned int)buf[1]<<8)|buf[0]);
|
||
if(timestamp > g_imu.timestamp)
|
||
g_imu.timestamp = timestamp;
|
||
else
|
||
g_imu.timestamp = (timestamp+0x1000000-g_imu.timestamp);
|
||
|
||
*tim_count = g_imu.timestamp;
|
||
}
|
||
}
|
||
|
||
void qmi8658_read_sensor_data(float acc[3], float gyro[3])
|
||
{
|
||
unsigned char buf_reg[12];
|
||
short raw_acc_xyz[3];
|
||
short raw_gyro_xyz[3];
|
||
|
||
qmi8658_read_reg(Qmi8658Register_Ax_L, buf_reg, 12);
|
||
raw_acc_xyz[0] = (short)((unsigned short)(buf_reg[1]<<8) |( buf_reg[0]));
|
||
raw_acc_xyz[1] = (short)((unsigned short)(buf_reg[3]<<8) |( buf_reg[2]));
|
||
raw_acc_xyz[2] = (short)((unsigned short)(buf_reg[5]<<8) |( buf_reg[4]));
|
||
|
||
raw_gyro_xyz[0] = (short)((unsigned short)(buf_reg[7]<<8) |( buf_reg[6]));
|
||
raw_gyro_xyz[1] = (short)((unsigned short)(buf_reg[9]<<8) |( buf_reg[8]));
|
||
raw_gyro_xyz[2] = (short)((unsigned short)(buf_reg[11]<<8) |( buf_reg[10]));
|
||
|
||
#if defined(QMI8658_UINT_MG_DPS)
|
||
// mg
|
||
acc[0] = (float)(raw_acc_xyz[0]*1000.0f)/g_imu.ssvt_a;
|
||
acc[1] = (float)(raw_acc_xyz[1]*1000.0f)/g_imu.ssvt_a;
|
||
acc[2] = (float)(raw_acc_xyz[2]*1000.0f)/g_imu.ssvt_a;
|
||
#else
|
||
// m/s2
|
||
acc[0] = (float)(raw_acc_xyz[0]*ONE_G)/g_imu.ssvt_a;
|
||
acc[1] = (float)(raw_acc_xyz[1]*ONE_G)/g_imu.ssvt_a;
|
||
acc[2] = (float)(raw_acc_xyz[2]*ONE_G)/g_imu.ssvt_a;
|
||
#endif
|
||
|
||
#if defined(QMI8658_UINT_MG_DPS)
|
||
// dps
|
||
gyro[0] = (float)(raw_gyro_xyz[0]*1.0f)/g_imu.ssvt_g;
|
||
gyro[1] = (float)(raw_gyro_xyz[1]*1.0f)/g_imu.ssvt_g;
|
||
gyro[2] = (float)(raw_gyro_xyz[2]*1.0f)/g_imu.ssvt_g;
|
||
#else
|
||
// rad/s
|
||
gyro[0] = (float)(raw_gyro_xyz[0]*M_PI)/(g_imu.ssvt_g*180); // *pi/180
|
||
gyro[1] = (float)(raw_gyro_xyz[1]*M_PI)/(g_imu.ssvt_g*180);
|
||
gyro[2] = (float)(raw_gyro_xyz[2]*M_PI)/(g_imu.ssvt_g*180);
|
||
#endif
|
||
}
|
||
|
||
void qmi8658_read_xyz(float acc[3], float gyro[3])
|
||
{
|
||
unsigned char status;
|
||
unsigned char data_ready = 0;
|
||
|
||
#if defined(QMI8658_SYNC_SAMPLE_MODE)
|
||
qmi8658_read_reg(Qmi8658Register_StatusInt, &status, 1);
|
||
if(status&0x01)
|
||
{
|
||
data_ready = 1;
|
||
qmi8658_delay_us(6); // delay 6us
|
||
}
|
||
#else
|
||
qmi8658_read_reg(Qmi8658Register_Status0, &status, 1);
|
||
if(status&0x03)
|
||
{
|
||
data_ready = 1;
|
||
}
|
||
#endif
|
||
if(data_ready)
|
||
{
|
||
qmi8658_read_sensor_data(acc, gyro);
|
||
qmi8658_axis_convert(acc, gyro, 0);
|
||
#if defined(QMI8658_USE_CALI)
|
||
qmi8658_data_cali(1, acc);
|
||
qmi8658_data_cali(2, gyro);
|
||
#endif
|
||
g_imu.imu[0] = acc[0];
|
||
g_imu.imu[1] = acc[1];
|
||
g_imu.imu[2] = acc[2];
|
||
g_imu.imu[3] = gyro[0];
|
||
g_imu.imu[4] = gyro[1];
|
||
g_imu.imu[5] = gyro[2];
|
||
}
|
||
else
|
||
{
|
||
acc[0] = g_imu.imu[0];
|
||
acc[1] = g_imu.imu[1];
|
||
acc[2] = g_imu.imu[2];
|
||
gyro[0] = g_imu.imu[3];
|
||
gyro[1] = g_imu.imu[4];
|
||
gyro[2] = g_imu.imu[5];
|
||
qmi8658_log("data ready fail!\n");
|
||
}
|
||
}
|
||
|
||
|
||
void qmi8658_enableSensors(unsigned char enableFlags)
|
||
{
|
||
#if defined(QMI8658_SYNC_SAMPLE_MODE)
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl7, enableFlags | 0x80);
|
||
#elif defined(QMI8658_USE_FIFO)
|
||
//qmi8658_write_reg(Qmi8658Register_Ctrl7, enableFlags|QMI8658_DRDY_DISABLE);
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl7, enableFlags);
|
||
#else
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl7, enableFlags);
|
||
#endif
|
||
g_imu.cfg.enSensors = enableFlags&0x03;
|
||
|
||
qmi8658_delay(1);
|
||
}
|
||
|
||
void qmi8658_dump_reg(void)
|
||
{
|
||
unsigned char read_data[8];
|
||
|
||
qmi8658_read_reg(Qmi8658Register_Ctrl1, read_data, 8);
|
||
qmi8658_log("Ctrl1[0x%x]\nCtrl2[0x%x]\nCtrl3[0x%x]\nCtrl4[0x%x]\nCtrl5[0x%x]\nCtrl6[0x%x]\nCtrl7[0x%x]\nCtrl8[0x%x]\n",
|
||
read_data[0],read_data[1],read_data[2],read_data[3],read_data[4],read_data[5],read_data[6],read_data[7]);
|
||
}
|
||
|
||
//void qmi8658_soft_reset(void)
|
||
//{
|
||
// qmi8658_log("qmi8658_soft_reset \n");
|
||
// qmi8658_write_reg(Qmi8658Register_Reset, 0xb0);
|
||
// qmi8658_delay(2000);
|
||
// qmi8658_write_reg(Qmi8658Register_Reset, 0x00);
|
||
// qmi8658_delay(5);
|
||
//}
|
||
|
||
void qmi8658_on_demand_cali(void)
|
||
{
|
||
qmi8658_log("qmi8658_on_demand_cali start\n");
|
||
qmi8658_write_reg(Qmi8658Register_Reset, 0xb0);
|
||
qmi8658_delay(10); // delay
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl9, (unsigned char)qmi8658_Ctrl9_Cmd_On_Demand_Cali);
|
||
qmi8658_delay(2200); // delay 2000ms above
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl9, (unsigned char)qmi8658_Ctrl9_Cmd_NOP);
|
||
qmi8658_delay(100); // delay
|
||
qmi8658_log("qmi8658_on_demand_cali done\n");
|
||
}
|
||
|
||
void qmi8658_config_reg(unsigned char low_power)
|
||
{
|
||
qmi8658_enableSensors(QMI8658_DISABLE_ALL);
|
||
if(low_power)
|
||
{
|
||
g_imu.cfg.enSensors = QMI8658_ACC_ENABLE;
|
||
g_imu.cfg.accRange = Qmi8658AccRange_8g;
|
||
g_imu.cfg.accOdr = Qmi8658AccOdr_LowPower_21Hz;
|
||
g_imu.cfg.gyrRange = Qmi8658GyrRange_1024dps;
|
||
g_imu.cfg.gyrOdr = Qmi8658GyrOdr_250Hz;
|
||
}
|
||
else
|
||
{
|
||
g_imu.cfg.enSensors = QMI8658_ACCGYR_ENABLE;
|
||
g_imu.cfg.accRange = Qmi8658AccRange_8g;
|
||
g_imu.cfg.accOdr = Qmi8658AccOdr_250Hz;
|
||
g_imu.cfg.gyrRange = Qmi8658GyrRange_1024dps;
|
||
g_imu.cfg.gyrOdr = Qmi8658GyrOdr_250Hz;
|
||
}
|
||
|
||
if(g_imu.cfg.enSensors & QMI8658_ACC_ENABLE)
|
||
{
|
||
qmi8658_config_acc(g_imu.cfg.accRange, g_imu.cfg.accOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable);
|
||
}
|
||
if(g_imu.cfg.enSensors & QMI8658_GYR_ENABLE)
|
||
{
|
||
qmi8658_config_gyro(g_imu.cfg.gyrRange, g_imu.cfg.gyrOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable);
|
||
}
|
||
}
|
||
|
||
|
||
unsigned char qmi8658_get_id(void)
|
||
{
|
||
unsigned char qmi8658_chip_id = 0x00;
|
||
unsigned char qmi8658_revision_id = 0x00;
|
||
unsigned char qmi8658_slave[2] = {QMI8658_SLAVE_ADDR_L, QMI8658_SLAVE_ADDR_H};
|
||
int retry = 0;
|
||
unsigned char iCount = 0;
|
||
unsigned char firmware_id[3];
|
||
unsigned char uuid[6];
|
||
unsigned int uuid_low, uuid_high;
|
||
|
||
while(iCount<2)
|
||
{
|
||
g_imu.slave = qmi8658_slave[iCount];
|
||
retry = 0;
|
||
while((qmi8658_chip_id != 0x05)&&(retry++ < 5))
|
||
{
|
||
qmi8658_read_reg(Qmi8658Register_WhoAmI, &qmi8658_chip_id, 1);
|
||
qmi8658_log("Qmi8658Register_WhoAmI = 0x%x\n", qmi8658_chip_id);
|
||
}
|
||
if(qmi8658_chip_id == 0x05)
|
||
{
|
||
qmi8658_on_demand_cali();
|
||
|
||
g_imu.cfg.ctrl8_value = 0xc0;
|
||
//QMI8658_INT1_ENABLE, QMI8658_INT2_ENABLE
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl1, 0x60|QMI8658_INT2_ENABLE|QMI8658_INT1_ENABLE);
|
||
qmi8658_read_reg(Qmi8658Register_Revision, &qmi8658_revision_id, 1);
|
||
qmi8658_read_reg(Qmi8658Register_firmware_id, firmware_id, 3);
|
||
qmi8658_read_reg(Qmi8658Register_uuid, uuid, 6);
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl7, 0x00);
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl8, g_imu.cfg.ctrl8_value);
|
||
uuid_low = (unsigned int)((unsigned int)(uuid[2]<<16)|(unsigned int)(uuid[1]<<8)|(uuid[0]));
|
||
uuid_high = (unsigned int)((unsigned int)(uuid[5]<<16)|(unsigned int)(uuid[4]<<8)|(uuid[3]));
|
||
qmi8658_log("qmi8658_init slave=0x%x Revision=0x%x\n", g_imu.slave, qmi8658_revision_id);
|
||
qmi8658_log("Firmware ID[0x%x 0x%x 0x%x]\n", firmware_id[2], firmware_id[1],firmware_id[0]);
|
||
qmi8658_log("UUID[0x%x %x]\n", uuid_high ,uuid_low);
|
||
|
||
break;
|
||
}
|
||
iCount++;
|
||
}
|
||
|
||
return qmi8658_chip_id;
|
||
}
|
||
|
||
#if defined(QMI8658_USE_AMD)
|
||
void qmi8658_config_amd(void)
|
||
{
|
||
g_imu.cfg.ctrl8_value &= (~QMI8658_CTRL8_ANYMOTION_EN);
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl8, g_imu.cfg.ctrl8_value);
|
||
|
||
qmi8658_write_reg(Qmi8658Register_Cal1_L, 0x03); // any motion X threshold U 3.5 first three bit(uint 1g) last five bit (uint 1/32 g)
|
||
qmi8658_write_reg(Qmi8658Register_Cal1_H, 0x03); // any motion Y threshold U 3.5 first three bit(uint 1g) last five bit (uint 1/32 g)
|
||
qmi8658_write_reg(Qmi8658Register_Cal2_L, 0x03); // any motion Z threshold U 3.5 first three bit(uint 1g) last five bit (uint 1/32 g)
|
||
qmi8658_write_reg(Qmi8658Register_Cal2_H, 0x02); // no motion X threshold U 3.5 first three bit(uint 1g) last five bit (uint 1/32 g)
|
||
qmi8658_write_reg(Qmi8658Register_Cal3_L, 0x02);
|
||
qmi8658_write_reg(Qmi8658Register_Cal3_H, 0x02);
|
||
|
||
qmi8658_write_reg(Qmi8658Register_Cal4_L, 0xf7); // MOTION_MODE_CTRL
|
||
qmi8658_write_reg(Qmi8658Register_Cal4_H, 0x01); // value 0x01
|
||
|
||
qmi8658_send_ctl9cmd(qmi8658_Ctrl9_Cmd_Motion);
|
||
|
||
qmi8658_write_reg(Qmi8658Register_Cal1_L, 0x03); // AnyMotionWindow.
|
||
qmi8658_write_reg(Qmi8658Register_Cal1_H, 0x01); // NoMotionWindow
|
||
qmi8658_write_reg(Qmi8658Register_Cal2_L, 0x2c); // SigMotionWaitWindow[7:0]
|
||
qmi8658_write_reg(Qmi8658Register_Cal2_H, 0x01); // SigMotionWaitWindow [15:8]
|
||
qmi8658_write_reg(Qmi8658Register_Cal3_L, 0x64); // SigMotionConfirmWindow[7:0]
|
||
qmi8658_write_reg(Qmi8658Register_Cal3_H, 0x00); // SigMotionConfirmWindow[15:8]
|
||
//qmi8658_write_reg(Qmi8658Register_Cal4_L, 0xf7);
|
||
qmi8658_write_reg(Qmi8658Register_Cal4_H, 0x02); // value 0x02
|
||
|
||
qmi8658_send_ctl9cmd(qmi8658_Ctrl9_Cmd_Motion);
|
||
}
|
||
|
||
void qmi8658_enable_amd(unsigned char enable, enum qmi8658_Interrupt int_map, unsigned char low_power)
|
||
{
|
||
if(int_map == qmi8658_Int1)
|
||
{
|
||
g_imu.cfg.ctrl8_value &= (~QMI8658_CTRL8_ANYMOTION_EN);
|
||
g_imu.cfg.ctrl8_value |= QMI8658_CTRL8_DATAVALID_EN;
|
||
}
|
||
else if(int_map == qmi8658_Int2)
|
||
{
|
||
g_imu.cfg.ctrl8_value &= (~QMI8658_CTRL8_ANYMOTION_EN);
|
||
g_imu.cfg.ctrl8_value &= (~QMI8658_CTRL8_DATAVALID_EN);
|
||
}
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl8, g_imu.cfg.ctrl8_value);
|
||
qmi8658_delay(2);
|
||
|
||
if(enable)
|
||
{
|
||
unsigned char ctrl1;
|
||
|
||
qmi8658_enableSensors(QMI8658_DISABLE_ALL);
|
||
qmi8658_config_reg(low_power);
|
||
|
||
qmi8658_read_reg(Qmi8658Register_Ctrl1, &ctrl1, 1);
|
||
if(int_map == qmi8658_Int1)
|
||
{
|
||
ctrl1 |= QMI8658_INT1_ENABLE;
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl1, ctrl1);// enable int for dev-E
|
||
}
|
||
else if(int_map == qmi8658_Int2)
|
||
{
|
||
ctrl1 |= QMI8658_INT2_ENABLE;
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl1, ctrl1);// enable int for dev-E
|
||
}
|
||
g_imu.cfg.ctrl8_value |= QMI8658_CTRL8_ANYMOTION_EN;
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl8, g_imu.cfg.ctrl8_value);
|
||
|
||
qmi8658_delay(1);
|
||
qmi8658_enableSensors(g_imu.cfg.enSensors);
|
||
}
|
||
else
|
||
{
|
||
|
||
}
|
||
}
|
||
#endif
|
||
|
||
#if defined(QMI8658_USE_PEDOMETER)
|
||
void qmi8658_config_pedometer(unsigned short odr)
|
||
{
|
||
float finalRate = (float)(200.0f/odr); //14.285
|
||
unsigned short ped_sample_cnt = (unsigned short)(0x0032 / finalRate);//6;//(unsigned short)(0x0032 / finalRate) ;
|
||
unsigned short ped_fix_peak2peak = 0x00AC;//0x0006;//0x00CC;
|
||
unsigned short ped_fix_peak = 0x00AC;//0x0006;//0x00CC;
|
||
unsigned short ped_time_up = (unsigned short)(200 / finalRate);
|
||
unsigned char ped_time_low = (unsigned char) (20 / finalRate);
|
||
unsigned char ped_time_cnt_entry = 8;
|
||
unsigned char ped_fix_precision = 0;
|
||
unsigned char ped_sig_count = 1;//<2F>Ʋ<EFBFBD><C6B2><EFBFBD><EFBFBD><EFBFBD>1
|
||
|
||
qmi8658_write_reg(Qmi8658Register_Cal1_L, ped_sample_cnt & 0xFF);
|
||
qmi8658_write_reg(Qmi8658Register_Cal1_H, (ped_sample_cnt >> 8) & 0xFF);
|
||
qmi8658_write_reg(Qmi8658Register_Cal2_L, ped_fix_peak2peak & 0xFF);
|
||
qmi8658_write_reg(Qmi8658Register_Cal2_H, (ped_fix_peak2peak >> 8) & 0xFF);
|
||
qmi8658_write_reg(Qmi8658Register_Cal3_L, ped_fix_peak & 0xFF);
|
||
qmi8658_write_reg(Qmi8658Register_Cal3_H, (ped_fix_peak >> 8) & 0xFF);
|
||
qmi8658_write_reg(Qmi8658Register_Cal4_H, 0x01);
|
||
qmi8658_write_reg(Qmi8658Register_Cal4_L, 0x02);
|
||
qmi8658_send_ctl9cmd(qmi8658_Ctrl9_Cmd_EnablePedometer);
|
||
|
||
qmi8658_write_reg(Qmi8658Register_Cal1_L, ped_time_up & 0xFF);
|
||
qmi8658_write_reg(Qmi8658Register_Cal1_H, (ped_time_up >> 8) & 0xFF);
|
||
qmi8658_write_reg(Qmi8658Register_Cal2_L, ped_time_low);
|
||
qmi8658_write_reg(Qmi8658Register_Cal2_H, ped_time_cnt_entry);
|
||
qmi8658_write_reg(Qmi8658Register_Cal3_L, ped_fix_precision);
|
||
qmi8658_write_reg(Qmi8658Register_Cal3_H, ped_sig_count);
|
||
qmi8658_write_reg(Qmi8658Register_Cal4_H, 0x02);
|
||
qmi8658_write_reg(Qmi8658Register_Cal4_L, 0x02);
|
||
qmi8658_send_ctl9cmd(qmi8658_Ctrl9_Cmd_EnablePedometer);
|
||
}
|
||
|
||
void qmi8658_enable_pedometer(unsigned char enable)
|
||
{
|
||
if(enable)
|
||
{
|
||
g_imu.cfg.ctrl8_value |= QMI8658_CTRL8_PEDOMETER_EN;
|
||
}
|
||
else
|
||
{
|
||
g_imu.cfg.ctrl8_value &= (~QMI8658_CTRL8_PEDOMETER_EN);
|
||
}
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl8, g_imu.cfg.ctrl8_value);
|
||
}
|
||
|
||
unsigned int qmi8658_read_pedometer(void)
|
||
{
|
||
unsigned char buf[3];
|
||
|
||
qmi8658_read_reg(Qmi8658Register_Pedo_L, buf, 3); // 0x5a
|
||
g_imu.step = (unsigned int)((buf[2]<<16)|(buf[1]<<8)|(buf[0]));
|
||
|
||
return g_imu.step;
|
||
}
|
||
#endif
|
||
|
||
#if defined(QMI8658_USE_FIFO)
|
||
void qmi8658_config_fifo(unsigned char watermark,enum qmi8658_FifoSize size,enum qmi8658_FifoMode mode,enum qmi8658_Interrupt int_map)
|
||
{
|
||
unsigned char ctrl1;
|
||
|
||
qmi8658_enableSensors(QMI8658_DISABLE_ALL);
|
||
qmi8658_read_reg(Qmi8658Register_Ctrl1, &ctrl1, 1);
|
||
if(int_map == qmi8658_Int1)
|
||
{
|
||
ctrl1 |= QMI8658_FIFO_MAP_INT1;
|
||
}
|
||
else if(int_map == qmi8658_Int2)
|
||
{
|
||
ctrl1 &= QMI8658_FIFO_MAP_INT2;
|
||
}
|
||
qmi8658_write_reg(Qmi8658Register_Ctrl1, ctrl1);
|
||
|
||
g_imu.cfg.fifo_ctrl = (unsigned char)(size | mode);
|
||
qmi8658_write_reg(Qmi8658Register_FifoCtrl, g_imu.cfg.fifo_ctrl);
|
||
qmi8658_write_reg(Qmi8658Register_FifoWmkTh, watermark);
|
||
|
||
qmi8658_send_ctl9cmd(qmi8658_Ctrl9_Cmd_Rst_Fifo);
|
||
qmi8658_enableSensors(QMI8658_ACCGYR_ENABLE);
|
||
}
|
||
|
||
unsigned short qmi8658_read_fifo(unsigned char* data)
|
||
{
|
||
unsigned char fifo_status[2] = {0,0};
|
||
unsigned char fifo_sensors = 1;
|
||
unsigned short fifo_bytes = 0;
|
||
unsigned short fifo_level = 0;
|
||
|
||
if((g_imu.cfg.fifo_ctrl&0x03)!=qmi8658_Fifo_Bypass)
|
||
{
|
||
//qmi8658_send_ctl9cmd(qmi8658_Ctrl9_Cmd_Req_Fifo);
|
||
|
||
qmi8658_read_reg(Qmi8658Register_FifoCount, fifo_status, 2);
|
||
fifo_bytes = (unsigned short)(((fifo_status[1]&0x03)<<8)|fifo_status[0]);
|
||
if((g_imu.cfg.enSensors == QMI8658_ACC_ENABLE)||(g_imu.cfg.enSensors == QMI8658_GYR_ENABLE))
|
||
{
|
||
fifo_sensors = 1;
|
||
}
|
||
else if(g_imu.cfg.enSensors == QMI8658_ACCGYR_ENABLE)
|
||
{
|
||
fifo_sensors = 2;
|
||
}
|
||
fifo_level = fifo_bytes/(3*fifo_sensors);
|
||
fifo_bytes = fifo_level*(6*fifo_sensors);
|
||
//qmi8658_log("fifo-level : %d\n", fifo_level);
|
||
if(fifo_level > 0)
|
||
{
|
||
qmi8658_send_ctl9cmd(qmi8658_Ctrl9_Cmd_Req_Fifo);
|
||
#if 1
|
||
for(int i=0; i<fifo_level; i++)
|
||
{
|
||
qmi8658_read_reg(Qmi8658Register_FifoData, &data[i*fifo_sensors*6], fifo_sensors*6);
|
||
}
|
||
#else
|
||
qmi8658_read_reg(Qmi8658Register_FifoData, data, fifo_bytes);
|
||
#endif
|
||
qmi8658_write_reg(Qmi8658Register_FifoCtrl, g_imu.cfg.fifo_ctrl);
|
||
}
|
||
qmi8658_send_ctl9cmd(qmi8658_Ctrl9_Cmd_Rst_Fifo);
|
||
}
|
||
|
||
return fifo_level;
|
||
}
|
||
#endif
|
||
|
||
#if defined(QMI8658_SOFT_SELFTEST)
|
||
unsigned char qmi8658_do_selftest(void)
|
||
{
|
||
float acc[3], gyr[3];
|
||
unsigned char status;
|
||
unsigned short st_count, retry, imu_fail_count;
|
||
float norm_acc, norm_gyo;
|
||
|
||
imu_fail_count = 0;
|
||
st_count = 0;
|
||
qmi8658_delay(50);
|
||
while(st_count++ < 20)
|
||
{
|
||
qmi8658_delay(1);
|
||
status = 0;
|
||
retry = 0;
|
||
while(!(status&0x03) && (retry++<50))
|
||
{
|
||
qmi8658_read_reg(Qmi8658Register_Status0, &status, 1);
|
||
qmi8658_delay(1);
|
||
}
|
||
if((status&0x03))
|
||
{
|
||
qmi8658_read_sensor_data(acc, gyr);
|
||
norm_acc = acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2];
|
||
norm_gyo = gyr[0]*gyr[0] + gyr[1]*gyr[1] + gyr[2]*gyr[2];
|
||
qmi8658_log("qmi8658_do_selftest-%d %f %f\n", st_count, norm_acc, norm_gyo);
|
||
if((norm_acc < 0.01f) || (norm_gyo < 0.000001f))
|
||
{
|
||
imu_fail_count++;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
imu_fail_count++;
|
||
}
|
||
}
|
||
|
||
if(imu_fail_count > 15)
|
||
{
|
||
qmi8658_log("qmi8658_do_selftest-fail\n");
|
||
return 0;
|
||
}
|
||
else
|
||
{
|
||
qmi8658_log("qmi8658_do_selftest-ok\n");
|
||
return 1;
|
||
}
|
||
}
|
||
#endif
|
||
|
||
unsigned char qmi8658_init(void)
|
||
{
|
||
if(qmi8658_get_id() == 0x05)
|
||
{
|
||
#if defined(QMI8658_USE_AMD)
|
||
qmi8658_config_amd();
|
||
#endif
|
||
#if defined(QMI8658_USE_PEDOMETER)
|
||
qmi8658_config_pedometer(125);
|
||
qmi8658_enable_pedometer(1);
|
||
#endif
|
||
qmi8658_config_reg(0);
|
||
qmi8658_enableSensors(g_imu.cfg.enSensors);
|
||
qmi8658_dump_reg();
|
||
#if defined(QMI8658_USE_CALI)
|
||
memset(&g_cali, 0, sizeof(g_cali));
|
||
#endif
|
||
return 1;
|
||
}
|
||
else
|
||
{
|
||
qmi8658_log("qmi8658_init fail\n");
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
|