mirror of
https://github.com/openmv/openmv.git
synced 2025-09-26 23:09:13 +08:00
1062 lines
28 KiB
C
1062 lines
28 KiB
C
/* Copyright (C) 2024 Alif Semiconductor - All Rights Reserved.
|
|
* Use, distribution and modification of this code is permitted under the
|
|
* terms stated in the Alif Semiconductor Software License Agreement
|
|
*
|
|
* You should have received a copy of the Alif Semiconductor Software
|
|
* License Agreement with this file. If not, please write to:
|
|
* contact@alifsemi.com, or visit: https://alifsemi.com/license
|
|
*/
|
|
|
|
/******************************************************************************
|
|
* @file ICM42670P_IMU.c
|
|
* @author Shreehari H K
|
|
* @email shreehari.hk@alifsemi.com
|
|
* @version V1.0.0
|
|
* @date 02-July-2024
|
|
* @brief Driver for Inertial Measurement Unit ICM42670P
|
|
* @bug None.
|
|
* @Note None.
|
|
******************************************************************************/
|
|
|
|
#include "RTE_Components.h"
|
|
#include "system_utils.h"
|
|
|
|
/* IMU Driver */
|
|
#include "Driver_IMU.h"
|
|
|
|
/* I3C Driver */
|
|
#include "Driver_I3C.h"
|
|
|
|
#if !defined (RTE_Drivers_ICM42670P)
|
|
#error "IMU ICM42670P is not configured in RTE_Components.h"
|
|
#endif
|
|
|
|
#define ARM_IMU_DRV_VERSION ARM_DRIVER_VERSION_MAJOR_MINOR(1, 2)
|
|
|
|
/* Timeout in Microsec */
|
|
#define IMU_I3C_TIMEOUT_US (100000)
|
|
|
|
#define ICM42670P_UPPER_DATA_BYTE_Pos (8)
|
|
|
|
/* Register offest */
|
|
#define ICM42670P_REG_IDX_SIZE (1)
|
|
|
|
/* ICM42670P Driver status */
|
|
#define ICM42670P_DRIVER_INITIALIZED (1 << 0U)
|
|
#define ICM42670P_DRIVER_POWERED (1 << 1U)
|
|
|
|
/* Target Slave Address */
|
|
#define ICM42670P_DEFAULT_ADDR (0x68)
|
|
|
|
/* Target Slave's Who Am I Reg addr and it's value */
|
|
#define ICM42670P_WHO_AM_I_REG (0x75)
|
|
#define ICM42670P_REG_WHO_AM_I_VAL (0x67)
|
|
|
|
/* Target Slave */
|
|
#define ICM42670P_PWR_MGMT0_REG (0x1F)
|
|
#define ICM42670P_PWR_MGMT0_VAL (0xFU)
|
|
|
|
/* Data Registers */
|
|
#define ICM42670P_TEMP_DATA1_REG (0x09)
|
|
#define ICM42670P_TEMP_DATA_SIZE (0x02)
|
|
#define ICM42670P_ACCEL_DATA1_REG (0x0B)
|
|
#define ICM42670P_ACCEL_DATA_SIZE (0x06)
|
|
#define ICM42670P_GYRO_DATA1_REG (0x11)
|
|
#define ICM42670P_GYRO_DATA_SIZE (0x06)
|
|
|
|
/* Data registers offset */
|
|
#define ICM42670P_TEMP_DATA1_OFFSET (0x00)
|
|
#define ICM42670P_TEMP_DATA0_OFFSET (0x01)
|
|
#define ICM42670P_ACCEL_DATA_X1_OFFSET (0x00)
|
|
#define ICM42670P_ACCEL_DATA_X0_OFFSET (0x01)
|
|
#define ICM42670P_ACCEL_DATA_Y1_OFFSET (0x02)
|
|
#define ICM42670P_ACCEL_DATA_Y0_OFFSET (0x03)
|
|
#define ICM42670P_ACCEL_DATA_Z1_OFFSET (0x04)
|
|
#define ICM42670P_ACCEL_DATA_Z0_OFFSET (0x05)
|
|
#define ICM42670P_GYRO_DATA_X1_OFFSET (0x00)
|
|
#define ICM42670P_GYRO_DATA_X0_OFFSET (0x01)
|
|
#define ICM42670P_GYRO_DATA_Y1_OFFSET (0x02)
|
|
#define ICM42670P_GYRO_DATA_Y0_OFFSET (0x03)
|
|
#define ICM42670P_GYRO_DATA_Z1_OFFSET (0x04)
|
|
#define ICM42670P_GYRO_DATA_Z0_OFFSET (0x05)
|
|
|
|
/* Configuration Registers */
|
|
#define ICM42670P_GYRO_CONFIG0_REG (0x20)
|
|
#define ICM42670P_GYRO_CONFIG0_VAL (0x06)
|
|
#define ICM42670P_GYRO_CONFIG1_REG (0x23)
|
|
#define ICM42670P_GYRO_CONFIG1_VAL (0x01)
|
|
#define ICM42670P_ACCEL_CONFIG0_REG (0x21)
|
|
#define ICM42670P_ACCEL_CONFIG0_VAL (0x06)
|
|
#define ICM42670P_ACCEL_CONFIG1_REG (0x24)
|
|
#define ICM42670P_ACCEL_CONFIG1_VAL (0x41)
|
|
#define ICM42670P_TEMP_CONFIG0_REG (0x22)
|
|
#define ICM42670P_TEMP_CONFIG0_VAL (0x00)
|
|
|
|
/* Memory selection registers */
|
|
#define ICM42670P_BLK_SEL_W_REG (0x79)
|
|
#define ICM42670P_BLK_SEL_W_VAL (0x0)
|
|
#define ICM42670P_MADDR_W_REG (0x7A)
|
|
#define ICM42670P_M_W_REG (0x7B)
|
|
|
|
#define ICM42670P_BLK_SEL_R_REG (0x7C)
|
|
#define ICM42670P_BLK_SEL_R_VAL (0x0)
|
|
#define ICM42670P_MADDR_R_REG (0x7D)
|
|
#define ICM42670P_M_R_REG (0x7E)
|
|
|
|
/* Interrupt config and src register*/
|
|
#define ICM42670P_INTF_CONFIG6_REG (0x23) /* IBI configuration Register */
|
|
#define ICM42670P_INTF_CONFIG6_VAL (0x14)
|
|
|
|
#define ICM42670P_INT_SOURCE8_REG (0x31) /* IBI Interrupt source Register */
|
|
#define ICM42670P_INT_SOURCE8_VAL (0x8) /* IBI for data availability */
|
|
|
|
#define ICM42670P_ACCEL_CALIB_VAL (2048U) /*Calibration for full scale output selection of +-16g */
|
|
#define ICM42670P_GYRO_CALIB_VAL (16.4) /*Calibration for full scale output selection of +-2kdps */
|
|
|
|
#define ICM42670P_ACCEL_VAL(x) ((x * 1000) / (ICM42670P_ACCEL_CALIB_VAL)) /* Acceleration value in mg*/
|
|
#define ICM42670P_GYRO_VAL(x) ((x * 1000) / (ICM42670P_GYRO_CALIB_VAL)) /* Gyro value in mdps */
|
|
#define ICM42670P_TEMPERATURE(x) ((x / 128.0) + 25) /* Temp value in C */
|
|
|
|
/* ICM42670P driver Info variable */
|
|
static struct ICM42670P_DRV_INFO
|
|
{
|
|
uint8_t state; /* Driver state */
|
|
uint8_t target_addr; /* Target slave's dynamic address */
|
|
uint8_t reserved[2]; /* Reserved */
|
|
volatile uint32_t imu_i3c_event; /* I3C Event status */
|
|
volatile ARM_IMU_STATUS status; /* Driver status */
|
|
} icm42670p_drv_info;
|
|
|
|
/* Driver version*/
|
|
static const ARM_DRIVER_VERSION DriverVersion =
|
|
{
|
|
ARM_IMU_API_VERSION,
|
|
ARM_IMU_DRV_VERSION
|
|
};
|
|
|
|
/* I3C driver */
|
|
extern ARM_DRIVER_I3C Driver_I3C;
|
|
static ARM_DRIVER_I3C *I3C_Driver = &Driver_I3C;
|
|
|
|
/* Driver Capabilities */
|
|
static const ARM_IMU_CAPABILITIES DriverCapabilities =
|
|
{
|
|
1, /* Supports Accelerometer data */
|
|
1, /* Supports Gyroscope data */
|
|
0, /* Doesn't support Magnetometer data */
|
|
1, /* Supports Temperature sens data */
|
|
0 /* reserved (must be zero)*/
|
|
};
|
|
|
|
/**
|
|
\fn ARM_DRIVER_VERSION ARM_IMU_GetVersion(void)
|
|
\brief Gets IMU driver version.
|
|
\return \ref ARM_DRIVER_VERSION.
|
|
*/
|
|
static ARM_DRIVER_VERSION ARM_IMU_GetVersion(void)
|
|
{
|
|
return DriverVersion;
|
|
}
|
|
|
|
/**
|
|
\fn ARM_IMU_CAPABILITIES ARM_IMU_GetCapabilities(void)
|
|
\brief Gets IMU driver capabilities.
|
|
\return \ref ARM_IMU_CAPABILITIES.
|
|
*/
|
|
static ARM_IMU_CAPABILITIES ARM_IMU_GetCapabilities(void)
|
|
{
|
|
return DriverCapabilities;
|
|
}
|
|
|
|
/**
|
|
\fn ARM_IMU_STATUS ARM_IMU_GetStatus(void)
|
|
\brief Gets IMU driver status.
|
|
\return \ref ARM_IMU_STATUS.
|
|
*/
|
|
static ARM_IMU_STATUS ARM_IMU_GetStatus(void)
|
|
{
|
|
return icm42670p_drv_info.status;
|
|
}
|
|
|
|
/**
|
|
\fn void IMU_I3CCallBack(uint32_t event)
|
|
\brief IMU driver I3C callback event.
|
|
\param[in] event: I3C Event.
|
|
\return None
|
|
*/
|
|
void IMU_I3CCallBack(uint32_t event)
|
|
{
|
|
icm42670p_drv_info.imu_i3c_event |= event;
|
|
|
|
if(icm42670p_drv_info.imu_i3c_event & ARM_I3C_EVENT_IBI_SLV_INTR_REQ)
|
|
{
|
|
/* Sets Data Rcvd to true */
|
|
icm42670p_drv_info.status.data_rcvd = 1U;
|
|
}
|
|
}
|
|
|
|
/**
|
|
\fn int32_t IMU_ResetDynAddr(void)
|
|
\brief Resets all slaves' dynamic address
|
|
\return \ref execution_status.
|
|
*/
|
|
static int32_t IMU_ResetDynAddr(void)
|
|
{
|
|
int32_t ret;
|
|
|
|
ARM_I3C_CMD i3c_cmd = {0};
|
|
|
|
/* Reset slave address */
|
|
i3c_cmd.rw = 0U;
|
|
i3c_cmd.cmd_id = I3C_CCC_RSTDAA(true);
|
|
i3c_cmd.len = 0U;
|
|
i3c_cmd.addr = 0;
|
|
|
|
ret = I3C_Driver->MasterSendCommand(&i3c_cmd);
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* wait for callback event. */
|
|
while(!((icm42670p_drv_info.imu_i3c_event & ARM_I3C_EVENT_TRANSFER_DONE) ||
|
|
(icm42670p_drv_info.imu_i3c_event & ARM_I3C_EVENT_TRANSFER_ERROR)));
|
|
if(icm42670p_drv_info.imu_i3c_event & ARM_I3C_EVENT_TRANSFER_ERROR)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
|
|
icm42670p_drv_info.imu_i3c_event = 0U;
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t IMU_SetDynAddr(void)
|
|
\brief Assigns Dynamic address to IMU.
|
|
\return \ref execution_status.
|
|
*/
|
|
static int32_t IMU_SetDynAddr(void)
|
|
{
|
|
int32_t ret;
|
|
|
|
/* I3C CCC (Common Command Codes) */
|
|
ARM_I3C_CMD i3c_cmd = {0};
|
|
|
|
i3c_cmd.rw = 0U;
|
|
i3c_cmd.cmd_id = I3C_CCC_SETDASA;
|
|
i3c_cmd.len = 1U;
|
|
|
|
/* Assign IMU's Static address */
|
|
i3c_cmd.addr = ICM42670P_DEFAULT_ADDR;
|
|
|
|
/* Assign Dynamic address */
|
|
ret = I3C_Driver->MasterAssignDA(&i3c_cmd);
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* wait for callback event. */
|
|
while(!((icm42670p_drv_info.imu_i3c_event & ARM_I3C_EVENT_TRANSFER_DONE) ||
|
|
(icm42670p_drv_info.imu_i3c_event & ARM_I3C_EVENT_TRANSFER_ERROR)));
|
|
if(icm42670p_drv_info.imu_i3c_event & ARM_I3C_EVENT_TRANSFER_ERROR)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
|
|
icm42670p_drv_info.imu_i3c_event = 0U;
|
|
|
|
/* Fetch the assigned dynamic address */
|
|
ret = I3C_Driver->GetSlaveDynAddr(ICM42670P_DEFAULT_ADDR,
|
|
&icm42670p_drv_info.target_addr);
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t IMU_Init(void)
|
|
\brief Initializes IMU driver.
|
|
This function will
|
|
- initialize i3c instance.
|
|
\return \ref Execution status.
|
|
*/
|
|
static int32_t IMU_Init(void)
|
|
{
|
|
int32_t ret;
|
|
ARM_DRIVER_VERSION version;
|
|
|
|
/* Get i3c driver version. */
|
|
version = I3C_Driver->GetVersion();
|
|
|
|
if((version.api < ARM_DRIVER_VERSION_MAJOR_MINOR(7U, 0U)) ||
|
|
(version.drv < ARM_DRIVER_VERSION_MAJOR_MINOR(7U, 0U)))
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
|
|
ret = I3C_Driver->Initialize(IMU_I3CCallBack);
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
ret = I3C_Driver->PowerControl(ARM_POWER_FULL);
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t IMU_Write(uint8_t tar_addr, uint16_t reg_addr,
|
|
uint8_t *reg_data, uint8_t len)
|
|
\brief Writes data to IMU registers.
|
|
\param[in] tar_addr : I3C slave address.
|
|
\param[in] reg_addr : Register address.
|
|
\param[in] reg_data : pointer to source data buf.
|
|
\param[in] len : Number of bytes to write.
|
|
\return \ref Execution status.
|
|
*/
|
|
static int32_t IMU_Write(uint8_t tar_addr, uint16_t reg_addr,
|
|
uint8_t *reg_data, uint8_t len)
|
|
|
|
{
|
|
int32_t ret = 0U;
|
|
uint8_t iter = 0U;
|
|
uint32_t counter = 0U;
|
|
uint8_t tx_buf[4];
|
|
|
|
/* Store register's address in 0th index */
|
|
tx_buf[0] = reg_addr;
|
|
|
|
if(len)
|
|
{
|
|
for(iter = 0U; iter < len; iter++)
|
|
{
|
|
tx_buf[iter + ICM42670P_REG_IDX_SIZE] = reg_data[iter];
|
|
}
|
|
}
|
|
|
|
icm42670p_drv_info.imu_i3c_event = 0U;
|
|
|
|
/* Send msg to slave */
|
|
ret = I3C_Driver->MasterTransmit(tar_addr,
|
|
tx_buf,
|
|
(len + ICM42670P_REG_IDX_SIZE));
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* wait for callback event. */
|
|
while(!((icm42670p_drv_info.imu_i3c_event & ARM_I3C_EVENT_TRANSFER_DONE) ||
|
|
(icm42670p_drv_info.imu_i3c_event & ARM_I3C_EVENT_TRANSFER_ERROR)))
|
|
{
|
|
if(counter++ < IMU_I3C_TIMEOUT_US)
|
|
{
|
|
sys_busy_loop_us(1);
|
|
}
|
|
else
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
}
|
|
if(icm42670p_drv_info.imu_i3c_event & ARM_I3C_EVENT_TRANSFER_ERROR)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
|
|
icm42670p_drv_info.imu_i3c_event = 0U;
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t IMU_Read(uint8_t tar_addr, uint16_t reg_addr,
|
|
uint8_t *reg_data, uint8_t len)
|
|
\brief Reads data from IMU registers.
|
|
\param[in] tar_addr : I3C slave address.
|
|
\param[in] reg_addr : Register address.
|
|
\param[in] reg_data : pointer to destination buf.
|
|
\param[in] len : Number of bytes to read.
|
|
\return \ref Execution status.
|
|
*/
|
|
static int32_t IMU_Read(uint8_t tar_addr, uint16_t reg_addr,
|
|
uint8_t *reg_data, uint8_t len)
|
|
{
|
|
int32_t ret = 0U;
|
|
uint32_t counter = 0U;
|
|
|
|
/* Send register address */
|
|
ret = IMU_Write(tar_addr, reg_addr, NULL, 0U);
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* Receive data from slave */
|
|
ret = I3C_Driver->MasterReceive(tar_addr, reg_data, len);
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* wait for callback event. */
|
|
while(!((icm42670p_drv_info.imu_i3c_event &
|
|
ARM_I3C_EVENT_TRANSFER_DONE) ||
|
|
(icm42670p_drv_info.imu_i3c_event &
|
|
ARM_I3C_EVENT_TRANSFER_ERROR)))
|
|
{
|
|
if(counter++ < IMU_I3C_TIMEOUT_US)
|
|
{
|
|
sys_busy_loop_us(1);
|
|
}
|
|
else
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
}
|
|
if(icm42670p_drv_info.imu_i3c_event & ARM_I3C_EVENT_TRANSFER_ERROR)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
icm42670p_drv_info.imu_i3c_event = 0U;
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t IMU_AccelConfig(void)
|
|
\brief Configures Accelerometer of IMU driver.
|
|
\return \ref Execution status.
|
|
*/
|
|
static int32_t IMU_AccelConfig(void)
|
|
{
|
|
uint8_t data[4];
|
|
|
|
/* Configure Accelerometer */
|
|
data[0] = ICM42670P_ACCEL_CONFIG0_VAL;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_ACCEL_CONFIG0_REG, data, 1U);
|
|
|
|
sys_busy_loop_us(10);
|
|
|
|
data[0] = 0;
|
|
IMU_Read(icm42670p_drv_info.target_addr,
|
|
ICM42670P_ACCEL_CONFIG0_REG, data, 1U);
|
|
|
|
if(data[0] != ICM42670P_ACCEL_CONFIG0_VAL)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
|
|
data[0] = ICM42670P_ACCEL_CONFIG1_VAL;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_ACCEL_CONFIG1_REG, data, 1U);
|
|
|
|
sys_busy_loop_us(10);
|
|
|
|
data[0] = 0;
|
|
IMU_Read(icm42670p_drv_info.target_addr,
|
|
ICM42670P_ACCEL_CONFIG1_REG, data, 1U);
|
|
|
|
if(data[0] != ICM42670P_ACCEL_CONFIG1_VAL)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t IMU_GyroConfig(void)
|
|
\brief Configures Gyroscope of IMU driver.
|
|
\return \ref Execution status.
|
|
*/
|
|
static int32_t IMU_GyroConfig(void)
|
|
{
|
|
uint8_t data[4];
|
|
|
|
/* Configure Gyroscope */
|
|
data[0] = ICM42670P_GYRO_CONFIG0_VAL;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_GYRO_CONFIG0_REG, data, 1U);
|
|
|
|
sys_busy_loop_us(10);
|
|
|
|
data[0] = 0;
|
|
IMU_Read(icm42670p_drv_info.target_addr,
|
|
ICM42670P_GYRO_CONFIG0_REG, data, 1U);
|
|
|
|
if(data[0] != ICM42670P_GYRO_CONFIG0_VAL)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
|
|
data[0] = ICM42670P_GYRO_CONFIG1_VAL;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_GYRO_CONFIG1_REG, data, 1U);
|
|
|
|
sys_busy_loop_us(10);
|
|
|
|
data[0] = 0;
|
|
IMU_Read(icm42670p_drv_info.target_addr,
|
|
ICM42670P_GYRO_CONFIG1_REG, data, 1U);
|
|
|
|
if(data[0] != ICM42670P_GYRO_CONFIG1_VAL)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t IMU_TempConfig(void)
|
|
\brief Configures Temp sensor of IMU driver.
|
|
\return \ref Execution status.
|
|
*/
|
|
static int32_t IMU_TempConfig(void)
|
|
{
|
|
uint8_t data[4];
|
|
|
|
/* Configure Temperature sensor */
|
|
data[0] = ICM42670P_TEMP_CONFIG0_VAL;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_TEMP_CONFIG0_REG, data, 1U);
|
|
|
|
sys_busy_loop_us(10);
|
|
|
|
data[0] = 0;
|
|
IMU_Read(icm42670p_drv_info.target_addr,
|
|
ICM42670P_TEMP_CONFIG0_REG, data, 1U);
|
|
|
|
if(data[0] != ICM42670P_TEMP_CONFIG0_VAL)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t IMU_IBIConfig(void)
|
|
\brief Configures In-Band-Interrupt of IMU.
|
|
\return \ref Execution status.
|
|
*/
|
|
static int32_t IMU_IBIConfig(void)
|
|
{
|
|
uint8_t data[4];
|
|
|
|
/* Below code is for IBI for data availability */
|
|
data[0] = ICM42670P_BLK_SEL_W_VAL;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_BLK_SEL_W_REG, data, 1U);
|
|
|
|
data[0] = ICM42670P_INT_SOURCE8_REG;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_MADDR_W_REG, data, 1U);
|
|
|
|
data[0] = ICM42670P_INT_SOURCE8_VAL;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_M_W_REG, data, 1U);
|
|
|
|
sys_busy_loop_us(10);
|
|
|
|
data[0] = ICM42670P_BLK_SEL_R_VAL;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_BLK_SEL_R_REG, data, 1U);
|
|
|
|
data[0] = ICM42670P_INT_SOURCE8_REG;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_MADDR_R_REG, data, 1U);
|
|
|
|
sys_busy_loop_us(10);
|
|
data[0] = 0;
|
|
IMU_Read(icm42670p_drv_info.target_addr,
|
|
ICM42670P_M_R_REG, data, 1U);
|
|
|
|
if(data[0] != ICM42670P_INT_SOURCE8_VAL)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
sys_busy_loop_us(10);
|
|
|
|
/* Below code is for for Enabling IBI */
|
|
data[0] = ICM42670P_BLK_SEL_W_VAL;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_BLK_SEL_W_REG, data, 1U);
|
|
|
|
data[0] = ICM42670P_INTF_CONFIG6_REG;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_MADDR_W_REG, data, 1U);
|
|
|
|
data[0] = ICM42670P_INTF_CONFIG6_VAL;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_M_W_REG, data, 1U);
|
|
|
|
sys_busy_loop_us(10);
|
|
|
|
data[0] = ICM42670P_BLK_SEL_R_VAL;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_BLK_SEL_R_REG, data, 1U);
|
|
|
|
data[0] = ICM42670P_INTF_CONFIG6_REG;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_MADDR_R_REG, data, 1U);
|
|
|
|
sys_busy_loop_us(10);
|
|
|
|
data[0] = 0;
|
|
IMU_Read(icm42670p_drv_info.target_addr,
|
|
ICM42670P_M_R_REG, data, 1U);
|
|
|
|
if(data[0] != ICM42670P_INTF_CONFIG6_VAL)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
|
|
sys_busy_loop_us(10);
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
/**
|
|
\fn int32_t IMU_Config(void)
|
|
\brief Configures IMU driver.
|
|
\return \ref Execution status.
|
|
*/
|
|
static int32_t IMU_Config(void)
|
|
{
|
|
int32_t ret;
|
|
uint8_t data[4];
|
|
|
|
/* Powers up ICM42670 */
|
|
data[0] = ICM42670P_PWR_MGMT0_VAL;
|
|
IMU_Write(icm42670p_drv_info.target_addr,
|
|
ICM42670P_PWR_MGMT0_REG, data, 1U);
|
|
|
|
sys_busy_loop_us(10);
|
|
|
|
data[0] = 0;
|
|
IMU_Read(icm42670p_drv_info.target_addr,
|
|
ICM42670P_PWR_MGMT0_REG, data, 1U);
|
|
|
|
if(data[0] != ICM42670P_PWR_MGMT0_VAL)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
|
|
/* Invokes Accelerometer configuration */
|
|
ret = IMU_AccelConfig();
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* Invokes Gyroscope configuration */
|
|
ret = IMU_GyroConfig();
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* Invokes Temperature sensor configuration */
|
|
ret = IMU_TempConfig();
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* Invokes IBI configuration */
|
|
ret = IMU_IBIConfig();
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t IMU_Setup(void)
|
|
\brief Sets up IMU driver.
|
|
This function will
|
|
- Sets up i3c instance.
|
|
\return \ref Execution status.
|
|
*/
|
|
static int32_t IMU_Setup(void)
|
|
{
|
|
int32_t ret;
|
|
uint8_t data[4];
|
|
|
|
/* Initializes I3C master */
|
|
ret = I3C_Driver->Control(I3C_MASTER_INIT, 0);
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* i3c Speed Mode Configuration: Slow mode*/
|
|
ret = I3C_Driver->Control(I3C_MASTER_SET_BUS_MODE,
|
|
I3C_BUS_SLOW_MODE);
|
|
|
|
/* Rejects Hot-Join request */
|
|
ret = I3C_Driver->Control(I3C_MASTER_SETUP_HOT_JOIN_ACCEPTANCE, 0);
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* Rejects Master request */
|
|
ret = I3C_Driver->Control(I3C_MASTER_SETUP_MR_ACCEPTANCE, 0);
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* Accepts Slave Interrupt request */
|
|
ret = I3C_Driver->Control(I3C_MASTER_SETUP_SIR_ACCEPTANCE, 1);
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* Resets IMU's address */
|
|
ret = IMU_ResetDynAddr();
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* Dynamic Addresses the IMU */
|
|
ret = IMU_SetDynAddr();
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* i3c Speed Mode Configuration: Normal mode*/
|
|
ret = I3C_Driver->Control(I3C_MASTER_SET_BUS_MODE,
|
|
I3C_BUS_NORMAL_MODE);
|
|
|
|
/* Reads Chip ID */
|
|
IMU_Read(icm42670p_drv_info.target_addr, ICM42670P_WHO_AM_I_REG, data, 1U);
|
|
|
|
if(data[0] != ICM42670P_REG_WHO_AM_I_VAL)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
|
|
/* Configures the slave */
|
|
ret = IMU_Config();
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t IMU_GetAccelData(ARM_IMU_COORDINATES *accel_data)
|
|
\brief Gets Accelerometer data from IMU driver.
|
|
\param[in] accel_data : Accelerometer data
|
|
\return \ref Execution status.
|
|
*/
|
|
static int32_t IMU_GetAccelData(ARM_IMU_COORDINATES *accel_data)
|
|
{
|
|
ARM_IMU_COORDINATES data;
|
|
uint8_t buf[6];
|
|
|
|
/* Reads Acceleromter data */
|
|
IMU_Read(icm42670p_drv_info.target_addr,
|
|
ICM42670P_ACCEL_DATA1_REG,
|
|
buf,
|
|
ICM42670P_ACCEL_DATA_SIZE);
|
|
|
|
/* Processes Accelerometer data */
|
|
data.x = (buf[ICM42670P_ACCEL_DATA_X1_OFFSET] <<
|
|
ICM42670P_UPPER_DATA_BYTE_Pos);
|
|
data.x |= buf[ICM42670P_ACCEL_DATA_X0_OFFSET];
|
|
data.y = (buf[ICM42670P_ACCEL_DATA_Y1_OFFSET] <<
|
|
ICM42670P_UPPER_DATA_BYTE_Pos);
|
|
data.y |= buf[ICM42670P_ACCEL_DATA_Y0_OFFSET];
|
|
data.z = (buf[ICM42670P_ACCEL_DATA_Z1_OFFSET] <<
|
|
ICM42670P_UPPER_DATA_BYTE_Pos);
|
|
data.z |= buf[ICM42670P_ACCEL_DATA_Z0_OFFSET];
|
|
|
|
accel_data->x = ICM42670P_ACCEL_VAL(data.x);
|
|
accel_data->y = ICM42670P_ACCEL_VAL(data.y);
|
|
accel_data->z = ICM42670P_ACCEL_VAL(data.z);
|
|
|
|
/* Resets data received status */
|
|
icm42670p_drv_info.status.data_rcvd = 0U;
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t IMU_GetGyroData(ARM_IMU_COORDINATES *gyro_data)
|
|
\brief Gets Gyroscope data from IMU driver.
|
|
\param[in] gyro_data : Gyroscope data
|
|
\return \ref Execution status.
|
|
*/
|
|
static int32_t IMU_GetGyroData(ARM_IMU_COORDINATES *gyro_data)
|
|
{
|
|
ARM_IMU_COORDINATES data;
|
|
uint8_t buf[6];
|
|
|
|
/* Reads Gyroscope data */
|
|
IMU_Read(icm42670p_drv_info.target_addr,
|
|
ICM42670P_GYRO_DATA1_REG,
|
|
buf,
|
|
ICM42670P_GYRO_DATA_SIZE);
|
|
|
|
/* Processes Gyroscope data */
|
|
data.x = (buf[ICM42670P_GYRO_DATA_X1_OFFSET] <<
|
|
ICM42670P_UPPER_DATA_BYTE_Pos);
|
|
data.x |= buf[ICM42670P_GYRO_DATA_X0_OFFSET];
|
|
data.y = (buf[ICM42670P_GYRO_DATA_Y1_OFFSET] <<
|
|
ICM42670P_UPPER_DATA_BYTE_Pos);
|
|
data.y |= buf[ICM42670P_GYRO_DATA_Y0_OFFSET];
|
|
data.z = (buf[ICM42670P_GYRO_DATA_Z1_OFFSET] <<
|
|
ICM42670P_UPPER_DATA_BYTE_Pos);
|
|
data.z |= buf[ICM42670P_GYRO_DATA_Z0_OFFSET];
|
|
|
|
gyro_data->x = ICM42670P_GYRO_VAL(data.x);
|
|
gyro_data->y = ICM42670P_GYRO_VAL(data.y);
|
|
gyro_data->z = ICM42670P_GYRO_VAL(data.z);
|
|
|
|
/* Resets data received status */
|
|
icm42670p_drv_info.status.data_rcvd = 0U;
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t IMU_GetTempData(float *temp_data)
|
|
\brief Gets Temperature sensor data from IMU driver.
|
|
\param[in] temp_data : Temperature sensor data
|
|
\return \ref Execution status.
|
|
*/
|
|
static int32_t IMU_GetTempData(float *temp_data)
|
|
{
|
|
int16_t ltemp;
|
|
uint8_t buf[2];
|
|
|
|
/* Reads Temperature Sensor data */
|
|
IMU_Read(icm42670p_drv_info.target_addr,
|
|
ICM42670P_TEMP_DATA1_REG,
|
|
buf,
|
|
ICM42670P_TEMP_DATA_SIZE);
|
|
|
|
/* Processes Temp data */
|
|
ltemp = (buf[ICM42670P_TEMP_DATA1_OFFSET] <<
|
|
ICM42670P_UPPER_DATA_BYTE_Pos);
|
|
ltemp |= buf[ICM42670P_TEMP_DATA0_OFFSET];
|
|
*temp_data = ICM42670P_TEMPERATURE(ltemp);
|
|
|
|
/* Resets data received status */
|
|
icm42670p_drv_info.status.data_rcvd = 0U;
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t IMU_Deinit(void)
|
|
\brief De-initializes IMU driver.
|
|
\return \ref Execution status.
|
|
*/
|
|
static int32_t IMU_Deinit(void)
|
|
{
|
|
int32_t ret;
|
|
|
|
ret = I3C_Driver->PowerControl(ARM_POWER_OFF);
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
ret = I3C_Driver->Uninitialize();
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t ARM_IMU_Initialize (void)
|
|
\brief Initializes IMU Interface.
|
|
\param[in] none.
|
|
\return \ref execution_status.
|
|
*/
|
|
static int32_t ARM_IMU_Initialize(void)
|
|
{
|
|
int32_t ret;
|
|
|
|
if(icm42670p_drv_info.state & ICM42670P_DRIVER_INITIALIZED)
|
|
{
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
ret = IMU_Init();
|
|
if (ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* Initializes driver state and data received status */
|
|
icm42670p_drv_info.state = ICM42670P_DRIVER_INITIALIZED;
|
|
icm42670p_drv_info.status.data_rcvd = 0U;
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t ARM_IMU_Uninitialize(void)
|
|
\brief Uninitializes IMU Interface.
|
|
\param[in] none.
|
|
\return \ref execution_status.
|
|
*/
|
|
static int32_t ARM_IMU_Uninitialize(void)
|
|
{
|
|
int32_t ret;
|
|
|
|
ret = IMU_Deinit();
|
|
if (ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* Resets driver state and data received status */
|
|
icm42670p_drv_info.state = 0U;
|
|
icm42670p_drv_info.status.data_rcvd = 0U;
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t ARM_IMU_PowerControl(ARM_POWER_STATE state)
|
|
\brief Controls power for IMU Interface.
|
|
\param[in] state : Power state
|
|
\return \ref execution_status.
|
|
*/
|
|
static int32_t ARM_IMU_PowerControl(ARM_POWER_STATE state)
|
|
{
|
|
int32_t ret;
|
|
|
|
/* Returns error if not initialized */
|
|
if(!(icm42670p_drv_info.state & ICM42670P_DRIVER_INITIALIZED))
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
|
|
switch (state)
|
|
{
|
|
case ARM_POWER_OFF:
|
|
if(!(icm42670p_drv_info.state & ICM42670P_DRIVER_POWERED))
|
|
{
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/* Sets state to Powered Down */
|
|
icm42670p_drv_info.state &= ~ICM42670P_DRIVER_POWERED;
|
|
break;
|
|
|
|
case ARM_POWER_FULL:
|
|
if(icm42670p_drv_info.state & ICM42670P_DRIVER_POWERED)
|
|
{
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/* Sets up IMU */
|
|
ret = IMU_Setup();
|
|
if(ret != ARM_DRIVER_OK)
|
|
{
|
|
return ret;
|
|
}
|
|
|
|
/* Sets state to Powered Up */
|
|
icm42670p_drv_info.state |= ICM42670P_DRIVER_POWERED;
|
|
break;
|
|
|
|
case ARM_POWER_LOW:
|
|
default:
|
|
{
|
|
return ARM_DRIVER_ERROR_UNSUPPORTED;
|
|
}
|
|
}
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
/**
|
|
\fn int32_t ARM_IMU_Control(uint32_t control, uint32_t arg)
|
|
\brief Control ICM42670P Slave
|
|
\param[in] control : Operation
|
|
\param[in] arg : Argument of operation
|
|
\return \ref execution_status
|
|
*/
|
|
static int32_t ARM_IMU_Control(uint32_t control, uint32_t arg)
|
|
{
|
|
ARM_IMU_COORDINATES *ptr;
|
|
float *temp_data;
|
|
|
|
switch(control)
|
|
{
|
|
case IMU_GET_ACCELEROMETER_DATA:
|
|
if(!arg)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
ptr = (ARM_IMU_COORDINATES*)arg;
|
|
IMU_GetAccelData(ptr);
|
|
break;
|
|
|
|
case IMU_GET_GYROSCOPE_DATA:
|
|
if(!arg)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
ptr = (ARM_IMU_COORDINATES*)arg;
|
|
IMU_GetGyroData(ptr);
|
|
break;
|
|
|
|
case IMU_GET_TEMPERATURE_DATA:
|
|
if(!arg)
|
|
{
|
|
return ARM_DRIVER_ERROR;
|
|
}
|
|
temp_data = (float*)arg;
|
|
IMU_GetTempData(temp_data);
|
|
break;
|
|
|
|
case IMU_GET_MAGNETOMETER_DATA:
|
|
default:
|
|
return ARM_DRIVER_ERROR_UNSUPPORTED;
|
|
}
|
|
|
|
return ARM_DRIVER_OK;
|
|
}
|
|
|
|
extern ARM_DRIVER_IMU ICM42670P;
|
|
|
|
ARM_DRIVER_IMU ICM42670P =
|
|
{
|
|
ARM_IMU_GetVersion,
|
|
ARM_IMU_GetCapabilities,
|
|
ARM_IMU_GetStatus,
|
|
ARM_IMU_Initialize,
|
|
ARM_IMU_Uninitialize,
|
|
ARM_IMU_PowerControl,
|
|
ARM_IMU_Control
|
|
};
|