485 lines
14 KiB
C
485 lines
14 KiB
C
/* Bosch BMI08X inertial measurement unit driver
|
|
*
|
|
* Copyright (c) 2022 Meta Platforms, Inc. and its affiliates
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#include <zephyr/drivers/sensor.h>
|
|
#include <zephyr/pm/device.h>
|
|
#include <zephyr/init.h>
|
|
#include <zephyr/kernel.h>
|
|
#include <zephyr/logging/log.h>
|
|
#include <zephyr/sys/__assert.h>
|
|
#include <zephyr/sys/byteorder.h>
|
|
|
|
#define DT_DRV_COMPAT bosch_bmi08x_gyro
|
|
#include "bmi08x.h"
|
|
|
|
LOG_MODULE_REGISTER(BMI08X_GYRO, CONFIG_SENSOR_LOG_LEVEL);
|
|
|
|
#if DT_ANY_INST_ON_BUS_STATUS_OKAY(i2c)
|
|
|
|
static int bmi08x_gyro_transceive_i2c(const struct device *dev, uint8_t reg, bool write, void *data,
|
|
size_t length)
|
|
{
|
|
const struct bmi08x_gyro_config *bmi08x = dev->config;
|
|
|
|
if (!write) {
|
|
return i2c_write_read_dt(&bmi08x->bus.i2c, ®, 1, data, length);
|
|
}
|
|
if (length > CONFIG_BMI08X_I2C_WRITE_BURST_SIZE) {
|
|
return -EINVAL;
|
|
}
|
|
uint8_t buf[1 + CONFIG_BMI08X_I2C_WRITE_BURST_SIZE];
|
|
|
|
buf[0] = reg;
|
|
memcpy(&buf[1], data, length);
|
|
return i2c_write_dt(&bmi08x->bus.i2c, buf, 1 + length);
|
|
}
|
|
|
|
static int bmi08x_bus_check_i2c(const union bmi08x_bus *bus)
|
|
{
|
|
return i2c_is_ready_dt(&bus->i2c) ? 0 : -ENODEV;
|
|
}
|
|
|
|
static const struct bmi08x_gyro_bus_io bmi08x_i2c_api = {
|
|
.check = bmi08x_bus_check_i2c,
|
|
.transceive = bmi08x_gyro_transceive_i2c,
|
|
};
|
|
|
|
#endif /* DT_ANY_INST_ON_BUS_STATUS_OKAY(i2c) */
|
|
|
|
#if DT_ANY_INST_ON_BUS_STATUS_OKAY(spi)
|
|
|
|
static int bmi08x_gyro_transceive_spi(const struct device *dev, uint8_t reg, bool write, void *data,
|
|
size_t length)
|
|
{
|
|
const struct bmi08x_gyro_config *bmi08x = dev->config;
|
|
const struct spi_buf tx_buf[2] = {{.buf = ®, .len = 1}, {.buf = data, .len = length}};
|
|
const struct spi_buf_set tx = {.buffers = tx_buf, .count = write ? 2 : 1};
|
|
|
|
if (!write) {
|
|
uint16_t dummy;
|
|
const struct spi_buf rx_buf[2] = {{.buf = &dummy, .len = 1},
|
|
{.buf = data, .len = length}};
|
|
const struct spi_buf_set rx = {.buffers = rx_buf, .count = 2};
|
|
|
|
return spi_transceive_dt(&bmi08x->bus.spi, &tx, &rx);
|
|
}
|
|
|
|
return spi_write_dt(&bmi08x->bus.spi, &tx);
|
|
}
|
|
|
|
static int bmi08x_bus_check_spi(const union bmi08x_bus *bus)
|
|
{
|
|
return spi_is_ready_dt(&bus->spi) ? 0 : -ENODEV;
|
|
}
|
|
|
|
static const struct bmi08x_gyro_bus_io bmi08x_spi_api = {
|
|
.check = bmi08x_bus_check_spi,
|
|
.transceive = bmi08x_gyro_transceive_spi,
|
|
};
|
|
|
|
#endif /* DT_ANY_INST_ON_BUS_STATUS_OKAY(spi) */
|
|
|
|
static inline int bmi08x_bus_check(const struct device *dev)
|
|
{
|
|
const struct bmi08x_gyro_config *config = dev->config;
|
|
|
|
return config->api->check(&config->bus);
|
|
}
|
|
|
|
static int bmi08x_gyro_transceive(const struct device *dev, uint8_t reg, bool write, void *data,
|
|
size_t length)
|
|
{
|
|
const struct bmi08x_gyro_config *cfg = dev->config;
|
|
|
|
return cfg->api->transceive(dev, reg, write, data, length);
|
|
}
|
|
|
|
int bmi08x_gyro_read(const struct device *dev, uint8_t reg_addr, uint8_t *data, uint8_t len)
|
|
{
|
|
return bmi08x_gyro_transceive(dev, reg_addr | BIT(7), false, data, len);
|
|
}
|
|
|
|
int bmi08x_gyro_byte_read(const struct device *dev, uint8_t reg_addr, uint8_t *byte)
|
|
{
|
|
return bmi08x_gyro_transceive(dev, reg_addr | BIT(7), false, byte, 1);
|
|
}
|
|
|
|
int bmi08x_gyro_byte_write(const struct device *dev, uint8_t reg_addr, uint8_t byte)
|
|
{
|
|
return bmi08x_gyro_transceive(dev, reg_addr & 0x7F, true, &byte, 1);
|
|
}
|
|
|
|
int bmi08x_gyro_word_write(const struct device *dev, uint8_t reg_addr, uint16_t word)
|
|
{
|
|
uint8_t tx_word[2] = {(uint8_t)(word & 0xff), (uint8_t)(word >> 8)};
|
|
|
|
return bmi08x_gyro_transceive(dev, reg_addr & 0x7F, true, tx_word, 2);
|
|
}
|
|
|
|
int bmi08x_gyro_reg_field_update(const struct device *dev, uint8_t reg_addr, uint8_t pos,
|
|
uint8_t mask, uint8_t val)
|
|
{
|
|
uint8_t old_val;
|
|
int ret;
|
|
|
|
ret = bmi08x_gyro_byte_read(dev, reg_addr, &old_val);
|
|
if (ret < 0) {
|
|
return ret;
|
|
}
|
|
|
|
return bmi08x_gyro_byte_write(dev, reg_addr, (old_val & ~mask) | ((val << pos) & mask));
|
|
}
|
|
|
|
static const struct bmi08x_range bmi08x_gyr_range_map[] = {
|
|
{125, BMI08X_GYR_RANGE_125DPS}, {250, BMI08X_GYR_RANGE_250DPS},
|
|
{500, BMI08X_GYR_RANGE_500DPS}, {1000, BMI08X_GYR_RANGE_1000DPS},
|
|
{2000, BMI08X_GYR_RANGE_2000DPS},
|
|
};
|
|
#define BMI08X_GYR_RANGE_MAP_SIZE ARRAY_SIZE(bmi08x_gyr_range_map)
|
|
|
|
int32_t bmi08x_gyr_reg_val_to_range(uint8_t reg_val)
|
|
{
|
|
return bmi08x_reg_val_to_range(reg_val, bmi08x_gyr_range_map, BMI08X_GYR_RANGE_MAP_SIZE);
|
|
}
|
|
|
|
static int bmi08x_gyr_odr_set(const struct device *dev, uint16_t freq_int, uint16_t freq_milli)
|
|
{
|
|
int odr = bmi08x_freq_to_odr_val(freq_int, freq_milli);
|
|
|
|
if (odr < 0) {
|
|
return odr;
|
|
}
|
|
|
|
if (odr < BMI08X_GYRO_BW_532_ODR_2000_HZ || odr > BMI08X_GYRO_BW_32_ODR_100_HZ) {
|
|
return -ENOTSUP;
|
|
}
|
|
|
|
return bmi08x_gyro_byte_write(dev, BMI08X_REG_GYRO_BANDWIDTH, (uint8_t)odr);
|
|
}
|
|
|
|
static int bmi08x_gyr_range_set(const struct device *dev, uint16_t range)
|
|
{
|
|
struct bmi08x_gyro_data *bmi08x = dev->data;
|
|
int32_t reg_val =
|
|
bmi08x_range_to_reg_val(range, bmi08x_gyr_range_map, BMI08X_GYR_RANGE_MAP_SIZE);
|
|
int ret;
|
|
|
|
if (reg_val < 0) {
|
|
return reg_val;
|
|
}
|
|
|
|
ret = bmi08x_gyro_byte_write(dev, BMI08X_REG_GYRO_RANGE, reg_val);
|
|
if (ret < 0) {
|
|
return ret;
|
|
}
|
|
|
|
bmi08x->scale = BMI08X_GYR_SCALE(range);
|
|
|
|
return ret;
|
|
}
|
|
|
|
static int bmi08x_gyr_config(const struct device *dev, enum sensor_channel chan,
|
|
enum sensor_attribute attr, const struct sensor_value *val)
|
|
{
|
|
switch (attr) {
|
|
case SENSOR_ATTR_FULL_SCALE:
|
|
return bmi08x_gyr_range_set(dev, sensor_rad_to_degrees(val));
|
|
case SENSOR_ATTR_SAMPLING_FREQUENCY:
|
|
return bmi08x_gyr_odr_set(dev, val->val1, val->val2 / 1000);
|
|
default:
|
|
LOG_DBG("Gyro attribute not supported.");
|
|
return -ENOTSUP;
|
|
}
|
|
}
|
|
|
|
static int bmi08x_attr_set(const struct device *dev, enum sensor_channel chan,
|
|
enum sensor_attribute attr, const struct sensor_value *val)
|
|
{
|
|
#ifdef CONFIG_PM_DEVICE
|
|
enum pm_device_state state;
|
|
|
|
(void)pm_device_state_get(dev, &state);
|
|
if (state != PM_DEVICE_STATE_ACTIVE) {
|
|
return -EBUSY;
|
|
}
|
|
#endif
|
|
|
|
switch (chan) {
|
|
case SENSOR_CHAN_GYRO_X:
|
|
case SENSOR_CHAN_GYRO_Y:
|
|
case SENSOR_CHAN_GYRO_Z:
|
|
case SENSOR_CHAN_GYRO_XYZ:
|
|
return bmi08x_gyr_config(dev, chan, attr, val);
|
|
default:
|
|
LOG_DBG("attr_set() not supported on this channel.");
|
|
return -ENOTSUP;
|
|
}
|
|
}
|
|
|
|
static int bmi08x_sample_fetch(const struct device *dev, enum sensor_channel chan)
|
|
{
|
|
struct bmi08x_gyro_data *bmi08x = dev->data;
|
|
size_t i;
|
|
int ret;
|
|
|
|
if (chan != SENSOR_CHAN_ALL && chan != SENSOR_CHAN_GYRO_XYZ) {
|
|
LOG_DBG("Unsupported sensor channel");
|
|
return -ENOTSUP;
|
|
}
|
|
|
|
ret = bmi08x_gyro_read(dev, BMI08X_REG_GYRO_X_LSB, (uint8_t *)bmi08x->gyr_sample,
|
|
sizeof(bmi08x->gyr_sample));
|
|
if (ret < 0) {
|
|
return ret;
|
|
}
|
|
|
|
/* convert samples to cpu endianness */
|
|
for (i = 0; i < ARRAY_SIZE(bmi08x->gyr_sample); i++) {
|
|
bmi08x->gyr_sample[i] = sys_le16_to_cpu(bmi08x->gyr_sample[i]);
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
static void bmi08x_to_fixed_point(int16_t raw_val, uint16_t scale, struct sensor_value *val)
|
|
{
|
|
int32_t converted_val;
|
|
|
|
/*
|
|
* maximum converted value we can get is: max(raw_val) * max(scale)
|
|
* max(raw_val) = +/- 2^15
|
|
* max(scale) = 4785
|
|
* max(converted_val) = 156794880 which is less than 2^31
|
|
*/
|
|
converted_val = raw_val * scale;
|
|
val->val1 = converted_val / 1000000;
|
|
val->val2 = converted_val % 1000000;
|
|
}
|
|
|
|
static void bmi08x_channel_convert(enum sensor_channel chan, uint16_t scale, uint16_t *raw_xyz,
|
|
struct sensor_value *val)
|
|
{
|
|
int i;
|
|
uint8_t ofs_start, ofs_stop;
|
|
|
|
switch (chan) {
|
|
case SENSOR_CHAN_GYRO_X:
|
|
ofs_start = ofs_stop = 0U;
|
|
break;
|
|
case SENSOR_CHAN_GYRO_Y:
|
|
ofs_start = ofs_stop = 1U;
|
|
break;
|
|
case SENSOR_CHAN_GYRO_Z:
|
|
ofs_start = ofs_stop = 2U;
|
|
break;
|
|
default:
|
|
ofs_start = 0U;
|
|
ofs_stop = 2U;
|
|
break;
|
|
}
|
|
|
|
for (i = ofs_start; i <= ofs_stop; i++, val++) {
|
|
bmi08x_to_fixed_point(raw_xyz[i], scale, val);
|
|
}
|
|
}
|
|
|
|
static inline void bmi08x_gyr_channel_get(const struct device *dev, enum sensor_channel chan,
|
|
struct sensor_value *val)
|
|
{
|
|
struct bmi08x_gyro_data *bmi08x = dev->data;
|
|
|
|
bmi08x_channel_convert(chan, bmi08x->scale, bmi08x->gyr_sample, val);
|
|
}
|
|
|
|
static int bmi08x_channel_get(const struct device *dev, enum sensor_channel chan,
|
|
struct sensor_value *val)
|
|
{
|
|
#ifdef CONFIG_PM_DEVICE
|
|
enum pm_device_state state;
|
|
|
|
(void)pm_device_state_get(dev, &state);
|
|
if (state != PM_DEVICE_STATE_ACTIVE) {
|
|
return -EBUSY;
|
|
}
|
|
#endif
|
|
|
|
switch ((int16_t)chan) {
|
|
case SENSOR_CHAN_GYRO_X:
|
|
case SENSOR_CHAN_GYRO_Y:
|
|
case SENSOR_CHAN_GYRO_Z:
|
|
case SENSOR_CHAN_GYRO_XYZ:
|
|
bmi08x_gyr_channel_get(dev, chan, val);
|
|
return 0;
|
|
default:
|
|
LOG_DBG("Channel not supported.");
|
|
return -ENOTSUP;
|
|
}
|
|
}
|
|
|
|
#ifdef CONFIG_PM_DEVICE
|
|
static int bmi08x_gyro_pm_action(const struct device *dev, enum pm_device_action action)
|
|
{
|
|
uint8_t reg_val;
|
|
int ret;
|
|
|
|
switch (action) {
|
|
case PM_DEVICE_ACTION_RESUME:
|
|
reg_val = BMI08X_GYRO_PM_NORMAL;
|
|
break;
|
|
case PM_DEVICE_ACTION_SUSPEND:
|
|
reg_val = BMI08X_GYRO_PM_SUSPEND;
|
|
break;
|
|
default:
|
|
return -ENOTSUP;
|
|
}
|
|
|
|
ret = bmi08x_gyro_byte_write(dev, BMI08X_REG_GYRO_LPM1, reg_val);
|
|
if (ret < 0) {
|
|
LOG_ERR("Failed to set power mode");
|
|
return ret;
|
|
}
|
|
k_msleep(BMI08X_GYRO_POWER_MODE_CONFIG_DELAY);
|
|
|
|
return ret;
|
|
}
|
|
#endif /* CONFIG_PM_DEVICE */
|
|
|
|
static const struct sensor_driver_api bmi08x_api = {
|
|
.attr_set = bmi08x_attr_set,
|
|
#ifdef CONFIG_BMI08X_GYRO_TRIGGER
|
|
.trigger_set = bmi08x_trigger_set_gyr,
|
|
#endif
|
|
.sample_fetch = bmi08x_sample_fetch,
|
|
.channel_get = bmi08x_channel_get,
|
|
};
|
|
|
|
int bmi08x_gyro_init(const struct device *dev)
|
|
{
|
|
const struct bmi08x_gyro_config *config = dev->config;
|
|
uint8_t val = 0U;
|
|
int ret;
|
|
|
|
ret = bmi08x_bus_check(dev);
|
|
if (ret < 0) {
|
|
LOG_ERR("Bus not ready for '%s'", dev->name);
|
|
return ret;
|
|
}
|
|
|
|
/* reboot the chip */
|
|
ret = bmi08x_gyro_byte_write(dev, BMI08X_REG_GYRO_SOFTRESET, BMI08X_SOFT_RESET_CMD);
|
|
if (ret < 0) {
|
|
LOG_ERR("Cannot reboot chip.");
|
|
return ret;
|
|
}
|
|
|
|
k_msleep(BMI08X_GYRO_SOFTRESET_DELAY);
|
|
|
|
ret = bmi08x_gyro_byte_read(dev, BMI08X_REG_GYRO_CHIP_ID, &val);
|
|
if (ret < 0) {
|
|
LOG_ERR("Failed to read chip id.");
|
|
return ret;
|
|
}
|
|
|
|
if (val != BMI08X_GYRO_CHIP_ID) {
|
|
LOG_ERR("Unsupported chip detected (0x%02x)!", val);
|
|
return -ENODEV;
|
|
}
|
|
|
|
/* set gyro default range */
|
|
ret = bmi08x_gyr_range_set(dev, config->gyro_fs);
|
|
if (ret < 0) {
|
|
LOG_ERR("Cannot set default range for gyroscope.");
|
|
return ret;
|
|
}
|
|
|
|
/* set gyro default bandwidth */
|
|
ret = bmi08x_gyro_byte_write(dev, BMI08X_REG_GYRO_BANDWIDTH, config->gyro_hz);
|
|
if (ret < 0) {
|
|
LOG_ERR("Failed to set gyro's default ODR.");
|
|
return ret;
|
|
}
|
|
|
|
#ifdef CONFIG_BMI08X_GYRO_TRIGGER
|
|
ret = bmi08x_gyr_trigger_mode_init(dev);
|
|
if (ret < 0) {
|
|
LOG_ERR("Cannot set up trigger mode.");
|
|
return ret;
|
|
}
|
|
#endif
|
|
/* with BMI08X_DATA_SYNC set, it is expected that the INT3 or INT4 is wired to either INT1
|
|
* or INT2
|
|
*/
|
|
#if defined(CONFIG_BMI08X_GYRO_TRIGGER) || BMI08X_GYRO_ANY_INST_HAS_DATA_SYNC
|
|
/* set gyro ints */
|
|
/* set ints */
|
|
ret = bmi08x_gyro_byte_write(dev, BMI08X_REG_GYRO_INT_CTRL, 0x80);
|
|
if (ret < 0) {
|
|
LOG_ERR("Failed to map interrupts.");
|
|
return ret;
|
|
}
|
|
ret = bmi08x_gyro_byte_write(dev, BMI08X_REG_GYRO_INT3_INT4_IO_CONF,
|
|
config->int3_4_conf_io);
|
|
if (ret < 0) {
|
|
LOG_ERR("Failed to map interrupts.");
|
|
return ret;
|
|
}
|
|
ret = bmi08x_gyro_byte_write(dev, BMI08X_REG_GYRO_INT3_INT4_IO_MAP, config->int3_4_map);
|
|
if (ret < 0) {
|
|
LOG_ERR("Failed to map interrupts.");
|
|
return ret;
|
|
}
|
|
#endif
|
|
|
|
return ret;
|
|
}
|
|
|
|
#define BMI08X_CONFIG_SPI(inst) \
|
|
.bus.spi = SPI_DT_SPEC_INST_GET( \
|
|
inst, SPI_OP_MODE_MASTER | SPI_TRANSFER_MSB | SPI_WORD_SET(8), 2),
|
|
|
|
#define BMI08X_CONFIG_I2C(inst) .bus.i2c = I2C_DT_SPEC_INST_GET(inst),
|
|
|
|
#define BMI08X_GYRO_TRIG(inst) \
|
|
.int3_4_map = DT_INST_PROP(inst, int3_4_map_io), \
|
|
.int3_4_conf_io = DT_INST_PROP(inst, int3_4_conf_io),
|
|
|
|
#if BMI08X_GYRO_ANY_INST_HAS_DATA_SYNC
|
|
/* the bmi08x-gyro should not have trigger mode with data-sync enabled */
|
|
BUILD_ASSERT(CONFIG_BMI08X_GYRO_TRIGGER_NONE,
|
|
"Only none trigger type allowed for bmi08x-gyro with data-sync enabled");
|
|
/* with data-sync, one of the int pins should be wired directory to the accel's int pins, their
|
|
* config should be defined
|
|
*/
|
|
#define BMI08X_GYRO_TRIGGER_PINS(inst) BMI08X_GYRO_TRIG(inst)
|
|
#else
|
|
#define BMI08X_GYRO_TRIGGER_PINS(inst) \
|
|
IF_ENABLED(CONFIG_BMI08X_GYRO_TRIGGER, (BMI08X_GYRO_TRIG(inst)))
|
|
#endif
|
|
|
|
#define BMI08X_CREATE_INST(inst) \
|
|
\
|
|
static struct bmi08x_gyro_data bmi08x_drv_##inst; \
|
|
\
|
|
static const struct bmi08x_gyro_config bmi08x_config_##inst = { \
|
|
COND_CODE_1(DT_INST_ON_BUS(inst, spi), (BMI08X_CONFIG_SPI(inst)), \
|
|
(BMI08X_CONFIG_I2C(inst))) \
|
|
.api = COND_CODE_1(DT_INST_ON_BUS(inst, spi), (&bmi08x_spi_api), \
|
|
(&bmi08x_i2c_api)), \
|
|
IF_ENABLED(CONFIG_BMI08X_GYRO_TRIGGER, \
|
|
(.int_gpio = GPIO_DT_SPEC_INST_GET(inst, int_gpios),)) \
|
|
.gyro_hz = DT_INST_ENUM_IDX(inst, gyro_hz), \
|
|
BMI08X_GYRO_TRIGGER_PINS(inst).gyro_fs = DT_INST_PROP(inst, gyro_fs), \
|
|
}; \
|
|
\
|
|
PM_DEVICE_DT_INST_DEFINE(inst, bmi08x_gyro_pm_action); \
|
|
SENSOR_DEVICE_DT_INST_DEFINE(inst, bmi08x_gyro_init, PM_DEVICE_DT_INST_GET(inst), \
|
|
&bmi08x_drv_##inst, &bmi08x_config_##inst, POST_KERNEL, \
|
|
CONFIG_SENSOR_INIT_PRIORITY, &bmi08x_api);
|
|
|
|
/* Create the struct device for every status "okay" node in the devicetree. */
|
|
DT_INST_FOREACH_STATUS_OKAY(BMI08X_CREATE_INST)
|