From 8271117841de5218ef53c2dfed7d0930b99d8f9c Mon Sep 17 00:00:00 2001 From: Huang Qi Date: Sat, 29 Nov 2025 21:53:48 +0800 Subject: [PATCH] drivers/sensors: Add QST QMI8658 6-axis IMU sensor support MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add complete driver support for the QST QMI8658 6-axis IMU sensor featuring 3-axis accelerometer and 3-axis gyroscope with I2C interface and uORB integration. Key features implemented: * Full I2C communication with configurable frequency (default 400kHz) * Multiple accelerometer ranges (±2g, ±4g, ±8g, ±16g) and ODR settings * Multiple gyroscope ranges (±16 to ±1024 dps) with high ODR support * Low-pass filter configuration for both sensors * Temperature sensing with 16-bit resolution * Self-test capability for both accelerometer and gyroscope * Calibration-on-demand support with offset registers * FIFO buffer management (framework ready) * Interrupt-driven and polling mode support * Complete uORB integration with sensor_accel and sensor_gyro topics Driver components added: * Core driver implementation (qmi8658_uorb.c) with register operations * Header file with register definitions and scale factors (qmi8658.h) * Kconfig options for driver configuration and polling mode * Build system integration (CMakeLists.txt, Make.defs) * Comprehensive documentation with API reference and usage examples The driver follows NuttX sensor framework conventions and provides robust error handling, mutex protection, and comprehensive debugging support through CONFIG_DEBUG_SENSORS. Signed-off-by: Huang Qi --- .../components/drivers/special/sensors.rst | 1 + .../drivers/special/sensors/qmi8658.rst | 232 +++ .../drivers/special/sensors/sensors_uorb.rst | 1 + drivers/sensors/CMakeLists.txt | 6 + drivers/sensors/Kconfig | 31 + drivers/sensors/Make.defs | 6 + drivers/sensors/qmi8658_uorb.c | 1750 +++++++++++++++++ include/nuttx/sensors/qmi8658.h | 132 ++ 8 files changed, 2159 insertions(+) create mode 100644 Documentation/components/drivers/special/sensors/qmi8658.rst create mode 100644 drivers/sensors/qmi8658_uorb.c create mode 100644 include/nuttx/sensors/qmi8658.h diff --git a/Documentation/components/drivers/special/sensors.rst b/Documentation/components/drivers/special/sensors.rst index c1857b9050667..064dcf077b366 100644 --- a/Documentation/components/drivers/special/sensors.rst +++ b/Documentation/components/drivers/special/sensors.rst @@ -30,6 +30,7 @@ general interface. sensors/mcp9600.rst sensors/mpl115a.rst sensors/nau7802.rst + sensors/qmi8658.rst sensors/sht4x.rst sensors/lsm6dso32.rst sensors/lis2mdl.rst diff --git a/Documentation/components/drivers/special/sensors/qmi8658.rst b/Documentation/components/drivers/special/sensors/qmi8658.rst new file mode 100644 index 0000000000000..cb11a46ce692a --- /dev/null +++ b/Documentation/components/drivers/special/sensors/qmi8658.rst @@ -0,0 +1,232 @@ +======= +QMI8658 +======= + +The QMI8658 is a high-performance 6-axis IMU sensor by QST featuring a 3-axis +accelerometer and 3-axis gyroscope. It supports both I2C and SPI interfaces, +although this driver currently supports I2C communication only. + +This driver uses the :doc:`uorb +` interface. It supports the +self-test capability for both the accelerometer and gyroscope. + +The driver supports comprehensive features including multiple full-scale ranges, +configurable ODR settings, low-pass filters, FIFO buffer management, temperature +sensing, and device calibration. + +.. note:: + The QMI8658 is a feature-rich sensor with advanced capabilities like + tap detection, motion detection, and various low-power modes. This driver + implements the core functionality for accelerometer and gyroscope data + acquisition with room for future feature extensions. + +Application Programming Interface +================================= + +.. code-block:: c + + #include + +The QMI8658 driver provides one registration function: + +- **uORB Interface**: ``qmi8658_uorb_register()`` + +Registration +------------ + +The uORB interface registers the driver and creates separate uORB topics for +accelerometer and gyroscope data under ``/dev/uorb/``: ``sensor_accel`` and +``sensor_gyro``, where ``n`` is the device number. + +.. code-block:: c + + /* Example uORB registration */ + + ret = qmi8658_uorb_register(0, i2c_bus, QMI8658_I2C_ADDR_DEFAULT); + if (ret < 0) + { + syslog(LOG_ERR, "Couldn't register QMI8658 uORB: %d\n", ret); + return ret; + } + +The uORB interface registers the driver and creates separate uORB topics for +accelerometer and gyroscope data under ``/dev/uorb/``: ``sensor_accel`` and +``sensor_gyro``, where ``n`` is the device number. + +.. code-block:: c + + /* Example uORB registration */ + + ret = qmi8658_uorb_register(0, i2c_bus, QMI8658_I2C_ADDR_DEFAULT); + if (ret < 0) + { + syslog(LOG_ERR, "Couldn't register QMI8658 uORB: %d\n", ret); + return ret; + } + +Configuration Options +===================== + +The QMI8658 driver supports several Kconfig options: + +* ``CONFIG_SENSORS_QMI8658``: Enable QMI8658 driver support (requires UORB) +* ``CONFIG_SENSORS_QMI8658_POLL``: Enable polling mode with configurable interval +* ``CONFIG_SENSORS_QMI8658_POLL_INTERVAL``: Set polling interval (default 1s) +* ``CONFIG_QMI8658_I2C_FREQUENCY``: Set I2C communication frequency (default 400kHz) + +Supported Features +================== + +Accelerometer +------------- + +* **Full-Scale Ranges**: ±2g, ±4g, ±8g, ±16g +* **Output Data Rates**: 1000Hz, 500Hz, 250Hz, 125Hz, 62.5Hz, 31.25Hz +* **Low-Power ODR**: 128Hz, 21Hz, 11Hz, 3Hz +* **Low-Pass Filters**: 4 different modes + OFF + +Gyroscope +--------- + +* **Full-Scale Ranges**: ±16, ±32, ±64, ±128, ±256, ±512, ±1024 dps +* **Output Data Rates**: 7174.4Hz, 3587.2Hz, 1793.6Hz, 896.8Hz, 448.4Hz, 224.2Hz, 112.1Hz, 56.05Hz, 28.025Hz +* **Low-Pass Filters**: 4 different modes + OFF + +Additional Features +-----------------== + +* **Temperature Sensor**: 16-bit temperature data with 256 LSB/°C scale factor +* **FIFO Buffer**: Configurable FIFO with watermark and interrupt support +* **Sampling Modes**: Synchronous and asynchronous sampling +* **Interrupt Support**: Data ready, FIFO watermark, motion detection +* **Self-Test**: Built-in self-test capability for both sensors +* **Calibration**: On-demand calibration support + +IOCTL Commands +============== + +uORB IOCTLs +----------- + +* ``SNIOC_SETFULLSCALE``: Set full-scale range (argument in g for accel, dps for gyro) +* ``SNIOC_SET_CALIBVALUE``: Set calibration offsets +* ``SNIOC_SELFTEST``: Perform sensor self-test +* ``SNIOC_WHO_AM_I``: Read device ID (should return 0x05) + +Scale Factors +============= + +The driver provides predefined scale factors for converting raw sensor data +to physical units: + +Accelerometer Scale Factors (LSB/g): + +.. code-block:: c + + #define QMI8658_ACC_SCALE_2G (16384.0f) + #define QMI8658_ACC_SCALE_4G (8192.0f) + #define QMI8658_ACC_SCALE_8G (4096.0f) + #define QMI8658_ACC_SCALE_16G (2048.0f) + +Gyroscope Scale Factors (LSB/dps): + +.. code-block:: c + + #define QMI8658_GYRO_SCALE_16DPS (2048.0f) + #define QMI8658_GYRO_SCALE_32DPS (1024.0f) + #define QMI8658_GYRO_SCALE_64DPS (512.0f) + #define QMI8658_GYRO_SCALE_128DPS (256.0f) + #define QMI8658_GYRO_SCALE_256DPS (128.0f) + #define QMI8658_GYRO_SCALE_512DPS (64.0f) + #define QMI8658_GYRO_SCALE_1024DPS (32.0f) + +Temperature Scale Factor (LSB/°C): + +.. code-block:: c + + #define QMI8658_TEMP_SCALE (256.0f) + +Data Conversion +=============== + +To convert raw sensor data to physical units: + +.. code-block:: c + + /* Convert accelerometer raw data to g */ + + float accel_x_g = (float)raw_accel_x / QMI8658_ACC_SCALE_8G; + float accel_y_g = (float)raw_accel_y / QMI8658_ACC_SCALE_8G; + float accel_z_g = (float)raw_accel_z / QMI8658_ACC_SCALE_8G; + + /* Convert gyroscope raw data to dps */ + + float gyro_x_dps = (float)raw_gyro_x / QMI8658_GYRO_SCALE_512DPS; + float gyro_y_dps = (float)raw_gyro_y / QMI8658_GYRO_SCALE_512DPS; + float gyro_z_dps = (float)raw_gyro_z / QMI8658_GYRO_SCALE_512DPS; + + /* Convert temperature raw data to °C */ + + float temp_c = (float)raw_temp / QMI8658_TEMP_SCALE; + +Debugging and Testing +===================== + +To debug the QMI8658 device, you can: + +1. **Enable Debug Output**: Set ``CONFIG_DEBUG_SENSORS`` and ``CONFIG_DEBUG_INFO`` +2. **Use uORB Listener**: Include ``uorb_listener`` application to monitor sensor data + +Performance Considerations +========================== + +* **I2C Frequency**: Default 400kHz, configurable via ``CONFIG_QMI8658_I2C_FREQUENCY`` +* **Polling Overhead**: Use interrupt-driven mode when possible for better efficiency +* **FIFO Usage**: Enable FIFO to reduce I2C traffic and CPU overhead +* **Power Management**: Utilize low-power ODR settings for battery-powered applications + +Limitations +=========== + +* Currently supports I2C interface only (SPI support can be added) +* Advanced features like tap detection and motion detection not yet implemented +* FIFO interrupt handling not fully implemented +* Some low-power modes require additional configuration + +Hardware Connections +==================== + +I2C Interface +-------------- + +* **VDD**: Power supply (1.71V to 3.6V) +* **GND**: Ground +* **SDA/SDI**: I2C Serial Data +* **SCL/SCLK**: I2C Serial Clock +* **CS**: Chip Select (connect to VDD for I2C mode) +* **INT1/INT2**: Interrupt pins (optional) + +Typical I2C addresses: +* **Primary**: 0x6B +* **Secondary**: 0x6D (when SDO/SA0 pin is high) + +.. note:: + Ensure proper pull-up resistors on SDA and SCL lines (typically 4.7kΩ for 3.3V). + +Troubleshooting +=============== + +**Device Not Responding** +* Check I2C address and connections +* Verify power supply voltage +* Ensure CS pin is properly configured for I2C mode + +**Incorrect Readings** +* Verify full-scale range settings +* Check scale factor calculations +* Ensure sensor is properly calibrated + +**Communication Errors** +* Reduce I2C frequency +* Check for signal integrity issues +* Verify pull-up resistor values diff --git a/Documentation/components/drivers/special/sensors/sensors_uorb.rst b/Documentation/components/drivers/special/sensors/sensors_uorb.rst index 75187b66541e0..ebdb66e55c86b 100644 --- a/Documentation/components/drivers/special/sensors/sensors_uorb.rst +++ b/Documentation/components/drivers/special/sensors/sensors_uorb.rst @@ -496,6 +496,7 @@ Implemented Drivers - mpu9250 - ms56xx - :doc:`nau7802` +- :doc:`qmi8658` - :doc:`sht4x` - :doc:`lsm6dso32` - wtgahrs2 diff --git a/drivers/sensors/CMakeLists.txt b/drivers/sensors/CMakeLists.txt index b3ca24e07b0c5..2a90d7555ddb3 100644 --- a/drivers/sensors/CMakeLists.txt +++ b/drivers/sensors/CMakeLists.txt @@ -344,6 +344,12 @@ if(CONFIG_SENSORS) list(APPEND SRCS tmp112.c) endif() + # QMI8658 6-axis IMU + + if(CONFIG_SENSORS_QMI8658) + list(APPEND SRCS qmi8658_uorb.c) + endif() + endif() # CONFIG_I2C # These drivers depend on SPI support diff --git a/drivers/sensors/Kconfig b/drivers/sensors/Kconfig index 3baefcd69482d..3aeb81c1dae11 100644 --- a/drivers/sensors/Kconfig +++ b/drivers/sensors/Kconfig @@ -2151,4 +2151,35 @@ config TMP112_I2C_FREQUENCY endif #SENSORS_TMP112 +config SENSORS_QMI8658 + bool "QST QMI8658 6-Axis IMU Sensor support" + default n + depends on I2C && UORB + ---help--- + Enable driver support for the QST QMI8658 6-axis IMU sensor. + +if SENSORS_QMI8658 + +config SENSORS_QMI8658_POLL + bool "Enables polling sensor data" + default n + ---help--- + Enables polling of sensor data. + +config SENSORS_QMI8658_POLL_INTERVAL + int "Polling interval in microseconds, default 1s" + depends on SENSORS_QMI8658_POLL + default 1000000 + range 0 4294967295 + ---help--- + The interval until a new sensor measurement will be triggered. + +config QMI8658_I2C_FREQUENCY + int "QMI8658 I2C frequency" + default 400000 + ---help--- + I2C frequency used to communicate with QMI8658. + +endif # SENSORS_QMI8658 + endif # SENSORS diff --git a/drivers/sensors/Make.defs b/drivers/sensors/Make.defs index 870afeb40dfac..c4505b7941d88 100644 --- a/drivers/sensors/Make.defs +++ b/drivers/sensors/Make.defs @@ -418,6 +418,12 @@ ifeq ($(CONFIG_SENSORS_MPU9250),y) CSRCS += mpu9250_uorb.c endif +# QMI8658 6-axis IMU + +ifeq ($(CONFIG_SENSORS_QMI8658),y) + CSRCS += qmi8658_uorb.c +endif + # Quadrature encoder upper half ifeq ($(CONFIG_SENSORS_QENCODER),y) diff --git a/drivers/sensors/qmi8658_uorb.c b/drivers/sensors/qmi8658_uorb.c new file mode 100644 index 0000000000000..3d091f63f51be --- /dev/null +++ b/drivers/sensors/qmi8658_uorb.c @@ -0,0 +1,1750 @@ +/**************************************************************************** + * drivers/sensors/qmi8658_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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* QMI8658 Register Addresses */ +#define QMI8658_REG_WHOAMI (0x00) /* Chip ID register */ +#define QMI8658_REG_REVISION (0x01) /* Revision register */ +#define QMI8658_REG_CTRL1 (0x02) /* Control register 1 */ +#define QMI8658_REG_CTRL2 (0x03) /* Control register 2 (Accelerometer) */ +#define QMI8658_REG_CTRL3 (0x04) /* Control register 3 (Gyroscope) */ +#define QMI8658_REG_CTRL5 (0x06) /* Control register 5 (LPF settings) */ +#define QMI8658_REG_CTRL7 (0x08) /* Control register 7 (Sensor enable) */ +#define QMI8658_REG_CTRL8 (0x09) /* Control register 8 (Motion detection) */ +#define QMI8658_REG_CTRL9 (0x0A) /* Control register 9 (Commands) */ +#define QMI8658_REG_CAL1_L (0x0B) /* Calibration register 1 low */ +#define QMI8658_REG_CAL1_H (0x0C) /* Calibration register 1 high */ +#define QMI8658_REG_CAL2_L (0x0D) /* Calibration register 2 low */ +#define QMI8658_REG_CAL2_H (0x0E) /* Calibration register 2 high */ +#define QMI8658_REG_CAL3_L (0x0F) /* Calibration register 3 low */ +#define QMI8658_REG_CAL3_H (0x10) /* Calibration register 3 high */ +#define QMI8658_REG_CAL4_L (0x11) /* Calibration register 4 low */ +#define QMI8658_REG_CAL4_H (0x12) /* Calibration register 4 high */ +#define QMI8658_REG_FIFO_WTM_TH (0x13) /* FIFO watermark threshold */ +#define QMI8658_REG_FIFO_CTRL (0x14) /* FIFO control */ +#define QMI8658_REG_FIFO_COUNT (0x15) /* FIFO sample count */ +#define QMI8658_REG_FIFO_STATUS (0x16) /* FIFO status */ +#define QMI8658_REG_FIFO_DATA (0x17) /* FIFO data */ +#define QMI8658_REG_STATUS_INT (0x2D) /* Status interrupt */ +#define QMI8658_REG_STATUS0 (0x2E) /* Status register 0 */ +#define QMI8658_REG_STATUS1 (0x2F) /* Status register 1 */ +#define QMI8658_REG_TIMESTAMP_L (0x30) /* Timestamp low */ +#define QMI8658_REG_TIMESTAMP_M (0x31) /* Timestamp middle */ +#define QMI8658_REG_TIMESTAMP_H (0x32) /* Timestamp high */ +#define QMI8658_REG_TEMPERATURE_L (0x33) /* Temperature low */ +#define QMI8658_REG_TEMPERATURE_H (0x34) /* Temperature high */ +#define QMI8658_REG_AX_L (0x35) /* Accelerometer X low */ +#define QMI8658_REG_AX_H (0x36) /* Accelerometer X high */ +#define QMI8658_REG_AY_L (0x37) /* Accelerometer Y low */ +#define QMI8658_REG_AY_H (0x38) /* Accelerometer Y high */ +#define QMI8658_REG_AZ_L (0x39) /* Accelerometer Z low */ +#define QMI8658_REG_AZ_H (0x3A) /* Accelerometer Z high */ +#define QMI8658_REG_GX_L (0x3B) /* Gyroscope X low */ +#define QMI8658_REG_GX_H (0x3C) /* Gyroscope X high */ +#define QMI8658_REG_GY_L (0x3D) /* Gyroscope Y low */ +#define QMI8658_REG_GY_H (0x3E) /* Gyroscope Y high */ +#define QMI8658_REG_GZ_L (0x3F) /* Gyroscope Z low */ +#define QMI8658_REG_GZ_H (0x40) /* Gyroscope Z high */ +#define QMI8658_REG_COD_STATUS (0x46) /* Calibration-on-demand status */ +#define QMI8658_REG_DQW_L (0x49) /* COD quaternion W low */ +#define QMI8658_REG_DQW_H (0x4A) /* COD quaternion W high */ +#define QMI8658_REG_DQX_L (0x4B) /* COD quaternion X low */ +#define QMI8658_REG_DQX_H (0x4C) /* COD quaternion X high */ +#define QMI8658_REG_RST_RESULT (0x4D) /* Reset result register */ +#define QMI8658_REG_DQY_L (0x4E) /* COD quaternion Y low */ +#define QMI8658_REG_DQY_H (0x4F) /* COD quaternion Y high */ +#define QMI8658_REG_DQZ_L (0x50) /* COD quaternion Z low */ +#define QMI8658_REG_DQZ_H (0x51) /* COD quaternion Z high */ +#define QMI8658_REG_DVX_L (0x52) /* Self-test X low */ +#define QMI8658_REG_DVX_H (0x53) /* Self-test X high */ +#define QMI8658_REG_DVY_L (0x54) /* Self-test Y low */ +#define QMI8658_REG_DVY_H (0x55) /* Self-test Y high */ +#define QMI8658_REG_DVZ_L (0x56) /* Self-test Z low */ +#define QMI8658_REG_DVZ_H (0x57) /* Self-test Z high */ +#define QMI8658_REG_TAP_STATUS (0x59) /* Tap status */ +#define QMI8658_REG_STEP_CNT_LOW (0x5A) /* Step counter low */ +#define QMI8658_REG_STEP_CNT_MID (0x5B) /* Step counter middle */ +#define QMI8658_REG_STEP_CNT_HIGH (0x5C) /* Step counter high */ +#define QMI8658_REG_RESET (0x60) /* Reset register */ + +/* Default values */ +#define QMI8658_REG_WHOAMI_DEFAULT (0x05) +#define QMI8658_REG_STATUS_DEFAULT (0x03) +#define QMI8658_REG_RESET_DEFAULT (0xB0) +#define QMI8658_REG_RST_RESULT_VAL (0x80) + +/* Control register bit definitions */ + +/* CTRL1 - Control Register 1 */ +#define QMI8658_CTRL1_ACC_EN (1 << 0) +#define QMI8658_CTRL1_POWER_DOWN (1 << 1) +#define QMI8658_CTRL1_FIFO_INT_EN (1 << 2) +#define QMI8658_CTRL1_INT1_EN (1 << 3) +#define QMI8658_CTRL1_INT2_EN (1 << 4) +#define QMI8658_CTRL1_ADDR_AI_EN (1 << 6) + +/* CTRL5 - Control Register 5 (Low-pass filters */ +#define QMI8658_ACCEL_LPF_MASK (0xF9) +#define QMI8658_GYRO_LPF_MASK (0x9F) + +/* CTRL7 - Control Register 7 (Enable/Disable) */ +#define QMI8658_CTRL7_ACC_EN (1 << 0) /* Accelerometer enable */ +#define QMI8658_CTRL7_GYRO_EN (1 << 1) /* Gyroscope enable */ +#define QMI8658_CTRL7_RESERVED (0x7C) /* Reserved bits 2-6 */ +#define QMI8658_CTRL7_SYNC_MODE (1 << 7) /* Synchronization mode */ + +/* CTRL7 bit masks */ +#define QMI8658_CTRL7_ACC_EN_MASK (0x01) /* Accelerometer enable mask */ +#define QMI8658_CTRL7_GYRO_EN_MASK (0x02) /* Gyroscope enable mask */ +#define QMI8658_CTRL7_EN_MASK (0x03) /* Enable bits mask */ + +/* CTRL9 - Control Register 9 (Commands) */ +#define QMI8658_CTRL9_CMD_ACK (0x00) /* Acknowledge command */ +#define QMI8658_CTRL9_CMD_RST_FIFO (0x04) /* Reset FIFO command */ +#define QMI8658_CTRL9_CMD_REQ_FIFO (0x05) /* Request FIFO data command */ +#define QMI8658_CTRL9_CMD_CALIB (0xA2) /* Calibration command */ + +/* STATUS0 - Status Register 0 (Data ready status) */ +#define QMI8658_STATUS0_ACC_DRDY (1 << 0) /* Accelerometer data ready */ +#define QMI8658_STATUS0_GYRO_DRDY (1 << 1) /* Gyroscope data ready */ +#define QMI8658_STATUS0_RESERVED (0xFC) /* Reserved bits 2-7 */ + +/* STATUS_INT - Status Interrupt Register */ +#define QMI8658_STATUS_INT_AVAIL (1 << 0) /* Interrupt available */ +#define QMI8658_STATUS_INT_LOCKED (1 << 1) /* Interrupt locked */ +#define QMI8658_STATUS_INT_RESERVED (0x7C) /* Reserved bits 2-6 */ +#define QMI8658_STATUS_INT_CMD_DONE (1 << 7) /* Command done interrupt */ + +/* Legacy compatibility definitions */ +#define STATUS0_ACCEL_AVAIL QMI8658_STATUS0_ACC_DRDY +#define STATUS0_GYRO_AVAIL QMI8658_STATUS0_GYRO_DRDY +#define STATUS_INT_CTRL9_CMD_DONE QMI8658_STATUS_INT_CMD_DONE +#define STATUS_INT_LOCKED QMI8658_STATUS_INT_LOCKED +#define STATUS_INT_AVAIL QMI8658_STATUS_INT_AVAIL + +/* Low-Pass Filter Modes */ +#define QMI8658_LPF_MODE_0 (0x00) +#define QMI8658_LPF_MODE_1 (0x01) +#define QMI8658_LPF_MODE_2 (0x02) +#define QMI8658_LPF_MODE_3 (0x03) +#define QMI8658_LPF_OFF (0x04) + +/* Sample Synchronization Modes */ +#define QMI8658_SYNC_MODE (0x00) +#define QMI8658_ASYNC_MODE (0x01) + +/* Scale factors are defined in nuttx/sensors/qmi8658.h */ + +/* Sensor indices */ + +enum qmi8658_idx_e +{ + QMI8658_ACCEL_IDX = 0, + QMI8658_GYRO_IDX, + QMI8658_MAX_IDX +}; + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* QMI8658 configuration structure */ + +struct qmi8658_config_s +{ + uint8_t acc_range; + uint8_t gyro_range; + uint8_t acc_enable; + uint8_t gyro_enable; + uint8_t temp_enable; + uint8_t lp_mode; +}; + +/* Scale factors structure */ + +struct qmi8658_scale_factors_s +{ + float acc_scale; + float gyro_scale; + float temp_scale; +}; + +struct qmi8658_dev_s +{ + FAR struct i2c_master_s *i2c; + uint8_t addr; + int freq; + + uint8_t acc_range; + uint8_t gyro_range; + uint8_t acc_odr; + uint8_t gyro_odr; + uint8_t acc_lpf; + uint8_t gyro_lpf; + uint8_t sample_mode; + + mutex_t dev_lock; + + bool accel_enabled; + bool gyro_enabled; +}; + +struct qmi8658_sensor_s +{ + struct sensor_lowerhalf_s lower; +#ifdef CONFIG_SENSORS_QMI8658_POLL + struct work_s work; + uint32_t interval; +#endif + float scale; + FAR struct qmi8658_dev_s *dev; + bool enabled; +}; + +struct qmi8658_uorb_dev_s +{ + struct qmi8658_dev_s base; + struct qmi8658_sensor_s priv[QMI8658_MAX_IDX]; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/* I2C Register Operations */ + +static int qmi8658_readreg8(FAR struct qmi8658_dev_s *priv, uint8_t regaddr, + FAR uint8_t *regval); +static int qmi8658_writereg8(FAR struct qmi8658_dev_s *priv, uint8_t regaddr, + uint8_t regval); +static int qmi8658_modifyreg8(FAR struct qmi8658_dev_s *priv, + uint8_t regaddr, uint8_t clearbits, + uint8_t setbits); +static int qmi8658_readregs(FAR struct qmi8658_dev_s *priv, uint8_t regaddr, + FAR uint8_t *buffer, uint8_t len); + +/* Basic Operations */ + +static int qmi8658_checkid(FAR struct qmi8658_dev_s *priv); +static int qmi8658_reset(FAR struct qmi8658_dev_s *priv); + +/* Sensor Configuration */ + +static int qmi8658_config_accelerometer(FAR struct qmi8658_dev_s *priv, + uint8_t range, + uint8_t odr, + uint8_t lpf_mode); +static int qmi8658_config_gyroscope(FAR struct qmi8658_dev_s *priv, + uint8_t range, + uint8_t odr, + uint8_t lpf_mode); + +/* Sensor Control */ + +static int qmi8658_set_accelerometer(FAR struct qmi8658_dev_s *priv, + bool enable); +static int qmi8658_set_gyroscope(FAR struct qmi8658_dev_s *priv, + bool enable); + +/* Sampling Mode */ + +static int qmi8658_set_sample_mode(FAR struct qmi8658_dev_s *priv, + bool sync); + +/* Data Reading Functions */ + +static int qmi8658_read_accel(FAR struct qmi8658_dev_s *priv, + FAR int16_t *data); +static int qmi8658_read_gyro(FAR struct qmi8658_dev_s *priv, + FAR int16_t *data); +static int qmi8658_read_temp(FAR struct qmi8658_dev_s *priv, + FAR int16_t *temperature); + +/* Scale Factors Functions */ + +static int qmi8658_get_scale_factors(FAR struct qmi8658_dev_s *priv, + FAR struct qmi8658_scale_factors_s *factors); + +/* Initialization */ + +static int qmi8658_initialize(FAR struct qmi8658_dev_s *priv); + +/* Sensor methods */ + +static int qmi8658_activate(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, bool enable); +#ifdef CONFIG_SENSORS_QMI8658_POLL +static int qmi8658_set_interval(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR uint32_t *period_us); +#else +static int qmi8658_fetch(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR char *buffer, size_t buflen); +#endif + +#ifdef CONFIG_SENSORS_QMI8658_POLL +static void qmi8658_accel_worker(FAR void *arg); +static void qmi8658_gyro_worker(FAR void *arg); +#endif + +static int qmi8658_read_imu(FAR struct qmi8658_dev_s *dev, + FAR struct sensor_accel *accel, + FAR struct sensor_gyro *gyro); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static const struct sensor_ops_s g_sensor_ops = +{ + NULL, /* open */ + NULL, /* close */ + qmi8658_activate, /* activate */ +#ifdef CONFIG_SENSORS_QMI8658_POLL + qmi8658_set_interval, /* set_interval */ +#else + NULL, /* set_interval */ +#endif + NULL, /* batch */ +#ifdef CONFIG_SENSORS_QMI8658_POLL + NULL, /* fetch */ +#else + qmi8658_fetch, /* fetch */ +#endif + NULL, /* flush */ + NULL, /* selftest */ + NULL, /* set_calibvalue */ + NULL, /* calibrate */ + NULL, /* get_info */ + NULL, /* control */ +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: qmi8658_readreg8 + * + * Description: + * Read from an 8-bit QMI8658 register + * + ****************************************************************************/ + +static int qmi8658_readreg8(FAR struct qmi8658_dev_s *priv, uint8_t regaddr, + FAR uint8_t *regval) +{ + struct i2c_config_s config; + int ret; + + sninfo("Reading reg 0x%02x (addr=0x%02x, freq=%d)\n", + regaddr, priv->addr, priv->freq); + + config.frequency = priv->freq; + config.address = priv->addr; + config.addrlen = 7; + + ret = i2c_write(priv->i2c, &config, ®addr, 1); + if (ret < 0) + { + snerr(" i2c_write failed: %d\n", ret); + return ret; + } + + ret = i2c_read(priv->i2c, &config, regval, 1); + if (ret < 0) + { + snerr(" i2c_read failed: %d\n", ret); + return ret; + } + + return OK; +} + +/**************************************************************************** + * Name: qmi8658_writereg8 + * + * Description: + * Write to an 8-bit QMI8658 register + * + ****************************************************************************/ + +static int qmi8658_writereg8(FAR struct qmi8658_dev_s *priv, uint8_t regaddr, + uint8_t regval) +{ + struct i2c_config_s config; + uint8_t data[2]; + int ret; + + sninfo("Writing reg 0x%02x = 0x%02x (addr=0x%02x, freq=%d)\n", + regaddr, regval, priv->addr, priv->freq); + + config.frequency = priv->freq; + config.address = priv->addr; + config.addrlen = 7; + + data[0] = regaddr; + data[1] = regval; + + ret = i2c_write(priv->i2c, &config, data, 2); + if (ret < 0) + { + snerr(" i2c_write failed: %d\n", ret); + return ret; + } + + return OK; +} + +/**************************************************************************** + * Name: qmi8658_modifyreg8 + * + * Description: + * Modify an 8-bit QMI8658 register (clear and set bits) + * + ****************************************************************************/ + +static int qmi8658_modifyreg8(FAR struct qmi8658_dev_s *priv, + uint8_t regaddr, uint8_t clearbits, + uint8_t setbits) +{ + uint8_t regval; + int ret; + + ret = qmi8658_readreg8(priv, regaddr, ®val); + if (ret < 0) + { + return ret; + } + + regval &= ~clearbits; + regval |= setbits; + + ret = qmi8658_writereg8(priv, regaddr, regval); + if (ret < 0) + { + return ret; + } + + return OK; +} + +/**************************************************************************** + * Name: qmi8658_readregs + * + * Description: + * Read multiple consecutive registers using address auto-increment + * + ****************************************************************************/ + +static int qmi8658_readregs(FAR struct qmi8658_dev_s *priv, uint8_t regaddr, + FAR uint8_t *buffer, uint8_t len) +{ + struct i2c_config_s config; + int ret; + + sninfo("Reading %d registers starting from 0x%02x " + "(addr=0x%02x, freq=%d)\n", + len, regaddr, priv->addr, priv->freq); + + config.frequency = priv->freq; + config.address = priv->addr; + config.addrlen = 7; + + ret = i2c_write(priv->i2c, &config, ®addr, 1); + if (ret < 0) + { + snerr(" i2c_write failed: %d\n", ret); + return ret; + } + + /* Read multiple consecutive registers (auto-increment enabled) */ + + ret = i2c_read(priv->i2c, &config, buffer, len); + if (ret < 0) + { + snerr(" i2c_read failed: %d\n", ret); + return ret; + } + + return OK; +} + +/**************************************************************************** + * Name: qmi8658_checkid + * + * Description: + * Check if the QMI8658 chip ID is correct + * + ****************************************************************************/ + +static int qmi8658_checkid(FAR struct qmi8658_dev_s *priv) +{ + uint8_t chip_id; + int ret; + int ret_lock; + + ret_lock = nxmutex_lock(&priv->dev_lock); + if (ret_lock < 0) + { + return ret_lock; + } + + ret = qmi8658_readreg8(priv, QMI8658_REG_WHOAMI, &chip_id); + if (ret < 0) + { + snerr(" Failed to read chip ID\n"); + goto err_unlock; + } + + if (chip_id != QMI8658_REG_WHOAMI_DEFAULT) + { + snerr(" Invalid chip ID: 0x%02x (expected 0x%02x)\n", + chip_id, QMI8658_REG_WHOAMI_DEFAULT); + ret = -ENODEV; + goto err_unlock; + } + + sninfo("QMI8658 chip ID verified: 0x%02x\n", chip_id); + ret = OK; + +err_unlock: + nxmutex_unlock(&priv->dev_lock); + return ret; +} + +/**************************************************************************** + * Name: qmi8658_reset + * + * Description: + * Reset the QMI8658 sensor with proper verification + * + ****************************************************************************/ + +static int qmi8658_reset(FAR struct qmi8658_dev_s *priv) +{ + int ret; + int ret_lock; + uint8_t chip_id; + + ret_lock = nxmutex_lock(&priv->dev_lock); + if (ret_lock < 0) + { + return ret_lock; + } + + /* Issue device reset command */ + + ret = qmi8658_writereg8(priv, QMI8658_REG_RESET, + QMI8658_REG_RESET_DEFAULT); + if (ret < 0) + { + snerr("Failed to write reset command to QMI8658: %d\n", ret); + goto err_unlock; + } + + /* Wait for reset to complete (15ms minimum delay per datasheet) */ + + up_mdelay(15); + + /* Verify device responsiveness after reset */ + + ret = qmi8658_readreg8(priv, QMI8658_REG_WHOAMI, &chip_id); + if (ret < 0) + { + snerr("Failed to read chip ID after reset: %d\n", ret); + goto err_unlock; + } + + if (chip_id != QMI8658_REG_WHOAMI_DEFAULT) + { + snerr("Invalid chip ID after reset: 0x%02x (expected " + "0x%02x)\n", chip_id, QMI8658_REG_WHOAMI_DEFAULT); + ret = -EIO; + goto err_unlock; + } + + /* Enable address auto-increment */ + + ret = qmi8658_modifyreg8(priv, QMI8658_REG_CTRL1, 0, + QMI8658_CTRL1_ADDR_AI_EN); + if (ret < 0) + { + snerr("Failed to enable address auto-increment: %d\n", ret); + goto err_unlock; + } + + sninfo("QMI8658 reset completed successfully (chip ID: 0x%02x)\n", + chip_id); + +err_unlock: + nxmutex_unlock(&priv->dev_lock); + return ret; +} + +/**************************************************************************** + * Name: qmi8658_config_accelerometer + * + * Description: + * Configure accelerometer settings (range, ODR, low-pass filter) + * + ****************************************************************************/ + +static int qmi8658_config_accelerometer(FAR struct qmi8658_dev_s *priv, + uint8_t range, uint8_t odr, + uint8_t lpf_mode) +{ + int ret; + int ret_lock; + + ret_lock = nxmutex_lock(&priv->dev_lock); + if (ret_lock < 0) + { + return ret_lock; + } + + sninfo("Configuring accelerometer range: %d (0x%02x)\n", + range, range << 4); + ret = qmi8658_modifyreg8(priv, QMI8658_REG_CTRL2, 0x30, + (range << 4)); + if (ret < 0) + { + snerr(" Failed to configure accelerometer range\n"); + goto err_unlock; + } + + priv->acc_range = range; + + sninfo("Configuring accelerometer ODR: %d (0x%02x)\n", odr, odr); + ret = qmi8658_modifyreg8(priv, QMI8658_REG_CTRL2, 0x0f, odr); + if (ret < 0) + { + snerr(" Failed to configure accelerometer ODR\n"); + goto err_unlock; + } + + uint8_t ctrl2_odr_val; + qmi8658_readreg8(priv, QMI8658_REG_CTRL2, &ctrl2_odr_val); + sninfo("CTRL2 after ODR config: 0x%02x\n", ctrl2_odr_val); + + priv->acc_odr = odr; + + uint8_t ctrl2_val; + ret = qmi8658_readreg8(priv, QMI8658_REG_CTRL2, &ctrl2_val); + if (ret < 0) + { + snerr(" Failed to read CTRL2 register\n"); + } + else + { + sninfo("CTRL2 after accel config: 0x%02x\n", ctrl2_val); + } + + if (lpf_mode != QMI8658_LPF_OFF) + { + ret = qmi8658_modifyreg8(priv, QMI8658_REG_CTRL5, + QMI8658_ACCEL_LPF_MASK, (lpf_mode << 1)); + if (ret < 0) + { + snerr(" Failed to configure accelerometer LPF\n"); + goto err_unlock; + } + } + + priv->acc_lpf = lpf_mode; + ret = OK; + +err_unlock: + nxmutex_unlock(&priv->dev_lock); + return ret; +} + +/**************************************************************************** + * Name: qmi8658_config_gyroscope + * + * Description: + * Configure gyroscope settings (range, ODR, low-pass filter) + * + ****************************************************************************/ + +static int qmi8658_config_gyroscope(FAR struct qmi8658_dev_s *priv, + uint8_t range, uint8_t odr, + uint8_t lpf_mode) +{ + int ret; + int ret_lock; + + ret_lock = nxmutex_lock(&priv->dev_lock); + if (ret_lock < 0) + { + return ret_lock; + } + + ret = qmi8658_modifyreg8(priv, QMI8658_REG_CTRL3, 0x30, + (range << 4)); + if (ret < 0) + { + snerr(" Failed to configure gyroscope range\n"); + goto err_unlock; + } + + priv->gyro_range = range; + + ret = qmi8658_modifyreg8(priv, QMI8658_REG_CTRL3, 0x0f, odr); + if (ret < 0) + { + snerr(" Failed to configure gyroscope ODR\n"); + goto err_unlock; + } + + priv->gyro_odr = odr; + + if (lpf_mode != QMI8658_LPF_OFF) + { + ret = qmi8658_modifyreg8(priv, QMI8658_REG_CTRL5, + QMI8658_GYRO_LPF_MASK, (lpf_mode << 5)); + if (ret < 0) + { + snerr(" Failed to configure gyroscope LPF\n"); + goto err_unlock; + } + } + + priv->gyro_lpf = lpf_mode; + ret = OK; + +err_unlock: + nxmutex_unlock(&priv->dev_lock); + return ret; +} + +/**************************************************************************** + * Name: qmi8658_set_accelerometer + * + * Description: + * Enable or disable the accelerometer + * + ****************************************************************************/ + +static int qmi8658_set_accelerometer(FAR struct qmi8658_dev_s *priv, + bool enable) +{ + int ret; + int ret_lock; + + ret_lock = nxmutex_lock(&priv->dev_lock); + if (ret_lock < 0) + { + return ret_lock; + } + + if (enable) + { + uint8_t ctrl7_val; + + ret = qmi8658_readreg8(priv, QMI8658_REG_CTRL7, &ctrl7_val); + if (ret < 0) + { + snerr(" Failed to read CTRL7 register\n"); + goto err_unlock; + } + + sninfo("CTRL7 before enable: 0x%02x\n", ctrl7_val); + + ret = qmi8658_modifyreg8(priv, QMI8658_REG_CTRL7, 0, + QMI8658_CTRL7_ACC_EN); + if (ret < 0) + { + snerr(" Failed to enable accelerometer\n"); + goto err_unlock; + } + + ret = qmi8658_readreg8(priv, QMI8658_REG_CTRL7, &ctrl7_val); + if (ret < 0) + { + snerr(" Failed to read CTRL7 register after enable\n"); + goto err_unlock; + } + + sninfo("CTRL7 after enable: 0x%02x\n", ctrl7_val); + + priv->accel_enabled = true; + sninfo("Accelerometer enabled\n"); + } + else + { + ret = qmi8658_modifyreg8(priv, QMI8658_REG_CTRL7, + QMI8658_CTRL7_ACC_EN, 0); + if (ret < 0) + { + snerr(" Failed to disable accelerometer\n"); + goto err_unlock; + } + + priv->accel_enabled = false; + sninfo("Accelerometer disabled\n"); + } + + ret = OK; + +err_unlock: + nxmutex_unlock(&priv->dev_lock); + return ret; +} + +/**************************************************************************** + * Name: qmi8658_set_gyroscope + * + * Description: + * Enable or disable the gyroscope + * + ****************************************************************************/ + +static int qmi8658_set_gyroscope(FAR struct qmi8658_dev_s *priv, bool enable) +{ + int ret; + int ret_lock; + + ret_lock = nxmutex_lock(&priv->dev_lock); + if (ret_lock < 0) + { + return ret_lock; + } + + if (enable) + { + ret = qmi8658_modifyreg8(priv, QMI8658_REG_CTRL7, 0, + QMI8658_CTRL7_GYRO_EN); + if (ret < 0) + { + snerr(" Failed to enable gyroscope\n"); + goto err_unlock; + } + + priv->gyro_enabled = true; + sninfo("Gyroscope enabled\n"); + } + else + { + ret = qmi8658_modifyreg8(priv, QMI8658_REG_CTRL7, + QMI8658_CTRL7_GYRO_EN, 0); + if (ret < 0) + { + snerr(" Failed to disable gyroscope\n"); + goto err_unlock; + } + + priv->gyro_enabled = false; + sninfo("Gyroscope disabled\n"); + } + + ret = OK; + +err_unlock: + nxmutex_unlock(&priv->dev_lock); + return ret; +} + +/**************************************************************************** + * Name: qmi8658_set_sample_mode + * + * Description: + * Enable or disable synchronous sampling mode. When disabled, the sensor + * operates in asynchronous mode where accelerometer and gyroscope data + * are sampled independently. + * + ****************************************************************************/ + +static int qmi8658_set_sample_mode(FAR struct qmi8658_dev_s *priv, bool sync) +{ + int ret; + int ret_lock; + + ret_lock = nxmutex_lock(&priv->dev_lock); + if (ret_lock < 0) + { + return ret_lock; + } + + if (sync) + { + ret = qmi8658_modifyreg8(priv, QMI8658_REG_CTRL7, 0, + QMI8658_CTRL7_SYNC_MODE); + if (ret < 0) + { + snerr(" Failed to enable synchronous mode\n"); + goto err_unlock; + } + + priv->sample_mode = QMI8658_SYNC_MODE; + sninfo("Synchronous mode enabled\n"); + } + else + { + ret = qmi8658_modifyreg8(priv, QMI8658_REG_CTRL7, + QMI8658_CTRL7_SYNC_MODE, 0); + if (ret < 0) + { + snerr(" Failed to disable synchronous mode\n"); + goto err_unlock; + } + + priv->sample_mode = QMI8658_ASYNC_MODE; + sninfo("Synchronous mode disabled\n"); + } + + ret = OK; + +err_unlock: + nxmutex_unlock(&priv->dev_lock); + return ret; +} + +/**************************************************************************** + * Name: qmi8658_read_accel + * + * Description: + * Read accelerometer data (16-bit values) using batch reading + * + ****************************************************************************/ + +static int qmi8658_read_accel(FAR struct qmi8658_dev_s *priv, + FAR int16_t *data) +{ + uint8_t buffer[6]; + int ret; + int ret_lock; + + ret_lock = nxmutex_lock(&priv->dev_lock); + if (ret_lock < 0) + { + return ret_lock; + } + + ret = qmi8658_readregs(priv, QMI8658_REG_AX_L, buffer, 6); + if (ret < 0) + { + snerr(" Failed to read accelerometer data registers\n"); + goto err_unlock; + } + + data[0] = (int16_t)((buffer[1] << 8) | buffer[0]); + data[1] = (int16_t)((buffer[3] << 8) | buffer[2]); + data[2] = (int16_t)((buffer[5] << 8) | buffer[4]); + + sninfo("Accel: X=%d, Y=%d, Z=%d (raw: %02x%02x, %02x%02x, %02x%02x)\n", + data[0], data[1], data[2], + buffer[1], buffer[0], buffer[3], buffer[2], + buffer[5], buffer[4]); + + ret = OK; + +err_unlock: + nxmutex_unlock(&priv->dev_lock); + return ret; +} + +/**************************************************************************** + * Name: qmi8658_read_gyro + * + * Description: + * Read gyroscope data (16-bit values) using batch reading + * + ****************************************************************************/ + +static int qmi8658_read_gyro(FAR struct qmi8658_dev_s *priv, + FAR int16_t *data) +{ + uint8_t buffer[6]; + int ret; + int ret_lock; + + ret_lock = nxmutex_lock(&priv->dev_lock); + if (ret_lock < 0) + { + return ret_lock; + } + + ret = qmi8658_readregs(priv, QMI8658_REG_GX_L, buffer, 6); + if (ret < 0) + { + snerr(" Failed to read gyroscope data registers\n"); + goto err_unlock; + } + + /* Convert to 16-bit signed values */ + + data[0] = (int16_t)((buffer[1] << 8) | buffer[0]); + data[1] = (int16_t)((buffer[3] << 8) | buffer[2]); + data[2] = (int16_t)((buffer[5] << 8) | buffer[4]); + + ret = OK; + +err_unlock: + nxmutex_unlock(&priv->dev_lock); + return ret; +} + +/**************************************************************************** + * Name: qmi8658_read_temp + * + * Description: + * Read temperature data from QMI8658 sensor + * + * Input Parameters: + * priv - Pointer to QMI8658 device structure + * temperature - Pointer to temperature data (16-bit signed) + * + * Returned Value: + * OK on success; ERROR on failure + * + * Notes: + * Temperature sensor is always enabled. Use QMI8658_TEMP_SCALE (256.0f) + * to convert raw value to degrees Celsius. + * + ****************************************************************************/ + +static int qmi8658_read_temp(FAR struct qmi8658_dev_s *priv, + FAR int16_t *temperature) +{ + uint8_t buffer[2]; + int ret; + int ret_lock; + + ret_lock = nxmutex_lock(&priv->dev_lock); + if (ret_lock < 0) + { + return ret_lock; + } + + /* Read temperature registers (0x33-0x34) */ + + ret = qmi8658_readregs(priv, QMI8658_REG_TEMPERATURE_L, buffer, 2); + if (ret < 0) + { + snerr(" Failed to read temperature data registers\n"); + goto err_unlock; + } + + /* Convert to 16-bit signed value (little-endian) */ + + *temperature = (int16_t)((buffer[1] << 8) | buffer[0]); + + sninfo("Temperature: %d (raw value)\n", *temperature); + ret = OK; + +err_unlock: + nxmutex_unlock(&priv->dev_lock); + return ret; +} + +/**************************************************************************** + * Name: qmi8658_initialize + * + * Description: + * Initialize the QMI8658 sensor with default configuration + * + ****************************************************************************/ + +static int qmi8658_initialize(FAR struct qmi8658_dev_s *priv) +{ + int ret; + + /* Initialize mutex */ + + nxmutex_init(&priv->dev_lock); + + ret = qmi8658_checkid(priv); + if (ret < 0) + { + return ret; + } + + ret = qmi8658_reset(priv); + if (ret < 0) + { + return ret; + } + + ret = qmi8658_config_accelerometer(priv, QMI8658_ACC_FS_4G, + QMI8658_ACC_ODR_1000Hz, + QMI8658_LPF_MODE_0); + if (ret < 0) + { + return ret; + } + + ret = qmi8658_config_gyroscope(priv, QMI8658_GYRO_FS_1024DPS, + QMI8658_GYRO_ODR_896_8Hz, + QMI8658_LPF_MODE_0); + if (ret < 0) + { + return ret; + } + + ret = qmi8658_set_accelerometer(priv, true); + if (ret < 0) + { + return ret; + } + + ret = qmi8658_set_gyroscope(priv, true); + if (ret < 0) + { + return ret; + } + + ret = qmi8658_set_sample_mode(priv, true); + if (ret < 0) + { + return ret; + } + + sninfo("QMI8658 initialization completed successfully\n"); + return OK; +} + +/**************************************************************************** + * Name: qmi8658_get_scale_factors + * + * Description: + * Get current scale factors for data conversion + * + ****************************************************************************/ + +static int qmi8658_get_scale_factors(FAR struct qmi8658_dev_s *priv, + FAR struct qmi8658_scale_factors_s *factors) +{ + if (!priv || !factors) + { + return -EINVAL; + } + + switch (priv->acc_range) + { + case QMI8658_ACC_FS_2G: + factors->acc_scale = QMI8658_ACC_SCALE_2G; + break; + case QMI8658_ACC_FS_4G: + factors->acc_scale = QMI8658_ACC_SCALE_4G; + break; + case QMI8658_ACC_FS_8G: + factors->acc_scale = QMI8658_ACC_SCALE_8G; + break; + case QMI8658_ACC_FS_16G: + factors->acc_scale = QMI8658_ACC_SCALE_16G; + break; + default: + factors->acc_scale = QMI8658_ACC_SCALE_2G; + break; + } + + switch (priv->gyro_range) + { + case QMI8658_GYRO_FS_16DPS: + factors->gyro_scale = QMI8658_GYRO_SCALE_16DPS; + break; + case QMI8658_GYRO_FS_32DPS: + factors->gyro_scale = QMI8658_GYRO_SCALE_32DPS; + break; + case QMI8658_GYRO_FS_64DPS: + factors->gyro_scale = QMI8658_GYRO_SCALE_64DPS; + break; + case QMI8658_GYRO_FS_128DPS: + factors->gyro_scale = QMI8658_GYRO_SCALE_128DPS; + break; + case QMI8658_GYRO_FS_256DPS: + factors->gyro_scale = QMI8658_GYRO_SCALE_256DPS; + break; + case QMI8658_GYRO_FS_512DPS: + factors->gyro_scale = QMI8658_GYRO_SCALE_512DPS; + break; + case QMI8658_GYRO_FS_1024DPS: + factors->gyro_scale = QMI8658_GYRO_SCALE_1024DPS; + break; + + default: + factors->gyro_scale = QMI8658_GYRO_SCALE_64DPS; + break; + } + + factors->temp_scale = QMI8658_TEMP_SCALE; + + return OK; +} + +/**************************************************************************** + * Name: qmi8658_read_imu + * + * Description: + * Read accelerometer and/or gyroscope data from the QMI8658 sensor. + * This function reads raw sensor data and applies appropriate scaling + * factors to convert to physical units. Temperature data is also + * read for sensor compensation. + * + * Input Parameters: + * dev - Pointer to the QMI8658 device structure + * accel - Pointer to accelerometer data structure (NULL if not needed) + * gyro - Pointer to gyroscope data structure (NULL if not needed) + * + * Returned Value: + * OK on success; negated errno on failure + * + * -EINVAL - Invalid device pointer + * -EIO - I2C communication failure + * + * Assumptions: + * The device must be properly initialized before calling this function. + * Either accel or gyro (or both) must be non-NULL. + * + ****************************************************************************/ + +static int qmi8658_read_imu(FAR struct qmi8658_dev_s *dev, + FAR struct sensor_accel *accel, + FAR struct sensor_gyro *gyro) +{ + struct qmi8658_scale_factors_s scale_factors; + int16_t accel_data[3]; + int16_t gyro_data[3]; + int16_t temperature; + int ret; + + if (!dev) + { + return -EINVAL; + } + + ret = qmi8658_get_scale_factors(dev, &scale_factors); + if (ret < 0) + { + return ret; + } + + /* Read temperature data (shared by both accel and gyro) */ + + ret = qmi8658_read_temp(dev, &temperature); + if (ret < 0) + { + return ret; + } + + if (accel) + { + /* Read accelerometer data */ + + ret = qmi8658_read_accel(dev, accel_data); + if (ret < 0) + { + return ret; + } + + accel->x = (float)accel_data[0] / scale_factors.acc_scale; + accel->y = (float)accel_data[1] / scale_factors.acc_scale; + accel->z = (float)accel_data[2] / scale_factors.acc_scale; + accel->temperature = (float)temperature / scale_factors.temp_scale; + accel->timestamp = sensor_get_timestamp(); + } + + if (gyro) + { + /* Read gyroscope data */ + + ret = qmi8658_read_gyro(dev, gyro_data); + if (ret < 0) + { + return ret; + } + + gyro->x = (float)gyro_data[0] / scale_factors.gyro_scale; + gyro->y = (float)gyro_data[1] / scale_factors.gyro_scale; + gyro->z = (float)gyro_data[2] / scale_factors.gyro_scale; + gyro->temperature = (float)temperature / scale_factors.temp_scale; + gyro->timestamp = sensor_get_timestamp(); + } + + return OK; +} + +/**************************************************************************** + * Name: qmi8658_activate + * + * Description: + * Enable or disable the sensor and manage polling work if enabled. + * When enabling a sensor in polling mode, this function starts a + * periodic work queue job to read sensor data at the configured + * interval. When disabling, it cancels any pending work. + * + * Input Parameters: + * lower - Pointer to the sensor lower-half structure + * filep - Pointer to the file structure (unused) + * enable - true to enable sensor, false to disable + * + * Returned Value: + * OK on success; negated errno on failure + * + * -EINVAL - Invalid pointer parameters + * -EIO - Work queue operation failed + * + * Assumptions: + * This function is called from the sensor framework when applications + * open/close the sensor device. + * + * In polling mode, the work queue must be available for scheduling + * periodic sensor reads. + * + ****************************************************************************/ + +static int qmi8658_activate(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, bool enable) +{ + FAR struct qmi8658_sensor_s *priv = (FAR struct qmi8658_sensor_s *)lower; + FAR struct qmi8658_uorb_dev_s *dev = + (FAR struct qmi8658_uorb_dev_s *)priv->dev; + int ret = OK; + + if (!priv || !dev) + { + return -EINVAL; + } + + int sensor_idx = priv - &dev->priv[0]; + + priv->enabled = enable; + +#ifdef CONFIG_SENSORS_QMI8658_POLL + if (enable) + { + if (priv->interval > 0) + { + uint32_t delay = priv->interval / USEC_PER_TICK; + if (sensor_idx == QMI8658_ACCEL_IDX) + { + ret = work_queue(HPWORK, &priv->work, qmi8658_accel_worker, + priv, delay); + } + else if (sensor_idx == QMI8658_GYRO_IDX) + { + ret = work_queue(HPWORK, &priv->work, qmi8658_gyro_worker, + priv, delay); + } + } + } + else + { + work_cancel(HPWORK, &priv->work); + } +#endif + + return ret; +} + +#ifdef CONFIG_SENSORS_QMI8658_POLL +/**************************************************************************** + * Name: qmi8658_set_interval + * + * Description: + * Set the polling interval for sensor data acquisition. + * This function updates the interval at which sensor data will be + * read when polling mode is enabled. The interval is specified + * in microseconds. + * + * Input Parameters: + * lower - Pointer to the sensor lower-half structure + * filep - Pointer to the file structure (unused) + * period_us - Pointer to the polling period in microseconds + * + * Returned Value: + * OK on success; negated errno on failure + * + * -EINVAL - Invalid pointer parameters + * + * Assumptions: + * This function is called from the sensor framework when applications + * set the sensor polling interval via IOCTL or similar interface. + * + * The new interval takes effect immediately for the next polling cycle. + * + * interval_us should be greater than 0 for meaningful operation. + * + * CONFIG_SENSORS_QMI8658_POLL must be enabled for this function + * to be available. + * + ****************************************************************************/ + +static int qmi8658_set_interval(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR uint32_t *period_us) +{ + FAR struct qmi8658_sensor_s *priv = (FAR struct qmi8658_sensor_s *)lower; + + if (!priv || !period_us) + { + return -EINVAL; + } + + priv->interval = *period_us; + + return OK; +} +#endif + +#ifndef CONFIG_SENSORS_QMI8658_POLL +/**************************************************************************** + * Name: qmi8658_fetch + * + * Description: + * Fetch sensor data when polling mode is disabled. + * This function reads accelerometer or gyroscope data from the + * QMI8658 sensor and returns it to the caller. The function + * determines which sensor data to read based on the sensor index. + * + * Input Parameters: + * lower - Pointer to the sensor lower-half structure + * filep - Pointer to the file structure (unused) + * buffer - Buffer to store the sensor data + * buflen - Size of the buffer in bytes + * + * Returned Value: + * Number of bytes read on success; negated errno on failure + * + * -EINVAL - Invalid pointer parameters or insufficient buffer size + * -EIO - I2C communication failure + * + * For accelerometer: sizeof(struct sensor_accel) bytes + * For gyroscope: sizeof(struct sensor_gyro) bytes + * + * Assumptions: + * This function is called when polling mode is not enabled. + * The application is responsible for providing a sufficiently + * large buffer to hold the sensor data structure. + * + * The function detects the sensor type based on the sensor index + * and reads the appropriate data (accelerometer or gyroscope). + * + * This is a blocking read operation that communicates with the + * hardware via I2C. + * + * CONFIG_SENSORS_QMI8658_POLL must be disabled for this function + * to be available. + * + ****************************************************************************/ + +static int qmi8658_fetch(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR char *buffer, size_t buflen) +{ + FAR struct qmi8658_sensor_s *priv = (FAR struct qmi8658_sensor_s *)lower; + FAR struct qmi8658_uorb_dev_s *dev = + (FAR struct qmi8658_uorb_dev_s *)priv->dev; + int ret; + + if (!priv || !dev || !buffer) + { + return -EINVAL; + } + + int sensor_idx = priv - &dev->priv[0]; + + if (sensor_idx == QMI8658_ACCEL_IDX) + { + struct sensor_accel accel; + ret = qmi8658_read_imu(&dev->base, &accel, NULL); + if (ret < 0) + { + return ret; + } + + if (buflen < sizeof(accel)) + { + return -EINVAL; + } + + memcpy(buffer, &accel, sizeof(accel)); + return sizeof(accel); + } + else if (sensor_idx == QMI8658_GYRO_IDX) + { + struct sensor_gyro gyro; + ret = qmi8658_read_imu(&dev->base, NULL, &gyro); + if (ret < 0) + { + return ret; + } + + if (buflen < sizeof(gyro)) + { + return -EINVAL; + } + + memcpy(buffer, &gyro, sizeof(gyro)); + return sizeof(gyro); + } + + return -EINVAL; +} +#endif + +#ifdef CONFIG_SENSORS_QMI8658_POLL +/**************************************************************************** + * Name: qmi8658_accel_worker + * + * Description: + * Worker function for accelerometer data polling. This function is + * scheduled by the work queue to periodically read accelerometer + * data from the QMI8658 sensor and push it to the sensor framework. + * + * Input Parameters: + * arg - Pointer to the qmi8658_sensor_s structure + * + * Returned Value: + * None + * + * Assumptions: + * The sensor is enabled and has a valid polling interval. + * The work queue is available for rescheduling. + * + ****************************************************************************/ + +static void qmi8658_accel_worker(FAR void *arg) +{ + FAR struct qmi8658_sensor_s *priv = (FAR struct qmi8658_sensor_s *)arg; + FAR struct qmi8658_uorb_dev_s *dev = + (FAR struct qmi8658_uorb_dev_s *)priv->dev; + struct sensor_accel accel; + int ret; + + DEBUGASSERT(priv != NULL); + DEBUGASSERT(dev != NULL); + + if (priv->enabled && priv->interval > 0) + { + uint32_t delay = priv->interval / USEC_PER_TICK; + work_queue(HPWORK, &priv->work, qmi8658_accel_worker, priv, delay); + } + + ret = qmi8658_read_imu(&dev->base, &accel, NULL); + if (ret < 0) + { + return; + } + + if (priv->lower.push_event && priv->lower.priv) + { + priv->lower.push_event(priv->lower.priv, &accel, sizeof(accel)); + } +} + +/**************************************************************************** + * Name: qmi8658_gyro_worker + * + * Description: + * Worker function for gyroscope data polling. This function is + * scheduled by the work queue to periodically read gyroscope + * data from the QMI8658 sensor and push it to the sensor framework. + * + * Input Parameters: + * arg - Pointer to the qmi8658_sensor_s structure + * + * Returned Value: + * None + * + * Assumptions: + * The sensor is enabled and has a valid polling interval. + * The work queue is available for rescheduling. + * + ****************************************************************************/ + +static void qmi8658_gyro_worker(FAR void *arg) +{ + FAR struct qmi8658_sensor_s *priv = (FAR struct qmi8658_sensor_s *)arg; + FAR struct qmi8658_uorb_dev_s *dev = + (FAR struct qmi8658_uorb_dev_s *)priv->dev; + struct sensor_gyro gyro; + int ret; + + DEBUGASSERT(priv != NULL); + DEBUGASSERT(dev != NULL); + + if (priv->enabled && priv->interval > 0) + { + uint32_t delay = priv->interval / USEC_PER_TICK; + work_queue(HPWORK, &priv->work, qmi8658_gyro_worker, priv, delay); + } + + ret = qmi8658_read_imu(&dev->base, NULL, &gyro); + if (ret < 0) + { + return; + } + + if (priv->lower.push_event && priv->lower.priv) + { + priv->lower.push_event(priv->lower.priv, &gyro, sizeof(gyro)); + } +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: qmi8658_uorb_register + * + * Description: + * Register the QMI8658 IMU sensor device with the NuttX sensor + * framework. This function initializes the device, sets up the + * accelerometer and gyroscope sensors, and registers them with + * the sensor subsystem. + * + * Input Parameters: + * devno - Device number for sensor registration + * i2c - Pointer to the I2C master interface + * addr - I2C slave address of the QMI8658 device + * + * Returned Value: + * OK on success; negated errno on failure + * + * -EINVAL - Invalid I2C pointer + * -ENOMEM - Memory allocation failure + * -EIO - Device initialization or registration failure + * + * Assumptions: + * The I2C bus is properly configured and available. + * The QMI8658 device is connected and powered. + * + ****************************************************************************/ + +int qmi8658_uorb_register(int devno, FAR struct i2c_master_s *i2c, + uint8_t addr) +{ + FAR struct qmi8658_uorb_dev_s *dev; + struct sensor_lowerhalf_s *lower; + struct qmi8658_scale_factors_s scale_factors; + int ret = OK; + int i; + + if (!i2c) + { + return -EINVAL; + } + + dev = (FAR struct qmi8658_uorb_dev_s *) + kmm_zalloc(sizeof(struct qmi8658_uorb_dev_s)); + if (!dev) + { + return -ENOMEM; + } + + dev->base.i2c = i2c; + dev->base.addr = addr; + dev->base.freq = CONFIG_QMI8658_I2C_FREQUENCY; + + for (i = 0; i < QMI8658_MAX_IDX; i++) + { + FAR struct qmi8658_sensor_s *sensor = &dev->priv[i]; + + sensor->dev = &dev->base; + sensor->enabled = false; +#ifdef CONFIG_SENSORS_QMI8658_POLL + sensor->interval = CONFIG_SENSORS_QMI8658_POLL_INTERVAL; + + memset(&sensor->work, 0, sizeof(sensor->work)); +#endif + + lower = &sensor->lower; + lower->type = (i == QMI8658_ACCEL_IDX) ? SENSOR_TYPE_ACCELEROMETER : + SENSOR_TYPE_GYROSCOPE; + lower->nbuffer = 2; + lower->ops = &g_sensor_ops; + + ret = qmi8658_get_scale_factors(&dev->base, &scale_factors); + if (ret < 0) + { + snerr("Failed to get scale factors: %d\n", ret); + goto errout; + } + + sensor->scale = (i == QMI8658_ACCEL_IDX) ? scale_factors.acc_scale : + scale_factors.gyro_scale; + } + + ret = qmi8658_initialize(&dev->base); + if (ret < 0) + { + snerr("Failed to initialize QMI8658: %d\n", ret); + goto errout; + } + + for (i = 0; i < QMI8658_MAX_IDX; i++) + { + FAR struct qmi8658_sensor_s *sensor = &dev->priv[i]; + FAR const char *devname; + + devname = (i == QMI8658_ACCEL_IDX) ? "qmi8658_accel" : "qmi8658_gyro"; + + ret = sensor_register(&sensor->lower, devno); + if (ret < 0) + { + snerr("Failed to register %s: %d\n", devname, ret); + goto errout; + } + } + + return ret; + +errout: + for (i = 0; i < QMI8658_MAX_IDX; i++) + { + if (dev->priv[i].lower.type != 0) + { + sensor_unregister(&dev->priv[i].lower, devno); + } + } + + kmm_free(dev); + + return ret; +} diff --git a/include/nuttx/sensors/qmi8658.h b/include/nuttx/sensors/qmi8658.h new file mode 100644 index 0000000000000..fe48210bd56c7 --- /dev/null +++ b/include/nuttx/sensors/qmi8658.h @@ -0,0 +1,132 @@ +/**************************************************************************** + * include/nuttx/sensors/qmi8658.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_QMI8658_H +#define __INCLUDE_NUTTX_SENSORS_QMI8658_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Accelerometer Full Scale Ranges */ +#define QMI8658_ACC_FS_2G (0x00) +#define QMI8658_ACC_FS_4G (0x01) +#define QMI8658_ACC_FS_8G (0x02) +#define QMI8658_ACC_FS_16G (0x03) + +/* Gyroscope Full Scale Ranges */ +#define QMI8658_GYRO_FS_16DPS (0x00) +#define QMI8658_GYRO_FS_32DPS (0x01) +#define QMI8658_GYRO_FS_64DPS (0x02) +#define QMI8658_GYRO_FS_128DPS (0x03) +#define QMI8658_GYRO_FS_256DPS (0x04) +#define QMI8658_GYRO_FS_512DPS (0x05) +#define QMI8658_GYRO_FS_1024DPS (0x06) + +/* ODR (Output Data Rate) Definitions */ + +/* Accelerometer ODR */ +#define QMI8658_ACC_ODR_1000Hz (0x03) +#define QMI8658_ACC_ODR_500Hz (0x04) +#define QMI8658_ACC_ODR_250Hz (0x05) +#define QMI8658_ACC_ODR_125Hz (0x06) +#define QMI8658_ACC_ODR_62_5Hz (0x07) +#define QMI8658_ACC_ODR_31_25Hz (0x08) +#define QMI8658_ACC_ODR_LOWPOWER_128Hz (0x0C) +#define QMI8658_ACC_ODR_LOWPOWER_21Hz (0x0D) +#define QMI8658_ACC_ODR_LOWPOWER_11Hz (0x0E) +#define QMI8658_ACC_ODR_LOWPOWER_3Hz (0x0F) + +/* Gyroscope ODR */ +#define QMI8658_GYRO_ODR_7174_4Hz (0x00) +#define QMI8658_GYRO_ODR_3587_2Hz (0x01) +#define QMI8658_GYRO_ODR_1793_6Hz (0x02) +#define QMI8658_GYRO_ODR_896_8Hz (0x03) +#define QMI8658_GYRO_ODR_448_4Hz (0x04) +#define QMI8658_GYRO_ODR_224_2Hz (0x05) +#define QMI8658_GYRO_ODR_112_1Hz (0x06) +#define QMI8658_GYRO_ODR_56_05Hz (0x07) +#define QMI8658_GYRO_ODR_28_025Hz (0x08) + +/* Scale Factors */ + +/* Accelerometer scale factors (LSB/g) */ +#define QMI8658_ACC_SCALE_2G (16384.0f) +#define QMI8658_ACC_SCALE_4G (8192.0f) +#define QMI8658_ACC_SCALE_8G (4096.0f) +#define QMI8658_ACC_SCALE_16G (2048.0f) + +/* Gyroscope scale factors (LSB/dps) */ +#define QMI8658_GYRO_SCALE_16DPS (2048.0f) +#define QMI8658_GYRO_SCALE_32DPS (1024.0f) +#define QMI8658_GYRO_SCALE_64DPS (512.0f) +#define QMI8658_GYRO_SCALE_128DPS (256.0f) +#define QMI8658_GYRO_SCALE_256DPS (128.0f) +#define QMI8658_GYRO_SCALE_512DPS (64.0f) +#define QMI8658_GYRO_SCALE_1024DPS (32.0f) + +/* Temperature Scale Factor (LSB/°C) */ +#define QMI8658_TEMP_SCALE (256.0f) + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#ifdef __cplusplus +extern "C" +{ +#endif + +/**************************************************************************** + * Name: qmi8658_uorb_register + * + * Description: + * Register the QMI8658 uORB sensor device + * + * Input Parameters: + * devno - Device number to use + * i2c - Pointer to the I2C master device + * addr - I2C address (use QMI8658_I2C_ADDR for default) + * + * Returned Value: + * Zero (OK) on success; a negated errno value on failure. + * + ****************************************************************************/ + +int qmi8658_uorb_register(int devno, FAR struct i2c_master_s *i2c, + uint8_t addr); + +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_NUTTX_SENSORS_QMI8658_H */