98 lines
2.7 KiB
C
98 lines
2.7 KiB
C
#include "imu_lsm6dsl.h"
|
|
|
|
#define LSM6DSL_REG_WHOAMI 0x0FU
|
|
#define LSM6DSL_REG_CTRL1_XL 0x10U
|
|
#define LSM6DSL_REG_OUT_TEMP_L 0x20U
|
|
#define LSM6DSL_REG_OUT_TEMP_H 0x21U
|
|
|
|
#define LSM6DSL_CONST_WHOAMI 0x6AU
|
|
|
|
#define LSM6DSL_MAXIMUM_MULTI_WRITE_SIZE 32
|
|
|
|
static ims_ret_t ims_lsm6dsl_whoami(ims_lsm6dsl_t *imu, uint8_t *whoami);
|
|
|
|
static ims_ret_t ims_lsm6dsl_read_register(ims_lsm6dsl_t *imu, uint8_t addr, uint8_t *data);
|
|
static ims_ret_t ims_lsm6dsl_read_registers(ims_lsm6dsl_t *imu, uint8_t addr, uint8_t *data, uint16_t len);
|
|
static ims_ret_t ims_lsm6dsl_write_register(ims_lsm6dsl_t *imu, uint8_t addr, uint8_t data);
|
|
static ims_ret_t ims_lsm6dsl_write_registers(ims_lsm6dsl_t *imu, uint8_t addr, uint8_t *data, uint16_t len);
|
|
|
|
ims_ret_t ims_lsm6dsl_init(ims_lsm6dsl_t *imu) {
|
|
ims_ret_t ret;
|
|
|
|
uint8_t who_am_i;
|
|
|
|
ret = ims_lsm6dsl_whoami(imu, &who_am_i);
|
|
if (ret != IMS_SUCCESS) {
|
|
return ret;
|
|
}
|
|
|
|
if (who_am_i != LSM6DSL_CONST_WHOAMI) {
|
|
return IMS_FAIL;
|
|
}
|
|
|
|
ret = ims_lsm6dsl_write_register(imu, LSM6DSL_REG_CTRL1_XL, 0xB0);
|
|
if (ret != IMS_SUCCESS) {
|
|
return IMS_FAIL;
|
|
}
|
|
|
|
return IMS_SUCCESS;
|
|
}
|
|
|
|
ims_ret_t ims_lsm6dsl_read_temperature(ims_lsm6dsl_t *imu, int16_t *temp) {
|
|
ims_ret_t ret;
|
|
|
|
uint8_t rx_temp[2];
|
|
|
|
ret = ims_lsm6dsl_read_registers(imu, LSM6DSL_REG_OUT_TEMP_L, rx_temp, 2);
|
|
if (ret != IMS_SUCCESS) {
|
|
return ret;
|
|
}
|
|
|
|
*temp = (int16_t)(uint16_t)((rx_temp[1] << 8U) | rx_temp[0]);
|
|
|
|
return IMS_SUCCESS;
|
|
}
|
|
|
|
static ims_ret_t ims_lsm6dsl_whoami(ims_lsm6dsl_t *imu, uint8_t *whoami) {
|
|
return ims_lsm6dsl_read_register(imu, LSM6DSL_REG_WHOAMI, whoami);
|
|
}
|
|
|
|
static ims_ret_t ims_lsm6dsl_read_register(ims_lsm6dsl_t *imu, uint8_t addr, uint8_t *data) {
|
|
return ims_lsm6dsl_read_registers(imu, addr, data, 1U);
|
|
}
|
|
|
|
static ims_ret_t ims_lsm6dsl_read_registers(ims_lsm6dsl_t *imu, uint8_t addr, uint8_t *data, uint16_t len) {
|
|
ims_i2c_xfer_desc_t xfer = {
|
|
.tx_size = 1U,
|
|
.tx_data = &addr,
|
|
.rx_size = len,
|
|
.rx_data = data,
|
|
};
|
|
|
|
return imu->cb.i2c_xfer(imu->pdev, &xfer);
|
|
}
|
|
|
|
static ims_ret_t ims_lsm6dsl_write_register(ims_lsm6dsl_t *imu, uint8_t addr, uint8_t data) {
|
|
return ims_lsm6dsl_write_registers(imu, addr, &data, 1U);
|
|
}
|
|
|
|
static ims_ret_t ims_lsm6dsl_write_registers(ims_lsm6dsl_t *imu, uint8_t addr, uint8_t *data, uint16_t len) {
|
|
if (len > LSM6DSL_MAXIMUM_MULTI_WRITE_SIZE) {
|
|
return IMS_FAIL;
|
|
}
|
|
|
|
uint8_t tx_buf[LSM6DSL_MAXIMUM_MULTI_WRITE_SIZE + 1];
|
|
|
|
tx_buf[0] = addr;
|
|
|
|
memcpy(&tx_buf[1], data, len);
|
|
|
|
ims_i2c_xfer_desc_t xfer = {
|
|
.tx_size = len + 1,
|
|
.tx_data = tx_buf,
|
|
.rx_size = 0U,
|
|
.rx_data = NULL,
|
|
};
|
|
|
|
return imu->cb.i2c_xfer(imu->pdev, &xfer);
|
|
} |