Feature/mpu6050 drdy interrupt

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

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

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;
 

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
 }