From c0d6d417e759e21109e1669f143d513eec2b4e0c Mon Sep 17 00:00:00 2001 From: Felipe Moura Date: Mon, 3 Aug 2026 19:01:01 -0300 Subject: [PATCH 1/2] drivers/sensors/mpu6050: deliver samples from the data ready interrupt fetch() timestamps a sample when the application asks for it, not when the device measured it, and reads accel and gyro separately so the two topics never share an instant. Add an optional push mode behind CONFIG_SENSORS_MPU6050_INT: the board supplies mpu6050_config_s::attach, the handler timestamps and defers to HPWORK, and the worker reads once and pushes both topics. The I2C read cannot run in the interrupt. The mode is chosen at build time, so fetch() is simply left out of the ops table and out of the build when the option is set: an instance uses one model or the other, never the mixture that made poll() unusable on l3gd20. A board that enables it without attach fails with -EINVAL. Also set CONFIG so the DLPF is on. Left at reset the gyroscope output is 8 kHz, not 1 kHz, so SMPLRT_DIV 9 gave 800 Hz rather than the documented 100 Hz; measured 833 Hz before and 101 Hz after. fetch() hid this since the application set the pace. Signed-off-by: Felipe Moura --- drivers/sensors/Kconfig | 14 +++ drivers/sensors/mpu6050_uorb.c | 167 +++++++++++++++++++++++++++++--- include/nuttx/sensors/mpu6050.h | 24 ++++- 3 files changed, 189 insertions(+), 16 deletions(-) diff --git a/drivers/sensors/Kconfig b/drivers/sensors/Kconfig index 6de33906550c8..1fb59146a5175 100644 --- a/drivers/sensors/Kconfig +++ b/drivers/sensors/Kconfig @@ -1573,6 +1573,20 @@ config SENSORS_MPU6050 ---help--- Enable uORB driver support for Invensense MPU6050 6-axis MotionTracker device over I2C. +if SENSORS_MPU6050 + +config SENSORS_MPU6050_INT + bool "Deliver samples from the data ready interrupt" + default n + select SCHED_HPWORK + ---help--- + Take samples from the MPU6050 INT pin instead of reading the device + on demand, which timestamps them when they were actually measured. + The board must wire that pin and supply mpu6050_config_s::attach; + registration fails with -EINVAL otherwise. + +endif # SENSORS_MPU6050 + config SENSORS_MPU9250 bool "Invensense MPU9250 Sensor support" default n diff --git a/drivers/sensors/mpu6050_uorb.c b/drivers/sensors/mpu6050_uorb.c index d416eda72bbe0..113677e02e101 100644 --- a/drivers/sensors/mpu6050_uorb.c +++ b/drivers/sensors/mpu6050_uorb.c @@ -39,6 +39,9 @@ #include #include #include +#ifdef CONFIG_SENSORS_MPU6050_INT +# include +#endif #ifdef CONFIG_SENSORS_MPU6050 @@ -111,6 +114,10 @@ struct mpu6050_uorb_dev_s struct mpu6050_dev_s base; struct mpu6050_sensor_s accel; struct mpu6050_sensor_s gyro; +#ifdef CONFIG_SENSORS_MPU6050_INT + struct work_s work; /* Bottom half: the I2C read cannot run in the ISR */ + uint64_t timestamp; /* When the sample became ready, taken in the ISR */ +#endif }; /**************************************************************************** @@ -119,22 +126,34 @@ struct mpu6050_uorb_dev_s static int mpu6050_activate(FAR struct sensor_lowerhalf_s *lower, FAR struct file *filep, bool enable); -static int mpu6050_fetch(FAR struct sensor_lowerhalf_s *lower, - FAR struct file *filep, - FAR char *buffer, size_t buflen); static int mpu6050_control(FAR struct sensor_lowerhalf_s *lower, FAR struct file *filep, int cmd, unsigned long arg); +#ifdef CONFIG_SENSORS_MPU6050_INT +static int mpu6050_interrupt(int irq, FAR void *context, FAR void *arg); +static void mpu6050_worker(FAR void *arg); +#else +static int mpu6050_fetch(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR char *buffer, size_t buflen); +#endif /**************************************************************************** * Private Data ****************************************************************************/ +/* With the INT pin wired the device pushes samples on its own, so fetch() + * is deliberately absent: a lower half implements one model or the other, + * never both. + */ + static const struct sensor_ops_s g_mpu6050_ops = { .activate = mpu6050_activate, - .fetch = mpu6050_fetch, .control = mpu6050_control, +#ifndef CONFIG_SENSORS_MPU6050_INT + .fetch = mpu6050_fetch, +#endif }; /**************************************************************************** @@ -227,6 +246,10 @@ static int mpu6050_activate(FAR struct sensor_lowerhalf_s *lower, { FAR struct mpu6050_sensor_s *priv = (FAR struct mpu6050_sensor_s *)lower; FAR struct mpu6050_dev_s *dev = priv->dev; +#ifdef CONFIG_SENSORS_MPU6050_INT + FAR struct mpu6050_uorb_dev_s *udev = (FAR struct mpu6050_uorb_dev_s *)dev; + bool other; +#endif int ret; ret = nxmutex_lock(&dev->dev_lock); @@ -235,14 +258,26 @@ static int mpu6050_activate(FAR struct sensor_lowerhalf_s *lower, return ret; } - if (enable) - { - ret = mpu6050_write_reg(dev, MPU6050_PWR_MGMT_1, 0x00); - } - else +#ifdef CONFIG_SENSORS_MPU6050_INT + + /* The data ready interrupt drives delivery here, so it stays enabled + * while either of the two sensors is still subscribed. + */ + + other = (priv == &udev->accel) ? udev->gyro.enabled : udev->accel.enabled; + if (enable || !other) { - ret = mpu6050_write_reg(dev, MPU6050_PWR_MGMT_1, 0x40); + ret = mpu6050_write_reg(dev, MPU6050_PWR_MGMT_1, + enable ? 0x00 : 0x40); + if (ret >= 0) + { + ret = mpu6050_write_reg(dev, MPU6050_INT_ENABLE, + enable ? 0x01 : 0x00); + } } +#else + ret = mpu6050_write_reg(dev, MPU6050_PWR_MGMT_1, enable ? 0x00 : 0x40); +#endif if (ret >= 0) { @@ -253,6 +288,85 @@ static int mpu6050_activate(FAR struct sensor_lowerhalf_s *lower, return ret; } +#ifdef CONFIG_SENSORS_MPU6050_INT + +/**************************************************************************** + * Name: mpu6050_interrupt + ****************************************************************************/ + +static int mpu6050_interrupt(int irq, FAR void *context, FAR void *arg) +{ + FAR struct mpu6050_uorb_dev_s *dev = arg; + + /* Timestamp here, where the sample really became ready. The read itself + * needs I2C, which may block, so it is deferred to the worker. + */ + + dev->timestamp = sensor_get_timestamp(); + return work_queue(HPWORK, &dev->work, mpu6050_worker, dev, 0); +} + +/**************************************************************************** + * Name: mpu6050_worker + ****************************************************************************/ + +static void mpu6050_worker(FAR void *arg) +{ + FAR struct mpu6050_uorb_dev_s *dev = arg; + struct sensor_accel accel; + struct sensor_gyro gyro; + uint8_t buf[14]; + float temp_c; + int ret; + + if (nxmutex_lock(&dev->base.dev_lock) < 0) + { + return; + } + + ret = mpu6050_read_regs(&dev->base, MPU6050_ACCEL_XOUT_H, buf, 14); + nxmutex_unlock(&dev->base.dev_lock); + + if (ret < 0) + { + snerr("ERROR: Failed to read measurement: %d\n", ret); + return; + } + + temp_c = ((float)(int16_t)((buf[6] << 8) | buf[7]) / 340.0f) + 36.53f; + + if (dev->accel.enabled) + { + accel.timestamp = dev->timestamp; + accel.x = (float)(int16_t)((buf[0] << 8) | buf[1]) * + dev->accel.scale; + accel.y = (float)(int16_t)((buf[2] << 8) | buf[3]) * + dev->accel.scale; + accel.z = (float)(int16_t)((buf[4] << 8) | buf[5]) * + dev->accel.scale; + accel.temperature = temp_c; + + dev->accel.lower.push_event(dev->accel.lower.priv, &accel, + sizeof(accel)); + } + + if (dev->gyro.enabled) + { + gyro.timestamp = dev->timestamp; + gyro.x = (float)(int16_t)((buf[8] << 8) | buf[9]) * + dev->gyro.scale; + gyro.y = (float)(int16_t)((buf[10] << 8) | buf[11]) * + dev->gyro.scale; + gyro.z = (float)(int16_t)((buf[12] << 8) | buf[13]) * + dev->gyro.scale; + gyro.temperature = temp_c; + + dev->gyro.lower.push_event(dev->gyro.lower.priv, &gyro, sizeof(gyro)); + } +} + +#else + /** * Name: mpu6050_fetch */ @@ -334,6 +448,8 @@ static int mpu6050_fetch(FAR struct sensor_lowerhalf_s *lower, return -EINVAL; } +#endif + /** * Name: mpu6050_control */ @@ -430,7 +546,8 @@ static int mpu6050_control(FAR struct sensor_lowerhalf_s *lower, * Name: mpu6050_register */ -int mpu6050_register(int devno, FAR struct i2c_master_s *i2c, uint8_t addr) +int mpu6050_register(int devno, FAR struct i2c_master_s *i2c, uint8_t addr, + FAR const struct mpu6050_config_s *config) { FAR struct mpu6050_uorb_dev_s *dev; FAR struct mpu6050_sensor_s *sensor; @@ -440,6 +557,18 @@ int mpu6050_register(int devno, FAR struct i2c_master_s *i2c, uint8_t addr) DEBUGASSERT(i2c != NULL); +#ifdef CONFIG_SENSORS_MPU6050_INT + /* The interrupt is the only source of samples in this build, so a board + * that did not wire it up is misconfigured rather than merely limited. + */ + + if (config == NULL || config->attach == NULL) + { + snerr("ERROR: CONFIG_SENSORS_MPU6050_INT needs config->attach\n"); + return -EINVAL; + } +#endif + dev = (FAR struct mpu6050_uorb_dev_s *) kmm_zalloc(sizeof(struct mpu6050_uorb_dev_s)); if (dev == NULL) @@ -470,6 +599,13 @@ int mpu6050_register(int devno, FAR struct i2c_master_s *i2c, uint8_t addr) /* Initialize MPU6050 */ mpu6050_write_reg(&dev->base, MPU6050_PWR_MGMT_1, 0x00); /* Wake up */ + + /* DLPF_CFG 1 puts the gyroscope output at 1 kHz, which is what the + * divider below assumes. Left at its 0 reset value that output is 8 kHz + * and the device samples at 800 Hz instead. + */ + + mpu6050_write_reg(&dev->base, MPU6050_CONFIG, 0x01); mpu6050_write_reg(&dev->base, MPU6050_SMPLRT_DIV, 9); /* 100 Hz */ mpu6050_write_reg(&dev->base, MPU6050_ACCEL_CONFIG, 0x00); /* ±2g */ mpu6050_write_reg(&dev->base, MPU6050_GYRO_CONFIG, 0x00); /* ±250°/s */ @@ -511,6 +647,15 @@ int mpu6050_register(int devno, FAR struct i2c_master_s *i2c, uint8_t addr) goto errout; } +#ifdef CONFIG_SENSORS_MPU6050_INT + ret = config->attach(config, mpu6050_interrupt, dev); + if (ret < 0) + { + syslog(LOG_ERR, "MPU6050: Failed to attach interrupt: %d\n", ret); + goto errout; + } +#endif + syslog(LOG_INFO, "MPU6050: uORB driver registered successfully\n"); return OK; diff --git a/include/nuttx/sensors/mpu6050.h b/include/nuttx/sensors/mpu6050.h index 1e1114f126867..13332d7ccdfe4 100644 --- a/include/nuttx/sensors/mpu6050.h +++ b/include/nuttx/sensors/mpu6050.h @@ -46,6 +46,18 @@ struct i2c_master_s; +/* Board specific configuration. With CONFIG_SENSORS_MPU6050_INT the board + * must supply attach(), which wires the MPU6050 INT pin to the given + * handler; the driver then delivers samples from that interrupt instead of + * reading the device on demand. + */ + +struct mpu6050_config_s +{ + CODE int (*attach)(FAR const struct mpu6050_config_s *config, + xcpt_t isr, FAR void *arg); +}; + /**************************************************************************** * Public Function Prototypes ****************************************************************************/ @@ -63,17 +75,19 @@ extern "C" * sensor framework. * * Input Parameters: - * devno - Device number for sensor registration (e.g. 0) - * i2c - Pointer to the I2C master interface - * addr - I2C slave address of the MPU6050 device + * devno - Device number for sensor registration (e.g. 0) + * i2c - Pointer to the I2C master interface + * addr - I2C slave address of the MPU6050 device + * config - Board configuration, required with CONFIG_SENSORS_MPU6050_INT + * and otherwise unused; may be NULL * * Returned Value: * Zero (OK) on success; a negated errno value on failure. * ****************************************************************************/ -int mpu6050_register(int devno, FAR struct i2c_master_s *i2c, - uint8_t addr); +int mpu6050_register(int devno, FAR struct i2c_master_s *i2c, uint8_t addr, + FAR const struct mpu6050_config_s *config); #ifdef __cplusplus } From 9482eb6308862d0fb4904ff8bcd047ef39fcdeb6 Mon Sep 17 00:00:00 2001 From: Felipe Moura Date: Mon, 3 Aug 2026 19:01:02 -0300 Subject: [PATCH 2/2] Documentation/mpu6050: document the data ready interrupt mode The registration example showed the three argument form, which no longer compiles, and fetch() as the only way samples are taken. Update it, add a section on the two acquisition modes and the attach() a board provides for the interrupt one, and state the 100 Hz sample rate. Signed-off-by: Felipe Moura --- .../drivers/special/sensors/mpu6050.rst | 48 +++++++++++++++++-- 1 file changed, 43 insertions(+), 5 deletions(-) diff --git a/Documentation/components/drivers/special/sensors/mpu6050.rst b/Documentation/components/drivers/special/sensors/mpu6050.rst index 6a422f5cc8c5a..4aa5ad8e0379d 100644 --- a/Documentation/components/drivers/special/sensors/mpu6050.rst +++ b/Documentation/components/drivers/special/sensors/mpu6050.rst @@ -33,24 +33,62 @@ Where ``n`` is the device number passed during device registration (e.g., 0). /* Example uORB sensor registration on I2C bus 0 at default address 0x68 */ - int err = mpu6050_register(0, i2c_master, MPU6050_ADDR_LOW); + int err = mpu6050_register(0, i2c_master, MPU6050_ADDR_LOW, NULL); if (err < 0) { syslog(LOG_ERR, "Failed to register MPU6050: %d\n", err); } +The device samples at 100 Hz. + +Acquisition Modes +================= + +The driver reads the device on demand by default: a sample is taken when the +application reads the topic, and is timestamped at that moment. + +With ``CONFIG_SENSORS_MPU6050_INT`` the device drives the acquisition instead, +through its data ready interrupt. Samples are then timestamped when they were +measured rather than when they were asked for, and both topics are published +from a single read, so accelerometer and gyroscope share one timestamp. This +requires the INT pin to be wired, and the board to pass a +``struct mpu6050_config_s`` whose ``attach`` member connects that pin to the +driver: + +.. code-block:: c + + static int board_mpu6050_attach(FAR const struct mpu6050_config_s *config, + xcpt_t isr, FAR void *arg) + { + /* Configure the GPIO for a rising edge and attach isr to it */ + } + + static const struct mpu6050_config_s g_mpu6050_config = + { + .attach = board_mpu6050_attach, + }; + + int err = mpu6050_register(0, i2c_master, MPU6050_ADDR_LOW, + &g_mpu6050_config); + +Registration fails with ``-EINVAL`` if the option is enabled and no ``attach`` +is supplied, since the interrupt is the only source of samples in that build. + Configuration Options ===================== - ``CONFIG_SENSORS_MPU6050`` - Enables uORB driver support for the InvenSense MPU6050 6-axis MotionTracker over I2C. +- ``CONFIG_SENSORS_MPU6050_INT`` - Takes samples from the data ready interrupt instead of reading the device on demand. Requires board support, see `Acquisition Modes`_. Supported Operations ==================== -The MPU6050 uORB driver supports standard sensor operations (`activate`, `fetch`, -`set_interval`, `batch`, and `control`). It acquires simultaneous 3-axis accelerometer -and 3-axis gyroscope samples, applies appropriate scale conversions, and publishes them -to their respective uORB topics. +The MPU6050 uORB driver supports standard sensor operations (`activate`, +`set_interval`, `batch`, and `control`), plus `fetch` when reading on demand. +An instance provides either `fetch` or interrupt driven delivery, never both. +It acquires simultaneous 3-axis accelerometer and 3-axis gyroscope samples, +applies appropriate scale conversions, and publishes them to their respective +uORB topics. Example Usage (`uorb_listener`) -------------------------------