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`) ------------------------------- 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 }