251 lines
6.7 KiB
C
251 lines
6.7 KiB
C
/*
|
|
* IMU_ IAM20680.c
|
|
*
|
|
* Created on: Nov 5, 2022
|
|
* Author: gcstk
|
|
*/
|
|
|
|
#include <IMU_IAM20680.h>
|
|
#include <main.h>
|
|
#include "user_iam20680.h"
|
|
|
|
static void IAM20680_spi_write(IAM20680_t *ctx, uint8_t reg, uint8_t *data_buf, uint8_t len);
|
|
|
|
static void IAM20680_spi_read(IAM20680_t *ctx, uint8_t reg, uint8_t *data_buf, uint8_t len);
|
|
|
|
extern SPI_HandleTypeDef hspi2;
|
|
|
|
|
|
// ------------------------------------------------ PUBLIC FUNCTION DEFINITIONS
|
|
uint8_t IAM20680_init(IAM20680_t *ctx, IAM20680_cfg_t *cfg)
|
|
{
|
|
uint8_t retry = 3;
|
|
|
|
while (retry != 0)
|
|
{
|
|
retry--;
|
|
HAL_Delay(150);
|
|
if (IAM20680_OK == IAM20680_lowlevel_init(ctx, cfg))
|
|
return IAM20680_OK;
|
|
}
|
|
return IAM20680_INIT_ERROR;
|
|
}
|
|
uint8_t IAM20680_lowlevel_init(IAM20680_t *ctx, IAM20680_cfg_t *cfg)
|
|
{
|
|
uint8_t device_id;
|
|
|
|
ctx->read_f = IAM20680_spi_read;
|
|
ctx->write_f = IAM20680_spi_write;
|
|
/* cheak IMU in board */
|
|
device_id = IAM20680_get_device_id(ctx);
|
|
if (device_id != IAM20680_DEVICE_ID && device_id != IAM20680HT_DEVICE_ID)
|
|
{
|
|
return IAM20680_INIT_ERROR;
|
|
}
|
|
|
|
/* initialize the sensor */
|
|
IAM20680_reg_write(ctx, IAM20680_REG_PWR_MGMT_1, 0x80, 0); // reset IMU
|
|
HAL_Delay(100);
|
|
IAM20680_set_spi_mode(ctx); // set to spi mode
|
|
IAM20680_reg_write(ctx, IAM20680_REG_PWR_MGMT_1, 0x01, 0); // Auto selects the best available clock source
|
|
|
|
/* Disable Accel and Gyro all axes */
|
|
IAM20680_reg_write(ctx, IAM20680_REG_PWR_MGMT_2, 0x3F, 1);
|
|
|
|
/* Set default full scale range */
|
|
IAM20680_set_gyro_measurement_range(ctx, IAM20680_GYRO_CONFIG_FS_SEL_2000dps);
|
|
IAM20680_set_accel_measurement_range(ctx, IAM20680_ACCEL_CONFIG_FS_SEL_2g);
|
|
|
|
/* Set default bandwidth */
|
|
IAM20680_set_gyro_DLPF_CFG(ctx, IAM20680_CONFIG_DLPF_CFG_92);
|
|
IAM20680_set_accel_A_DLPF_CFG(ctx, IAM20680_ACCEL_CONFIG2_A_DLPF_CFG_99);
|
|
|
|
/* Initial sampling rate to 100Hz*/
|
|
IAM20680_reg_write(ctx, IAM20680_REG_SMPLRT_DIV, 9, 1);
|
|
|
|
/* Set interrupt */
|
|
IAM20680_reg_write(ctx, IAM20680_REG_INT_PIN_CFG, 0x10, 1); // set interrupt pin mode
|
|
IAM20680_reg_write(ctx, IAM20680_REG_INT_ENABLE, 0x01, 1); // enable interrupt
|
|
|
|
/* Enable Accel and Gyro all axes */
|
|
IAM20680_reg_write(ctx, IAM20680_REG_PWR_MGMT_2, 0x00, 1);
|
|
|
|
return IAM20680_OK;
|
|
}
|
|
|
|
void IAM20680_generic_write(IAM20680_t *ctx, uint8_t reg, uint8_t *data_buf, uint8_t len)
|
|
{
|
|
ctx->write_f(ctx, reg, data_buf, len);
|
|
}
|
|
|
|
void IAM20680_generic_read(IAM20680_t *ctx, uint8_t reg, uint8_t *data_buf, uint8_t len)
|
|
{
|
|
ctx->read_f(ctx, reg, data_buf, len);
|
|
}
|
|
|
|
void IAM20680_set_config(IAM20680_t *ctx, uint8_t config_data)
|
|
{
|
|
config_data &= 0x7F;
|
|
|
|
IAM20680_generic_write(ctx, IAM20680_REG_CONFIG, &config_data, 1);
|
|
}
|
|
|
|
void IAM20680_set_gyro_measurement_range(IAM20680_t *ctx, IAM20680_GYRO_CONFIG_FS_SEL_t new_value)
|
|
{
|
|
uint8_t value;
|
|
|
|
IAM20680_generic_read(ctx, IAM20680_REG_GYRO_CONFIG, &value, 1);
|
|
value &= ~BIT_GYRO_FS_SEL_MASK;
|
|
value |= new_value;
|
|
IAM20680_generic_write(ctx, IAM20680_REG_GYRO_CONFIG, &value, 1);
|
|
}
|
|
|
|
void IAM20680_set_accel_measurement_range(IAM20680_t *ctx, IAM20680_ACCEL_CONFIG_FS_SEL_t new_value)
|
|
{
|
|
uint8_t value;
|
|
|
|
IAM20680_generic_read(ctx, IAM20680_REG_ACCEL_CONFIG, &value, 1);
|
|
value &= ~BIT_ACCEL_FS_SEL_MASK;
|
|
value |= new_value;
|
|
|
|
IAM20680_generic_write(ctx, IAM20680_REG_ACCEL_CONFIG, &value, 1);
|
|
}
|
|
|
|
void IAM20680_set_gyro_DLPF_CFG(IAM20680_t *ctx, IAM20680_CONFIG_DLPF_CFG_t new_value)
|
|
{
|
|
uint8_t value;
|
|
|
|
IAM20680_generic_read(ctx, IAM20680_REG_CONFIG, &value, 1);
|
|
value &= ~BIT_DLPF_CFG_MASK;
|
|
value |= new_value;
|
|
IAM20680_generic_write(ctx, IAM20680_REG_CONFIG, &value, 1);
|
|
}
|
|
|
|
void IAM20680_set_accel_A_DLPF_CFG(IAM20680_t *ctx, IAM20680_ACCEL_CONFIG2_A_DLPF_CFG_t new_value)
|
|
{
|
|
uint8_t value;
|
|
|
|
IAM20680_generic_read(ctx, IAM20680_REG_ACCEL_CONFIG_2, &value, 1);
|
|
value &= ~BIT_A_DLPF_CFG_MASK;
|
|
value |= new_value;
|
|
|
|
IAM20680_generic_write(ctx, IAM20680_REG_ACCEL_CONFIG_2, &value, 1);
|
|
}
|
|
|
|
uint8_t IAM20680_get_interrupt_status(IAM20680_t *ctx)
|
|
{
|
|
uint8_t temp;
|
|
|
|
IAM20680_generic_read(ctx, IAM20680_REG_INT_STATUS, &temp, 1);
|
|
|
|
return temp;
|
|
}
|
|
|
|
void IAM20680_set_spi_mode(IAM20680_t *ctx)
|
|
{
|
|
uint8_t temp;
|
|
|
|
IAM20680_generic_read(ctx, IAM20680_REG_USER_CTRL, &temp, 1);
|
|
|
|
temp |= 0x10;
|
|
|
|
IAM20680_generic_write(ctx, IAM20680_REG_USER_CTRL, &temp, 1);
|
|
|
|
temp = 0;
|
|
|
|
IAM20680_generic_read(ctx, IAM20680_REG_USER_CTRL, &temp, 1);
|
|
}
|
|
|
|
uint8_t IAM20680_get_device_id(IAM20680_t *ctx)
|
|
{
|
|
uint8_t temp;
|
|
|
|
IAM20680_generic_read(ctx, IAM20680_REG_WHO_AM_I, &temp, 1);
|
|
|
|
return temp;
|
|
}
|
|
|
|
int16_t IAM20680_get_axis(IAM20680_t *ctx, uint8_t addr_reg_msb, uint8_t addr_reg_lsb)
|
|
{
|
|
int16_t result;
|
|
uint16_t temp;
|
|
uint8_t read_buffer[2];
|
|
|
|
IAM20680_generic_read(ctx, addr_reg_msb, read_buffer, 2);
|
|
|
|
temp = read_buffer[0];
|
|
temp <<= 8;
|
|
temp |= read_buffer[1];
|
|
|
|
result = (int16_t) temp;
|
|
|
|
return result;
|
|
}
|
|
|
|
void IAM20680_get_accel_data(IAM20680_t *ctx, int16_t *p_accel_x, int16_t *p_accel_y, int16_t *p_accel_z)
|
|
{
|
|
*p_accel_x = IAM20680_get_axis(ctx, IAM20680_REG_ACCEL_XOUT_H, IAM20680_REG_ACCEL_XOUT_L);
|
|
*p_accel_y = IAM20680_get_axis(ctx, IAM20680_REG_ACCEL_YOUT_H, IAM20680_REG_ACCEL_YOUT_L);
|
|
*p_accel_z = IAM20680_get_axis(ctx, IAM20680_REG_ACCEL_ZOUT_H, IAM20680_REG_ACCEL_ZOUT_L);
|
|
}
|
|
|
|
void IAM20680_get_gyro_data(IAM20680_t *ctx, int16_t *p_gyro_x, int16_t *p_gyro_y, int16_t *p_gyro_z)
|
|
{
|
|
*p_gyro_x = IAM20680_get_axis(ctx, IAM20680_REG_GYRO_XOUT_H, IAM20680_REG_GYRO_XOUT_L);
|
|
*p_gyro_y = IAM20680_get_axis(ctx, IAM20680_REG_GYRO_YOUT_H, IAM20680_REG_GYRO_YOUT_L);
|
|
*p_gyro_z = IAM20680_get_axis(ctx, IAM20680_REG_GYRO_ZOUT_H, IAM20680_REG_GYRO_ZOUT_L);
|
|
}
|
|
|
|
static void IAM20680_spi_write(IAM20680_t *ctx, uint8_t reg, uint8_t *data_buf, uint8_t len)
|
|
{
|
|
uint8_t tx_buf[265];
|
|
uint8_t cnt;
|
|
|
|
tx_buf[0] = reg;
|
|
for (cnt = 1; cnt <= len; cnt++)
|
|
{
|
|
tx_buf[cnt] = data_buf[cnt - 1];
|
|
}
|
|
HAL_GPIO_WritePin(SPI2_CSN_GPIO_Port, SPI2_CSN_Pin, GPIO_PIN_RESET);
|
|
HAL_SPI_Transmit(&hspi2, tx_buf, len + 1, 0xff);
|
|
HAL_GPIO_WritePin(SPI2_CSN_GPIO_Port, SPI2_CSN_Pin, GPIO_PIN_SET);
|
|
}
|
|
|
|
static void IAM20680_spi_read(IAM20680_t *ctx, uint8_t reg, uint8_t *data_buf, uint8_t len)
|
|
{
|
|
uint8_t tx_buf[1];
|
|
uint8_t rx_buf[265] = { 0 };
|
|
uint8_t cnt;
|
|
|
|
tx_buf[0] = reg | 0x80;
|
|
|
|
HAL_GPIO_WritePin(SPI2_CSN_GPIO_Port, SPI2_CSN_Pin, GPIO_PIN_RESET);
|
|
HAL_SPI_Transmit(&hspi2, tx_buf, 1, 1000);
|
|
HAL_SPI_Receive(&hspi2, rx_buf, len, 1000);
|
|
HAL_GPIO_WritePin(SPI2_CSN_GPIO_Port, SPI2_CSN_Pin, GPIO_PIN_SET);
|
|
for (cnt = 0; cnt < len; cnt++)
|
|
{
|
|
data_buf[cnt] = rx_buf[cnt];
|
|
}
|
|
}
|
|
|
|
uint8_t IAM20680_reg_write(IAM20680_t *ctx, uint8_t reg, uint8_t reg_val, uint8_t cheak)
|
|
{
|
|
uint8_t ans = IAM20680_OK;
|
|
uint8_t RD_reg = 0;
|
|
|
|
IAM20680_generic_write(ctx, reg, ®_val, 1);
|
|
|
|
if (1 == cheak)
|
|
{
|
|
IAM20680_generic_read(ctx, reg, &RD_reg, 1);
|
|
if (RD_reg != reg_val)
|
|
{
|
|
ans = IAM20680_INIT_ERROR;
|
|
}
|
|
}
|
|
|
|
return ans;
|
|
}
|
|
// ------------------------------------------------------------------------- END
|