1847 lines
70 KiB
C
1847 lines
70 KiB
C
|
|
#include "app_config.h"
|
|
#include "asm/clock.h"
|
|
#include "asm/cpu.h"
|
|
#include "generic/typedef.h"
|
|
#include "generic/gpio.h"
|
|
#include "typedef.h"
|
|
#include "system/includes.h"
|
|
#include "media/includes.h"
|
|
#include "asm/iic_hw.h"
|
|
#include "asm/iic_soft.h"
|
|
#include "asm/timer.h"
|
|
#include "imuSensor_manage.h"
|
|
#include "qmi8658c.h"
|
|
|
|
// #include "spi1.h"
|
|
// #include "port_wkup.h"
|
|
|
|
|
|
|
|
#if TCFG_QMI8658_ENABLE
|
|
|
|
/*************Betterlife ic debug***********/
|
|
#undef LOG_TAG_CONST
|
|
#define LOG_TAG "[QMI8658]"
|
|
#define LOG_ERROR_ENABLE
|
|
#define LOG_INFO_ENABLE
|
|
#include "debug.h"
|
|
|
|
void delay(volatile u32 t);
|
|
void udelay(u32 us);
|
|
#define MDELAY(n) mdelay(n)
|
|
|
|
|
|
static qmi8658_param *qmi8658_info;
|
|
|
|
// static qmi8658_data qmi8658_raw_data={0};
|
|
|
|
/******************************************************************
|
|
* Description: I2C or SPI bus interface functions and delay time function
|
|
*
|
|
* Parameters:
|
|
* devAddr: I2C device address, modify the macro(QMI8658_ADDRESS @qmi8658.h) based on your I2C function.
|
|
* If SPI interface, please ingnore the parameter.
|
|
* regAddr: register address
|
|
* readLen: data length to read
|
|
* *readBuf: data buffer to read
|
|
* writeLen: data length to write
|
|
* *writeBuf: data buffer to write
|
|
*
|
|
******************************************************************/
|
|
|
|
#if (QMI8658_USER_INTERFACE==QMI8658_USE_I2C)
|
|
|
|
#if TCFG_QMI8658_USER_IIC_TYPE
|
|
#define iic_init(iic) hw_iic_init(iic)
|
|
#define iic_uninit(iic) hw_iic_uninit(iic)
|
|
#define iic_start(iic) hw_iic_start(iic)
|
|
#define iic_stop(iic) hw_iic_stop(iic)
|
|
#define iic_tx_byte(iic, byte) hw_iic_tx_byte(iic, byte)
|
|
#define iic_rx_byte(iic, ack) hw_iic_rx_byte(iic, ack)
|
|
#define iic_read_buf(iic, buf, len) hw_iic_read_buf(iic, buf, len)
|
|
#define iic_write_buf(iic, buf, len) hw_iic_write_buf(iic, buf, len)
|
|
#define iic_suspend(iic) hw_iic_suspend(iic)
|
|
#define iic_resume(iic) hw_iic_resume(iic)
|
|
#else
|
|
#define iic_init(iic) soft_iic_init(iic)
|
|
#define iic_uninit(iic) soft_iic_uninit(iic)
|
|
#define iic_start(iic) soft_iic_start(iic)
|
|
#define iic_stop(iic) soft_iic_stop(iic)
|
|
#define iic_tx_byte(iic, byte) soft_iic_tx_byte(iic, byte)
|
|
#define iic_rx_byte(iic, ack) soft_iic_rx_byte(iic, ack)
|
|
#define iic_read_buf(iic, buf, len) soft_iic_read_buf(iic, buf, len)
|
|
#define iic_write_buf(iic, buf, len) soft_iic_write_buf(iic, buf, len)
|
|
#define iic_suspend(iic) soft_iic_suspend(iic)
|
|
#define iic_resume(iic) soft_iic_resume(iic)
|
|
#endif
|
|
//起止信号间隔：standard mode(100khz):4.7us; fast mode(400khz):1.3us
|
|
//return: readLen:ok, other:fail
|
|
u16 qmi8658_I2C_Read_NBytes(unsigned char devAddr,
|
|
unsigned char regAddr,
|
|
unsigned char *readBuf,
|
|
u16 readLen)
|
|
{
|
|
u16 i = 0;
|
|
local_irq_disable();
|
|
iic_start(qmi8658_info->iic_hdl);
|
|
if (0 == iic_tx_byte(qmi8658_info->iic_hdl, devAddr)) {
|
|
log_error("qmi8658 iic read err1");
|
|
goto __iic_exit_r;
|
|
}
|
|
delay(qmi8658_info->iic_delay);
|
|
/* if (0 == iic_tx_byte(qmi8658_info->iic_hdl, regAddr |0x80)) {//|0x80地址自动递增 */
|
|
if (0 == iic_tx_byte(qmi8658_info->iic_hdl, regAddr)) {//|0x80地址自动递增
|
|
log_error("qmi8658 iic read err2");
|
|
goto __iic_exit_r;
|
|
}
|
|
delay(qmi8658_info->iic_delay);
|
|
|
|
iic_start(qmi8658_info->iic_hdl);
|
|
if (0 == iic_tx_byte(qmi8658_info->iic_hdl, devAddr + 1)) {
|
|
log_error("qmi8658 iic read err3");
|
|
goto __iic_exit_r;
|
|
}
|
|
for (i = 0; i < readLen; i++) {
|
|
delay(qmi8658_info->iic_delay);
|
|
if (i == (readLen - 1)) {
|
|
*readBuf++ = iic_rx_byte(qmi8658_info->iic_hdl, 0);
|
|
} else {
|
|
*readBuf++ = iic_rx_byte(qmi8658_info->iic_hdl, 1);
|
|
}
|
|
/* if(i%100==0)wdt_clear(); */
|
|
}
|
|
__iic_exit_r:
|
|
iic_stop(qmi8658_info->iic_hdl);
|
|
local_irq_enable();
|
|
|
|
return i;
|
|
}
|
|
|
|
//起止信号间隔：standard mode(100khz):4.7us; fast mode(400khz):1.3us
|
|
//return:1:ok, 0:fail
|
|
unsigned char qmi8658_I2C_Write_Byte(unsigned char devAddr, //芯片支持1byte写
|
|
unsigned char regAddr,
|
|
unsigned char byte)
|
|
{
|
|
u8 ret = 1;
|
|
local_irq_disable();
|
|
iic_start(qmi8658_info->iic_hdl);
|
|
if (0 == iic_tx_byte(qmi8658_info->iic_hdl, devAddr)) {
|
|
log_error("qmi8658 iic write err1");
|
|
ret = 0;
|
|
goto __iic_exit_w;
|
|
}
|
|
delay(qmi8658_info->iic_delay);
|
|
if (0 == iic_tx_byte(qmi8658_info->iic_hdl, regAddr)) {
|
|
log_error("qmi8658 iic write err2");
|
|
ret = 0;
|
|
goto __iic_exit_w;
|
|
}
|
|
delay(qmi8658_info->iic_delay);
|
|
if (0 == iic_tx_byte(qmi8658_info->iic_hdl, byte)) {
|
|
log_error("qmi8658 iic write err3");
|
|
ret = 0;
|
|
goto __iic_exit_w;
|
|
}
|
|
__iic_exit_w:
|
|
iic_stop(qmi8658_info->iic_hdl);
|
|
local_irq_enable();
|
|
return ret;
|
|
}
|
|
//起止信号间隔：standard mode(100khz):4.7us; fast mode(400khz):1.3us
|
|
//return: readLen:ok, other:fail
|
|
unsigned char qmi8658_I2C_Write_NBytes(unsigned char devAddr, //芯片支持1byte写
|
|
unsigned char regAddr,
|
|
unsigned char *writeBuf,
|
|
u16 writeLen)
|
|
{
|
|
u16 i;
|
|
for (i = 0; i < writeLen; i++) {
|
|
if (qmi8658_I2C_Write_Byte(devAddr, regAddr + i, writeBuf[i]) == 0) {
|
|
log_error("qmi8658_I2C_Write_NBytes error N:%d", i);
|
|
return i;
|
|
}
|
|
udelay(5);//delay5us
|
|
}
|
|
return i;
|
|
}
|
|
IMU_read Qmi8658_read = qmi8658_I2C_Read_NBytes;
|
|
IMU_write Qmi8658_write = qmi8658_I2C_Write_Byte;
|
|
|
|
#elif (QMI8658_USER_INTERFACE==QMI8658_USE_SPI)
|
|
|
|
#define spi_cs_init() \
|
|
do { \
|
|
gpio_write(qmi8658_info->spi_cs_pin, 1); \
|
|
gpio_set_direction(qmi8658_info->spi_cs_pin, 0); \
|
|
gpio_set_die(qmi8658_info->spi_cs_pin, 1); \
|
|
} while (0)
|
|
#define spi_cs_uninit() \
|
|
do { \
|
|
gpio_set_die(qmi8658_info->spi_cs_pin, 0); \
|
|
gpio_set_direction(qmi8658_info->spi_cs_pin, 1); \
|
|
gpio_set_pull_up(qmi8658_info->spi_cs_pin, 0); \
|
|
gpio_set_pull_down(qmi8658_info->spi_cs_pin, 0); \
|
|
} while (0)
|
|
#define spi_cs_h() gpio_write(qmi8658_info->spi_cs_pin, 1)
|
|
#define spi_cs_l() gpio_write(qmi8658_info->spi_cs_pin, 0)
|
|
|
|
#define spi_read_byte() spi_recv_byte(qmi8658_info->spi_hdl, NULL)
|
|
#define spi_write_byte(x) spi_send_byte(qmi8658_info->spi_hdl, x)
|
|
#define spi_dma_read(x, y) spi_dma_recv(qmi8658_info->spi_hdl, x, y)
|
|
#define spi_dma_write(x, y) spi_dma_send(qmi8658_info->spi_hdl, x, y)
|
|
#define spi_set_width(x) spi_set_bit_mode(qmi8658_info->spi_hdl, x)
|
|
#define spi_init() spi_open(qmi8658_info->spi_hdl)
|
|
#define spi_closed() spi_close(qmi8658_info->spi_hdl)
|
|
#define spi_suspend() hw_spi_suspend(qmi8658_info->spi_hdl)
|
|
#define spi_resume() hw_spi_resume(qmi8658_info->spi_hdl)
|
|
|
|
u16 qmi8658_SPI_readNBytes(unsigned char devAddr,
|
|
unsigned char regAddr,
|
|
unsigned char *readBuf,
|
|
u16 readLen)
|
|
{
|
|
spi_cs_l();
|
|
spi_write_byte(regAddr | 0x80);//| 0x80:read mode
|
|
spi_dma_read(readBuf, readLen);
|
|
spi_cs_h();
|
|
//SPIRead((regAddr | 0x80), readBuf, readLen);
|
|
return (readLen);
|
|
}
|
|
unsigned char qmi8658_SPI_writeByte(unsigned char devAddr,
|
|
unsigned char regAddr,
|
|
unsigned char writebyte)
|
|
{
|
|
spi_cs_l();
|
|
spi_write_byte((regAddr) & 0x7F);
|
|
spi_write_byte(writebyte);
|
|
spi_cs_h();
|
|
udelay(5);//delay5us
|
|
return (1);
|
|
}
|
|
unsigned char qmi8658_SPI_writeNBytes(unsigned char devAddr,
|
|
unsigned char regAddr,
|
|
unsigned char *writeBuf,
|
|
u16 writeLen)
|
|
{
|
|
#if 0 //多字节dma写
|
|
spi_cs_l();
|
|
spi_write_byte(regAddr & 0x7F);
|
|
spi_dma_write(writeBuf, writeLen);
|
|
spi_cs_h();
|
|
#else
|
|
u16 i = 0;
|
|
for (; i < writeLen; i++) {
|
|
qmi8658_SPI_writeByte(devAddr, regAddr + i, writeBuf[i]);
|
|
}
|
|
#endif
|
|
//SPIWrite((regAddr & 0x7F), writeBuf, writeLen);
|
|
return (i);
|
|
}
|
|
|
|
IMU_read Qmi8658_read = qmi8658_SPI_readNBytes;
|
|
IMU_write Qmi8658_write = qmi8658_SPI_writeByte;
|
|
#else
|
|
//I3C
|
|
#endif
|
|
|
|
void Qmi8658_delay(int ms)
|
|
{
|
|
//your delay code(mSecond: millisecond):
|
|
MDELAY(ms);
|
|
}
|
|
|
|
static unsigned short acc_lsb_div = 0;
|
|
static unsigned short gyro_lsb_div = 0;
|
|
static unsigned short ae_q_lsb_div = (1 << 14);
|
|
static unsigned short ae_v_lsb_div = (1 << 10);
|
|
static unsigned int imu_timestamp = 0;
|
|
static struct Qmi8658Config qmi8658_config;
|
|
|
|
enum {
|
|
AXIS_X = 0,
|
|
AXIS_Y = 1,
|
|
AXIS_Z = 2,
|
|
|
|
AXIS_TOTAL
|
|
};
|
|
#if 0
|
|
typedef struct {
|
|
short sign[AXIS_TOTAL];
|
|
unsigned short map[AXIS_TOTAL];
|
|
} qst_imu_layout;
|
|
|
|
static qst_imu_layout imu_map;
|
|
|
|
void Qmi8658_set_layout(short layout)
|
|
{
|
|
if (layout == 0) {
|
|
imu_map.sign[AXIS_X] = 1;
|
|
imu_map.sign[AXIS_Y] = 1;
|
|
imu_map.sign[AXIS_Z] = 1;
|
|
imu_map.map[AXIS_X] = AXIS_X;
|
|
imu_map.map[AXIS_Y] = AXIS_Y;
|
|
imu_map.map[AXIS_Z] = AXIS_Z;
|
|
} else if (layout == 1) {
|
|
imu_map.sign[AXIS_X] = -1;
|
|
imu_map.sign[AXIS_Y] = 1;
|
|
imu_map.sign[AXIS_Z] = 1;
|
|
imu_map.map[AXIS_X] = AXIS_Y;
|
|
imu_map.map[AXIS_Y] = AXIS_X;
|
|
imu_map.map[AXIS_Z] = AXIS_Z;
|
|
} else if (layout == 2) {
|
|
imu_map.sign[AXIS_X] = -1;
|
|
imu_map.sign[AXIS_Y] = -1;
|
|
imu_map.sign[AXIS_Z] = 1;
|
|
imu_map.map[AXIS_X] = AXIS_X;
|
|
imu_map.map[AXIS_Y] = AXIS_Y;
|
|
imu_map.map[AXIS_Z] = AXIS_Z;
|
|
} else if (layout == 3) {
|
|
imu_map.sign[AXIS_X] = 1;
|
|
imu_map.sign[AXIS_Y] = -1;
|
|
imu_map.sign[AXIS_Z] = 1;
|
|
imu_map.map[AXIS_X] = AXIS_Y;
|
|
imu_map.map[AXIS_Y] = AXIS_X;
|
|
imu_map.map[AXIS_Z] = AXIS_Z;
|
|
} else if (layout == 4) {
|
|
imu_map.sign[AXIS_X] = -1;
|
|
imu_map.sign[AXIS_Y] = 1;
|
|
imu_map.sign[AXIS_Z] = -1;
|
|
imu_map.map[AXIS_X] = AXIS_X;
|
|
imu_map.map[AXIS_Y] = AXIS_Y;
|
|
imu_map.map[AXIS_Z] = AXIS_Z;
|
|
} else if (layout == 5) {
|
|
imu_map.sign[AXIS_X] = 1;
|
|
imu_map.sign[AXIS_Y] = 1;
|
|
imu_map.sign[AXIS_Z] = -1;
|
|
imu_map.map[AXIS_X] = AXIS_Y;
|
|
imu_map.map[AXIS_Y] = AXIS_X;
|
|
imu_map.map[AXIS_Z] = AXIS_Z;
|
|
} else if (layout == 6) {
|
|
imu_map.sign[AXIS_X] = 1;
|
|
imu_map.sign[AXIS_Y] = -1;
|
|
imu_map.sign[AXIS_Z] = -1;
|
|
imu_map.map[AXIS_X] = AXIS_X;
|
|
imu_map.map[AXIS_Y] = AXIS_Y;
|
|
imu_map.map[AXIS_Z] = AXIS_Z;
|
|
} else if (layout == 7) {
|
|
imu_map.sign[AXIS_X] = -1;
|
|
imu_map.sign[AXIS_Y] = -1;
|
|
imu_map.sign[AXIS_Z] = -1;
|
|
imu_map.map[AXIS_X] = AXIS_Y;
|
|
imu_map.map[AXIS_Y] = AXIS_X;
|
|
imu_map.map[AXIS_Z] = AXIS_Z;
|
|
} else {
|
|
imu_map.sign[AXIS_X] = 1;
|
|
imu_map.sign[AXIS_Y] = 1;
|
|
imu_map.sign[AXIS_Z] = 1;
|
|
imu_map.map[AXIS_X] = AXIS_X;
|
|
imu_map.map[AXIS_Y] = AXIS_Y;
|
|
imu_map.map[AXIS_Z] = AXIS_Z;
|
|
}
|
|
}
|
|
#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:
|
|
acc_lsb_div = (1 << 14);
|
|
break;
|
|
case Qmi8658AccRange_4g:
|
|
acc_lsb_div = (1 << 13);
|
|
break;
|
|
case Qmi8658AccRange_8g:
|
|
acc_lsb_div = (1 << 12);
|
|
break;
|
|
case Qmi8658AccRange_16g:
|
|
acc_lsb_div = (1 << 11);
|
|
break;
|
|
default:
|
|
range = Qmi8658AccRange_8g;
|
|
acc_lsb_div = (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(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl2, ctl_dada);
|
|
// set LPF & HPF
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, 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(QMI8658_SLAVE_ADDRESS, 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:
|
|
gyro_lsb_div = 2048;
|
|
break;
|
|
case Qmi8658GyrRange_32dps:
|
|
gyro_lsb_div = 1024;
|
|
break;
|
|
case Qmi8658GyrRange_64dps:
|
|
gyro_lsb_div = 512;
|
|
break;
|
|
case Qmi8658GyrRange_128dps:
|
|
gyro_lsb_div = 256;
|
|
break;
|
|
case Qmi8658GyrRange_256dps:
|
|
gyro_lsb_div = 128;
|
|
break;
|
|
case Qmi8658GyrRange_512dps:
|
|
gyro_lsb_div = 64;
|
|
break;
|
|
case Qmi8658GyrRange_1024dps:
|
|
gyro_lsb_div = 32;
|
|
break;
|
|
case Qmi8658GyrRange_2048dps:
|
|
gyro_lsb_div = 16;
|
|
break;
|
|
// case Qmi8658GyrRange_4096dps:
|
|
// gyro_lsb_div = 8;
|
|
// break;
|
|
default:
|
|
range = Qmi8658GyrRange_512dps;
|
|
gyro_lsb_div = 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(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl3, ctl_dada);
|
|
|
|
// Conversion from degrees/s to rad/s if necessary
|
|
// set LPF & HPF
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, 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(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl5, ctl_dada);
|
|
// set LPF & HPF
|
|
}
|
|
|
|
void Qmi8658_config_mag(enum Qmi8658_MagDev device, enum Qmi8658_MagOdr odr)
|
|
{
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl4, device | odr);
|
|
}
|
|
|
|
void Qmi8658_config_ae(enum Qmi8658_AeOdr odr)
|
|
{
|
|
//Qmi8658_config_acc(Qmi8658AccRange_8g, AccOdr_1000Hz, Lpf_Enable, St_Enable);
|
|
//Qmi8658_config_gyro(Qmi8658GyrRange_2048dps, GyrOdr_1000Hz, Lpf_Enable, St_Enable);
|
|
Qmi8658_config_acc(qmi8658_config.accRange, qmi8658_config.accOdr, Qmi8658Lpf_Enable, Qmi8658St_Disable);
|
|
Qmi8658_config_gyro(qmi8658_config.gyrRange, qmi8658_config.gyrOdr, Qmi8658Lpf_Enable, Qmi8658St_Disable);
|
|
Qmi8658_config_mag(qmi8658_config.magDev, qmi8658_config.magOdr);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl6, odr);
|
|
}
|
|
|
|
void Qmi8658_send_ctl9cmd(enum Qmi8658_Ctrl9Command cmd) //cmd=0x0d
|
|
{
|
|
unsigned char status1 = 0x00;
|
|
unsigned short count = 0;
|
|
//void* pi2c = qmi8658_i2c_init();
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl9, (unsigned char)cmd); // write commond to ctrl9
|
|
#if defined(QMI8658_HANDSHAKE_NEW)
|
|
#if defined(QMI8658_HANDSHAKE_TO_STATUS)
|
|
unsigned char status_reg = Qmi8658Register_StatusInt;
|
|
unsigned char cmd_done = 0x80;
|
|
#else
|
|
unsigned char status_reg = Qmi8658Register_Status1;// Qmi8658Register_Status1
|
|
unsigned char cmd_done = 0x01;
|
|
#endif
|
|
//count = 0;
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, status_reg, &status1, 1);
|
|
/* log_info("status1=%x\n",status1); */
|
|
while (((status1 & cmd_done) != cmd_done) && (count++ < 200)) { // read statusINT until bit7 is 1
|
|
Qmi8658_delay(1);
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, status_reg, &status1, 1);
|
|
//log_info("first %d status_reg: 0x%x\n", count,status1);
|
|
}
|
|
/* log_info("Qmi8658_config_fifo ctrl9 done-1: %d\n", count); */
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl9, Qmi8658_Ctrl9_Cmd_NOP); // write commond 0x00 to ctrl9
|
|
count = 0;
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, status_reg, &status1, 1);
|
|
while (((status1 & cmd_done) == cmd_done) && (count++ < 200)) { // read statusINT until bit7 is 0
|
|
Qmi8658_delay(1);
|
|
/* log_info("second %d status_reg: 0x%x\n", count,status1); */
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, status_reg, &status1, 1);
|
|
}
|
|
/* log_info("ctrl9 done-2: %d\n", count); */
|
|
#else
|
|
while (((status1 & QMI8658_STATUS1_CMD_DONE) == 0) && (count++ < 200)) {
|
|
Qmi8658_delay(2);
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Status1, &status1, sizeof(status1));
|
|
}
|
|
//log_info("fifo rst done : %d\n", count);
|
|
#endif
|
|
|
|
//qmi8658_i2c_deinit(pi2c);
|
|
|
|
}
|
|
|
|
|
|
#if (QMI8658_USE_FIFO_EN)
|
|
/*
|
|
* if mode=Qmi8658_Fifo_Stream then watermark <= size 否则不会输出中断信息
|
|
* */
|
|
void Qmi8658_config_fifo(uint8_t watermark, enum Qmi8658_FifoSize size, enum Qmi8658_FifoMode mode, enum Qmi8658_fifo_format format)
|
|
{
|
|
/* Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, 0x00); */
|
|
Qmi8658_delay(1);
|
|
#if (QMI8658_NEW_HANDSHAKE == 1)
|
|
u8 temp_data;
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl1, &temp_data, 1);
|
|
#if (QMI8658_FIFO_INT_OMAP_INT1 == 1)
|
|
temp_data |= BIT(2); //
|
|
#else
|
|
temp_data &= ~BIT(2); //
|
|
#endif
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl1, temp_data);
|
|
#endif
|
|
|
|
qmi8658_config.fifo_format = format; //QMI8658_FORMAT_12_BYTES;
|
|
qmi8658_config.fifo_ctrl = (uint8_t)size | (uint8_t)mode;
|
|
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_FifoCtrl, qmi8658_config.fifo_ctrl);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_FifoWmkTh, (unsigned char)watermark);
|
|
Qmi8658_send_ctl9cmd(Qmi8658_Ctrl9_Cmd_Rst_Fifo);
|
|
|
|
/* if(format == QMI8658_FORMAT_ACCEL_6_BYTES) */
|
|
/* Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, 0x01); */
|
|
/* else if(format == QMI8658_FORMAT_GYRO_6_BYTES) */
|
|
/* Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, 0x02); */
|
|
/* else if(format == QMI8658_FORMAT_12_BYTES) */
|
|
/* Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, 0x03); */
|
|
}
|
|
|
|
/*
|
|
*sensor data formal:
|
|
* 3 sensor:acc, gyro, mag
|
|
* AX_L[0] AX_H[0] AY_L[0] AY_H[0] AZ_L[0] AZ_H[0]
|
|
* GX_L[0] GX_H[0] GY_L[0] GY_H[0] GZ_L[0] GZ_H[0]
|
|
* MX_L[0] MX_H[0] MY_L[0] MY_H[0] MZ_L[0] MZ_H[0]
|
|
* AX_L[1] AX_H[1] ......
|
|
* 2 sensor:acc, gyro
|
|
* AX_L[0] AX_H[0] AY_L[0] AY_H[0] AZ_L[0] AZ_H[0]
|
|
* GX_L[0] GX_H[0] GY_L[0] GY_H[0] GZ_L[0] GZ_H[0]
|
|
* AX_L[1] AX_H[1] ......
|
|
* 1 sensor:acc or gyro
|
|
* AX_L[0] AX_H[0]] AY_L[0] AY_H[0] AZ_L [0] AZ_H[0] AX_L[1] ......
|
|
* */
|
|
unsigned short Qmi8658_read_fifo(unsigned char *data)
|
|
{
|
|
unsigned char fifo_status[2] = {0, 0};
|
|
unsigned short fifo_bytes = 0;
|
|
unsigned short fifo_level = 0;
|
|
//unsigned short i;
|
|
|
|
Qmi8658_send_ctl9cmd(Qmi8658_Ctrl9_Cmd_Req_Fifo);
|
|
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_FifoCount, fifo_status, 2);
|
|
fifo_bytes = (unsigned short)(((fifo_status[1] & 0x03) << 8) | fifo_status[0]);
|
|
fifo_bytes *= 2;
|
|
if ((qmi8658_config.fifo_format == QMI8658_FORMAT_ACCEL_6_BYTES) || (qmi8658_config.fifo_format == QMI8658_FORMAT_GYRO_6_BYTES) || (qmi8658_config.fifo_format == QMI8658_FORMAT_MAG_6_BYTES)) {
|
|
fifo_level = fifo_bytes / 6; // one sensor
|
|
fifo_bytes = fifo_level * 6;
|
|
} else if (qmi8658_config.fifo_format == QMI8658_FORMAT_12_BYTES) {
|
|
fifo_level = fifo_bytes / 12; // two sensor
|
|
fifo_bytes = fifo_level * 12;
|
|
} else if (qmi8658_config.fifo_format == QMI8658_FORMAT_18_BYTES) {
|
|
fifo_level = fifo_bytes / 18; // three sensor
|
|
fifo_bytes = fifo_level * 18;
|
|
}
|
|
/* log_info("fifo byte=%d level=%d\n", fifo_bytes, fifo_level); */
|
|
if (fifo_level > 0) {
|
|
#if 0
|
|
for (i = 1; i < fifo_level; i++) {
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_FifoData, data + i * 12, 12);
|
|
}
|
|
#else
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_FifoData, data, fifo_bytes);
|
|
#endif
|
|
}
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_FifoCtrl, qmi8658_config.fifo_ctrl);
|
|
|
|
return fifo_bytes;
|
|
}
|
|
|
|
|
|
void Qmi8658_get_fifo_format(enum Qmi8658_fifo_format *format)
|
|
{
|
|
if (format) {
|
|
*format = qmi8658_config.fifo_format;
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
/* status0:46(0x2e)
|
|
* BIT: 7:4 3 2 1 0
|
|
* reserved AE(new data) mag gyro acc
|
|
*/
|
|
unsigned char Qmi8658_readStatus0(void)
|
|
{
|
|
unsigned char status[2];
|
|
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Status0, status, sizeof(status));
|
|
log_info("status[0:0x%x 1:0x%x]", status[0], status[1]);
|
|
|
|
return status[0];
|
|
}
|
|
/*!status1:47(0x2f)
|
|
* \brief Blocking read of data status register 1 (::Qmi8658Register_Status1).
|
|
* bit7~bit1: reserved
|
|
* bit0: Used to indicate ctrl9 Command was done.
|
|
* \returns Status byte \see STATUS1 for flag definitions.
|
|
*/
|
|
unsigned char Qmi8658_readStatus1(void)
|
|
{
|
|
unsigned char status;
|
|
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Status1, &status, sizeof(status));
|
|
/* log_info("status1[0x%x]\n",status); */
|
|
|
|
return status;
|
|
}
|
|
|
|
/*
|
|
* Sample time stamp. Count incremented by one for each sample
|
|
(x, y, z data set) from sensor with highest ODR (circular register
|
|
0x0-0xFFFFFF)
|
|
*/
|
|
unsigned char Qmi8658_read_time_stamp_reg(void)
|
|
{
|
|
unsigned char time_stamp[3];
|
|
u32 sample_time = 0;
|
|
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Timestamp_L, time_stamp, sizeof(time_stamp));
|
|
sample_time = (u32)(time_stamp[2] << 16) | (u32)(time_stamp[1] << 8) | time_stamp[0];
|
|
/* log_info("Sample time stamp =[0x%x]\n",sample_time); */
|
|
return sample_time;
|
|
}
|
|
/************************0x59地址文档未介绍*****************************/
|
|
unsigned char Qmi8658_read_temptap_reg(void)
|
|
{
|
|
unsigned char status;
|
|
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, 0x59, &status, sizeof(status));
|
|
log_info("tap_temp_reg =[0x%x]\n", status);
|
|
|
|
return status;
|
|
}
|
|
|
|
float Qmi8658_readTemp(void)
|
|
{
|
|
unsigned char buf[2];
|
|
short temp = 0;
|
|
float temp_f = 0;
|
|
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Tempearture_L, buf, 2);
|
|
temp = ((short)buf[1] << 8) | buf[0];
|
|
temp_f = (float)temp / 256.0f;
|
|
|
|
return temp_f;
|
|
}
|
|
|
|
void Qmi8658_read_acc_xyz(float acc_xyz[3])
|
|
{
|
|
unsigned char buf_reg[6];
|
|
short raw_acc_xyz[3];
|
|
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ax_L, buf_reg, 6); // 0x35, 53
|
|
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]));
|
|
|
|
acc_xyz[0] = (raw_acc_xyz[0] * ONE_G) / acc_lsb_div;
|
|
acc_xyz[1] = (raw_acc_xyz[1] * ONE_G) / acc_lsb_div;
|
|
acc_xyz[2] = (raw_acc_xyz[2] * ONE_G) / acc_lsb_div;
|
|
|
|
//log_info("fis210x acc: %f %f %f\n", acc_xyz[0], acc_xyz[1], acc_xyz[2]);
|
|
}
|
|
|
|
void Qmi8658_read_gyro_xyz(float gyro_xyz[3])
|
|
{
|
|
unsigned char buf_reg[6];
|
|
short raw_gyro_xyz[3];
|
|
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Gx_L, buf_reg, 6); // 0x3b, 59
|
|
raw_gyro_xyz[0] = (short)((unsigned short)(buf_reg[1] << 8) | (buf_reg[0]));
|
|
raw_gyro_xyz[1] = (short)((unsigned short)(buf_reg[3] << 8) | (buf_reg[2]));
|
|
raw_gyro_xyz[2] = (short)((unsigned short)(buf_reg[5] << 8) | (buf_reg[4]));
|
|
|
|
/* log_info("gyro: %d %d %d\n", raw_gyro_xyz[0], raw_gyro_xyz[1], raw_gyro_xyz[2]); */
|
|
|
|
gyro_xyz[0] = (raw_gyro_xyz[0] * 1.0f) / gyro_lsb_div;
|
|
gyro_xyz[1] = (raw_gyro_xyz[1] * 1.0f) / gyro_lsb_div;
|
|
gyro_xyz[2] = (raw_gyro_xyz[2] * 1.0f) / gyro_lsb_div;
|
|
|
|
//log_info("fis210x gyro: %f %f %f\n", gyro_xyz[0], gyro_xyz[1], gyro_xyz[2]);
|
|
}
|
|
|
|
/******
|
|
* tim_count: Sample time stamp. Count incremented by one for each sample
|
|
(x, y, z data set) from sensor with highest ODR (circular register
|
|
0x0-0xFFFFFF).
|
|
*****/
|
|
void Qmi8658_read_xyz(float acc[3], float gyro[3], unsigned int *tim_count)
|
|
{
|
|
unsigned char buf_reg[12];
|
|
short raw_acc_xyz[3];
|
|
short raw_gyro_xyz[3];
|
|
// float acc_t[3];
|
|
// float gyro_t[3];
|
|
|
|
if (tim_count) {
|
|
unsigned char buf[3];
|
|
unsigned int timestamp;
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Timestamp_L, buf, 3); // 0x18 24
|
|
timestamp = (unsigned int)(((unsigned int)buf[2] << 16) | ((unsigned int)buf[1] << 8) | buf[0]);
|
|
if (timestamp > imu_timestamp) {
|
|
imu_timestamp = timestamp;
|
|
} else {
|
|
imu_timestamp = (timestamp + 0x1000000 - imu_timestamp);
|
|
}
|
|
|
|
*tim_count = imu_timestamp;
|
|
}
|
|
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ax_L, buf_reg, 12); // 0x19, 25
|
|
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 (QMI8658_UINT_MG_DPS)
|
|
// mg
|
|
acc[AXIS_X] = (float)(raw_acc_xyz[AXIS_X] * 1000.0f) / acc_lsb_div;
|
|
acc[AXIS_Y] = (float)(raw_acc_xyz[AXIS_Y] * 1000.0f) / acc_lsb_div;
|
|
acc[AXIS_Z] = (float)(raw_acc_xyz[AXIS_Z] * 1000.0f) / acc_lsb_div;
|
|
#else
|
|
// m/s2
|
|
acc[AXIS_X] = (float)(raw_acc_xyz[AXIS_X] * ONE_G) / acc_lsb_div;
|
|
acc[AXIS_Y] = (float)(raw_acc_xyz[AXIS_Y] * ONE_G) / acc_lsb_div;
|
|
acc[AXIS_Z] = (float)(raw_acc_xyz[AXIS_Z] * ONE_G) / acc_lsb_div;
|
|
#endif
|
|
// acc[AXIS_X] = imu_map.sign[AXIS_X]*acc_t[imu_map.map[AXIS_X]];
|
|
// acc[AXIS_Y] = imu_map.sign[AXIS_Y]*acc_t[imu_map.map[AXIS_Y]];
|
|
// acc[AXIS_Z] = imu_map.sign[AXIS_Z]*acc_t[imu_map.map[AXIS_Z]];
|
|
|
|
#if (QMI8658_UINT_MG_DPS)
|
|
// dps
|
|
gyro[0] = (float)(raw_gyro_xyz[0] * 1.0f) / gyro_lsb_div;
|
|
gyro[1] = (float)(raw_gyro_xyz[1] * 1.0f) / gyro_lsb_div;
|
|
gyro[2] = (float)(raw_gyro_xyz[2] * 1.0f) / gyro_lsb_div;
|
|
#else
|
|
// rad/s
|
|
gyro[AXIS_X] = (float)(raw_gyro_xyz[AXIS_X] * 0.01745f) / gyro_lsb_div; // *pi/180
|
|
gyro[AXIS_Y] = (float)(raw_gyro_xyz[AXIS_Y] * 0.01745f) / gyro_lsb_div;
|
|
gyro[AXIS_Z] = (float)(raw_gyro_xyz[AXIS_Z] * 0.01745f) / gyro_lsb_div;
|
|
#endif
|
|
// gyro[AXIS_X] = imu_map.sign[AXIS_X]*gyro_t[imu_map.map[AXIS_X]];
|
|
// gyro[AXIS_Y] = imu_map.sign[AXIS_Y]*gyro_t[imu_map.map[AXIS_Y]];
|
|
// gyro[AXIS_Z] = imu_map.sign[AXIS_Z]*gyro_t[imu_map.map[AXIS_Z]];
|
|
}
|
|
|
|
/******
|
|
* tim_count: Sample time stamp. Count incremented by one for each sample
|
|
(x, y, z data set) from sensor with highest ODR (circular register
|
|
0x0-0xFFFFFF).
|
|
*****/
|
|
void Qmi8658_read_xyz_raw(short raw_acc_xyz[3], short raw_gyro_xyz[3], unsigned int *tim_count)
|
|
{
|
|
unsigned char buf_reg[12];
|
|
|
|
if (tim_count) {
|
|
unsigned char buf[3];
|
|
unsigned int timestamp;
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Timestamp_L, buf, 3); // 0x18 24
|
|
timestamp = (unsigned int)(((unsigned int)buf[2] << 16) | ((unsigned int)buf[1] << 8) | buf[0]);
|
|
if (timestamp > imu_timestamp) {
|
|
imu_timestamp = timestamp;
|
|
} else {
|
|
imu_timestamp = (timestamp + 0x1000000 - imu_timestamp);
|
|
}
|
|
|
|
*tim_count = imu_timestamp;
|
|
}
|
|
if (raw_acc_xyz && raw_gyro_xyz) {
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ax_L, buf_reg, 12); // 0x19, 25
|
|
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]));
|
|
} else if (raw_acc_xyz) {
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ax_L, buf_reg, 6); // 0x19, 25
|
|
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]));
|
|
} else if (raw_gyro_xyz) {
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Gx_L, buf_reg, 6); // 0x19, 25
|
|
raw_gyro_xyz[0] = (short)((unsigned short)(buf_reg[1] << 8) | (buf_reg[0]));
|
|
raw_gyro_xyz[1] = (short)((unsigned short)(buf_reg[3] << 8) | (buf_reg[2]));
|
|
raw_gyro_xyz[2] = (short)((unsigned short)(buf_reg[5] << 8) | (buf_reg[4]));
|
|
}
|
|
}
|
|
|
|
void Qmi8658_read_raw_acc_xyz(void *acc_data)
|
|
{
|
|
unsigned char buf_reg[8];
|
|
imu_axis_data_t *raw_acc_xyz = (imu_axis_data_t *)acc_data;
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ax_L, buf_reg, 6); // 53
|
|
raw_acc_xyz->x = (short)((unsigned short)(buf_reg[1] << 8) | (buf_reg[0]));
|
|
raw_acc_xyz->y = (short)((unsigned short)(buf_reg[3] << 8) | (buf_reg[2]));
|
|
raw_acc_xyz->z = (short)((unsigned short)(buf_reg[5] << 8) | (buf_reg[4]));
|
|
|
|
/* log_info("Qmi8658 acc: %d %d %d\n", raw_acc_xyz->x, raw_acc_xyz->y, raw_acc_xyz->z); */
|
|
}
|
|
void Qmi8658_read_raw_gyro_xyz(void *gyro_data)
|
|
{
|
|
unsigned char buf_reg[6];
|
|
imu_axis_data_t *raw_gyro_xyz = (imu_axis_data_t *)gyro_data;
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Gx_L, buf_reg, 6); // 53
|
|
raw_gyro_xyz->x = (short)((unsigned short)(buf_reg[1] << 8) | (buf_reg[0]));
|
|
raw_gyro_xyz->y = (short)((unsigned short)(buf_reg[3] << 8) | (buf_reg[2]));
|
|
raw_gyro_xyz->z = (short)((unsigned short)(buf_reg[5] << 8) | (buf_reg[4]));
|
|
|
|
/* log_info("Qmi8658 gyro: %d %d %d\n", raw_gyro_xyz->x, raw_gyro_xyz->y, raw_gyro_xyz->z); */
|
|
}
|
|
void Qmi8658_read_raw_acc_gyro_xyz(void *raw_data)
|
|
{
|
|
unsigned char buf_reg[14];
|
|
short temp = 0;
|
|
imu_sensor_data_t *raw_sensor_data = (imu_sensor_data_t *)raw_data;
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Tempearture_L, buf_reg, 14); // 0x3b, 59
|
|
temp = ((short)buf_reg[1] << 8) | buf_reg[0];
|
|
raw_sensor_data->acc.x = (short)((unsigned short)(buf_reg[3] << 8) | (buf_reg[2]));
|
|
raw_sensor_data->acc.y = (short)((unsigned short)(buf_reg[5] << 8) | (buf_reg[4]));
|
|
raw_sensor_data->acc.z = (short)((unsigned short)(buf_reg[7] << 8) | (buf_reg[6]));
|
|
|
|
raw_sensor_data->gyro.x = (short)((unsigned short)(buf_reg[9] << 8) | (buf_reg[8]));
|
|
raw_sensor_data->gyro.y = (short)((unsigned short)(buf_reg[11] << 8) | (buf_reg[10]));
|
|
raw_sensor_data->gyro.z = (short)((unsigned short)(buf_reg[13] << 8) | (buf_reg[12]));
|
|
raw_sensor_data->temp_data = (float)temp / 256.0f;
|
|
/* log_info("Qmi8658 raw:acc_x:%d acc_y:%d acc_z:%d gyro_x:%d gyro_y:%d gyro_z:%d", raw_sensor_data->acc.x, raw_sensor_data->acc.y, raw_sensor_data->acc.z, raw_sensor_data->gyro.x, raw_sensor_data->gyro.y, raw_sensor_data->gyro.z); */
|
|
/* log_info("Qmi8658 temp:%d.%d\n", (u16)(raw_sensor_data->temp_data), (u16)(((u16)((raw_sensor_data->temp_data)* 100)) % 100)); */
|
|
}
|
|
|
|
|
|
void Qmi8658_read_ae(float quat[4], float velocity[3])
|
|
{
|
|
unsigned char buf_reg[14];
|
|
short raw_q_xyz[4];
|
|
short raw_v_xyz[3];
|
|
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Q1_L, buf_reg, 14);
|
|
raw_q_xyz[0] = (short)((unsigned short)(buf_reg[1] << 8) | (buf_reg[0]));
|
|
raw_q_xyz[1] = (short)((unsigned short)(buf_reg[3] << 8) | (buf_reg[2]));
|
|
raw_q_xyz[2] = (short)((unsigned short)(buf_reg[5] << 8) | (buf_reg[4]));
|
|
raw_q_xyz[3] = (short)((unsigned short)(buf_reg[7] << 8) | (buf_reg[6]));
|
|
|
|
raw_v_xyz[1] = (short)((unsigned short)(buf_reg[9] << 8) | (buf_reg[8]));
|
|
raw_v_xyz[2] = (short)((unsigned short)(buf_reg[11] << 8) | (buf_reg[10]));
|
|
raw_v_xyz[2] = (short)((unsigned short)(buf_reg[13] << 8) | (buf_reg[12]));
|
|
|
|
quat[0] = (float)(raw_q_xyz[0] * 1.0f) / ae_q_lsb_div;
|
|
quat[1] = (float)(raw_q_xyz[1] * 1.0f) / ae_q_lsb_div;
|
|
quat[2] = (float)(raw_q_xyz[2] * 1.0f) / ae_q_lsb_div;
|
|
quat[3] = (float)(raw_q_xyz[3] * 1.0f) / ae_q_lsb_div;
|
|
|
|
velocity[0] = (float)(raw_v_xyz[0] * 1.0f) / ae_v_lsb_div;
|
|
velocity[1] = (float)(raw_v_xyz[1] * 1.0f) / ae_v_lsb_div;
|
|
velocity[2] = (float)(raw_v_xyz[2] * 1.0f) / ae_v_lsb_div;
|
|
}
|
|
|
|
void Qmi8658_enableWakeOnMotion(enum Qmi8658_Interrupt int_set, enum Qmi8658_WakeOnMotionThreshold threshold, unsigned char blankingTime)
|
|
{
|
|
unsigned char cal1_1_reg = (unsigned char)threshold;
|
|
unsigned char cal1_2_reg = (unsigned char)int_set | (blankingTime & 0x3F);
|
|
unsigned char status1 = 0;
|
|
int count = 0;
|
|
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, QMI8658_CTRL7_DISABLE_ALL);
|
|
Qmi8658_config_acc(Qmi8658AccRange_8g, Qmi8658AccOdr_LowPower_21Hz, Qmi8658Lpf_Disable, Qmi8658St_Disable);
|
|
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_L, cal1_1_reg);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_H, cal1_2_reg);
|
|
// ctrl9 wom setting
|
|
Qmi8658_send_ctl9cmd(Qmi8658_Ctrl9_Cmd_WoM_Setting);
|
|
// ctrl9 wom setting
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, QMI8658_CTRL7_ACC_ENABLE);
|
|
}
|
|
|
|
void Qmi8658_disableWakeOnMotion(void)
|
|
{
|
|
unsigned char status1 = 0;
|
|
int count = 0;
|
|
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, QMI8658_CTRL7_DISABLE_ALL);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_L, 0);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_H, 0);
|
|
|
|
Qmi8658_send_ctl9cmd(Qmi8658_Ctrl9_Cmd_WoM_Setting);
|
|
}
|
|
|
|
void Qmi8658_enableSensors(unsigned char enableFlags)
|
|
{
|
|
|
|
if (enableFlags & QMI8658_CONFIG_AE_ENABLE) {
|
|
enableFlags |= QMI8658_CTRL7_ACC_ENABLE | QMI8658_CTRL7_GYR_ENABLE;
|
|
}
|
|
#if (QMI8658_SYNC_SAMPLE_MODE)
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, enableFlags | 0x80);
|
|
#else
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, enableFlags & QMI8658_CTRL7_ENABLE_MASK);
|
|
#endif
|
|
}
|
|
|
|
|
|
void Qmi8658_enable_acc_Sensors(unsigned char enableFlags)
|
|
{
|
|
unsigned char ctrl7 = 0;
|
|
if (enableFlags) {
|
|
// Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Timestamp_L, &ctrl7, 1);//
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, &ctrl7, 1);
|
|
ctrl7 |= QMI8658_CTRL7_ACC_ENABLE;
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, ctrl7 & QMI8658_CTRL7_ENABLE_MASK);
|
|
} else { // close acc
|
|
// Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Timestamp_L, &ctrl7, 1);
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, &ctrl7, 1);
|
|
ctrl7 &= (~QMI8658_CTRL7_ACC_ENABLE);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, ctrl7 & QMI8658_CTRL7_ENABLE_MASK);
|
|
}
|
|
|
|
}
|
|
|
|
void Qmi8658_enable_gyro_Sensors(unsigned char enableFlags)
|
|
{
|
|
unsigned char ctrl7 = 0;
|
|
if (enableFlags) {
|
|
// Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Timestamp_L, &ctrl7, 1);
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, &ctrl7, 1);
|
|
ctrl7 |= QMI8658_CTRL7_GYR_ENABLE;
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, ctrl7 & QMI8658_CTRL7_ENABLE_MASK);
|
|
} else { // close gyro
|
|
// Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Timestamp_L, &ctrl7, 1);
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, &ctrl7, 1);
|
|
ctrl7 &= (~QMI8658_CTRL7_GYR_ENABLE);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, ctrl7 & QMI8658_CTRL7_ENABLE_MASK);
|
|
}
|
|
|
|
}
|
|
|
|
/*************************************************
|
|
*************************************************
|
|
模式切换具体时间查数据手册(差异较大)
|
|
*************************************************
|
|
*************************************************/
|
|
//15ms 后完成
|
|
void Qmi8658_reset(void)//
|
|
{
|
|
log_info("qmi8658_soft_reset \n");
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Reset, 0xb0);//
|
|
Qmi8658_delay(20); // delay
|
|
}
|
|
|
|
//如果是spi3wire模式，执行powerdown后, 必须配置spi为3wire(Qmi8658Register_Ctrl1[bit7=1])
|
|
/* void Qmi8658_power_down(void)//??? */
|
|
/* { */
|
|
/* Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl1, 0);//clk disable??? */
|
|
/* Qmi8658_enableSensors(0); */
|
|
/* } */
|
|
//进入: 只能Power-On Default 进入该模式
|
|
//退出: 先soft reset再init
|
|
u8 Qmi8658_power_down(void)//
|
|
{
|
|
u8 temp_data = 0;
|
|
/* Qmi8658_reset(); */
|
|
log_info("qmi8658_power_down\n");
|
|
//sensorDisable
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl1, &temp_data, 1);
|
|
temp_data |= BIT(0); //sensorDisable
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl1, temp_data);//
|
|
//close acc,gyro,,
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, &temp_data, 1);
|
|
temp_data &= ~0x0f;
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, temp_data);//
|
|
|
|
Qmi8658_delay(10); // delay
|
|
return 1;
|
|
}
|
|
|
|
//进入: 只能Power-On Default, Low Power Accel Only进入该模式
|
|
//退出: 先soft reset再init
|
|
void Qmi8658_low_power(void)//
|
|
{
|
|
u8 temp_data = 0;
|
|
/* Qmi8658_reset(); */
|
|
log_info("qmi8658_low_power\n");
|
|
//sensorenable
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl1, &temp_data, 1);
|
|
temp_data &= ~BIT(0); //sensor enable
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl1, temp_data);//
|
|
//close acc,gyro,,
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, &temp_data, 1);
|
|
temp_data &= ~0x0f;
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, temp_data);//
|
|
//aODR = 11xx
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl2, &temp_data, 1);
|
|
temp_data |= 0x0f;
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl2, temp_data);//
|
|
|
|
Qmi8658_delay(10); // delay
|
|
}
|
|
|
|
|
|
/*
|
|
* spi set:
|
|
* ctrl1: bit7: 3_wire_en
|
|
* bit6: iic/spi_addr_auto_inc
|
|
* bit5: spi_read_big_endian_en
|
|
* 4:1 : reserved
|
|
* bit0: disable internal 2 MHz oscillator
|
|
*/
|
|
void Qmi8658_spi_mode_set(u8 spi_3_wire_en)//仅上电初始化调用
|
|
{
|
|
unsigned char ctrl1 = 0;
|
|
if (spi_3_wire_en) {
|
|
ctrl1 = 0xa0; //默认开大端模式
|
|
} else {
|
|
ctrl1 = 0x20;
|
|
}
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl1, ctrl1);
|
|
}
|
|
void Qmi8658_spi_mode_switch(u8 spi_3_wire_en)
|
|
{
|
|
unsigned char ctrl1 = 0;
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl1, &ctrl1, 1);
|
|
if (spi_3_wire_en) {
|
|
ctrl1 |= 0xa0; //默认开大端模式
|
|
} else {
|
|
ctrl1 &= ~0x80;
|
|
ctrl1 |= 0x20;
|
|
}
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl1, ctrl1);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void Qmi8658_Config_apply(struct Qmi8658Config const *config)
|
|
{
|
|
unsigned char fisSensors = config->inputSelection;
|
|
|
|
Qmi8658_enableSensors(QMI8658_CTRL7_DISABLE_ALL);
|
|
if (fisSensors & QMI8658_CONFIG_AE_ENABLE) {
|
|
Qmi8658_config_ae(config->aeOdr);
|
|
} else {
|
|
if (config->inputSelection & QMI8658_CONFIG_ACC_ENABLE) {
|
|
Qmi8658_config_acc(config->accRange, config->accOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable);
|
|
}
|
|
if (config->inputSelection & QMI8658_CONFIG_GYR_ENABLE) {
|
|
Qmi8658_config_gyro(config->gyrRange, config->gyrOdr, Qmi8658Lpf_Enable, Qmi8658St_Disable);
|
|
}
|
|
}
|
|
|
|
if (config->inputSelection & QMI8658_CONFIG_MAG_ENABLE) {
|
|
Qmi8658_config_mag(config->magDev, config->magOdr);
|
|
}
|
|
|
|
Qmi8658_enableSensors(fisSensors);
|
|
}
|
|
|
|
|
|
#if (QMI8658_USE_STEP_EN)
|
|
unsigned char stepConfig(unsigned short odr) //11hz=0x0 ???
|
|
{
|
|
unsigned short ped_fix_peak, ped_sample_cnt, ped_time_up, ped_fix_peak2peak;
|
|
unsigned char ped_time_low, ped_time_cnt_entry, ped_fix_precision, ped_sig_count, basedODR;
|
|
//void* pi2c = qmi8658_i2c_init();
|
|
|
|
float finalRate;
|
|
//finalRate = 100.0/odr;
|
|
finalRate = 100.0 / odr; //14.285
|
|
ped_sample_cnt = (unsigned short)(0x0032 / finalRate) ;//6;//(unsigned short)(0x0032 / finalRate) ;
|
|
ped_fix_peak2peak = 0x00CC;
|
|
ped_fix_peak = 0x00CC;
|
|
ped_time_up = (unsigned short)(200.0 / finalRate);
|
|
ped_time_low = (unsigned char)(20 / finalRate) ;
|
|
ped_time_cnt_entry = 1;
|
|
ped_fix_precision = 0;
|
|
ped_sig_count = 1;//?????
|
|
|
|
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_L, ped_sample_cnt & 0xFF);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_H, (ped_sample_cnt >> 8) & 0xFF);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_L, ped_fix_peak2peak & 0xFF);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_H, (ped_fix_peak2peak >> 8) & 0xFF);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_L, ped_fix_peak & 0xFF);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_H, (ped_fix_peak >> 8) & 0xFF);
|
|
//Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_L, 0x01);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_H, 0x01);
|
|
log_info("+");
|
|
|
|
|
|
Qmi8658_send_ctl9cmd(Qmi8658_Ctrl9_Cmd_EnablePedometer);
|
|
log_info(".");
|
|
///////////////////////////////
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_L, ped_time_up & 0xFF);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_H, (ped_time_up >> 8) & 0xFF);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_L, ped_time_low);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_H, ped_time_cnt_entry);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_L, ped_fix_precision);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_H, ped_sig_count);
|
|
//Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_L, 0x02 );
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_H, 0x02);
|
|
log_info("-");
|
|
Qmi8658_send_ctl9cmd(Qmi8658_Ctrl9_Cmd_EnablePedometer);
|
|
log_info("*");
|
|
// qmi8658_i2c_deinit(pi2c);
|
|
|
|
|
|
}
|
|
|
|
|
|
uint32_t Qmi8658_step_read_stepcounter(uint32_t *step)
|
|
{
|
|
uint8_t reg_data[3] = {0};
|
|
uint8_t reg_47 = 0;
|
|
uint32_t rslt;
|
|
|
|
#if 1
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_AccEl_X, reg_data, 3);
|
|
#else
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_AccEl_X, reg_data, 1);
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_AccEl_Y, ®_data[1], 1);
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_AccEl_Z, ®_data[2], 1);
|
|
#endif
|
|
|
|
rslt = reg_data[2] << 16 | reg_data[1] << 8 | reg_data[0];
|
|
*step = rslt;
|
|
|
|
return rslt;
|
|
}
|
|
|
|
void step_disable(void)
|
|
{
|
|
unsigned char ctrl8_data = 0x00;
|
|
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl8, &ctrl8_data, 1);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl8, ctrl8_data & (~0x10)); // 0xD0
|
|
|
|
|
|
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_L, 0x00);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_H, 0x00);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_L, 0x00);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_H, 0x00);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_L, 0x00);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_H, 0x00);
|
|
//Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_L, 0x01);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_H, 0x01);
|
|
log_info("+");
|
|
|
|
|
|
Qmi8658_send_ctl9cmd(Qmi8658_Ctrl9_Cmd_Reset_Pedometer);
|
|
log_info(".");
|
|
///////////////////////////////
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_L, 0x00);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_H, 0x00);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_L, 0x00);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_H, 0x00);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_L, 0x00);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_H, 0x00);
|
|
//Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_L, 0x02 );
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_H, 0x02);
|
|
log_info("-");
|
|
Qmi8658_send_ctl9cmd(Qmi8658_Ctrl9_Cmd_Reset_Pedometer);
|
|
log_info("*");
|
|
|
|
|
|
}
|
|
|
|
|
|
#endif
|
|
|
|
|
|
/*****************************
|
|
*********** any motion config ctrl9
|
|
First CTRL9 Command:
|
|
write to CAL1_L the value of AnyMotionXThr.
|
|
write to CAL1_H the value of AnyMotionYThr.
|
|
write to CAL2_L the value of AnyMotionZThr.
|
|
write to CAL2_H the value of NoMotionXThr.
|
|
write to CAL3_L the value of NoMotionYThr.
|
|
write to CAL3_H the value of NoMotionZThr.
|
|
write to CAL4_L the value of MOTION_MODE_CTRL
|
|
write to CAL4_H the value of 0x01
|
|
write to CTRL9 the value of 0x0E // CTRL_CMD_CONFIGURE_MOTION = 0x0E
|
|
|
|
Second CTRL9 Command:
|
|
write to CAL1_L the value of AnyMotionWindow.
|
|
write to CAL1_H the value of NoMotionWindow
|
|
write to CAL2_L the value of SigMotionWaitWindow[7:0]
|
|
write to CAL2_H the value of SigMotionWaitWindow [15:8]
|
|
write to CAL3_L the value of SigMotionConfirmWindow[7:0]
|
|
write to CAL3_H the value of SigMotionConfirmWindow[15:8]
|
|
write to CAL4_H the value of 0x02
|
|
write to CTRL9 the value of 0x0E // CTRL_CMD_CONFIGURE_MOTION 0x0E
|
|
|
|
************************************/
|
|
#if (QMI8658_USE_ANYMOTION_EN)
|
|
/*******/
|
|
/*motion_g_th is bit5~bit7 , 1 bit is 1G . such as : 001 is 1G, 111 is 7G . */
|
|
/*motion_mg_th is bit0 ~bit4, 1bit is 1/32G = 32.25mg; 00001 is 32.25mg , 00002 is 32.25*2mg . motion_mg_th set range is 0~ 31 */
|
|
/*****/
|
|
unsigned char anymotion_Config(enum Qmi8658_anymotion_G_TH motion_g_th, unsigned char motion_mg_th) //11hz=0x0 ?????
|
|
{
|
|
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_L, (motion_g_th + motion_mg_th)); //0x03 // any motion X threshold U 3.5 first three bit(uint 1g) last five bit (uint 1/32 g)
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_H, (motion_g_th + motion_mg_th)); //0x03 //the value of AnyMotionYThr.
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_L, (motion_g_th + motion_mg_th)); //0x03 //the value of AnyMotionZThr.
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_H, 0x03);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_L, 0x03);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_H, 0x03);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_L, 0x07);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_H, 0x01);
|
|
log_info("+");
|
|
|
|
|
|
Qmi8658_send_ctl9cmd(Qmi8658_Ctrl9_Cmd_Motion);
|
|
log_info(".");
|
|
///////////////////////////////
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_L, 0x03); //0x05
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_H, 0x1a);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_L, 0x2c);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_H, 0x01);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_L, 0x64);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_H, 0x00);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_H, 0x02);
|
|
log_info("-");
|
|
Qmi8658_send_ctl9cmd(Qmi8658_Ctrl9_Cmd_Motion);
|
|
log_info("*");
|
|
return 1;
|
|
}
|
|
|
|
|
|
void anymotion_lowpwr_config(void)
|
|
{
|
|
qmi8658_config.inputSelection = QMI8658_CONFIG_ACC_ENABLE;//QMI8658_CONFIG_ACCGYR_ENABLE;
|
|
qmi8658_config.accRange = Qmi8658AccRange_8g;
|
|
qmi8658_config.accOdr = Qmi8658AccOdr_LowPower_11Hz;//Qmi8658AccOdr_LowPower_3Hz;//Qmi8658AccOdr_LowPower_11Hz;
|
|
qmi8658_config.gyrRange = Qmi8658GyrRange_1024dps; //Qmi8658GyrRange_2048dps Qmi8658GyrRange_1024dps
|
|
qmi8658_config.gyrOdr = Qmi8658GyrOdr_250Hz;
|
|
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl8, 0xc2); // 0xC2
|
|
anymotion_Config(Qmi8658_1G, 3) ; // th = 1.1g;
|
|
|
|
//Qmi8658_Config_apply(&qmi8658_config);
|
|
}
|
|
|
|
|
|
void anymotion_high_odr_enable(void)
|
|
{
|
|
unsigned char ctrl8_data = 0x00;
|
|
|
|
/*
|
|
qmi8658_config.inputSelection = QMI8658_CONFIG_ACC_ENABLE;//QMI8658_CONFIG_ACCGYR_ENABLE;
|
|
qmi8658_config.accRange = Qmi8658AccRange_8g;
|
|
qmi8658_config.accOdr = Qmi8658AccOdr_125Hz;
|
|
qmi8658_config.gyrRange = Qmi8658GyrRange_1024dps; //Qmi8658GyrRange_2048dps Qmi8658GyrRange_1024dps
|
|
qmi8658_config.gyrOdr = Qmi8658GyrOdr_125Hz;
|
|
// qmi8658_config.magOdr = Qmi8658MagOdr_125Hz;
|
|
// qmi8658_config.magDev = MagDev_AKM09918;
|
|
// qmi8658_config.aeOdr = Qmi8658AeOdr_128Hz;
|
|
*/
|
|
|
|
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl8, &ctrl8_data, 1);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl8, ctrl8_data | 0xC2); // 0xC2
|
|
anymotion_Config(Qmi8658_1G, 3) ; //// th = 1.1g;
|
|
|
|
//Qmi8658_Config_apply(&qmi8658_config);
|
|
}
|
|
|
|
|
|
void anymotion_disable(void)
|
|
{
|
|
unsigned char ctrl8_data = 0x00;
|
|
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl8, &ctrl8_data, 1);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl8, ctrl8_data & (~0x02)); // 0xC2
|
|
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_L, 0x23); //0x03 // any motion X threshold U 3.5 first three bit(uint 1g) last five bit (uint 1/32 g)
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_H, 0x23); //0x03 //the value of AnyMotionYThr.
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_L, 0x23); //0x03 //the value of AnyMotionZThr.
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_H, 0x03);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_L, 0x03);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_H, 0x03);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_L, 0x00);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_H, 0x01);
|
|
log_info("+");
|
|
|
|
|
|
Qmi8658_send_ctl9cmd(Qmi8658_Ctrl9_Cmd_Motion);
|
|
log_info(".");
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_L, 0x03); //0x05
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_H, 0x1a);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_L, 0x2c);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_H, 0x01);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_L, 0x64);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_H, 0x00);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_H, 0x02);
|
|
log_info("-");
|
|
Qmi8658_send_ctl9cmd(Qmi8658_Ctrl9_Cmd_Motion);
|
|
log_info("*");
|
|
|
|
|
|
}
|
|
|
|
#endif
|
|
/***********************************
|
|
First CTRL9 Command:
|
|
write to CAL1_L the value of peakWindow. //30表示， 30 ms at 1000 Hz .就是ODR的个数。
|
|
write to CAL1_H the value of priority.
|
|
write to CAL2_L the value of TapWindow low byte. //100表示， 100 ms at 1000 Hz 就是ODR的个数。
|
|
write to CAL2_H the value of TapWindow high byte.
|
|
write to CAL3_L the value of DTapWindow low byte.
|
|
write to CAL3_H the value of DTapWindow high byte.
|
|
write to CAL4_H the value of 0x01
|
|
write to CTRL9 the value of 0x0C // CTRL_CMD_CONFIGURE_TAP = 0x0C
|
|
|
|
Second CTRL9 Command:
|
|
write to CAL1_L the value of alpha. //更新下个数据的百分比
|
|
write to CAL1_H the value of gamma. //差值数据更新的百分比
|
|
write to CAL2_L the value of peakMagThr low byte. xyz 模值的阀值。 敲的力度阀值
|
|
write to CAL2_H the value of peakMagThr high byte.
|
|
write to CAL3_L the value of UDMThr low byte. //停止敲击的，阀值，gamma与其有关。值设置越大，越容易满足条件。
|
|
write to CAL3_H the value of UDMThr high byte.
|
|
write to CAL4_H the value of 0x02
|
|
write to CTRL9 the value of 0x0C // CTRL_CMD_CONFIGURE_TAP 0x0C
|
|
|
|
|
|
*************************************************/
|
|
|
|
#if (QMI8658_USE_TAP_EN)
|
|
unsigned char tap_Config(void) //11hz=0x0e ????
|
|
{
|
|
#if 1 //alpha tap param
|
|
uint8_t peakWindow = 0x0e; //0x1e 30ms at 1000hz ODR.
|
|
uint8_t priority = 0x04;
|
|
uint16_t TapWindow = 0x000c; //0x0016 //0x0064 100MS at 100hz odr
|
|
uint16_t DTapWindow = 0x0058; //
|
|
uint8_t alpha = 0x40; //0x40; //0x08; //0.065 //alpha 百分比 =alpha[7] + alpha[6~0]* (1/128)
|
|
uint8_t gamma = 0x20; //0x10; //0x20;//0x20; //0.25 //gamma 百分比 =gamma[7] + gamma[6~0]* (1/128)
|
|
uint16_t peakMagThr = 0x0069;//0x0200;//0x0599//0x0599; // 1.4g
|
|
uint16_t UDMThr = 0x0049; //0x0100; //0x0199; // 0.4g
|
|
#endif //
|
|
|
|
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_L, peakWindow); //0x1E
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_H, priority); //0x03
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_L, TapWindow & 0xff); //0x64
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_H, (TapWindow >> 8) & 0xff);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_L, DTapWindow & 0xff);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_H, (DTapWindow >> 8) & 0xff);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_H, 0x01);
|
|
log_info("+");
|
|
|
|
|
|
Qmi8658_send_ctl9cmd(Qmi8658_Ctrl9_Cmd_EnableTap);
|
|
log_info(".");
|
|
///////////////////////////////
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_L, alpha);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_H, gamma); //0x20
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_L, peakMagThr & 0xff);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_H, (peakMagThr >> 8) & 0xff); //0x05
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_L, UDMThr & 0xff);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_H, (UDMThr >> 8) & 0xff); //0x01
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal4_H, 0x02);
|
|
log_info("-");
|
|
Qmi8658_send_ctl9cmd(Qmi8658_Ctrl9_Cmd_EnableTap);
|
|
log_info("*");
|
|
|
|
}
|
|
|
|
void tap_enable(void)
|
|
{
|
|
unsigned char ctrl8_data = 0x00;
|
|
|
|
/*
|
|
qmi8658_config.inputSelection = QMI8658_CONFIG_ACCGYR_ENABLE;//QMI8658_CONFIG_ACCGYR_ENABLE;
|
|
qmi8658_config.accRange = Qmi8658AccRange_2g;
|
|
qmi8658_config.accOdr = Qmi8658AccOdr_125Hz;
|
|
qmi8658_config.gyrRange = Qmi8658GyrRange_1024dps; //Qmi8658GyrRange_2048dps Qmi8658GyrRange_1024dps
|
|
qmi8658_config.gyrOdr = Qmi8658GyrOdr_125Hz;
|
|
//qmi8658_config.magOdr = Qmi8658MagOdr_125Hz;
|
|
//qmi8658_config.magDev = MagDev_AKM09918;
|
|
//qmi8658_config.aeOdr = Qmi8658AeOdr_128Hz;
|
|
*/
|
|
|
|
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl8, &ctrl8_data, 1);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl8, ctrl8_data | 0xc1); // 0xC2
|
|
tap_Config() ;
|
|
|
|
//Qmi8658_Config_apply(&qmi8658_config);
|
|
}
|
|
#endif
|
|
#if (QMI8658_NEW_HANDSHAKE)
|
|
void qmi8658_on_demand_cali(void)
|
|
{
|
|
unsigned char cod_status = 0x00;
|
|
unsigned char cod_data[6];
|
|
|
|
log_info("qmi8658_on_demand_cali start");
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl9, (unsigned char)Qmi8658_Ctrl9_Cmd_On_Demand_Cali);
|
|
Qmi8658_delay(2200); // delay 2000ms above
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl9, 0x00);//qmi8658_Ctrl9_Cmd_Ack
|
|
Qmi8658_delay(10); // delay
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, 0x70/* Qmi8658Register_Cod_Status */, &cod_status, 1);
|
|
if (cod_status) {
|
|
log_info("qmi8658_on_demand_cali fail! status=0x%x", cod_status);
|
|
} else {
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Dvx_L, &cod_data[0], 6);//qmi8658_get_gyro_gain
|
|
log_info("qmi8658_on_demand_cali done! cod[%d %d %d]",
|
|
(unsigned short)(cod_data[1] << 8 | cod_data[0]),
|
|
(unsigned short)(cod_data[3] << 8 | cod_data[2]),
|
|
(unsigned short)(cod_data[5] << 8 | cod_data[4]));
|
|
}
|
|
}
|
|
void qmi8658_apply_gyr_gain(unsigned char cod_data[6])
|
|
{
|
|
u8 cod_data_temp[8];
|
|
|
|
/* Qmi8658_enableSensors(QMI8658_CTRL7_DISABLE_ALL); */
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_L, cod_data[0]);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal1_H, cod_data[1]);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_L, cod_data[2]);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal2_H, cod_data[3]);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_L, cod_data[4]);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Cal3_H, cod_data[5]);
|
|
|
|
Qmi8658_send_ctl9cmd(0xaa);//(qmi8658_Ctrl9_Cmd_Apply_Gyro_Gain);
|
|
/* Qmi8658_enableSensors(QMI8658_CONFIG_ACCGYR_ENABLE); */
|
|
}
|
|
#endif
|
|
unsigned char qmi8658_init(u8 demand_cali_en)
|
|
{
|
|
unsigned char qmi8658_chip_id = 0x00;
|
|
unsigned char qmi8658_revision_id = 0x00;
|
|
unsigned char iCount = 0;
|
|
|
|
unsigned char fw_version[3] = {0};
|
|
unsigned char usid_version[6] = {0};
|
|
#if (QMI8658_USER_INTERFACE==QMI8658_USE_SPI)
|
|
if (qmi8658_info->spi_work_mode) {
|
|
Qmi8658_spi_mode_set(1);//使能spi_3_wire
|
|
} else {
|
|
Qmi8658_spi_mode_set(0);//使能spi_4_wire
|
|
}
|
|
#endif
|
|
while ((qmi8658_chip_id == 0x00) && (iCount < 10)) {
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_WhoAmI, &qmi8658_chip_id, 1);
|
|
if (qmi8658_chip_id == 0x05) {
|
|
break;
|
|
}
|
|
iCount++;
|
|
mdelay(1);
|
|
}
|
|
|
|
/* Qmi8658_read(QMI8658_SLAVE_ADDRESS, 0x49, fw_version, 3); */
|
|
/* log_info("Qmi8658_fw_version(Quaternion Increment dQW?)=0x%x, 0x%x, 0x%x \n", fw_version[0],fw_version[1],fw_version[2]); */
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Revision, &qmi8658_revision_id, 1);
|
|
|
|
if (qmi8658_chip_id == 0x05) {
|
|
log_info("Qmi8658_init slave=0x%x Qmi8658Register_WhoAmI=0x%x 0x%x\n", QMI8658_SLAVE_ADDRESS, qmi8658_chip_id, qmi8658_revision_id);
|
|
|
|
/* Qmi8658_read(QMI8658_SLAVE_ADDRESS, 0x51, usid_version, 6); */
|
|
/* log_info("Qmi8658_usid_version(Velocity Increment along XYZ?)=0x%x, 0x%x, 0x%x ,0x%x, 0x%x, 0x%x \n", usid_version[0],usid_version[1],usid_version[2] ,usid_version[3],usid_version[4],usid_version[5]); */
|
|
|
|
#if (QMI8658_NEW_HANDSHAKE==0)
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl1, &iCount, 1);
|
|
iCount |= 0x60;
|
|
iCount &= ~ 0x01;
|
|
/* iCount |= 0x04; */
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl1, iCount);//bit7:1:spi_3_wire, bit6:addr_auto_inc, bit5:spi_read_big_endian(大端),bit0:0:clkosc
|
|
#else
|
|
/* Qmi8658_reset(); */
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl1, 0x60 | 0x08 | 0x10); // 另一种qmi8658
|
|
if (demand_cali_en) {
|
|
qmi8658_on_demand_cali();
|
|
/* printf("set cod"); */
|
|
} else {
|
|
/* qmi8658_apply_gyr_gain(cod_data_copy); */
|
|
/* printf("restore cod"); */
|
|
}
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, 0x00);
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl8, 0xc0);
|
|
#endif
|
|
qmi8658_config.inputSelection = QMI8658_CONFIG_ACCGYR_ENABLE;//QMI8658_CONFIG_ACCGYR_ENABLE;
|
|
qmi8658_config.accRange = Qmi8658AccRange_16g;
|
|
qmi8658_config.accOdr = Qmi8658AccOdr_125Hz;
|
|
qmi8658_config.gyrRange = Qmi8658GyrRange_2048dps; //Qmi8658GyrRange_2048dps Qmi8658GyrRange_1024dps
|
|
qmi8658_config.gyrOdr = Qmi8658GyrOdr_125Hz;
|
|
|
|
#if (QMI8658_USE_TAP_EN)
|
|
tap_enable();
|
|
#endif
|
|
|
|
#if (QMI8658_USE_ANYMOTION_EN)
|
|
//anymotion_high_odr_enable();
|
|
anymotion_lowpwr_config();
|
|
#endif
|
|
|
|
#if (QMI8658_USE_FIFO_EN)
|
|
Qmi8658_config_fifo(128, Qmi8658_Fifo_128, Qmi8658_Fifo_Stream, QMI8658_FORMAT_12_BYTES);
|
|
#endif
|
|
|
|
#if (QMI8658_USE_STEP_EN)
|
|
qmi8658_config.inputSelection = QMI8658_CONFIG_ACC_ENABLE;
|
|
qmi8658_config.accRange = Qmi8658AccRange_8g;
|
|
qmi8658_config.accOdr = Qmi8658AccOdr_LowPower_21Hz;//Qmi8658AccOdr_62_5Hz;//Qmi8658AccOdr_LowPower_21Hz;
|
|
qmi8658_config.gyrRange = Qmi8658GyrRange_1024dps; //Qmi8658GyrRange_2048dps Qmi8658GyrRange_1024dps
|
|
qmi8658_config.gyrOdr = Qmi8658GyrOdr_500Hz;
|
|
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, 0x00); //
|
|
Qmi8658_write(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl8, 0xD0);
|
|
stepConfig(21); //ODR = 21hz
|
|
//stepConfig(62); //ODR = 62.5HZ
|
|
#endif
|
|
|
|
Qmi8658_Config_apply(&qmi8658_config);
|
|
#if 0
|
|
unsigned char read_data = 0x00;
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl1, &read_data, 1);
|
|
log_info("Qmi8658Register_Ctrl1=0x%x \n", read_data);
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl2, &read_data, 1);
|
|
log_info("Qmi8658Register_Ctrl2=0x%x \n", read_data);
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl3, &read_data, 1);
|
|
log_info("Qmi8658Register_Ctrl3=0x%x \n", read_data);
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl4, &read_data, 1);
|
|
log_info("Qmi8658Register_Ctrl4=0x%x \n", read_data);
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl5, &read_data, 1);
|
|
log_info("Qmi8658Register_Ctrl5=0x%x \n", read_data);
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl6, &read_data, 1);
|
|
log_info("Qmi8658Register_Ctrl6=0x%x \n", read_data);
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_Ctrl7, &read_data, 1);
|
|
log_info("Qmi8658Register_Ctrl7=0x%x \n", read_data);
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, 77, &qmi8658_revision_id, 1);
|
|
log_info("teset:0x%x", qmi8658_revision_id);
|
|
#endif
|
|
// Qmi8658_set_layout(2);
|
|
return 1;
|
|
} else {
|
|
log_error("Qmi8658_init fail\n");
|
|
qmi8658_chip_id = 0;
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
static u8 qmi8658_int_pin1 = IO_PORTB_04;
|
|
static u8 qmi8658_int_pin2 = IO_PORTB_03;
|
|
//return:0:fail, 1:ok
|
|
u8 qmi8658_sensor_init(void *priv)
|
|
{
|
|
if (priv == NULL) {
|
|
log_error("qmi8658 init fail(no param)\n");
|
|
return 0;
|
|
}
|
|
qmi8658_info = (qmi8658_param *)priv;
|
|
|
|
#if (QMI8658_USER_INTERFACE==QMI8658_USE_I2C) //iic interface
|
|
iic_init(qmi8658_info->iic_hdl);
|
|
#elif (QMI8658_USER_INTERFACE==QMI8658_USE_SPI)//spi interface
|
|
spi_cs_init();
|
|
spi_init();
|
|
#else
|
|
//I3C
|
|
#endif
|
|
gpio_set_die(qmi8658_int_pin1, 1);
|
|
gpio_set_direction(qmi8658_int_pin1, 1);
|
|
// gpio_set_pull_up(qmi8658_int_pin1,1);
|
|
gpio_set_die(qmi8658_int_pin2, 1);
|
|
gpio_set_direction(qmi8658_int_pin2, 1);
|
|
// gpio_set_pull_up(qmi8658_int_pin2,1);
|
|
return qmi8658_init(1);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static volatile u32 qmi8658_init_flag ;
|
|
static u8 imu_busy = 0;
|
|
static qmi8658_param qmi8658_info_data;
|
|
/* #define SENSORS_MPU_BUFF_LEN 14 */
|
|
/* u8 read_qmi8658_buf[SENSORS_MPU_BUFF_LEN]; */
|
|
#if (QMI8658_USE_FIFO_EN)
|
|
u8 fifo_data[1024];//size:Qmi8658_FifoSize * 6 * Number_of_enabled_sensors
|
|
#endif
|
|
void qmi8658_int_callback()
|
|
{
|
|
if (qmi8658_init_flag == 0) {
|
|
log_error("qmi8658 init fail!");
|
|
return ;
|
|
}
|
|
if (imu_busy) {
|
|
log_error("qmi8658 busy!");
|
|
return ;
|
|
}
|
|
imu_busy = 1;
|
|
|
|
imu_sensor_data_t raw_sensor_datas;
|
|
float TempData = 0.0;
|
|
u8 status_temp = 0;
|
|
#if (QMI8658_USE_FIFO_EN)
|
|
u16 fifo_level = 0;
|
|
fifo_level = Qmi8658_read_fifo(fifo_data);
|
|
TempData = Qmi8658_readTemp();
|
|
/* log_info("qmi8658 raw:acc_x:%d acc_y:%d acc_z:%d gyro_x:%d gyro_y:%d gyro_z:%d, tim_count:%d", (s16)(fifo_data[1]<<8|fifo_data[0]), (s16)(fifo_data[3]<<8|fifo_data[2]), (s16)(fifo_data[5]<<8|fifo_data[4]), (s16)(fifo_data[7]<<8|fifo_data[6]), (s16)(fifo_data[9]<<8|fifo_data[8]), (s16)(fifo_data[11]<<8|fifo_data[10]),fifo_level);//共fifo_level组数据, 只打印第一组数据 */
|
|
#else
|
|
status_temp = Qmi8658_readStatus0();
|
|
/* log_info("status:0x%x", status_temp); */
|
|
if (status_temp & 0x03) {
|
|
Qmi8658_read_raw_acc_gyro_xyz(&raw_sensor_datas);//获取原始值
|
|
/* TempData=Qmi8658_readTemp(); */
|
|
TempData = raw_sensor_datas.temp_data;
|
|
/* log_info("qmi8658 raw:acc_x:%d acc_y:%d acc_z:%d gyro_x:%d gyro_y:%d gyro_z:%d", raw_sensor_datas.acc.x, raw_sensor_datas.acc.y, raw_sensor_datas.acc.z, raw_sensor_datas.gyro.x, raw_sensor_datas.gyro.y, raw_sensor_datas.gyro.z); */
|
|
}
|
|
#endif
|
|
/* log_info("qmi8658 temp:%d.%d\n", (u16)TempData, (u16)(((u16)(TempData * 100)) % 100)); */
|
|
imu_busy = 0;
|
|
}
|
|
|
|
s8 qmi8658_dev_init(void *arg)
|
|
{
|
|
if (arg == NULL) {
|
|
log_error("qmi8658 init fail(no arg)\n");
|
|
return -1;
|
|
}
|
|
#if (QMI8658_USER_INTERFACE==QMI8658_USE_I2C)
|
|
qmi8658_info_data.iic_hdl = ((struct imusensor_platform_data *)arg)->peripheral_hdl;
|
|
qmi8658_info_data.iic_delay = ((struct imusensor_platform_data *)arg)->peripheral_param0; //这个延时并非影响iic的时钟频率，而是2Byte数据之间的延时
|
|
// u8 iic_clk; //iic_clk: <=400kHz
|
|
#elif (QMI8658_USER_INTERFACE==QMI8658_USE_SPI)
|
|
qmi8658_info_data.spi_hdl = ((struct imusensor_platform_data *)arg)->peripheral_hdl, //SPIx (role:master)
|
|
qmi8658_info_data.spi_cs_pin = ((struct imusensor_platform_data *)arg)->peripheral_param0; //IO_PORTA_05
|
|
qmi8658_info_data.spi_work_mode = ((struct imusensor_platform_data *)arg)->peripheral_param1; //1:3wire(SPI_MODE_UNIDIR_1BIT) or 0:4wire(SPI_MODE_BIDIR_1BIT) (与spi结构体一样)
|
|
// u8 port; //SPIx group:A,B,C,D (spi结构体)
|
|
// U8 spi_clk; //spi_clk: <=15MHz (spi结构体)
|
|
#else
|
|
//I3C
|
|
#endif
|
|
qmi8658_int_pin2 = ((struct imusensor_platform_data *)arg)->imu_sensor_int_io;
|
|
qmi8658_int_pin1 = qmi8658_int_pin2;
|
|
if (imu_busy) {
|
|
log_error("qmi8658 busy!");
|
|
return -1;
|
|
}
|
|
imu_busy = 1;
|
|
if (qmi8658_sensor_init(&qmi8658_info_data)) {
|
|
#if (QMI8658_USE_INT_EN) //中断模式,暂不支持
|
|
log_info("int mode en!");
|
|
/* port_wkup_enable(qmi8658_int_pin2, 1, qmi8658_int_callback); //PA08-IO中断，1:下降沿触发，回调函数qmi8658_int_callback*/
|
|
#ifdef CONFIG_CPU_BR23
|
|
io_ext_interrupt_init(qmi8658_int_pin2, 1, qmi8658_int_callback);
|
|
#elif defined(CONFIG_CPU_BR28)
|
|
// br28外部中断回调函数，按照现在的外部中断注册方式
|
|
// io配置在板级，定义在板级头文件，这里只是注册回调函数
|
|
/* port_edge_wkup_set_callback_by_index(3, qmi8658_int_callback); // 序号需要和板级配置中的wk_param对应上 */
|
|
port_edge_wkup_set_callback(qmi8658_int_callback);
|
|
#elif defined(CONFIG_CPU_BR27)
|
|
port_edge_wkup_set_callback(qmi8658_int_callback);
|
|
#endif
|
|
#else //定时
|
|
|
|
#endif
|
|
qmi8658_init_flag = 1;
|
|
imu_busy = 0;
|
|
log_info("qmi8658 Device init success!%d\n", qmi8658_init_flag);
|
|
return 0;
|
|
} else {
|
|
log_info("qmi8658 Device init fail!\n");
|
|
imu_busy = 0;
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
|
|
int qmi8658_dev_ctl(u8 cmd, void *arg);
|
|
REGISTER_IMU_SENSOR(qmi8658_sensor) = {
|
|
.logo = "qmi8658",
|
|
.imu_sensor_init = qmi8658_dev_init,
|
|
.imu_sensor_check = NULL,
|
|
.imu_sensor_ctl = qmi8658_dev_ctl,
|
|
};
|
|
|
|
int qmi8658_dev_ctl(u8 cmd, void *arg)
|
|
{
|
|
int ret = -1;
|
|
u8 status_temp = 0;
|
|
if (qmi8658_init_flag == 0) {
|
|
log_error("qmi8658 init fail!");
|
|
return ret;//0:ok,,<0:err
|
|
}
|
|
if (imu_busy) {
|
|
log_error("qmi8658 busy!");
|
|
return ret;//0:ok,,<0:err
|
|
}
|
|
imu_busy = 1;
|
|
switch (cmd) {
|
|
case IMU_GET_SENSOR_NAME:
|
|
memcpy((u8 *)arg, &(qmi8658_sensor.logo), 20);
|
|
ret = 0;
|
|
break;
|
|
case IMU_SENSOR_ENABLE: //enable demand cali
|
|
/* cbuf_init(&hrsensor_cbuf, hrsensorcbuf, 24 * sizeof(int)); */
|
|
qmi8658_init(1);
|
|
ret = 0;
|
|
break;
|
|
case IMU_SENSOR_DISABLE:
|
|
/* cbuf_clear(&hrsensor_cbuf); */
|
|
//close power
|
|
Qmi8658_power_down();
|
|
ret = 0;
|
|
break;
|
|
case IMU_SENSOR_RESET://reset后必须enable
|
|
Qmi8658_reset();
|
|
ret = 0;
|
|
break;
|
|
case IMU_SENSOR_SLEEP:
|
|
if (Qmi8658_power_down()) {
|
|
log_info("qmi8658 enter sleep ok!");
|
|
ret = 0;
|
|
} else {
|
|
log_error("qmi8658 enter sleep fail!");
|
|
}
|
|
break;
|
|
case IMU_SENSOR_WAKEUP: //disable demand cali
|
|
if (qmi8658_init(0)) {
|
|
log_info("qmi8658 wakeup ok!");
|
|
ret = 0;
|
|
} else {
|
|
log_error("qmi8658 wakeup fail!");
|
|
}
|
|
break;
|
|
case IMU_SENSOR_INT_DET://传感器中断状态检查
|
|
break;
|
|
case IMU_SENSOR_DATA_READY://传感器数据准备就绪待读
|
|
break;
|
|
case IMU_SENSOR_CHECK_DATA://检查传感器缓存buf是否存满
|
|
break;
|
|
case IMU_SENSOR_READ_DATA://默认读传感器所有数据
|
|
status_temp = Qmi8658_readStatus0();
|
|
/* log_info("status:0x%x", status_temp); */
|
|
if (status_temp & 0x03) {
|
|
Qmi8658_read_raw_acc_gyro_xyz(arg);//获取原始值
|
|
ret = 0;
|
|
}
|
|
break;
|
|
case IMU_GET_ACCEL_DATA://加速度数据
|
|
/* float TempData = 0.0; */
|
|
status_temp = Qmi8658_readStatus0();
|
|
/* log_info("status:0x%x", status_temp); */
|
|
if (status_temp & 0x01) {
|
|
Qmi8658_read_raw_acc_xyz(arg);//获取原始值
|
|
ret = 0;
|
|
/* TempData = Qmi8658_readTemp(); */
|
|
}
|
|
break;
|
|
case IMU_GET_GYRO_DATA://陀螺仪数据
|
|
status_temp = Qmi8658_readStatus0();
|
|
/* log_info("status:0x%x", status_temp); */
|
|
if (status_temp & 0x02) {
|
|
Qmi8658_read_raw_gyro_xyz(arg);//获取原始值
|
|
ret = 0;
|
|
}
|
|
break;
|
|
case IMU_GET_MAG_DATA://磁力计数据
|
|
log_error("qmi8658 have no mag!\n");
|
|
break;
|
|
case IMU_SENSOR_SEARCH://检查传感器id
|
|
Qmi8658_read(QMI8658_SLAVE_ADDRESS, Qmi8658Register_WhoAmI, (u8 *)arg, 1);
|
|
if (*(u8 *)arg == 0x05) {
|
|
ret = 0;
|
|
log_info("qmi8658 online!\n");
|
|
} else {
|
|
log_error("qmi8658 offline!\n");
|
|
}
|
|
break;
|
|
case IMU_GET_SENSOR_STATUS://获取传感器状态
|
|
status_temp = Qmi8658_readStatus0();
|
|
*(u8 *)arg = status_temp;
|
|
ret = 0;
|
|
break;
|
|
case IMU_SET_SENSOR_FIFO_CONFIG://配置传感器FIFO
|
|
#if (QMI8658_USE_FIFO_EN)
|
|
u8 *tmp = (u8 *)arg;
|
|
u8 wm_th = tmp[0];
|
|
u8 fifo_size = tmp[1];//Qmi8658_Fifo_64
|
|
u8 fifo_mode = tmp[2];//Qmi8658_Fifo_Stream
|
|
u8 fifo_format = tmp[3];//QMI8658_FORMAT_12_BYTES
|
|
Qmi8658_config_fifo(wm_th, fifo_size, fifo_mode, fifo_format);
|
|
ret = 0;
|
|
#endif
|
|
break;
|
|
case IMU_GET_SENSOR_READ_FIFO://读取传感器FIFO数据
|
|
#if (QMI8658_USE_FIFO_EN)
|
|
ret = Qmi8658_read_fifo((u8 *)arg);
|
|
#endif
|
|
break;
|
|
case IMU_SET_SENSOR_TEMP_DISABLE://温度传感器无法控制
|
|
u8 temp = *(u8 *)arg;
|
|
ret = 0;
|
|
break;
|
|
default:
|
|
log_error("--cmd err!\n");
|
|
break;
|
|
}
|
|
imu_busy = 0;
|
|
return ret;//0:ok,,<0:err
|
|
|
|
}
|
|
|
|
|
|
|
|
#endif
|
|
|