Skip to content

GitLab

B
BA001-sixAxisDriver
Commit 7e636aa8 authored by Evandeng's avatar Evandeng
Browse files
Options

增加Gsensor的fifo配置方式

parent b7a0d953
Hide whitespace changes
Inline Side-by-side
Showing with 117 additions and 92 deletions
SW/drive/icm40608_step/Invn/Drivers/Icm406xx/Icm406xxDriver_HL.c
View file @ 7e636aa8
......@@ -867,12 +867,15 @@ int inv_icm406xx_get_data_from_fifo(struct inv_icm406xx * s)
}
}
} else {
status |= inv_icm406xx_read_reg(s, MPUREG_FIFO_DATA, packet_size * packet_count, s->fifo_data);
if(status) {
/* sensor data is in FIFO according to FIFO_COUNT but failed to read FIFO,
reset FIFO and try next chance */
inv_icm406xx_reset_fifo(s);
return status;
for(packet_count_i = 0;packet_count_i < packet_count;packet_count_i++)
{
status |= inv_icm406xx_read_reg(s, MPUREG_FIFO_DATA, packet_size, &s->fifo_data[packet_count_i*packet_size]);
if(status) {
/* sensor data is in FIFO according to FIFO_COUNT but failed to read FIFO,
reset FIFO and try next chance */
inv_icm406xx_reset_fifo(s);
return status;
}
}
}
......@@ -1364,7 +1367,7 @@ int inv_icm406xx_configure_fifo(struct inv_icm406xx * s, INV_ICM406XX_FIFO_CONFI
data |= (uint8_t)ICM406XX_FIFO_CONFIG1_WM_GT_TH_EN;
status |= inv_icm406xx_write_reg(s, MPUREG_FIFO_CONFIG1, 1, &data);
/* Configure FIFO WM so that INT is triggered for each packet */
data = 0x1;
data = 127;
status |= inv_icm406xx_write_reg(s, MPUREG_FIFO_CONFIG2, 1, &data);
/* Disable Data Ready Interrupt */
......
SW/drive/icm40608_step/icm40608.c
View file @ 7e636aa8
......@@ -349,7 +349,6 @@ typedef union {
int16_t data;
} data_union;
Icm40608data_st icm40608data = {0};
struct inv_icm406xx_serif icm406xx_serif;
......@@ -359,9 +358,10 @@ typedef struct
{
iic_write write_fun;
iic_read read_fun;
systick_get inv_systick_get;
sys_sleep_us inv_systick_sleep;
int_get_rawdata get_rawdata;
icm40608_close_cb close_cb;
}InvIicOpera;
static InvIicOpera inviicopera = {0};
......@@ -403,28 +403,7 @@ static uint8_t Icm40608_read_ID(void)
return id;
}
void Icm40608_init(void)
{
if (DEVICE_ID == Icm40608_read_ID())
{
// NRF_LOG_INFO("Icm40608Init successful");
}
else
{
// NRF_LOG_INFO("Icm40608Init fail");
}
inv_icm406xx_device_reset(&icm_driver);
// PSYS_Parm sys_value = GlobalParam_Get();
// sys_value->runing->accel_sample_rate = DF_RATE_HZ;
// sys_value->runing->gyro_sample_rate = DF_RATE_HZ;
// sys_value->runing->mag_sample_rate = DF_RATE_HZ;
// Icm40608_set_sample_rate(sys_value->runing->accel_sample_rate, sys_value->runing->gyro_sample_rate);
//acc LN mode
//gyro LN mode
//icm40608_register_write(PWR_MGMT0, ACC_LP_GYRO_LN);
}
int8_t Icm40608_set_sample_rate(uint8_t accel_rate, uint8_t gyro_rate)
{
......@@ -760,43 +739,6 @@ void Icm40608_Gyro_Value_Get(float *gyr_real_x, float *gyr_real_y, float *gyr_re
*gyr_real_z = 0.0;
}
static void GYR_Value2Rawdata(float real_x, float real_y, float real_z, int16_t *x, int16_t *y, int16_t *z)
{
float resolution = 0;
uint8_t data;
double tmp_x = 0, tmp_y = 0, tmp_z = 0;
icm40608_register_read(GYRO_CONFIG0, &data, 1);
uint32_t range = (data & (0x07 << 5));
switch (range)
{
case ICM40608_GYRO_RANGE_2000:
resolution = 16.4f; //2000
break;
case ICM40608_GYRO_RANGE_1000:
resolution = 32.8f; //1000
break;
case ICM40608_GYRO_RANGE_500:
resolution = 65.6f; //500
break;
case ICM40608_GYRO_RANGE_250:
resolution = 131.2f; //250
break;
case ICM40608_GYRO_RANGE_125:
resolution = 262.4f; //125
break;
default:
resolution = 16.4f; //2000
break;
}
tmp_x = real_x * resolution;
*x = (int16_t)tmp_x;
tmp_y = real_y * resolution;
*y = (int16_t)tmp_y;
tmp_z = real_z * resolution;
*z = (int16_t)tmp_z;
}
void Icm40608_ACC_Value_Get(float *acc_real_x, float *acc_real_y, float *acc_real_z)
{
......@@ -976,12 +918,11 @@ void Icm40608_Gyro_GetRawData(uint8_t *data)
*/
uint64_t inv_icm406xx_get_time_us(void)
{
uint32_t systick;
if(inviicopera.inv_systick_get)
{
inviicopera.inv_systick_get(&systick);
return inviicopera.inv_systick_get();
}
return systick;
return 0;
}
/*
* Icm406xx driver needs a sleep feature from external device. Thus inv_icm406xx_sleep_us
......@@ -995,6 +936,21 @@ void inv_icm406xx_sleep_us(uint32_t us)
}
}
static void sensor_event_cb(inv_icm406xx_sensor_event_t * event)
{
if(inviicopera.get_rawdata)
{
inviicopera.get_rawdata(event->accel,event->gyro);
}
}
int icm40608_getdata_fifo(void)
{
return inv_icm406xx_get_data_from_fifo(&icm_driver);
}
/* --------------------------------------------------------------------------------------
* Functions definition
* -------------------------------------------------------------------------------------- */
......@@ -1010,7 +966,7 @@ int SetupInvDevice(struct inv_icm406xx_serif *icm_serif)
/* Init device */
// NRF_LOG_INFO("Initialize Icm406xx");
rc = inv_icm406xx_init(&icm_driver, icm_serif, NULL);
rc = inv_icm406xx_init(&icm_driver, icm_serif, sensor_event_cb);
if (rc != INV_ERROR_SUCCESS)
{
// NRF_LOG_INFO("!!! ERROR : failed to initialize Icm406xx.");
......@@ -1136,6 +1092,25 @@ int ConfigureInvDevice(ICM406XX_APEX_CONFIG0_DMP_ODR_t pedometer_freq,
return rc;
}
int ConfigureInvDevice_fifo(ICM406XX_ACCEL_CONFIG0_FS_SEL_t acc_fsr_g,
ICM406XX_GYRO_CONFIG0_FS_SEL_t gyr_fsr_dps,
ICM406XX_ACCEL_CONFIG0_ODR_t acc_freq,
ICM406XX_GYRO_CONFIG0_ODR_t gyr_freq)
{
int rc = 0;
rc |= inv_icm406xx_set_accel_fsr(&icm_driver, acc_fsr_g);
rc |= inv_icm406xx_set_gyro_fsr(&icm_driver, gyr_fsr_dps);
rc |= inv_icm406xx_set_accel_frequency(&icm_driver, acc_freq);
rc |= inv_icm406xx_set_gyro_frequency(&icm_driver, gyr_freq);
rc |= inv_icm406xx_enable_accel_low_power_mode(&icm_driver);
rc |= inv_icm406xx_enable_gyro_low_noise_mode(&icm_driver);
return rc;
}
void icm40608_open(void)
{
// inv_icm406xx_enable_accel_low_power_mode(&icm_driver);
......@@ -1160,6 +1135,12 @@ void icm40608_open(void)
}
int icm40608_fifo_config(ACC_RANGE_E accrange,GYRO_RANGE_E gyrorange,ACC_CONFIG0_ODR_t accodr,GYRO_CONFIG0_ODR_t gyroodr)
{
SetupInvDevice(&icm406xx_serif);
return ConfigureInvDevice_fifo((ICM406XX_ACCEL_CONFIG0_FS_SEL_t)accrange,(ICM406XX_GYRO_CONFIG0_FS_SEL_t)gyrorange,(ICM406XX_ACCEL_CONFIG0_ODR_t)accodr,(ICM406XX_GYRO_CONFIG0_ODR_t)gyroodr);
}
void icm40608_close(void)
{
......@@ -1168,6 +1149,11 @@ void icm40608_close(void)
status = inv_icm406xx_disable_gyro(&icm_driver);
// NRF_LOG_INFO("status:%d\n",status);
if(inviicopera.close_cb)
{
inviicopera.close_cb();
}
}
/* I2C slave address for INV device */
......@@ -1213,13 +1199,27 @@ void icm40608_config(void)
icm40608_close();
}
void icm40608_init(iic_write write_fun,iic_read read_fun,systick_get systick_get_fun,sys_sleep_us sys_sleep_us_fun)
void icm40608_init(iic_write write_fun,iic_read read_fun,systick_get systick_get_fun,sys_sleep_us sys_sleep_us_fun,int_get_rawdata get_rawdata_fun)
{
inviicopera.read_fun = read_fun;
inviicopera.write_fun = write_fun;
inviicopera.inv_systick_get = systick_get_fun;
inviicopera.inv_systick_sleep = sys_sleep_us_fun;
inviicopera.get_rawdata = get_rawdata_fun;
icm40608_config();
}
void icm40608_close_cb_register(icm40608_close_cb close_cb)
{
inviicopera.close_cb = close_cb;
}
int Icm40608_Acc_Odr_Set(ACC_CONFIG0_ODR_t odr)
{
return inv_icm406xx_set_accel_frequency(&icm_driver,(ICM406XX_ACCEL_CONFIG0_ODR_t)odr);
}
int Icm40608_Gyro_Odr_Set(ACC_CONFIG0_ODR_t odr)
{
return inv_icm406xx_set_gyro_frequency(&icm_driver, (ICM406XX_GYRO_CONFIG0_ODR_t)odr);
}
SW/drive/icm40608_step/icm40608.h
View file @ 7e636aa8
......@@ -16,6 +16,9 @@ typedef enum
typedef enum
{
ICM40608_GYRO_RANGE_16 = 0x07 << 5,
ICM40608_GYRO_RANGE_31 = 0x06 << 5,
ICM40608_GYRO_RANGE_64 = 0x05 << 5,
ICM40608_GYRO_RANGE_125 = 0x04 << 5,
ICM40608_GYRO_RANGE_250 = 0x03 << 5,
ICM40608_GYRO_RANGE_500 = 0x02 << 5,
......@@ -34,6 +37,30 @@ typedef enum
ICM40608_ACCEL_LP ,
}ICM40608_WORKMODE_SET_E;
typedef enum
{
GYRO_CONFIG0_ODR_500_HZ = 0x0F, /*!< 500 Hz (2 ms)*/
GYRO_CONFIG0_ODR_12_5_HZ = 0x0B, /*!< 12.5 Hz (80 ms)*/
GYRO_CONFIG0_ODR_25_HZ = 0x0A, /*!< 25 Hz (40 ms)*/
GYRO_CONFIG0_ODR_50_HZ = 0x09, /*!< 50 Hz (20 ms)*/
GYRO_CONFIG0_ODR_100_HZ = 0x08, /*!< 100 Hz (10 ms)*/
GYRO_CONFIG0_ODR_200_HZ = 0x07, /*!< 200 Hz (5 ms)*/
GYRO_CONFIG0_ODR_1_KHZ = 0x06, /*!< 1 KHz (1 ms)*/
}GYRO_CONFIG0_ODR_t;
typedef enum
{
ACCEL_CONFIG0_ODR_500_HZ = 0xF, /*!< 500 Hz (2 ms)*/
ACCEL_CONFIG0_ODR_1_5625_HZ = 0xE, /*!< 1.5625 Hz (640 ms)*/
ACCEL_CONFIG0_ODR_3_125_HZ = 0xD, /*!< 3.125 Hz (320 ms)*/
ACCEL_CONFIG0_ODR_6_25_HZ = 0xC, /*!< 6.25 Hz (160 ms)*/
ACCEL_CONFIG0_ODR_12_5_HZ = 0xB, /*!< 12.5 Hz (80 ms)*/
ACCEL_CONFIG0_ODR_25_HZ = 0xA, /*!< 25 Hz (40 ms)*/
ACCEL_CONFIG0_ODR_50_HZ = 0x9, /*!< 50 Hz (20 ms)*/
ACCEL_CONFIG0_ODR_100_HZ = 0x8, /*!< 100 Hz (10 ms)*/
ACCEL_CONFIG0_ODR_200_HZ = 0x7, /*!< 200 Hz (5 ms)*/
ACCEL_CONFIG0_ODR_1_KHZ = 0x6, /*!< 1 KHz (1 ms)*/
}ACC_CONFIG0_ODR_t;
typedef struct
......@@ -61,7 +88,6 @@ typedef struct
}Icm40608Data_St;
typedef Icm40608Data_St* PIcm40608Data_St;
void Icm40608_init(void);
void Icm40608_Read_AllData(PIcm40608Data_St pmpu6050data);
......@@ -75,41 +101,37 @@ void Icm40608_Gyro_Value_Get(float *gyr_real_x, float *gyr_real_y, float *gyr_re
void Icm40608_ACC_Value_Get(float *acc_real_x, float *acc_real_y, float *acc_real_z);
void Icm40608_Pedometer_Get(void);
typedef struct
{
uint8_t dmp_odr_hz;
uint8_t wom_smd_mask; /**< This variable keeps track if wom or smd is enabled */
uint8_t wom_enable;
bool dmpisopen;
uint8_t pid;
}Icm40608data_st, * PIcm40608data_st;
typedef void (*iic_write)(uint8_t writeaddress, uint8_t *pdata, uint8_t length, bool no_stop);
typedef void (*iic_read)(uint8_t readaddress, uint8_t *pdata, uint8_t length);
typedef void (*int_get_rawdata)(int16_t *,int16_t*);
typedef void (*icm40608_close_cb)(void);
typedef void (*systick_get)(uint32_t *);
typedef uint32_t (*systick_get)(void);
typedef void (*sys_sleep_us)(uint32_t);
void Icm40608_Acc_GetRawData(uint8_t* data);
void Icm40608_Gyro_GetRawData(uint8_t* data);
uint16_t Icm40608_Gyro_Range_Get(void);
uint8_t Icm40608_Acc_Range_get(void);
void Icm40608_Gyro_Range_Set(GYRO_RANGE_E range);
void Icm40608_Acc_Range_Set(ACC_RANGE_E range);
uint8_t Icm40608_Acc_Range_Get(void);
void Icm40608_Gyro_Range_Set(GYRO_RANGE_E range);
int8_t Icm40608_set_sample_rate(uint8_t accel_rate, uint8_t gyro_rate);
int Icm40608_Acc_Odr_Set(ACC_CONFIG0_ODR_t);
int Icm40608_Gyro_Odr_Set(ACC_CONFIG0_ODR_t);
void Icm40608_Gyro_Poweroff(void);
void icm40608_init(iic_write write_fun,iic_read read_fun,systick_get systick_get_fun,sys_sleep_us sys_sleep_us_fun);
int icm40608_getdata_fifo(void);
int icm40608_fifo_config(ACC_RANGE_E,GYRO_RANGE_E,ACC_CONFIG0_ODR_t,GYRO_CONFIG0_ODR_t);
void icm40608_init(iic_write ,iic_read ,systick_get ,sys_sleep_us ,int_get_rawdata);
void icm40608_close_cb_register(icm40608_close_cb);
void icm40608_close(void);
void icm40608_open(void);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment