/**
*!
* \file b_drv_ICM20948.c
* \version v0.0.1
* \date 2020/03/25
* \author Bean(notrynohigh@outlook.com)
*******************************************************************************
* @attention
*
* Copyright (c) 2020 Bean
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*******************************************************************************
*/
/*Includes ----------------------------------------------*/
#include "drivers/inc/b_drv_icm20948.h"
/**
* \addtogroup B_DRIVER
* \{
*/
/**
* \addtogroup ICM20948
* \{
*/
/**
* \defgroup ICM20948_Private_TypesDefinitions
* \{
*/
/**
* \}
*/
/**
* \defgroup ICM20948_Private_Defines
* \{
*/
#define DRIVER_NAME ICM20948
/* define ICM-20948 Register */
#define ICM20948_ID 0xEAU
#define I2C_ADD_ICM20948 0xD0
#define I2C_ADD_ICM20948_AK09916 0x0C
#define I2C_ADD_ICM20948_AK09916_READ 0x80
/* user bank 0 register */
#define REG_ADD_WHO_AM_I 0x00
#define REG_VAL_WIA 0xEA
#define REG_ADD_USER_CTRL 0x03
#define REG_VAL_BIT_DMP_EN 0x80
#define REG_VAL_BIT_FIFO_EN 0x40
#define REG_VAL_BIT_I2C_MST_EN 0x20
#define REG_VAL_BIT_I2C_IF_DIS 0x10
#define REG_VAL_BIT_DMP_RST 0x08
#define REG_VAL_BIT_DIAMOND_DMP_RST 0x04
#define REG_ADD_PWR_MIGMT_1 0x06
#define REG_VAL_ALL_RGE_RESET 0x80
#define REG_VAL_RUN_MODE 0x01 // Non low-power mode
#define REG_ADD_LP_CONFIG 0x05
#define REG_ADD_PWR_MGMT_1 0x06
#define REG_ADD_PWR_MGMT_2 0x07
#define REG_VAL_MGMNT_2_SEN_ENABLE 0x00
#define REG_ADD_ACCEL_XOUT_H 0x2D
#define REG_ADD_ACCEL_XOUT_L 0x2E
#define REG_ADD_ACCEL_YOUT_H 0x2F
#define REG_ADD_ACCEL_YOUT_L 0x30
#define REG_ADD_ACCEL_ZOUT_H 0x31
#define REG_ADD_ACCEL_ZOUT_L 0x32
#define REG_ADD_GYRO_XOUT_H 0x33
#define REG_ADD_GYRO_XOUT_L 0x34
#define REG_ADD_GYRO_YOUT_H 0x35
#define REG_ADD_GYRO_YOUT_L 0x36
#define REG_ADD_GYRO_ZOUT_H 0x37
#define REG_ADD_GYRO_ZOUT_L 0x38
#define REG_ADD_EXT_SENS_DATA_00 0x3B
#define REG_ADD_REG_BANK_SEL 0x7F
#define REG_VAL_REG_BANK_0 0x00
#define REG_VAL_REG_BANK_1 0x10
#define REG_VAL_REG_BANK_2 0x20
#define REG_VAL_REG_BANK_3 0x30
/* user bank 1 register */
/* user bank 2 register */
#define REG_ADD_GYRO_SMPLRT_DIV 0x00
#define REG_ADD_GYRO_CONFIG_1 0x01
#define REG_VAL_BIT_GYRO_DLPCFG_2 0x10 /* bit[5:3] */
#define REG_VAL_BIT_GYRO_DLPCFG_4 0x20 /* bit[5:3] */
#define REG_VAL_BIT_GYRO_DLPCFG_6 0x30 /* bit[5:3] */
#define REG_VAL_BIT_GYRO_FS_250DPS 0x00 /* bit[2:1] */
#define REG_VAL_BIT_GYRO_FS_500DPS 0x02 /* bit[2:1] */
#define REG_VAL_BIT_GYRO_FS_1000DPS 0x04 /* bit[2:1] */
#define REG_VAL_BIT_GYRO_FS_2000DPS 0x06 /* bit[2:1] */
#define REG_VAL_BIT_GYRO_DLPF 0x01 /* bit[0] */
#define REG_ADD_ACCEL_SMPLRT_DIV_2 0x11
#define REG_ADD_ACCEL_CONFIG 0x14
#define REG_VAL_BIT_ACCEL_DLPCFG_2 0x10 /* bit[5:3] */
#define REG_VAL_BIT_ACCEL_DLPCFG_4 0x20 /* bit[5:3] */
#define REG_VAL_BIT_ACCEL_DLPCFG_6 0x30 /* bit[5:3] */
#define REG_VAL_BIT_ACCEL_FS_2g 0x00 /* bit[2:1] */
#define REG_VAL_BIT_ACCEL_FS_4g 0x02 /* bit[2:1] */
#define REG_VAL_BIT_ACCEL_FS_8g 0x04 /* bit[2:1] */
#define REG_VAL_BIT_ACCEL_FS_16g 0x06 /* bit[2:1] */
#define REG_VAL_BIT_ACCEL_DLPF 0x01 /* bit[0] */
/* user bank 3 register */
#define REG_ADD_I2C_MST_CTRL 0x01
#define REG_ADD_I2C_MST_CTRL_CLK_400KHZ \
0x07 // Gives 345.6kHz and is recommended to achieve max 400kHz
#define REG_ADD_I2C_SLV0_ADDR 0x03
#define REG_ADD_I2C_SLV0_REG 0x04
#define REG_ADD_I2C_SLV0_CTRL 0x05
#define REG_VAL_BIT_SLV0_EN 0x80
#define REG_VAL_BIT_MASK_LEN 0x07
#define REG_ADD_I2C_SLV0_DO 0x06
#define REG_ADD_I2C_SLV1_ADDR 0x07
#define REG_ADD_I2C_SLV1_REG 0x08
#define REG_ADD_I2C_SLV1_CTRL 0x09
#define REG_ADD_I2C_SLV1_DO 0x0A
#define REG_ADD_MAG_WIA1 0x00
#define REG_VAL_MAG_WIA1 0x48
#define REG_ADD_MAG_WIA2 0x01
#define REG_VAL_MAG_WIA2 0x09
#define REG_ADD_MAG_ST2 0x10
#define REG_ADD_MAG_DATA 0x11
#define REG_ADD_MAG_CNTL2 0x31
#define REG_VAL_MAG_MODE_PD 0x00
#define REG_VAL_MAG_MODE_SM 0x01
#define REG_VAL_MAG_MODE_10HZ 0x02
#define REG_VAL_MAG_MODE_20HZ 0x04
#define REG_VAL_MAG_MODE_50HZ 0x05
#define REG_VAL_MAG_MODE_100HZ 0x08
#define REG_VAL_MAG_MODE_ST 0x10
#define REG_ADD_MAG_CNTL3 0x32
#define REG_VAL_MAG_CNTL3_RESET 0x01
#define TEMP_SENSITIVITY_REG 333.87f
/**
* \}
*/
/**
* \defgroup ICM20948_Private_Macros
* \{
*/
#define F_IIC_WriteByte(a, b, c) \
do \
{ \
w_data = c; \
_bICM20948WriteRegs(pdrv, b, &w_data, 1); \
} while (0);
#define F_IIC_ReadByte(a, b, e) \
do \
{ \
e = _bICM20948ReadRegs(pdrv, b, &r_data, 1); \
} while (0);
#define ICM20948_Mag_Read(a, b, c, d) \
do \
{ \
bICM20948_Mag_Read(pdrv, a, b, c, d); \
} while (0);
#define ICM20948_Mag_Write(a, b, c) \
do \
{ \
bICM20948_Mag_Write(pdrv, a, b, c); \
} while (0);
#define ICM20948_Mag_Reset() \
do \
{ \
bICM20948_Mag_Reset(pdrv); \
} while (0);
#define F_IIC_ReadBytes(a, b, c, d, e) \
do \
{ \
e = _bICM20948ReadRegs(pdrv, b, c, d); \
} while (0);
#define F_Delay_ms(a) bHalDelayMs(a)
/**
* \}
*/
/**
* \defgroup ICM20948_Private_Variables
* \{
*/
bDRIVER_HALIF_TABLE(bICM20948_HalIf_t, DRIVER_NAME);
/**
* \}
*/
/**
* \defgroup ICM20948_Private_FunctionPrototypes
* \{
*/
/**
* \}
*/
/**
* \defgroup ICM20948_Private_Functions
* \{
*/
//----------------------------------------------------------------
static int _bICM20948ReadRegs(bDriverInterface_t *pdrv, uint8_t reg, uint8_t *data, uint16_t len)
{
bDRIVER_GET_HALIF(_if, bICM20948_HalIf_t, pdrv);
// reg = reg | 0x80;
if (bHalI2CMemRead(_if, reg, 1, data, len) < 0)
{
return -1;
}
else
{
return len;
}
}
static int _bICM20948WriteRegs(bDriverInterface_t *pdrv, uint8_t reg, uint8_t *data, uint16_t len)
{
bDRIVER_GET_HALIF(_if, bICM20948_HalIf_t, pdrv);
bHalI2CMemWrite(_if, reg, 1, data, len);
return len;
}
static int _ICM20948GetID(bDriverInterface_t *pdrv, uint8_t *id)
{
int retval = 0;
// uint8_t w_data = 0;
uint8_t r_data = 0;
// F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0);
F_IIC_ReadByte(I2C_ADD_ICM20948, REG_ADD_WHO_AM_I, retval);
if (retval <= 0)
{
return -1;
}
*id = r_data;
return retval;
}
int F_ICM20948_GetData(bDriverInterface_t *pdrv, int32_t *pData_acc, int32_t *pData_gyro,
int32_t *pData_mag, float *tempreature)
{
int retval = 0;
uint8_t tempBuf[20];
int16_t pDataRaw_x[3];
int16_t pDataRaw_g[3];
int16_t pDataRaw_m[3];
int16_t tempreature_r;
F_IIC_ReadBytes(I2C_ADD_ICM20948, REG_ADD_ACCEL_XOUT_H, &tempBuf[0], 20, retval);
if (retval < 0)
{
return -1;
}
pDataRaw_x[0] = ((((int16_t)tempBuf[0]) << 8) + (int16_t)tempBuf[1]);
pDataRaw_x[1] = ((((int16_t)tempBuf[2]) << 8) + (int16_t)tempBuf[3]);
pDataRaw_x[2] = ((((int16_t)tempBuf[4]) << 8) + (int16_t)tempBuf[5]);
pDataRaw_g[0] = ((((int16_t)tempBuf[6]) << 8) + (int16_t)tempBuf[7]);
pDataRaw_g[1] = ((((int16_t)tempBuf[8]) << 8) + (int16_t)tempBuf[9]);
pDataRaw_g[2] = ((((int16_t)tempBuf[10]) << 8) + (int16_t)tempBuf[11]);
tempreature_r = ((((int16_t)tempBuf[12]) << 8) + (int16_t)tempBuf[13]);
pDataRaw_m[0] = ((((int16_t)tempBuf[15]) << 8) + (int16_t)tempBuf[14]);
pDataRaw_m[1] = ((((int16_t)tempBuf[17]) << 8) + (int16_t)tempBuf[16]);
pDataRaw_m[2] = ((((int16_t)tempBuf[19]) << 8) + (int16_t)tempBuf[18]);
*tempreature = (float)(tempreature_r / TEMP_SENSITIVITY_REG);
pData_gyro[0] = pDataRaw_g[0] * 60.9756097561f;
pData_gyro[1] = pDataRaw_g[1] * 60.9756097561f;
pData_gyro[2] = pDataRaw_g[2] * 60.9756097561f;
pData_acc[0] = (int32_t)(pDataRaw_x[0] * 0.061035156f);
pData_acc[1] = (int32_t)(pDataRaw_x[1] * 0.061035156f);
pData_acc[2] = (int32_t)(pDataRaw_x[2] * 0.061035156f);
pData_mag[0] = (int32_t)(pDataRaw_m[0] * 1.0f);
pData_mag[1] = (int32_t)(pDataRaw_m[1] * 1.0f);
pData_mag[2] = (int32_t)(pDataRaw_m[2] * 1.0f);
return retval;
}
static void bICM20948_Mag_Read(bDriverInterface_t *pdrv, uint8_t I2CAddr, uint8_t RegAddr,
uint8_t Len, uint8_t *pdata)
{
uint8_t w_data = 0;
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3); // swtich bank3
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_ADDR,
I2CAddr | I2C_ADD_ICM20948_AK09916_READ);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_REG, RegAddr);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_CTRL, REG_VAL_BIT_SLV0_EN | Len);
F_Delay_ms(10);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0); // swtich bank0
for (uint8_t i = 0; i < Len; i++)
{
_bICM20948ReadRegs(pdrv, REG_ADD_EXT_SENS_DATA_00 + i, pdata + i, 1);
}
}
static void bICM20948_Mag_Write(bDriverInterface_t *pdrv, uint8_t I2CAddr, uint8_t RegAddr,
uint8_t data)
{
uint8_t w_data = 0;
uint8_t r_data = 0;
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3); // swtich bank3
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_ADDR, I2CAddr);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_REG, RegAddr);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_DO, data);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_CTRL, REG_VAL_BIT_SLV0_EN | 1);
F_Delay_ms(50);
ICM20948_Mag_Read(I2CAddr, RegAddr, 1, &r_data);
}
static void bICM20948_Mag_Reset(bDriverInterface_t *pdrv)
{
uint8_t w_data = 0;
ICM20948_Mag_Write(I2C_ADD_ICM20948_AK09916, REG_ADD_MAG_CNTL2, 0x00);
F_Delay_ms(10);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_PWR_MGMT_1, REG_VAL_ALL_RGE_RESET);
F_Delay_ms(10);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_PWR_MGMT_1, REG_VAL_RUN_MODE);
F_Delay_ms(10);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_PWR_MGMT_2, REG_VAL_MGMNT_2_SEN_ENABLE);
F_Delay_ms(10);
ICM20948_Mag_Write(I2C_ADD_ICM20948_AK09916, REG_ADD_MAG_CNTL3, REG_VAL_MAG_CNTL3_RESET);
F_Delay_ms(10);
}
static int _bICM20948Read(bDriverInterface_t *pdrv, uint32_t off, uint8_t *pbuf, uint32_t len)
{
int retval = 0;
bICM20948_9Axis_t *ptmp = (bICM20948_9Axis_t *)pbuf;
if (len < sizeof(bICM20948_9Axis_t))
{
return 0;
}
retval =
F_ICM20948_GetData(pdrv, ptmp->acc_arr, ptmp->gyro_arr, ptmp->mag_arr, &ptmp->temperature);
return retval;
}
static int _bICM20948Ctl(struct bDriverIf *pdrv, uint8_t cmd, void *param)
{
int retval = 0;
switch (cmd)
{
case bCMD_ICM20948_SET_STATUS_ERR:
{
pdrv->status = -1;
}
break;
default:
retval = -1;
break;
}
return retval;
}
/**
* \}
*/
/**
* \addtogroup ICM20948_Exported_Functions
* \{
*/
int bICM20948_Init(bDriverInterface_t *pdrv)
{
uint8_t w_data = 0;
uint8_t r_data = 0;
uint8_t data[8];
bDRIVER_STRUCT_INIT(pdrv, DRIVER_NAME, bICM20948_Init);
pdrv->read = _bICM20948Read;
pdrv->ctl = _bICM20948Ctl;
if (_ICM20948GetID(pdrv, &r_data) < 0)
{
return -1;
}
if (r_data != ICM20948_ID)
{
return -1;
}
ICM20948_Mag_Reset();
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3);
/* Reset I2C master clock. */
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_I2C_MST_CTRL, 0);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0);
_bICM20948ReadRegs(pdrv, 0x03, &r_data, 1);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_USER_CTRL, r_data | REG_VAL_BIT_I2C_MST_EN);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_I2C_MST_CTRL, REG_ADD_I2C_MST_CTRL_CLK_400KHZ);
F_Delay_ms(10);
// configure gyro
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_2);
F_IIC_WriteByte(
I2C_ADD_ICM20948, REG_ADD_GYRO_CONFIG_1,
REG_VAL_BIT_GYRO_DLPCFG_6 | REG_VAL_BIT_GYRO_FS_2000DPS | REG_VAL_BIT_GYRO_DLPF);
// configure acc
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_ACCEL_SMPLRT_DIV_2, 0x00);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_ACCEL_CONFIG,
REG_VAL_BIT_ACCEL_DLPCFG_6 | REG_VAL_BIT_ACCEL_FS_4g | REG_VAL_BIT_ACCEL_DLPF);
// checkMag
uint8_t ret[2];
ICM20948_Mag_Read(I2C_ADD_ICM20948_AK09916, REG_ADD_MAG_WIA1, 2, ret);
if (!((ret[0] == REG_VAL_MAG_WIA1) && (ret[1] == REG_VAL_MAG_WIA2)))
{
return -2;
}
F_Delay_ms(10);
ICM20948_Mag_Write(I2C_ADD_ICM20948_AK09916, REG_ADD_MAG_CNTL2, REG_VAL_MAG_MODE_100HZ);
F_Delay_ms(10);
ICM20948_Mag_Read(I2C_ADD_ICM20948_AK09916, REG_ADD_MAG_DATA, 8, data);
F_IIC_WriteByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0);
return 0;
}
#ifdef BSECTION_NEED_PRAGMA
#pragma section driver_init
#endif
bDRIVER_REG_INIT(B_DRIVER_ICM20948, bICM20948_Init);
#ifdef BSECTION_NEED_PRAGMA
#pragma section
#endif
/**
* \}
*/
/**
* \}
*/
/**
* \}
*/
/************************ Copyright (c) 2020 Bean *****END OF FILE****/