This is an automated email from the ASF dual-hosted git repository. acassis pushed a commit to branch master in repository https://gitbox.apache.org/repos/asf/nuttx.git
commit 6a12133160e37d393472939775e909b789ff780b Author: Felipe Moura <[email protected]> AuthorDate: Mon Aug 3 19:01:01 2026 -0300 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 <[email protected]> --- 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 6de33906550..1fb59146a51 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 d416eda72bb..113677e02e1 100644 --- a/drivers/sensors/mpu6050_uorb.c +++ b/drivers/sensors/mpu6050_uorb.c @@ -39,6 +39,9 @@ #include <nuttx/mutex.h> #include <nuttx/sensors/mpu6050.h> #include <nuttx/sensors/sensor.h> +#ifdef CONFIG_SENSORS_MPU6050_INT +# include <nuttx/wqueue.h> +#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 1e1114f1268..13332d7ccdf 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 }
