From 9dfb7d2dbe5e3d4e9c591c9783ffd9ee2c3b4716 Mon Sep 17 00:00:00 2001 From: jerryslhao Date: Thu, 9 Jan 2025 15:37:14 +0800 Subject: [PATCH] drivers/sensors/bmi088: add driver for Bosch BMI088 IMU This adds a driver for the Bosch BMI088 IMU sensor. The driver supports spi. Signed-off-by: jerryslhao --- drivers/sensors/CMakeLists.txt | 10 + drivers/sensors/Kconfig | 70 +++ drivers/sensors/Make.defs | 9 + drivers/sensors/bmi088.c | 442 ++++++++++++++++ drivers/sensors/bmi088_base.c | 426 +++++++++++++++ drivers/sensors/bmi088_base.h | 261 ++++++++++ drivers/sensors/bmi088_uorb.c | 923 +++++++++++++++++++++++++++++++++ include/nuttx/sensors/bmi088.h | 173 ++++++ 8 files changed, 2314 insertions(+) create mode 100644 drivers/sensors/bmi088.c create mode 100644 drivers/sensors/bmi088_base.c create mode 100644 drivers/sensors/bmi088_base.h create mode 100644 drivers/sensors/bmi088_uorb.c create mode 100644 include/nuttx/sensors/bmi088.h diff --git a/drivers/sensors/CMakeLists.txt b/drivers/sensors/CMakeLists.txt index 0bb8ed7954..e6bf460ec3 100644 --- a/drivers/sensors/CMakeLists.txt +++ b/drivers/sensors/CMakeLists.txt @@ -370,6 +370,16 @@ if(CONFIG_SENSORS) list(APPEND SRCS as5048a.c) endif() + if(CONFIG_SENSORS_BMI088) + list(APPEND SRCS bmi088_base.c) + if(CONFIG_SENSORS_BMI088_UORB) + list(APPEND SRCS bmi088_uorb.c) + else() + list(APPEND SRCS bmi088.c) + endif() + + endif() + endif() # CONFIG_SPI # These drivers depend on 1WIRE support diff --git a/drivers/sensors/Kconfig b/drivers/sensors/Kconfig index b2a0fbae78..b43ba06f61 100644 --- a/drivers/sensors/Kconfig +++ b/drivers/sensors/Kconfig @@ -260,6 +260,76 @@ endchoice # I2C Address endif # SENSORS_BMI160 +config SENSORS_BMI088 + bool "Bosch BMI088 Gyroscope Sensor support" + default n + select SPI + ---help--- + Enable driver support for the Bosch BMG088 gyroscope sensor. + +if SENSORS_BMI088 + +config SENSORS_BMI088_UORB + bool "BMI088 UORB Interface" + default n + ---help--- + Enables Work with the UORB or Character Device interface. + If not set, the Character Device is used by default. + +choice + prompt "BMI088 Interface" + default SENSORS_BMI088_SPI + +config SENSORS_BMI088_I2C + bool "BMI088 I2C Interface" + select I2C + ---help--- + Enables support for the I2C interface + +config SENSORS_BMI088_SPI + bool "BMI088 SPI Interface" + select SPI + ---help--- + Enables support for the SPI interface + +endchoice # BMI088 Interface + +choice + prompt "I2C Address" + default BMI160_I2C_ACC_ADDR_18 + +config BMI160_I2C_ACC_ADDR_18 + bool "0x18" + ---help--- + Default address. + If SDO pin is pulled to GND, use 0x18 + +config BMI160_I2C_ACC_ADDR_19 + bool "0x19" + ---help--- + If SDO pin is pulled to VDDIO, use 0x19 + +endchoice # I2C Address + +choice + prompt "I2C Address" + default BMI088_I2C_GYRO_ADDR_68 + +config BMI088_I2C_GYRO_ADDR_68 + bool "0x68" + ---help--- + Default address. + If SDO pin is pulled to GND, use 0x68 + +config BMI088_I2C_GYRO_ADDR_69 + bool "0x69" + ---help--- + If SDO pin is pulled to VDDIO, use 0x69 + +endchoice # I2C Address + +endif # SENSORS_BMI088 + config SENSORS_BMI270 bool "Bosch BMI270 Inertial Measurement Sensor support" default n diff --git a/drivers/sensors/Make.defs b/drivers/sensors/Make.defs index 8f317d49d8..f20424b4c5 100644 --- a/drivers/sensors/Make.defs +++ b/drivers/sensors/Make.defs @@ -174,6 +174,15 @@ else endif endif +ifeq ($(CONFIG_SENSORS_BMI088),y) + CSRCS += bmi088_base.c +ifeq ($(CONFIG_SENSORS_BMI088_UORB),y) + CSRCS += bmi088_uorb.c +else + CSRCS += bmi088.c +endif +endif + ifeq ($(CONFIG_SENSORS_BMP180),y) CSRCS += bmp180_base.c ifeq ($(CONFIG_SENSORS_BMP180_UORB),y) diff --git a/drivers/sensors/bmi088.c b/drivers/sensors/bmi088.c new file mode 100644 index 0000000000..0247290658 --- /dev/null +++ b/drivers/sensors/bmi088.c @@ -0,0 +1,442 @@ +/**************************************************************************** + * drivers/sensors/bmi088.c + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include "bmi088_base.h" + +#if defined(CONFIG_SENSORS_BMI088) + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +static uint8_t acc_range = BMI088_ACC_RANGE_6G; +static uint8_t gyro_range = BMI088_GYRO_RANGE_2000DPS; +static uint8_t gyro_bandwidth = BMI088_GYRO_BANDWIDTH_2000HZ_532HZ; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/* Character driver methods */ + +static int bmi088_acc_open(FAR struct file *filep); +static int bmi088_gyro_open(FAR struct file *filep); +static int bmi088_acc_close(FAR struct file *filep); +static int bmi088_gyro_close(FAR struct file *filep); +static ssize_t bmi088_acc_read(FAR struct file *filep, FAR char *buffer, + size_t len); +static ssize_t bmi088_gyro_read(FAR struct file *filep, FAR char *buffer, + size_t len); +static int bmi088_acc_ioctl(FAR struct file *filep, int cmd, + unsigned long arg); +static int bmi088_gyro_ioctl(FAR struct file *filep, int cmd, + unsigned long arg); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* This the vtable that supports the character driver interface */ + +static const struct file_operations g_bmi088_acc_fops = +{ + bmi088_acc_open, /* open */ + bmi088_acc_close, /* close */ + bmi088_acc_read, /* read */ + NULL, /* write */ + NULL, /* seek */ + bmi088_acc_ioctl, /* ioctl */ +}; + +/* This the vtable that supports the character driver interface */ + +static const struct file_operations g_bmi088_gyro_fops = +{ + bmi088_gyro_open, /* open */ + bmi088_gyro_close, /* close */ + bmi088_gyro_read, /* read */ + NULL, /* write */ + NULL, /* seek */ + bmi088_gyro_ioctl, /* ioctl */ +}; + +/**************************************************************************** + * Name: bmi088_acc_open + * + * Description: + * Standard character driver open method. + * + ****************************************************************************/ + +static int bmi088_acc_open(FAR struct file *filep) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct bmi088_dev_s *priv = inode->i_private; + + up_mdelay(1); + bmi088_put_acc_reg8(priv, BMI088_ACC_PWR_CTRL , + BMI088_ACC_PWR_CTRL_ACC_ENABLE); + up_mdelay(5); + bmi088_put_acc_reg8(priv, BMI088_ACC_RANGE , acc_range); + up_mdelay(50); + bmi088_put_acc_reg8(priv, BMI088_ACC_PWR_CONF , + BMI088_ACC_PWR_CONF_ACTIVE_MODE); + + bmi088_put_acc_reg8(priv, BMI088_INT1_IO_CONF , 0x08); + bmi088_put_acc_reg8(priv, BMI088_INT1_INT2_MAP_DATA, 0x04); + + return OK; +} + +/**************************************************************************** + * Name: bmi088_gyro_open + * + * Description: + * Standard character driver open method. + * + ****************************************************************************/ + +static int bmi088_gyro_open(FAR struct file *filep) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct bmi088_dev_s *priv = inode->i_private; + + /* emable and config acc */ + + bmi088_put_gyro_reg8(priv, BMI088_GYRO_LPM1 , BMI088_GYRO_PM_NORMAL); + bmi088_put_gyro_reg8(priv, BMI088_GYRO_RANGE , gyro_range); + bmi088_put_gyro_reg8(priv, BMI088_GYRO_BANDWIDTH, gyro_bandwidth); + + bmi088_put_gyro_reg8(priv, BMI088_INT3_INT4_IO_CONF, 0x00); + bmi088_put_gyro_reg8(priv, BMI088_INT3_INT4_IO_MAP , 0x01); + bmi088_put_gyro_reg8(priv, BMI088_GYRO_INT_CTRL , 0x80); + + return OK; +} + +/**************************************************************************** + * Name: bmi088_acc_close + * + * Description: + * Standard character driver close method. + * + ****************************************************************************/ + +static int bmi088_acc_close(FAR struct file *filep) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct bmi088_dev_s *priv = inode->i_private; + + /* Set suspend mode to each sensors. */ + + bmi088_put_acc_reg8(priv, BMI088_ACC_INT_STAT_1, 0x00); + + bmi088_put_acc_reg8(priv, BMI088_ACC_PWR_CONF , + BMI088_ACC_PWR_CONF_SUSPEND_MODE); + up_mdelay(5); + bmi088_put_acc_reg8(priv, BMI088_ACC_PWR_CTRL , + BMI088_ACC_PWR_CTRL_ACC_DISABLE); + up_mdelay(5); + + return OK; +} + +/**************************************************************************** + * Name: bmi088_gyro_close + * + * Description: + * Standard character driver close method. + * + ****************************************************************************/ + +static int bmi088_gyro_close(FAR struct file *filep) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct bmi088_dev_s *priv = inode->i_private; + + /* Set suspend mode to each sensors. */ + + bmi088_put_gyro_reg8(priv, BMI088_GYRO_LPM1, BMI088_GYRO_PM_SUSPEND); + up_mdelay(30); + + return OK; +} + +/**************************************************************************** + * Name: bmi088_acc_read + * + * Description: + * Standard character driver read method. + * + ****************************************************************************/ + +static ssize_t bmi088_acc_read(FAR struct file *filep, FAR char *buffer, + size_t len) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct bmi088_dev_s *priv = inode->i_private; + FAR struct acc_gyro_st_s *p = (FAR struct acc_gyro_st_s *)buffer; + + if (len < sizeof(struct acc_gyro_st_s)) + { + snerr("Expected buffer size is %zu\n", sizeof(struct acc_gyro_st_s)); + return 0; + } + + /* read and caculate acc */ + + bmi088_get_acc_regs(priv, BMI088_ACC_X_LSB, (uint8_t *)&p->acc_source, 6); + p->accel.x = p->acc_source.x / 32768.0 * ((1 << (acc_range)) * 3.0); + p->accel.y = p->acc_source.y / 32768.0 * ((1 << (acc_range)) * 3.0); + p->accel.z = p->acc_source.z / 32768.0 * ((1 << (acc_range)) * 3.0); + + return len; +} + +/**************************************************************************** + * Name: bmi088_acc_read + * + * Description: + * Standard character driver read method. + * + ****************************************************************************/ + +static ssize_t bmi088_gyro_read(FAR struct file *filep, FAR char *buffer, + size_t len) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct bmi088_dev_s *priv = inode->i_private; + FAR struct acc_gyro_st_s *p = (FAR struct acc_gyro_st_s *)buffer; + + if (len < sizeof(struct gyro_t)) + { + snerr("Expected buffer size is %zu\n", sizeof(struct acc_gyro_st_s)); + return 0; + } + + bmi088_get_gyro_regs(priv, BMI088_GYRO_X_LSB, + (uint8_t *)&p->gyro_source, 6); + p->gyro.x = p->gyro_source.x / 32768.0 * (2000.0 / (1 << (gyro_range))); + p->gyro.y = p->gyro_source.y / 32768.0 * (2000.0 / (1 << (gyro_range))); + p->gyro.z = p->gyro_source.z / 32768.0 * (2000.0 / (1 << (gyro_range))); + + return len; +} + +/**************************************************************************** + * Name: bmi088_acc_ioctl + * + * Description: + * Standard character driver ioctl method. + * + ****************************************************************************/ + +static int bmi088_acc_ioctl(FAR struct file *filep, int cmd, + unsigned long arg) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct bmi088_dev_s *priv = inode->i_private; + int ret = OK; + + switch (cmd) + { + case SNIOC_ACC_ENABLE: + break; + case SNIOC_ACC_DISABLE: + break; + default: + snerr("Unrecognized cmd: %d\n", cmd); + ret = -ENOTTY; + break; + } + + return ret; +} + +/**************************************************************************** + * Name: bmi088_gyro_ioctl + * + * Description: + * Standard character driver ioctl method. + * + ****************************************************************************/ + +static int bmi088_gyro_ioctl(FAR struct file *filep, int cmd, + unsigned long arg) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct bmi088_dev_s *priv = inode->i_private; + int ret = OK; + + switch (cmd) + { + case SNIOC_GYRO_ENABLE: + break; + case SNIOC_GYRO_DISABLE: + break; + default: + snerr("Unrecognized cmd: %d\n", cmd); + ret = -ENOTTY; + break; + } + + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: bmi088_acc_register + * + * Description: + * Register the BMI088 character device as 'devpath' + * + * Input Parameters: + * devpath - The full path to the driver to register. E.g., "/dev/press0" + * dev - An instance of the SPI interface to use to communicate with + * BMI088 + * + * Returned Value: + * Zero (OK) on success; a negated errno value on failure. + * + ****************************************************************************/ + +#ifdef CONFIG_SENSORS_BMI088_I2C +int bmi088_acc_register(FAR const char *devpath, + FAR struct i2c_master_s *dev) +#else /* CONFIG_SENSORS_BMI088_SPI */ +int bmi088_acc_register(FAR const char *devpath, FAR struct spi_dev_s *dev) +#endif +{ + FAR struct bmi088_dev_s *priv; + int ret; + + priv = (FAR struct bmi088_dev_s *)kmm_malloc(sizeof(struct bmi088_dev_s)); + if (!priv) + { + snerr("Failed to allocate instance\n"); + return -ENOMEM; + } + +#ifdef CONFIG_SENSORS_BMI088_I2C + priv->i2c = dev; + priv->addr = BMI088_I2C_ACC_ADDR; + priv->freq = BMI088_I2C_FREQ; + +#else /* CONFIG_SENSORS_BMI088_SPI */ + priv->spi = dev; +#endif + + ret = bmi088_get_acc_reg8(priv, BMI088_ACC_CHIP_ID); + if (ret != 0x1e) + { + snerr("Wrong Device ID!\n"); + kmm_free(priv); + return ret; + } + + ret = register_driver(devpath, &g_bmi088_acc_fops, 0666, priv); + if (ret < 0) + { + snerr("Failed to register driver: %d\n", ret); + kmm_free(priv); + } + + sninfo("BMI088 driver loaded successfully!\n"); + return OK; +} + +/**************************************************************************** + * Name: bmi088_gyroregister + * + * Description: + * Register the BMI088 character device as 'devpath' + * + * Input Parameters: + * devpath - The full path to the driver to register. E.g., "/dev/press0" + * dev - An instance of the SPI interface to use to communicate with + * BMI088 + * + * Returned Value: + * Zero (OK) on success; a negated errno value on failure. + * + ****************************************************************************/ + +#ifdef CONFIG_SENSORS_BMI088_I2C +int bmi088_gyro_register(FAR const char *devpath, + FAR struct i2c_master_s *dev) +#else /* CONFIG_SENSORS_BMI088_SPI */ +int bmi088_gyro_register(FAR const char *devpath, FAR struct spi_dev_s *dev) +#endif +{ + FAR struct bmi088_dev_s *priv; + int ret; + + priv = (FAR struct bmi088_dev_s *)kmm_malloc(sizeof(struct bmi088_dev_s)); + if (!priv) + { + snerr("Failed to allocate instance\n"); + return -ENOMEM; + } + +#ifdef CONFIG_SENSORS_BMI088_I2C + priv->i2c = dev; + priv->addr = BMI088_I2C_GY_ADDR; + priv->freq = BMI088_I2C_FREQ; + +#else /* CONFIG_SENSORS_BMI088_SPI */ + priv->spi = dev; + +#endif + + ret = bmi088_get_gyro_reg8(priv, BMI088_GYRO_CHIP_ID); + if (ret != 0x0f) + { + snerr("Wrong Device ID!\n"); + kmm_free(priv); + return ret; + } + + ret = register_driver(devpath, &g_bmi088_gyro_fops, 0666, priv); + if (ret < 0) + { + snerr("Failed to register driver: %d\n", ret); + kmm_free(priv); + } + + sninfo("BMI088 driver loaded successfully!\n"); + return OK; +} + +#endif /* CONFIG_SENSORS_BMI088 */ diff --git a/drivers/sensors/bmi088_base.c b/drivers/sensors/bmi088_base.c new file mode 100644 index 0000000000..849e591e55 --- /dev/null +++ b/drivers/sensors/bmi088_base.c @@ -0,0 +1,426 @@ +/**************************************************************************** + * drivers/sensors/bmi088_base.c + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "bmi088_base.h" + +#if defined(CONFIG_SENSORS_BMI088) + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: bmi088_configspi + * + * Description: + * + ****************************************************************************/ + +#ifdef CONFIG_SENSORS_BMI088_SPI +static void bmi088_configspi(FAR struct spi_dev_s *spi) +{ + /* Configure SPI for the BMI088 */ + + SPI_SETMODE(spi, SPIDEV_MODE0); + SPI_SETBITS(spi, 8); + SPI_HWFEATURES(spi, 0); + SPI_SETFREQUENCY(spi, BMI088_SPI_MAXFREQUENCY); +} +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: bmi088_get_acc_reg8 + * + * Description: + * Read from an 8-bit BMI088 ACC register + * + ****************************************************************************/ + +uint8_t bmi088_get_acc_reg8(FAR struct bmi088_dev_s *priv, uint8_t regaddr) +{ + uint8_t regval = 0; + +#ifdef CONFIG_SENSORS_BMI088_I2C + struct i2c_msg_s msg[2]; + int ret; + + msg[0].frequency = priv->freq; + msg[0].addr = priv->addr; + msg[0].flags = I2C_M_NOSTOP; + msg[0].buffer = ®addr; + msg[0].length = 1; + + msg[1].frequency = priv->freq; + msg[1].addr = priv->addr; + msg[1].flags = I2C_M_READ; + msg[1].buffer = ®val; + msg[1].length = 1; + + ret = I2C_TRANSFER(priv->i2c, msg, 2); + if (ret < 0) + { + snerr("I2C_TRANSFER failed: %d\n", ret); + } + +#else /* CONFIG_SENSORS_BMI088_SPI */ + /* If SPI bus is shared then lock and configure it */ + + SPI_LOCK(priv->spi, true); + bmi088_configspi(priv->spi); + + /* Select the BMI088 */ + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(BMI088_SPI_DEV_ACC), true); + + /* Send register to read and get the next byte */ + + SPI_SEND(priv->spi, regaddr | 0x80); + SPI_RECVBLOCK(priv->spi, ®val, 1); + SPI_RECVBLOCK(priv->spi, ®val, 1); + + /* Deselect the BMI088 */ + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(BMI088_SPI_DEV_ACC), false); + + /* Unlock bus */ + + SPI_LOCK(priv->spi, false); +#endif + + return regval; +} + +/**************************************************************************** + * Name: bmi088_get_gyro_reg8 + * + * Description: + * Read from an 8-bit BMI088 GYRO register + * + ****************************************************************************/ + +uint8_t bmi088_get_gyro_reg8(FAR struct bmi088_dev_s *priv, uint8_t regaddr) +{ + uint8_t regval = 0; + +#ifdef CONFIG_SENSORS_BMI088_I2C + struct i2c_msg_s msg[2]; + int ret; + + msg[0].frequency = priv->freq; + msg[0].addr = priv->addr; + msg[0].flags = I2C_M_NOSTOP; + msg[0].buffer = ®addr; + msg[0].length = 1; + + msg[1].frequency = priv->freq; + msg[1].addr = priv->addr; + msg[1].flags = I2C_M_READ; + msg[1].buffer = ®val; + msg[1].length = 1; + + ret = I2C_TRANSFER(priv->i2c, msg, 2); + if (ret < 0) + { + snerr("I2C_TRANSFER failed: %d\n", ret); + } + +#else /* CONFIG_SENSORS_BMI088_SPI */ + /* If SPI bus is shared then lock and configure it */ + + SPI_LOCK(priv->spi, true); + bmi088_configspi(priv->spi); + + /* Select the BMI088 */ + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(BMI088_SPI_DEV_GYRO), true); + + /* Send register to read and get the next byte */ + + SPI_SEND(priv->spi, regaddr | 0x80); + SPI_RECVBLOCK(priv->spi, ®val, 1); + + /* Deselect the BMI088 */ + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(BMI088_SPI_DEV_GYRO), false); + + /* Unlock bus */ + + SPI_LOCK(priv->spi, false); +#endif + + return regval; +} + +/**************************************************************************** + * Name: bmi088_put_acc_reg8 + * + * Description: + * Write a value to an 8-bit BMI088 ACC register + * + ****************************************************************************/ + +void bmi088_put_acc_reg8(FAR struct bmi088_dev_s *priv, uint8_t regaddr, + uint8_t regval) +{ +#ifdef CONFIG_SENSORS_BMI088_I2C + struct i2c_msg_s msg[2]; + int ret; + uint8_t txbuffer[2]; + + txbuffer[0] = regaddr; + txbuffer[1] = regval; + + msg[0].frequency = priv->freq; + msg[0].addr = priv->addr; + msg[0].flags = 0; + msg[0].buffer = txbuffer; + msg[0].length = 2; + + ret = I2C_TRANSFER(priv->i2c, msg, 1); + if (ret < 0) + { + snerr("I2C_TRANSFER failed: %d\n", ret); + } + +#else /* CONFIG_SENSORS_BMI088_SPI */ + /* If SPI bus is shared then lock and configure it */ + + SPI_LOCK(priv->spi, true); + bmi088_configspi(priv->spi); + + /* Select the BMI088 */ + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(BMI088_SPI_DEV_ACC), true); + + /* Send register address and set the value */ + + SPI_SEND(priv->spi, regaddr); + SPI_SEND(priv->spi, regval); + + /* Deselect the BMI088 */ + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(BMI088_SPI_DEV_ACC), false); + + /* Unlock bus */ + + SPI_LOCK(priv->spi, false); + +#endif +} + +/**************************************************************************** + * Name: bmi088_put_gyro_reg8 + * + * Description: + * Write a value to an 8-bit BMI088 GYRO register + * + ****************************************************************************/ + +void bmi088_put_gyro_reg8(FAR struct bmi088_dev_s *priv, uint8_t regaddr, + uint8_t regval) +{ +#ifdef CONFIG_SENSORS_BMI088_I2C + struct i2c_msg_s msg[2]; + int ret; + uint8_t txbuffer[2]; + + txbuffer[0] = regaddr; + txbuffer[1] = regval; + + msg[0].frequency = priv->freq; + msg[0].addr = priv->addr; + msg[0].flags = 0; + msg[0].buffer = txbuffer; + msg[0].length = 2; + + ret = I2C_TRANSFER(priv->i2c, msg, 1); + if (ret < 0) + { + snerr("I2C_TRANSFER failed: %d\n", ret); + } + +#else /* CONFIG_SENSORS_BMI088_SPI */ + /* If SPI bus is shared then lock and configure it */ + + SPI_LOCK(priv->spi, true); + bmi088_configspi(priv->spi); + + /* Select the BMI088 */ + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(BMI088_SPI_DEV_GYRO), true); + + /* Send register address and set the value */ + + SPI_SEND(priv->spi, regaddr); + SPI_SEND(priv->spi, regval); + + /* Deselect the BMI088 */ + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(BMI088_SPI_DEV_GYRO), false); + + /* Unlock bus */ + + SPI_LOCK(priv->spi, false); + +#endif +} + +/**************************************************************************** + * Name: bmi088_get_acc_regs + * + * Description: + * Read cnt bytes from specified dev_addr and reg_addr + * + ****************************************************************************/ + +void bmi088_get_acc_regs(FAR struct bmi088_dev_s *priv, uint8_t regaddr, + uint8_t *regval, int len) +{ +#ifdef CONFIG_SENSORS_BMI088_I2C + struct i2c_msg_s msg[2]; + int ret; + + msg[0].frequency = priv->freq; + msg[0].addr = priv->addr; + msg[0].flags = I2C_M_NOSTOP; + msg[0].buffer = ®addr; + msg[0].length = 1; + + msg[1].frequency = priv->freq; + msg[1].addr = priv->addr; + msg[1].flags = I2C_M_READ; + msg[1].buffer = regval; + msg[1].length = len; + + ret = I2C_TRANSFER(priv->i2c, msg, 2); + if (ret < 0) + { + snerr("I2C_TRANSFER failed: %d\n", ret); + } + +#else /* CONFIG_SENSORS_BMI088_SPI */ + /* If SPI bus is shared then lock and configure it */ + + SPI_LOCK(priv->spi, true); + bmi088_configspi(priv->spi); + + /* Select the BMI088 */ + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(BMI088_SPI_DEV_ACC), true); + + /* Send register to read and get the next 2 bytes */ + + SPI_SEND(priv->spi, regaddr | 0x80); + SPI_RECVBLOCK(priv->spi, regval, 1); + SPI_RECVBLOCK(priv->spi, regval, len); + + /* Deselect the BMI088 */ + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(BMI088_SPI_DEV_ACC), false); + + /* Unlock bus */ + + SPI_LOCK(priv->spi, false); + +#endif +} + +/**************************************************************************** + * Name: bmi088_get_gyro_regs + * + * Description: + * Read cnt bytes from specified dev_addr and reg_addr + * + ****************************************************************************/ + +void bmi088_get_gyro_regs(FAR struct bmi088_dev_s *priv, uint8_t regaddr, + uint8_t *regval, int len) +{ +#ifdef CONFIG_SENSORS_BMI088_I2C + struct i2c_msg_s msg[2]; + int ret; + + msg[0].frequency = priv->freq; + msg[0].addr = priv->addr; + msg[0].flags = I2C_M_NOSTOP; + msg[0].buffer = ®addr; + msg[0].length = 1; + + msg[1].frequency = priv->freq; + msg[1].addr = priv->addr; + msg[1].flags = I2C_M_READ; + msg[1].buffer = regval; + msg[1].length = len; + + ret = I2C_TRANSFER(priv->i2c, msg, 2); + if (ret < 0) + { + snerr("I2C_TRANSFER failed: %d\n", ret); + } + +#else /* CONFIG_SENSORS_BMI088_SPI */ + /* If SPI bus is shared then lock and configure it */ + + SPI_LOCK(priv->spi, true); + bmi088_configspi(priv->spi); + + /* Select the BMI088 */ + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(BMI088_SPI_DEV_GYRO), true); + + /* Send register to read and get the next 2 bytes */ + + SPI_SEND(priv->spi, regaddr | 0x80); + SPI_RECVBLOCK(priv->spi, regval, len); + + /* Deselect the BMI088 */ + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(BMI088_SPI_DEV_GYRO), false); + + /* Unlock bus */ + + SPI_LOCK(priv->spi, false); + +#endif +} + +#endif /* CONFIG_SENSORS_BMI088 */ diff --git a/drivers/sensors/bmi088_base.h b/drivers/sensors/bmi088_base.h new file mode 100644 index 0000000000..8e3c4f0f59 --- /dev/null +++ b/drivers/sensors/bmi088_base.h @@ -0,0 +1,261 @@ +/**************************************************************************** + * drivers/sensors/bmi088_base.h + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __INCLUDE_NUTTX_SENSORS_BMI088_COMMOM_H +#define __INCLUDE_NUTTX_SENSORS_BMI088_COMMOM_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define DEVID_ACC 0x1E +#define DEVID_GYRO 0x0F + +/* I2C ACC Address + * + * NOTE: If SDO1 pin is pulled to VDDIO, use 0x19 + */ +#ifdef CONFIG_BMI088_I2C_ACC_ADDR_18 +#define BMI088_I2C_ACC_ADDR 0x18 +#else +#define BMI088_I2C_ACC_ADDR 0x19 +#endif + +/* I2C GY Address + * + * NOTE: If SDO2 pin is pulled to VDDIO, use 0x69 + */ + +#ifdef CONFIG_BMI088_I2C_GY_ADDR_68 +#define BMI088_I2C_GY_ADDR 0x68 +#else +#define BMI088_I2C_GY_ADDR 0x69 +#endif + +#define BMI088_I2C_FREQ 400000 + +#define BMI088_SPI_DEV_ACC 0x00 +#define BMI088_SPI_DEV_GYRO 0x01 + +/* Accelerometer Registers */ + +#define BMI088_ACC_CHIP_ID 0x00 +#define BMI088_ACC_ERR_REG 0x02 +#define BMI088_ACC_STATUS 0x03 +#define BMI088_ACC_X_LSB 0x12 +#define BMI088_ACC_X_MSB 0x13 +#define BMI088_ACC_Y_LSB 0x14 +#define BMI088_ACC_Y_MSB 0x15 +#define BMI088_ACC_Z_LSB 0x16 +#define BMI088_ACC_Z_MSB 0x17 +#define BMI088_SENSORTIME_0 0x18 +#define BMI088_SENSORTIME_1 0x19 +#define BMI088_SENSORTIME_2 0x1A +#define BMI088_ACC_INT_STAT_1 0x1D +#define BMI088_TEMP_MSB 0x22 +#define BMI088_TEMP_LSB 0x23 +#define BMI088_ACC_CONF 0x40 +#define BMI088_ACC_RANGE 0x41 +#define BMI088_INT1_IO_CONF 0x53 +#define BMI088_INT2_IO_CONF 0x54 +#define BMI088_INT1_INT2_MAP_DATA 0x58 +#define BMI088_ACC_SELF_TEST 0x6D +#define BMI088_ACC_PWR_CONF 0x7C +#define BMI088_ACC_PWR_CTRL 0x7D +#define BMI088_ACC_SOFTRESET 0x7E + +/* Gyroscope Registers */ + +#define BMI088_GYRO_CHIP_ID 0x00 +#define BMI088_GYRO_X_LSB 0x02 +#define BMI088_GYRO_X_MSB 0x03 +#define BMI088_GYRO_Y_LSB 0x04 +#define BMI088_GYRO_Y_MSB 0x05 +#define BMI088_GYRO_Z_LSB 0x06 +#define BMI088_GYRO_Z_MSB 0x07 +#define BMI088_GYRO_INT_STAT_1 0x0A +#define BMI088_GYRO_RANGE 0x0F +#define BMI088_GYRO_BANDWIDTH 0x10 +#define BMI088_GYRO_LPM1 0x11 +#define BMI088_GYRO_SOFTRESET 0x14 +#define BMI088_GYRO_INT_CTRL 0x15 +#define BMI088_INT3_INT4_IO_CONF 0x16 +#define BMI088_INT3_INT4_IO_MAP 0x18 +#define BMI088_GYRO_SELF_TEST 0x3C + +/* additional constants */ + +#define BMI088_SOFTRESET_CMD 0xB6 +#define BMI088_ACC_ENABLE 0x04 +#define BMI088_GYRO_PM_NORMAL 0x00 +#define BMI088_GYRO_PM_SUSPEND 0x80 +#define BMI088_GYRO_PM_DEEPSUSPEND 0x20 + +/* ACC_ERR_REG (0x02) Accelerometer Error Register */ + +#define BMI088_ACC_ERR_REG_NO_ERROR (0x00) +#define BMI088_ACC_ERR_REG_ERROR_OCCURRED (0x01) + +/* ACC_CONF (0x40) Accelerometer configuration register */ + +#define BMI088_ACC_CONF_BWP_OSR4 (0x00 << 4) +#define BMI088_ACC_CONF_BWP_OSR2 (0x01 << 4) +#define BMI088_ACC_CONF_BWP_NORMAL (0x02 << 4) +#define BMI088_ACC_CONF_ODR_12_5 (0x05 << 0) +#define BMI088_ACC_CONF_ODR_25 (0x06 << 0) +#define BMI088_ACC_CONF_ODR_50 (0x07 << 0) +#define BMI088_ACC_CONF_ODR_100 (0x08 << 0) +#define BMI088_ACC_CONF_ODR_200 (0x09 << 0) +#define BMI088_ACC_CONF_ODR_400 (0x0A << 0) +#define BMI088_ACC_CONF_ODR_800 (0x0B << 0) +#define BMI088_ACC_CONF_ODR_1600 (0x0C << 0) + +/* ACC_RANGE (0x41) Accelerometer Range Setting Register */ + +#define BMI088_ACC_RANGE_3G (0x00 << 0) +#define BMI088_ACC_RANGE_6G (0x01 << 0) +#define BMI088_ACC_RANGE_12G (0x02 << 0) +#define BMI088_ACC_RANGE_24G (0x03 << 0) + +/* INT1_IO_CONF (0x53) Configures the input/output pin INT1 */ + +#define BMI088_INT1_IO_CONF_INT1_INPUT (0x01<<4) +#define BMI088_INT1_IO_CONF_INT1_OUTPUT (0x01<<3) +#define BMI088_INT1_IO_CONF_INT1_PP (0x00<<2) +#define BMI088_INT1_IO_CONF_INT1_OD (0x01<<2) +#define BMI088_INT1_IO_CONF_INT1_LVL (0x00<<1) +#define BMI088_INT1_IO_CONF_INT1_HL (0x01<<1) + +/* INT1_IO_CONF (0x54) Configures the input/output pin INT1 */ + +#define BMI088_INT2_IO_CONF_INT2_INPUT (0x01<<4) +#define BMI088_INT2_IO_CONF_INT2_OUTPUT (0x01<<3) +#define BMI088_INT2_IO_CONF_INT2_PP (0x00<<2) +#define BMI088_INT2_IO_CONF_INT2_OD (0x01<<2) +#define BMI088_INT2_IO_CONF_INT2_LVL (0x00<<1) +#define BMI088_INT2_IO_CONF_INT2_HL (0x01<<1) + +/* ACC_SELF_TEST (0x6D) Enables the sensor self-test signal */ + +#define BMI088_ACC_SELF_TEST_OFF (0x00 << 0) +#define BMI088_ACC_SELF_TEST_POSITIVE (0x0D << 0) +#define BMI088_ACC_SELF_TEST_NEGATIVE (0x09 << 0) + +/* ACC_PWR_CONF (0x7C) */ + +#define BMI088_ACC_PWR_CONF_ACTIVE_MODE (0x00 << 0) +#define BMI088_ACC_PWR_CONF_SUSPEND_MODE (0x03 << 0) + +/* ACC_PWR_CTRL (0x7D) Switches accelerometer ON or OFF. */ + +#define BMI088_ACC_PWR_CTRL_ACC_DISABLE (0x00 << 0) +#define BMI088_ACC_PWR_CTRL_ACC_ENABLE (0x04 << 0) + +/* GYRO_RANGE (0x0F) Angular rate range and resolution. */ + +#define BMI088_GYRO_RANGE_2000DPS (0x00 << 0) +#define BMI088_GYRO_RANGE_1000DPS (0x01 << 0) +#define BMI088_GYRO_RANGE_500DPS (0x02 << 0) +#define BMI088_GYRO_RANGE_250DPS (0x03 << 0) +#define BMI088_GYRO_RANGE_125DPS (0x04 << 0) + +/* GYRO_BANDWIDTH (0x10) */ + +#define BMI088_GYRO_BANDWIDTH_2000HZ_532HZ (0x00 << 0) +#define BMI088_GYRO_BANDWIDTH_2000HZ_230HZ (0x01 << 0) +#define BMI088_GYRO_BANDWIDTH_1000HZ_116HZ (0x02 << 0) +#define BMI088_GYRO_BANDWIDTH_400HZ_47HZ (0x03 << 0) +#define BMI088_GYRO_BANDWIDTH_200HZ_23HZ (0x04 << 0) +#define BMI088_GYRO_BANDWIDTH_100HZ_12HZ (0x05 << 0) +#define BMI088_GYRO_BANDWIDTH_200HZ_64HZ (0x06 << 0) +#define BMI088_GYRO_BANDWIDTH_100HZ_32HZ (0x07 << 0) + +/* GYRO_LPM1 (0x11) Selection of the main power modes. */ + +#define BMI088_GYRO_LPM1_NORMAL_MODE (0x00 << 0) +#define BMI088_GYRO_LPM1_SUSPEND_MODE (0x80 << 0) +#define BMI088_GYRO_LPM1_DEEP_SUSPEND_MODE (0x20 << 0) + +/* GYRO_INT_CTRL (0x15) Control the data ready interrupt generation. */ + +#define BMI088_GYRO_INT_CTRL_DATA_RDY_INT_DISABLE (0x00 << 0) +#define BMI088_GYRO_INT_CTRL_DATA_RDY_INT_ENABLE (0x80 << 0) + +/* INT3_INT4_IO_MAP (0x18) */ + +#define BMI088_INT3_INT4_IO_MAP_NONE (0x00 << 0) +#define BMI088_INT3_INT4_IO_MAP_INT3_ONLY (0x01 << 0) +#define BMI088_INT3_INT4_IO_MAP_INT4_ONLY (0x80 << 0) +#define BMI088_INT3_INT4_IO_MAP_INT3_AND_INT4 (0x81 << 0) + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +struct bmi088_dev_s +{ +#ifdef CONFIG_SENSORS_BMI088_I2C + FAR struct i2c_master_s *i2c; /* I2C interface */ + uint8_t addr; /* I2C address */ + int freq; /* Frequency <= 3.4MHz */ + +#else /* CONFIG_SENSORS_BMI088_SPI */ + FAR struct spi_dev_s *spi; /* SPI interface */ + +#endif +}; + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +uint8_t bmi088_get_acc_reg8(FAR struct bmi088_dev_s *priv, uint8_t regaddr); +uint8_t bmi088_get_gyro_reg8(FAR struct bmi088_dev_s *priv, uint8_t regaddr); +void bmi088_put_acc_reg8(FAR struct bmi088_dev_s *priv, uint8_t regaddr, + uint8_t regval); +void bmi088_put_gyro_reg8(FAR struct bmi088_dev_s *priv, uint8_t regaddr, + uint8_t regval); +void bmi088_get_acc_regs(FAR struct bmi088_dev_s *priv, uint8_t regaddr, + uint8_t *regval, int len); +void bmi088_get_gyro_regs(FAR struct bmi088_dev_s *priv, uint8_t regaddr, + uint8_t *regval, int len); + +#endif /* __INCLUDE_NUTTX_SENSORS_BMI088_COMMOM_H */ diff --git a/drivers/sensors/bmi088_uorb.c b/drivers/sensors/bmi088_uorb.c new file mode 100644 index 0000000000..57acb83634 --- /dev/null +++ b/drivers/sensors/bmi088_uorb.c @@ -0,0 +1,923 @@ +/**************************************************************************** + * drivers/sensors/bmi088_uorb.c + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "bmi088_base.h" +#include +#include +#include +#include + +#if defined(CONFIG_SENSORS_BMI088_UORB) + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define BMI088_DEFAULT_INTERVAL 10000 /* Default conversion interval. */ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* Sensor parameter */ + +struct bmi088_para_s +{ + uint8_t regval; /* the data of register */ + uint32_t parameter; +}; + +/* Device struct */ + +struct bmi088_acc_dev_uorb_s +{ + /* sensor_lowerhalf_s must be in the first line. */ + + struct sensor_lowerhalf_s lower; /* Lower half sensor driver. */ + + struct work_s work; /* Interrupt handler worker. */ + uint32_t interval; /* acc acquisition interval. */ + uint32_t range; /* acc range */ + uint32_t bwp; /* acc low pass filter */ + + struct bmi088_dev_s dev; +}; + +struct bmi088_gyro_dev_uorb_s +{ + /* sensor_lowerhalf_s must be in the first line. */ + + struct sensor_lowerhalf_s lower; /* Lower half sensor driver. */ + struct work_s work; /* Interrupt handler worker. */ + uint32_t interval; /* gyro acquisition interval. */ + uint32_t range; /* gyro range */ + + struct bmi088_dev_s dev; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/* Sensor handle functions */ + +static void bmi088_accel_enable(FAR struct bmi088_acc_dev_uorb_s *priv, + bool enable); +static void bmi088_gyro_enable(FAR struct bmi088_gyro_dev_uorb_s *priv, + bool enable); + +/* Sensor ops functions */ + +static int bmi088_set_accel_interval(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR uint32_t *period_us); +static int bmi088_set_gyro_interval(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR uint32_t *period_us); +static int bmi088_accel_activate(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + bool enable); +static int bmi088_gyro_activate(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + bool enable); + +/* Sensor poll functions */ + +static void bmi088_accel_worker(FAR void *arg); +static void bmi088_gyro_worker(FAR void *arg); +static int bmi088_find_parameter(uint32_t parameter, + FAR const struct bmi088_para_s *parameter_s, + int len); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static const struct sensor_ops_s g_bmi088_accel_ops = +{ + .activate = bmi088_accel_activate, /* Enable/disable sensor. */ + .set_interval = bmi088_set_accel_interval, /* Set output data period. */ +}; + +static const struct sensor_ops_s g_bmi088_gyro_ops = +{ + .activate = bmi088_gyro_activate, /* Enable/disable sensor. */ + .set_interval = bmi088_set_gyro_interval, /* Set output data period. */ +}; + +static const struct bmi088_para_s g_bmi088_accel_range[] = +{ + { BMI088_ACC_RANGE_3G, 3}, /* range is 3g. */ + { BMI088_ACC_RANGE_6G, 6}, /* range is 6g. */ + { BMI088_ACC_RANGE_12G, 12}, /* range is 12g. */ + { BMI088_ACC_RANGE_24G, 24}, /* range is 24g. */ +}; + +static const struct bmi088_para_s g_bmi088_acc_bwp[] = +{ + { BMI088_ACC_CONF_BWP_OSR4 , 4}, /* 4-fold oversampling. */ + { BMI088_ACC_CONF_BWP_OSR2 , 2}, /* 2-fold oversampling. */ + { BMI088_ACC_CONF_BWP_NORMAL, 1}, /* 1-fold oversampling. */ +}; + +static const struct bmi088_para_s g_bmi088_accel_odr[] = +{ + { BMI088_ACC_CONF_ODR_12_5, 80000}, /* interval is 80.0ms. */ + { BMI088_ACC_CONF_ODR_25, 40000}, /* interval is 40.0ms. */ + { BMI088_ACC_CONF_ODR_50, 20000}, /* interval is 20.0ms. */ + { BMI088_ACC_CONF_ODR_100, 10000}, /* interval is 10.0ms. */ + { BMI088_ACC_CONF_ODR_200, 5000}, /* interval is 5.0ms. */ + { BMI088_ACC_CONF_ODR_400, 2500}, /* interval is 2.5ms. */ + { BMI088_ACC_CONF_ODR_800, 1250}, /* interval is 1.25ms. */ + { BMI088_ACC_CONF_ODR_1600, 625}, /* interval is 0.625ms. */ +}; + +static const struct bmi088_para_s g_bmi088_gyro_range[] = +{ + { BMI088_GYRO_RANGE_2000DPS, 2000}, /* range is -2000 ~ 2000. */ + { BMI088_GYRO_RANGE_1000DPS, 1000}, /* range is -1000 ~ 1000. */ + { BMI088_GYRO_RANGE_500DPS , 500}, /* range is -500 ~ 500. */ + { BMI088_GYRO_RANGE_250DPS , 250}, /* range is -250 ~ 250. */ + { BMI088_GYRO_RANGE_125DPS , 125}, /* range is -125 ~ 125. */ +}; + +static const struct bmi088_para_s g_bmi088_gyro_odr[] = +{ + { BMI088_GYRO_BANDWIDTH_100HZ_12HZ, 10000}, /* interval is 10ms. */ + { BMI088_GYRO_BANDWIDTH_100HZ_32HZ, 10000}, /* interval is 10ms. */ + { BMI088_GYRO_BANDWIDTH_200HZ_23HZ, 5000}, /* interval is 5ms. */ + { BMI088_GYRO_BANDWIDTH_200HZ_64HZ, 5000}, /* interval is 10ms. */ + { BMI088_GYRO_BANDWIDTH_400HZ_47HZ, 2500}, /* interval is 5ms. */ + { BMI088_GYRO_BANDWIDTH_1000HZ_116HZ, 1000}, /* interval is 2.5ms. */ + { BMI088_GYRO_BANDWIDTH_2000HZ_230HZ, 500}, /* interval is 1ms. */ + { BMI088_GYRO_BANDWIDTH_2000HZ_532HZ, 500}, /* interval is 0.5ms. */ +}; + +/**************************************************************************** + * Name: bmi088_find_parameter + * + * Description: + * Find the parameter that matches best. + * + * Input Parameters: + * parameter - Desired parameter. + * parameter_s - Array of sensor pa.rameter + * len - Array length. + * + * Returned Value: + * Index of the best fit parameter. + * + * Assumptions/Limitations: + * none. + * + ****************************************************************************/ + +static int bmi088_find_parameter(uint32_t parameter, + FAR const struct bmi088_para_s *parameter_s, + int len) +{ + int i; + + for (i = 0; i < len; i++) + { + if (parameter == parameter_s[i].parameter) + { + return i; + } + } + + return i - 1; +} + +/**************************************************************************** + * Name: bmi088_accel_enable + * + * Description: + * Enable or disable sensor device. when enable sensor, sensor will + * work in current mode(if not set, use default mode). when disable + * sensor, it will disable sense path and stop convert. + * + * Input Parameters: + * priv - The instance of lower half sensor driver + * enable - true(enable) and false(disable) + * + * Returned Value: + * Return 0 if the driver was success; A negated errno + * value is returned on any failure. + * + * Assumptions/Limitations: + * none. + * + ****************************************************************************/ + +static void bmi088_accel_enable(FAR struct bmi088_acc_dev_uorb_s *priv, + bool enable) +{ + int idx_odr; + int idx_range; + int idx_bwp; + + if (enable) + { + /* Set accel as normal mode. */ + + up_mdelay(1); + bmi088_put_acc_reg8(&priv->dev, BMI088_ACC_PWR_CTRL, + BMI088_ACC_PWR_CTRL_ACC_ENABLE); + up_mdelay(5); + + idx_odr = bmi088_find_parameter(priv->interval, g_bmi088_accel_odr, + nitems(g_bmi088_accel_odr)); + + idx_bwp = bmi088_find_parameter(priv->bwp, g_bmi088_acc_bwp, + nitems(g_bmi088_acc_bwp)); + + idx_range = bmi088_find_parameter(priv->range, g_bmi088_accel_range, + nitems(g_bmi088_accel_range)); + + bmi088_put_acc_reg8(&priv->dev, BMI088_ACC_CONF, + g_bmi088_acc_bwp[idx_bwp].regval | + g_bmi088_accel_odr[idx_odr].regval); + + bmi088_put_acc_reg8(&priv->dev, BMI088_ACC_RANGE, + g_bmi088_accel_range[idx_range].regval); + + up_mdelay(5); + bmi088_put_acc_reg8(&priv->dev, BMI088_ACC_PWR_CONF , + BMI088_ACC_PWR_CONF_ACTIVE_MODE); + + work_queue(HPWORK, &priv->work, bmi088_accel_worker, + priv, + priv->interval / USEC_PER_TICK); + } + else + { + /* Set suspend mode to sensors. */ + + work_cancel(HPWORK, &priv->work); + bmi088_put_acc_reg8(&priv->dev, BMI088_ACC_PWR_CONF , + BMI088_ACC_PWR_CONF_SUSPEND_MODE); + up_mdelay(5); + bmi088_put_acc_reg8(&priv->dev, BMI088_ACC_PWR_CTRL , + BMI088_ACC_PWR_CTRL_ACC_DISABLE); + up_mdelay(5); + } +} + +/**************************************************************************** + * Name: bmi088_gyro_enable + * + * Description: + * Enable or disable sensor device. when enable sensor, sensor will + * work in current mode(if not set, use default mode). when disable + * sensor, it will disable sense path and stop convert. + * + * Input Parameters: + * priv - The instance of lower half sensor driver + * enable - true(enable) and false(disable) + * + * Returned Value: + * Return 0 if the driver was success; A negated errno + * value is returned on any failure. + * + * Assumptions/Limitations: + * none. + * + ****************************************************************************/ + +static void bmi088_gyro_enable(FAR struct bmi088_gyro_dev_uorb_s *priv, + bool enable) +{ + int idx_odr; + int idx_range; + + if (enable) + { + /* Set gyro as normal mode. */ + + bmi088_put_gyro_reg8(&priv->dev, BMI088_GYRO_LPM1 , + BMI088_GYRO_PM_NORMAL); + + nxsig_usleep(30000); + + idx_odr = bmi088_find_parameter(priv->interval, + g_bmi088_gyro_odr, + nitems(g_bmi088_gyro_odr)); + + idx_range = bmi088_find_parameter(priv->range, + g_bmi088_gyro_range, + nitems(g_bmi088_gyro_range)); + + bmi088_put_gyro_reg8(&priv->dev, BMI088_GYRO_RANGE, + g_bmi088_gyro_odr[idx_range].regval); + + bmi088_put_gyro_reg8(&priv->dev, BMI088_GYRO_BANDWIDTH, + g_bmi088_gyro_range[idx_odr].regval); + + bmi088_put_gyro_reg8(&priv->dev, BMI088_GYRO_LPM1, + BMI088_GYRO_LPM1_NORMAL_MODE); + + work_queue(HPWORK, &priv->work, + bmi088_gyro_worker, + priv, + priv->interval / USEC_PER_TICK); + } + else + { + work_cancel(HPWORK, &priv->work); + + /* Set suspend mode to sensors. */ + + bmi088_put_gyro_reg8(&priv->dev, BMI088_GYRO_LPM1, + BMI088_GYRO_LPM1_SUSPEND_MODE); + } +} + +/**************************************************************************** + * Name: bmi088_set_accel_interval + * + * Description: + * Set the sensor output data period in microseconds for a given sensor. + * If *period_us > max_delay it will be truncated to max_delay and if + * *period_us < min_delay it will be replaced by min_delay. + * + * Input Parameters: + * lower - The instance of lower half sensor driver. + * filep - The pointer of file, represents each user using the sensor. + * period_us - The time between report data, in us. It may by overwrite + * by lower half driver. + * + * Returned Value: + * Return 0 if the driver was success; A negated errno + * value is returned on any failure. + * + * Assumptions/Limitations: + * none. + * + ****************************************************************************/ + +static int bmi088_set_accel_interval(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR uint32_t *period_us) +{ + FAR struct bmi088_acc_dev_uorb_s *priv = \ + (FAR struct bmi088_acc_dev_uorb_s *)lower; + int num; + + /* Sanity check. */ + + if (NULL == priv || NULL == period_us) + { + return -EINVAL; + } + + num = bmi088_find_parameter(*period_us, g_bmi088_accel_odr, + nitems(g_bmi088_accel_odr)); + + bmi088_put_gyro_reg8(&priv->dev, BMI088_ACC_CONF, + BMI088_ACC_CONF_BWP_NORMAL | + g_bmi088_accel_odr[num].regval); + + priv->interval = g_bmi088_accel_odr[num].parameter; + *period_us = priv->interval; + return OK; +} + +/**************************************************************************** + * Name: bmi088_set_gyro_interval + * + * Description: + * Set the sensor output data period in microseconds for a given sensor. + * If *period_us > max_delay it will be truncated to max_delay and if + * *period_us < min_delay it will be replaced by min_delay. + * + * Input Parameters: + * lower - The instance of lower half sensor driver. + * filep - The pointer of file, represents each user using the sensor. + * period_us - The time between report data, in us. It may by overwrite + * by lower half driver. + * + * Returned Value: + * Return 0 if the driver was success; A negated errno + * value is returned on any failure. + * + * Assumptions/Limitations: + * none. + * + ****************************************************************************/ + +static int bmi088_set_gyro_interval(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR uint32_t *period_us) +{ + FAR struct bmi088_gyro_dev_uorb_s *priv = \ + (FAR struct bmi088_gyro_dev_uorb_s *)lower; + int num; + + /* Sanity check. */ + + if (NULL == priv || NULL == period_us) + { + return -EINVAL; + } + + num = bmi088_find_parameter(*period_us, + g_bmi088_gyro_odr, + nitems(g_bmi088_gyro_odr)); + bmi088_put_gyro_reg8(&priv->dev, + BMI088_GYRO_BANDWIDTH, + g_bmi088_gyro_odr[num].regval); + + priv->interval = g_bmi088_gyro_odr[num].parameter; + *period_us = priv->interval; + return OK; +} + +/**************************************************************************** + * Name: bmi088_gyro_activate + * + * Description: + * Enable or disable sensor device. when enable sensor, sensor will + * work in current mode(if not set, use default mode). when disable + * sensor, it will disable sense path and stop convert. + * + * Input Parameters: + * lower - The instance of lower half sensor driver. + * filep - The pointer of file, represents each user using the sensor. + * enable - true(enable) and false(disable). + * + * Returned Value: + * Return 0 if the driver was success; A negated errno + * value is returned on any failure. + * + * Assumptions/Limitations: + * none. + * + ****************************************************************************/ + +static int bmi088_gyro_activate(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + bool enable) +{ + FAR struct bmi088_gyro_dev_uorb_s *priv = \ + (FAR struct bmi088_gyro_dev_uorb_s *)lower; + + bmi088_gyro_enable(priv, enable); + + return OK; +} + +/**************************************************************************** + * Name: bmi088_accel_activate + * + * Description: + * Enable or disable sensor device. when enable sensor, sensor will + * work in current mode(if not set, use default mode). when disable + * sensor, it will disable sense path and stop convert. + * + * Input Parameters: + * lower - The instance of lower half sensor driver. + * filep - The pointer of file, represents each user using the sensor. + * enable - true(enable) and false(disable). + * + * Returned Value: + * Return 0 if the driver was success; A negated errno + * value is returned on any failure. + * + * Assumptions/Limitations: + * none. + * + ****************************************************************************/ + +static int bmi088_accel_activate(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + bool enable) +{ + FAR struct bmi088_acc_dev_uorb_s *priv = \ + (FAR struct bmi088_acc_dev_uorb_s *)lower; + + bmi088_accel_enable(priv, enable); + + return OK; +} + +/* Sensor poll functions */ + +/**************************************************************************** + * Name: bmi088_accel_worker + * + * Description: + * Task the worker with retrieving the latest sensor data. We should not do + * this in a interrupt since it might take too long. Also we cannot lock + * the I2C bus from within an interrupt. + * + * Input Parameters: + * arg - Device struct. + * + * Returned Value: + * none. + * + * Assumptions/Limitations: + * none. + * + ****************************************************************************/ + +static void bmi088_accel_worker(FAR void *arg) +{ + FAR struct bmi088_acc_dev_uorb_s *priv = arg; + struct sensor_accel accel; + struct acc_source_t p; + uint32_t time; + uint8_t temp[2]; + + DEBUGASSERT(priv != NULL); + + work_queue(HPWORK, &priv->work, + bmi088_accel_worker, + priv, + priv->interval / USEC_PER_TICK); + + bmi088_get_acc_regs(&priv->dev, + BMI088_ACC_X_LSB, + (FAR uint8_t *)&p, 6); + + bmi088_get_acc_regs(&priv->dev, + BMI088_SENSORTIME_0, + (FAR uint8_t *)&time, 3); + + bmi088_get_acc_regs(&priv->dev, + BMI088_TEMP_MSB, + temp, 2); + + int idx_range = bmi088_find_parameter(priv->range, + g_bmi088_accel_range, + nitems(g_bmi088_accel_range)); + uint8_t range = g_bmi088_accel_range[idx_range].regval; + + accel.x = p.x / 32768.0 * ((1 << (range)) * 3.0); + accel.y = p.y / 32768.0 * ((1 << (range)) * 3.0); + accel.z = p.z / 32768.0 * ((1 << (range)) * 3.0); + + accel.timestamp = time * 39.0625; + + uint16_t t = temp[0] * 8 + temp[1] / 32; + + if (t > 1023) + { + accel.temperature = (t - 2048) * 0.125 + 23.0; + } + else + { + accel.temperature = t * 0.125 + 23; + } + + priv->lower.push_event(priv->lower.priv, &accel, sizeof(accel)); +} + +/**************************************************************************** + * Name: bmi088_gyro_worker + * + * Description: + * Task the worker with retrieving the latest sensor data. We should not do + * this in a interrupt since it might take too long. Also we cannot lock + * the I2C bus from within an interrupt. + * + * Input Parameters: + * arg - Device struct. + * + * Returned Value: + * none. + * + * Assumptions/Limitations: + * none. + * + ****************************************************************************/ + +static void bmi088_gyro_worker(FAR void *arg) +{ + FAR struct bmi088_gyro_dev_uorb_s *priv = arg; + struct sensor_gyro gyro; + struct gyro_source_t p; + int idx_range; + uint8_t range; + uint32_t time; + uint8_t temp[2]; + + DEBUGASSERT(priv != NULL); + + work_queue(HPWORK, &priv->work, + bmi088_gyro_worker, + priv, + priv->interval / USEC_PER_TICK); + + bmi088_get_gyro_regs(&priv->dev, + BMI088_GYRO_X_LSB, + (FAR uint8_t *)&p, 6); + + bmi088_get_acc_regs(&priv->dev, + BMI088_SENSORTIME_0, + (FAR uint8_t *)&time, 3); + + bmi088_get_acc_regs(&priv->dev, + BMI088_TEMP_MSB, + temp, 2); + + idx_range = bmi088_find_parameter(priv->range, + g_bmi088_gyro_range, + nitems(g_bmi088_gyro_range)); + + range = g_bmi088_gyro_range[idx_range].regval; + + gyro.x = p.x / 32768.0 * (2000.0 / (1 << (range))); + gyro.y = p.y / 32768.0 * (2000.0 / (1 << (range))); + gyro.z = p.z / 32768.0 * (2000.0 / (1 << (range))); + + gyro.timestamp = time * 39.0625; + + uint16_t t = temp[0] * 8 + temp[1] / 32; + + if (t > 1023) + { + gyro.temperature = (t - 2048) * 0.125 + 23; + } + else + { + gyro.temperature = t * 0.125 + 23; + } + + priv->lower.push_event(priv->lower.priv, &gyro, sizeof(gyro)); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: bmi088_register_accel + * + * Description: + * Register the BMI088 accel sensor. + * + * Input Parameters: + * devno - Sensor device number. + * config - Interrupt fuctions. + * + * Returned Value: + * Description of the value returned by this function (if any), + * including an enumeration of all possible error values. + * + * Assumptions/Limitations: + * none. + * + ****************************************************************************/ + +#ifdef CONFIG_SENSORS_BMI088_I2C +static int bmi088_register_accel(int devno, + FAR struct i2c_master_s *dev) +#else /* CONFIG_BMI088_SPI */ +static int bmi088_register_accel(int devno, + FAR struct spi_dev_s *dev) +#endif +{ + FAR struct bmi088_acc_dev_uorb_s *priv; + int ret; + + /* Sanity check */ + + DEBUGASSERT(dev != NULL); + + /* Initialize the BMI088 device structure */ + + priv = kmm_zalloc(sizeof(*priv)); + if (priv == NULL) + { + return -ENOMEM; + } + + /* config accelerometer */ + +#ifdef CONFIG_SENSORS_BMI088_I2C + priv->dev_acc.i2c = priv_acc; + priv->dev_acc.addr = BMI088_I2C_ACC_ADDR; + priv->dev_acc.freq = BMI088_I2C_FREQ; + +#else /* CONFIG_SENSORS_BMI088_SPI */ + priv->dev.spi = dev; + + /* BMI088 detects communication bus is SPI by rising edge of CS. */ + + bmi088_get_acc_reg8(&priv->dev , BMI088_ACC_CHIP_ID); + nxsig_usleep(200); + +#endif + + priv->lower.ops = &g_bmi088_accel_ops; + priv->lower.type = SENSOR_TYPE_ACCELEROMETER; + priv->lower.uncalibrated = true; + priv->interval = BMI088_DEFAULT_INTERVAL; + priv->lower.nbuffer = 1; + + /* Read and verify the deviceid */ + + ret = bmi088_get_acc_reg8(&priv->dev, BMI088_ACC_CHIP_ID); + if (ret != 0x1e) + { + snerr("Wrong Device ID!\n"); + kmm_free(priv); + return ret; + } + + /* Register the character driver */ + + ret = sensor_register(&priv->lower, devno); + if (ret < 0) + { + snerr("Failed to register accel driver: %d\n", ret); + kmm_free(priv); + } + + bmi088_accel_enable(priv, true); + + return ret; +} + +/**************************************************************************** + * Name: bmi088_register_gyro + * + * Description: + * Register the BMI088 gyro sensor. + * + * Input Parameters: + * devno - Sensor device number. + * config - Interrupt fuctions. + * + * Returned Value: + * Description of the value returned by this function (if any), + * including an enumeration of all possible error values. + * + * Assumptions/Limitations: + * none. + * + ****************************************************************************/ + +#ifdef CONFIG_SENSORS_BMI088_I2C +static int bmi088_register_gyro(int devno, + FAR struct i2c_master_s *dev) +#else /* CONFIG_BMI088_SPI */ +static int bmi088_register_gyro(int devno, + FAR struct spi_dev_s *dev) +#endif +{ + FAR struct bmi088_gyro_dev_uorb_s *priv; + int ret ; + + /* Sanity check */ + + DEBUGASSERT(dev != NULL); + + /* Initialize the device structure */ + + priv = kmm_zalloc(sizeof(*priv)); + if (priv == NULL) + { + return -ENOMEM; + } + + /* config gyroscope */ + +#ifdef CONFIG_SENSORS_BMI088_I2C + priv->dev.i2c = dev; + priv->dev.addr = BMI088_I2C_GY_ADDR; + priv->dev.freq = BMI088_I2C_FREQ; + +#else /* CONFIG_SENSORS_BMI088_SPI */ + priv->dev.spi = dev; +#endif + + priv->lower.ops = &g_bmi088_gyro_ops; + priv->lower.type = SENSOR_TYPE_GYROSCOPE; + priv->lower.uncalibrated = true; + priv->interval = BMI088_DEFAULT_INTERVAL; + priv->lower.nbuffer = 1; + + /* Read and verify the deviceid */ + + ret = bmi088_get_gyro_reg8(&priv->dev, BMI088_GYRO_CHIP_ID); + if (ret != 0x0f) + { + snerr("Wrong Device ID!\n"); + kmm_free(priv); + return ret; + } + + /* Register the character driver */ + + ret = sensor_register(&priv->lower, devno); + if (ret < 0) + { + snerr("Failed to register gyro driver: %d\n", ret); + kmm_free(priv); + } + + bmi088_gyro_enable(priv, true); + + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: bmi088_register_acc_uorb + * + * Description: + * Register the BMI088 accel sensor. + * + * Input Parameters: + * devno - Sensor device number. + * dev - An instance of the SPI or I2C interface to use to communicate + * with BMI088 + * + * Returned Value: + * Description of the value returned by this function (if any), + * including an enumeration of all possible error values. + * + * Assumptions/Limitations: + * none. + * + ****************************************************************************/ + +#ifdef CONFIG_SENSORS_BMI088_I2C +int bmi088_register_acc_uorb(int devno, FAR struct i2c_master_s *dev) +#else /* CONFIG_BMI088_SPI */ +int bmi088_register_acc_uorb(int devno, FAR struct spi_dev_s *dev) +#endif +{ + int ret; + + ret = bmi088_register_accel(devno, dev); + DEBUGASSERT(ret >= 0); + + sninfo("BMI088 acc driver loaded successfully!\n"); + return ret; +} + +/**************************************************************************** + * Name: bmi088_register_gyro_uorb + * + * Description: + * Register the BMI088 gyro sensor. + * + * Input Parameters: + * devno - Sensor device number. + * dev - An instance of the SPI or I2C interface to use to communicate + * with BMI088 + * + * Returned Value: + * Description of the value returned by this function (if any), + * including an enumeration of all possible error values. + * + * Assumptions/Limitations: + * none. + * + ****************************************************************************/ + +#ifdef CONFIG_SENSORS_BMI088_I2C +int bmi088_register_gyro_uorb(int devno, FAR struct i2c_master_s *dev) +#else /* CONFIG_BMI088_SPI */ +int bmi088_register_gyro_uorb(int devno, FAR struct spi_dev_s *dev) +#endif +{ + int ret; + + ret = bmi088_register_gyro(devno, dev); + DEBUGASSERT(ret >= 0); + + sninfo("BMI088 gyro driver loaded successfully!\n"); + return ret; +} +#endif /* CONFIG_SENSORS_BMI088_UORB */ diff --git a/include/nuttx/sensors/bmi088.h b/include/nuttx/sensors/bmi088.h new file mode 100644 index 0000000000..b6c96e3de1 --- /dev/null +++ b/include/nuttx/sensors/bmi088.h @@ -0,0 +1,173 @@ +/**************************************************************************** + * include/nuttx/sensors/bmi088.h + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __INCLUDE_NUTTX_SENSORS_BMI088_H +#define __INCLUDE_NUTTX_SENSORS_BMI088_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#if defined(CONFIG_SENSORS_BMI088) + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define BMI088_SPI_MAXFREQUENCY 1000000 + +/* Configuration ************************************************************/ + +/* Power mode */ + +#define BMI088_PM_SUSPEND (0x00) +#define BMI088_PM_NORMAL (0x01) +#define BMI088_PM_LOWPOWER (0x02) +#define BMI088_PM_FASTSTARTUP (0x03) + +/* Output data rate */ + +#define BMI088_ACCEL_ODR_0_78HZ (0x01) +#define BMI088_ACCEL_ODR_1_56HZ (0x02) +#define BMI088_ACCEL_ODR_3_12HZ (0x03) +#define BMI088_ACCEL_ODR_6_25HZ (0x04) +#define BMI088_ACCEL_ODR_12_5HZ (0x05) +#define BMI088_ACCEL_ODR_25HZ (0x06) +#define BMI088_ACCEL_ODR_50HZ (0x07) +#define BMI088_ACCEL_ODR_100HZ (0x08) +#define BMI088_ACCEL_ODR_200HZ (0x09) +#define BMI088_ACCEL_ODR_400HZ (0x0A) +#define BMI088_ACCEL_ODR_800HZ (0x0B) +#define BMI088_ACCEL_ODR_1600HZ (0x0C) + +/* IOCTL Commands ***********************************************************/ + +#define SNIOC_ACC_ENABLE _SNIOC(0x0001) +#define SNIOC_ACC_DISABLE _SNIOC(0x0002) + +#define SNIOC_GYRO_ENABLE _SNIOC(0x0101) +#define SNIOC_GYRO_DISABLE _SNIOC(0x0102) + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * struct 6-axis data + ****************************************************************************/ + +#pragma pack(1) + +struct acc_source_t +{ + int16_t x; + int16_t y; + int16_t z; +}; + +struct acc_t +{ + float x; + float y; + float z; +}; + +struct gyro_source_t +{ + int16_t x; + int16_t y; + int16_t z; +}; + +struct gyro_t +{ + float x; + float y; + float z; +}; + +struct acc_gyro_st_s +{ + struct acc_source_t acc_source; + uint32_t sensor_time; + struct gyro_source_t gyro_source; + struct acc_t accel; + struct gyro_t gyro; +}; + +#pragma pack() + +struct spi_dev_s; +struct i2c_master_s; + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/**************************************************************************** + * Name: bmi088_register + * + * Description: + * Register the BMI088 character device as 'devpath' + * + * Input Parameters: + * devpath - The full path to the driver to register. E.g., "/dev/accel0" + * dev - An instance of the SPI or I2C interface to use to communicate + * with BMI088 + * + * Returned Value: + * Zero (OK) on success; a negated errno value on failure. + * + ****************************************************************************/ + +#ifdef CONFIG_SENSORS_BMI088_I2C +# ifdef CONFIG_SENSORS_BMI088_UORB +int bmi088_register_acc_uorb(int devno, FAR struct i2c_master_s *dev); +int bmi088_register_gyro_uorb(int devno, FAR struct i2c_master_s *dev); +# else +int bmi088_acc_register (FAR const char *devpath, + FAR struct i2c_master_s *dev); +int bmi088_gyro_register(FAR const char *devpath, + FAR struct i2c_master_s *dev); +# endif /* CONFIG_SENSORS_BMI088_UORB */ +#else /* CONFIG_BMI088_SPI */ +# ifdef CONFIG_SENSORS_BMI088_UORB +int bmi088_register_acc_uorb(int devno, FAR struct spi_dev_s *dev); +int bmi088_register_gyro_uorb(int devno, FAR struct spi_dev_s *dev); +# else +int bmi088_acc_register (FAR const char *devpath, + FAR struct spi_dev_s *dev); +int bmi088_gyro_register(FAR const char *devpath, + FAR struct spi_dev_s *dev); +# endif /* CONFIG_SENSORS_BMI088_UORB */ +#endif + +#endif /* CONFIG_SENSORS_BMI088 */ +#endif /* __INCLUDE_NUTTX_SENSORS_BMI088_H */