You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nuttx.apache.org by ac...@apache.org on 2023/04/10 14:25:49 UTC
[nuttx] branch master updated: drivers/sensors: add support of InvenSense MPU-9250 sensor
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
The following commit(s) were added to refs/heads/master by this push:
new 90ff76dfd6 drivers/sensors: add support of InvenSense MPU-9250 sensor
90ff76dfd6 is described below
commit 90ff76dfd671264019f2cd183d615e3beacbe0c6
Author: zouboan <ff...@feedforward.com.cn>
AuthorDate: Fri Apr 7 20:22:42 2023 +0800
drivers/sensors: add support of InvenSense MPU-9250 sensor
---
drivers/sensors/Kconfig | 104 ++
drivers/sensors/Make.defs | 4 +
drivers/sensors/mpu9250.c | 2045 +++++++++++++++++++++++++++++++++++++++
include/nuttx/sensors/mpu9250.h | 170 ++++
4 files changed, 2323 insertions(+)
diff --git a/drivers/sensors/Kconfig b/drivers/sensors/Kconfig
index f15da78928..8491ec8227 100644
--- a/drivers/sensors/Kconfig
+++ b/drivers/sensors/Kconfig
@@ -791,6 +791,110 @@ config MPU60X0_ACCEL_AFS_SEL
endif # SENSORS_MPU60X0
+config SENSORS_MPU9250
+ bool "Invensense MPU9250 Sensor support"
+ default n
+ ---help---
+ Enable driver support for Invensense MPU9250 MotionTracker device.
+
+if SENSORS_MPU9250
+
+choice
+ prompt "MPU9250 Interface"
+ default MPU9250_SPI
+
+config MPU9250_SPI
+ bool "MPU9250 SPI Interface"
+ select SPI
+ ---help---
+ Enables support for the SPI interface
+
+config MPU9250_I2C
+ bool "MPU9250 I2C Interface"
+ select I2C
+ ---help---
+ Enables support for the I2C interface
+
+endchoice
+
+config MPU9250_I2C_FREQ
+ int "MPU9250 I2C Frequency"
+ depends on MPU9250_I2C
+ default 400000
+
+config MPU9250_MEASURE_FREQ
+ int "Default data output rate in Hz"
+ default 1000
+ ---help---
+ Default data output rate in Hz
+
+config MPU9250_THREAD_STACKSIZE
+ int "Worker thread stack size"
+ default 1024
+ ---help---
+ The stack size for the worker thread
+
+config MPU9250_EXT_SYNC_SET
+ int "MPU9250 frame sync bit position"
+ default 0
+ ---help---
+ EXT_SYNC_SET[2..0]
+ EXT_SYNC_SET: frame sync bit position
+
+config MPU9250_DLPF_CFG
+ int "MPU9250 gyroscope and temperature low-pass filter setting"
+ default 1
+ ---help---
+ DLPF_CFG[2..0]
+ DLPF_CFG: digital low-pass filter bandwidth
+
+config MPU9250_ACCEL_FCHOICE_B
+ int "Inverted version of accel_fchoice"
+ default 0
+ ---help---
+ The data output rate of the DLPF filter block can be further
+ reduced by a factor of 1/(1+SMPLRT_DIV).
+ where SMPLRT_DIV is an 8-bit integer
+
+config MPU9250_A_DLPF_CFG
+ int "MPU9250 Accelerometer low pass filter setting"
+ default 1
+ ---help---
+ A_DLPF_CFG[2..0]
+ A_DLPF_CFG: Accelerometer digital low-pass filter bandwidth
+
+config MPU9250_GYRO_FS_SEL
+ int "MPU9250 Gyro FS_SEL"
+ default 2
+ ---help---
+ Sets the @fs_sel bit in GYRO_CONFIG to the value provided. Per
+ the datasheet, the meaning of @fs_sel is as follows:
+ GYRO_CONFIG(0x1b) : XG_ST YG_ST ZG_ST FS_SEL1 FS_SEL0 x x x
+ XG_ST, YG_ST, ZG_ST : self-test (unsupported in this driver)
+ 1 -> activate self-test on X, Y, and/or Z gyros
+ FS_SEL[10] : full-scale range select
+ 0 -> ± 250 deg/sec
+ 1 -> ± 500 deg/sec
+ 2 -> ± 1000 deg/sec
+ 3 -> ± 2000 deg/sec
+
+config MPU9250_ACCEL_AFS_SEL
+ int "MPU9250 Accelerometer AFS_SEL"
+ default 2
+ ---help---
+ Sets the @afs_sel bit in ACCEL_CONFIG to the value provided. Per
+ the datasheet, the meaning of @afs_sel is as follows:
+ ACCEL_CONFIG(0x1c) : XA_ST YA_ST ZA_ST AFS_SEL1 AFS_SEL0 x x x
+ XA_ST, YA_ST, ZA_ST : self-test (unsupported in this driver)
+ 1 -> activate self-test on X, Y, and/or Z accelerometers
+ AFS_SEL[10] : full-scale range select
+ 0 -> ± 2 g
+ 1 -> ± 4 g
+ 2 -> ± 8 g
+ 3 -> ± 16 g
+
+endif # SENSORS_MPU9250
+
config SENSORS_MAX44009
bool "Maxim MAX44009 ALS sensor"
default n
diff --git a/drivers/sensors/Make.defs b/drivers/sensors/Make.defs
index 593f32f783..18e2545be2 100644
--- a/drivers/sensors/Make.defs
+++ b/drivers/sensors/Make.defs
@@ -302,6 +302,10 @@ ifeq ($(CONFIG_SENSORS_MPU60X0),y)
CSRCS += mpu60x0.c
endif
+ifeq ($(CONFIG_SENSORS_MPU9250),y)
+ CSRCS += mpu9250.c
+endif
+
# Quadrature encoder upper half
ifeq ($(CONFIG_SENSORS_QENCODER),y)
diff --git a/drivers/sensors/mpu9250.c b/drivers/sensors/mpu9250.c
new file mode 100644
index 0000000000..d6855fe267
--- /dev/null
+++ b/drivers/sensors/mpu9250.c
@@ -0,0 +1,2045 @@
+/****************************************************************************
+ * drivers/sensors/mpu9250.c
+ *
+ * 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 <nuttx/config.h>
+
+#include <errno.h>
+#include <math.h>
+#include <stdio.h>
+#include <debug.h>
+#include <string.h>
+#include <limits.h>
+#include <nuttx/mutex.h>
+#include <nuttx/signal.h>
+
+#include <nuttx/compiler.h>
+#include <nuttx/kmalloc.h>
+#include <nuttx/kthread.h>
+
+#ifdef CONFIG_MPU9250_SPI
+#include <nuttx/spi/spi.h>
+#else
+#include <nuttx/i2c/i2c_master.h>
+#endif
+#include <nuttx/fs/fs.h>
+#include <nuttx/sensors/mpu9250.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define MPU9250_AKM_DEV_ID 0x48 /* Magnetometer device ID */
+#define MIN(x, y) (x) > (y) ? (y) : (x)
+
+/* 16bit mode: 0.15uTesla/LSB, 100 uTesla == 1 Gauss */
+
+#define MAG_RAW_TO_GAUSS (0.15f / 100.0f)
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+enum mpu9250_idx_e
+{
+ MPU9250_ACCEL_IDX = 0,
+ MPU9250_GYRO_IDX,
+ MPU9250_MAG_IDX,
+ MPU9250_MAX_IDX
+};
+
+enum mpu9250_regaddr_e
+{
+ SELF_TEST_G_X = 0x00,
+ SELF_TEST_G_Y = 0x01,
+ SELF_TEST_G_Z = 0x02,
+
+ SELF_TEST_A_X = 0x0d,
+ SELF_TEST_A_Y = 0x0e,
+ SELF_TEST_A_Z = 0x0f,
+
+ XG_OFFSETH = 0x13,
+ XG_OFFSETL = 0x14,
+ YG_OFFSETH = 0x15,
+ YG_OFFSETL = 0x16,
+ ZG_OFFSETH = 0x17,
+ ZG_OFFSETL = 0x18,
+
+ SMPLRT_DIV = 0x19,
+
+ /* _SHIFT : number of empty bits to the right of the field
+ * _WIDTH : width of the field, in bits
+ *
+ * single-bit fields don't have _SHIFT or _mask
+ */
+
+ MPU9250_CONFIG = 0x1a,
+ CONFIG_EXT_SYNC_SET_SHIFT = 3,
+ CONFIG_EXT_SYNC_SET_WIDTH = 3,
+ CONFIG_DLPF_CFG_SHIFT = 0,
+ CONFIG_DLPF_CFG_WIDTH = 3,
+
+ GYRO_CONFIG = 0x1b,
+ GYRO_CONFIG_XG_ST = BIT(7),
+ GYRO_CONFIG_YG_ST = BIT(6),
+ GYRO_CONFIG_ZG_ST = BIT(5),
+ GYRO_CONFIG_FS_SEL_SHIFT = 3,
+ GYRO_CONFIG_FS_SEL_WIDTH = 2,
+
+ ACCEL_CONFIG = 0x1c,
+ ACCEL_CONFIG_XA_ST = BIT(7),
+ ACCEL_CONFIG_YA_ST = BIT(6),
+ ACCEL_CONFIG_ZA_ST = BIT(5),
+ ACCEL_CONFIG_AFS_SEL_SHIFT = 3,
+ ACCEL_CONFIG_AFS_SEL_WIDTH = 2,
+
+ ACCEL_CONFIG2 = 0x1d,
+ ACCEL_CONFIG2_FCHOICE_B = BIT(3),
+ CONFIG2_A_DLPF_CFG_SHIFT = 0,
+ CONFIG2_A_DLPF_CFG_WIDTH = 3,
+ LPACCEL_ODR = 0x1e,
+ WOM_THR = 0x1f,
+
+ FIFO_EN = 0x23,
+ BITS_FIFO_ENABLE_TEMP_OUT = BIT(7),
+ BITS_FIFO_ENABLE_GYRO_XOUT = BIT(6),
+ BITS_FIFO_ENABLE_GYRO_YOUT = BIT(5),
+ BITS_FIFO_ENABLE_GYRO_ZOUT = BIT(4),
+ BITS_FIFO_ENABLE_ACCEL = BIT(3),
+ BITS_FIFO_ENABLE_SLV2 = BIT(2),
+ BITS_FIFO_ENABLE_SLV1 = BIT(1),
+ BITS_FIFO_ENABLE_SLV0 = BIT(0),
+
+ I2C_MST_CTRL = 0x24,
+ I2C_SLV0_ADDR = 0x25,
+ I2C_SLV0_REG = 0x26,
+ I2C_SLV0_CTRL = 0x27,
+ BITS_I2C_SLV0_EN = BIT(7),
+ BITS_I2C_SLV0_READ_8BYTES = BIT(3),
+ I2C_SLV1_ADDR = 0x28,
+ I2C_SLV1_REG = 0x29,
+ I2C_SLV1_CTRL = 0x2a,
+ BITS_I2C_SLV1_EN = BIT(7),
+ I2C_SLV2_ADDR = 0x2b,
+ I2C_SLV2_REG = 0x2c,
+ I2C_SLV2_CTRL = 0x2d,
+ BITS_I2C_SLV2_EN = BIT(7),
+ I2C_SLV3_ADDR = 0x2e,
+ I2C_SLV3_REG = 0x2f,
+ I2C_SLV3_CTRL = 0x30,
+ I2C_SLV4_ADDR = 0x31,
+ I2C_SLV4_REG = 0x32,
+ I2C_SLV4_DO = 0x33,
+ I2C_SLV4_CTRL = 0x34,
+ BITS_I2C_SLV4_EN = BIT(7),
+ BITS_I2C_SLV4_DONE = BIT(6),
+ I2C_SLV4_DI = 0x35, /* RO */
+ I2C_MST_STATUS = 0x36, /* RO */
+
+ INT_PIN_CFG = 0x37,
+ INT_PIN_CFG_INT_ACTL = BIT(7),
+ INT_PIN_CFG_INT_OPEN = BIT(6),
+ INT_PIN_CFG_LATCH_INT_EN = BIT(5),
+ INT_PIN_CFG_INT_RD_CLEAR = BIT(4),
+ INT_PIN_CFG_FSYNC_INT_LEVEL = BIT(3),
+ INT_PIN_CFG_FSYNC_INT_EN = BIT(2),
+ INT_PIN_CFG_I2C_BYPASS_EN = BIT(1),
+
+ INT_ENABLE = 0x38,
+ INT_STATUS = 0x3a, /* RO */
+
+ ACCEL_XOUT_H = 0x3b, /* RO */
+ ACCEL_XOUT_L = 0x3c, /* RO */
+ ACCEL_YOUT_H = 0x3d, /* RO */
+ ACCEL_YOUT_L = 0x3e, /* RO */
+ ACCEL_ZOUT_H = 0x3f, /* RO */
+ ACCEL_ZOUT_L = 0x40, /* RO */
+ TEMP_OUT_H = 0x41, /* RO */
+ TEMP_OUT_L = 0x42, /* RO */
+ GYRO_XOUT_H = 0x43, /* RO */
+ GYRO_XOUT_L = 0x44, /* RO */
+ GYRO_YOUT_H = 0x45, /* RO */
+ GYRO_YOUT_L = 0x46, /* RO */
+ GYRO_ZOUT_H = 0x47, /* RO */
+ GYRO_ZOUT_L = 0x48, /* RO */
+
+ EXT_SENS_DATA_00 = 0x49, /* RO */
+ EXT_SENS_DATA_01 = 0x4a, /* RO */
+ EXT_SENS_DATA_02 = 0x4b, /* RO */
+ EXT_SENS_DATA_03 = 0x4c, /* RO */
+ EXT_SENS_DATA_04 = 0x4d, /* RO */
+ EXT_SENS_DATA_05 = 0x4e, /* RO */
+ EXT_SENS_DATA_06 = 0x4f, /* RO */
+ EXT_SENS_DATA_07 = 0x50, /* RO */
+ EXT_SENS_DATA_08 = 0x51, /* RO */
+ EXT_SENS_DATA_09 = 0x52, /* RO */
+ EXT_SENS_DATA_10 = 0x53, /* RO */
+ EXT_SENS_DATA_11 = 0x54, /* RO */
+ EXT_SENS_DATA_12 = 0x55, /* RO */
+ EXT_SENS_DATA_13 = 0x56, /* RO */
+ EXT_SENS_DATA_14 = 0x57, /* RO */
+ EXT_SENS_DATA_15 = 0x58, /* RO */
+ EXT_SENS_DATA_16 = 0x59, /* RO */
+ EXT_SENS_DATA_17 = 0x5a, /* RO */
+ EXT_SENS_DATA_18 = 0x5b, /* RO */
+ EXT_SENS_DATA_19 = 0x5c, /* RO */
+ EXT_SENS_DATA_20 = 0x5d, /* RO */
+ EXT_SENS_DATA_21 = 0x5e, /* RO */
+ EXT_SENS_DATA_22 = 0x5f, /* RO */
+ EXT_SENS_DATA_23 = 0x60, /* RO */
+
+ I2C_SLV0_DO = 0x63,
+ I2C_SLV1_DO = 0x64,
+ I2C_SLV2_DO = 0x65,
+ I2C_SLV3_DO = 0x66,
+ I2C_MST_DELAY_CTRL = 0x67,
+ BITS_SLV4_DLY_EN = BIT(4),
+ BITS_SLV3_DLY_EN = BIT(3),
+ BITS_SLV2_DLY_EN = BIT(2),
+ BITS_SLV1_DLY_EN = BIT(1),
+ BITS_SLV0_DLY_EN = BIT(0),
+
+ SIGNAL_PATH_RESET = 0x68,
+ SIGNAL_PATH_RESET_GYRO_RESET = BIT(2),
+ SIGNAL_PATH_RESET_ACCEL_RESET = BIT(1),
+ SIGNAL_PATH_RESET_TEMP_RESET = BIT(0),
+ SIGNAL_PATH_RESET_ALL_RESET = BIT(3) - 1,
+
+ MOT_DETECT_CTRL = 0x69,
+
+ USER_CTRL = 0x6a,
+ USER_CTRL_FIFO_EN = BIT(6),
+ USER_CTRL_I2C_MST_EN = BIT(5),
+ USER_CTRL_I2C_IF_DIS = BIT(4),
+ USER_CTRL_FIFO_RST = BIT(2),
+ USER_CTRL_I2C_MST_RST = BIT(1),
+ USER_CTRL_SIG_COND_RST = BIT(0),
+
+ PWR_MGMT_1 = 0x6b, /* Reset: 0x40 */
+ PWR_MGMT_1_DEVICE_RESET = BIT(7),
+ PWR_MGMT_1_SLEEP = BIT(6),
+ PWR_MGMT_1_CYCLE = BIT(5),
+ PWR_MGMT_1_GYRO_STANDBY = BIT(4),
+ PWR_MGMT_1_PD_PTAT = BIT(3),
+ PWR_MGMT_1_CLK_SEL_SHIFT = 0,
+ PWR_MGMT_1_CLK_SEL_WIDTH = 3,
+
+ PWR_MGMT_2 = 0x6c,
+ FIFO_COUNTH = 0x72,
+ FIFO_COUNTL = 0x73,
+ FIFO_R_W = 0x74,
+ WHO_AM_I = 0x75, /* RO reset: 0x68 */
+
+ XA_OFFSETH = 0x77,
+ XA_OFFSETL = 0x78,
+ YA_OFFSETH = 0x7a,
+ YA_OFFSETL = 0x7b,
+ ZA_OFFSETH = 0x7d,
+ ZA_OFFSETL = 0x7e,
+
+ /* Magnetometer device address */
+
+ MPU9250_AK8963_I2C_ADDR = 0x0c,
+ MPU9250_AK8963_I2C_READ = 0x80,
+ MPU9250_AK8963_I2C_WRITE = 0x00,
+
+ /* AK8963 Magnetometer Register Addresses */
+
+ MPU9250_MAG_REG_WIA = 0x00,
+ MPU9250_MAG_REG_ST1 = 0x02,
+ MPU9250_MAG_REG_DATA = 0x03,
+ MPU9250_MAG_REG_HXL = 0x03,
+ MPU9250_MAG_REG_ST2 = 0x09,
+ MPU9250_MAG_REG_CNTL1 = 0x0a,
+ MPU9250_MAG_REG_CNTL2 = 0x0b,
+ MPU9250_MAG_REG_ASAX = 0x10,
+ MPU9250_MAG_REG_ASAY = 0x11,
+ MPU9250_MAG_REG_ASAZ = 0x12,
+
+ /* Bit definitions for the magnetometer registers */
+
+ BIT_MAG_CNTL1_MODE_POWER_DOWN = 0x0,
+ BIT_MAG_CNTL1_MODE_SINGLE_MEASURE_MODE = 0x1,
+ BIT_MAG_CNTL1_MODE_CONTINUOUS_MEASURE_MODE_1 = 0x2,
+ BIT_MAG_CNTL1_MODE_CONTINUOUS_MEASURE_MODE_2 = 0x6,
+ BIT_MAG_CNTL1_FUSE_ROM_ACCESS_MODE = 0xf,
+ BIT_MAG_CNTL1_16_BITS = 0x10,
+ BIT_MAG_HOFL = 0x08,
+
+ BIT_MAG_CNTL2_SOFT_RESET = 0x01,
+};
+
+/* Describes the mpu9250 sensor register file. This structure reflects
+ * the underlying hardware, so don't change it!
+ */
+
+#pragma pack(push, 1)
+struct sensor_data_s
+{
+ uint16_t x_accel;
+ uint16_t y_accel;
+ uint16_t z_accel;
+ uint16_t temp;
+ uint16_t x_gyro;
+ uint16_t y_gyro;
+ uint16_t z_gyro;
+ char mag_st1; /* 14 mag ST1 (1B) */
+ int16_t x_mag; /* 15-16 (2B) */
+ int16_t y_mag; /* 17-18 (2B) */
+ int16_t z_mag; /* 19-20 (2B) */
+ char mag_st2; /* 21 mag ST2 (1B) */
+};
+#pragma pack(pop)
+
+struct mpu9250_sensor_s
+{
+ struct sensor_lowerhalf_s lower;
+ uint64_t last_update;
+ bool enabled;
+ float scale;
+ float adj[3];
+ unsigned long interval;
+ FAR void *dev; /* The pointer to common device data of mpu9250 */
+};
+
+/* Used by the driver to manage the device */
+
+struct mpu9250_dev_s
+{
+ struct mpu9250_sensor_s priv[MPU9250_MAX_IDX];
+ struct mpu9250_config_s config; /* board-specific information */
+ sem_t run; /* Locks sensor thread */
+ mutex_t lock; /* mutex for this structure */
+};
+
+/****************************************************************************
+ * Private Function Function Prototypes
+ ****************************************************************************/
+
+/* Sensor methods */
+
+static int mpu9250_set_interval(FAR struct sensor_lowerhalf_s *lower,
+ FAR struct file *filep,
+ FAR unsigned long *period_us);
+static int mpu9250_activate(FAR struct sensor_lowerhalf_s *lower,
+ FAR struct file *filep, bool enable);
+static int mpu9250_control(FAR struct sensor_lowerhalf_s *lower,
+ FAR struct file *filep,
+ int cmd, unsigned long arg);
+
+static inline int mpu9250_write_gyro_range(FAR struct mpu9250_dev_s *dev,
+ uint8_t fs_sel);
+static void mpu9250_gyro_scale(FAR struct mpu9250_sensor_s *priv,
+ enum gyro_config_bit scale);
+static inline int mpu9250_write_accel_range(FAR struct mpu9250_dev_s *dev,
+ uint8_t afs_sel);
+static void mpu9250_accel_scale(FAR struct mpu9250_sensor_s *priv,
+ enum accel_config_bit scale);
+static int ak8963_initialize(FAR struct mpu9250_dev_s *dev,
+ int measure_freq);
+static int read_ak8963_reg(FAR struct mpu9250_dev_s *dev,
+ enum mpu9250_regaddr_e reg, uint8_t *val);
+static int write_ak8963_reg(FAR struct mpu9250_dev_s *dev,
+ enum mpu9250_regaddr_e reg, uint8_t val);
+static int get_mag_adjustment(FAR struct mpu9250_dev_s *dev);
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static const struct sensor_ops_s g_mpu9250_ops =
+{
+ NULL, /* open */
+ NULL, /* close */
+ mpu9250_activate, /* activate */
+ mpu9250_set_interval, /* set_interval */
+ NULL, /* batch */
+ NULL, /* fetch */
+ NULL, /* selftest */
+ NULL, /* set_calibvalue */
+ NULL, /* calibrate */
+ mpu9250_control /* control */
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: mpu9250_activate
+ *
+ * Description: Activate the sensor.
+ *
+ * Return:
+ * OK - on success
+ ****************************************************************************/
+
+static int mpu9250_activate(FAR struct sensor_lowerhalf_s *lower,
+ FAR struct file *filep, bool enable)
+{
+ FAR struct mpu9250_sensor_s *priv = (FAR struct mpu9250_sensor_s *)lower;
+ FAR struct mpu9250_dev_s *dev = (FAR struct mpu9250_dev_s *)(priv->dev);
+ bool start_thread = false;
+
+ if (enable)
+ {
+ if (!priv->enabled)
+ {
+ start_thread = true;
+ priv->last_update = sensor_get_timestamp();
+ }
+ }
+
+ priv->enabled = enable;
+
+ if (start_thread)
+ {
+ /* Wake up the thread */
+
+ nxsem_post(&dev->run);
+ }
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: mpu9250_set_interval
+ *
+ * Description: Set data output interval of sensor.
+ *
+ * Return:
+ * OK - on success
+ ****************************************************************************/
+
+static int mpu9250_set_interval(FAR struct sensor_lowerhalf_s *lower,
+ FAR struct file *filep,
+ FAR unsigned long *interval)
+{
+ FAR struct mpu9250_sensor_s *priv = (FAR struct mpu9250_sensor_s *)lower;
+
+ priv->interval = *interval;
+
+ return 0;
+}
+
+/****************************************************************************
+ * Name: mpu9250_control
+ *
+ * Description: Interface function of struct sensor_ops_s.
+ *
+ * Return:
+ * OK - on success
+ ****************************************************************************/
+
+static int mpu9250_control(FAR struct sensor_lowerhalf_s *lower,
+ FAR struct file *filep,
+ int cmd, unsigned long arg)
+{
+ FAR struct mpu9250_sensor_s *priv = (FAR struct mpu9250_sensor_s *)lower;
+ FAR struct mpu9250_dev_s *dev = (FAR struct mpu9250_dev_s *)(priv->dev);
+ int ret;
+
+ switch (cmd)
+ {
+ /* Set full scale command */
+
+ case SNIOC_SET_SCALE_XL:
+ {
+ switch (priv->lower.type)
+ {
+ /* Set gyroscope full scale */
+
+ case SENSOR_TYPE_GYROSCOPE:
+ {
+ ret = mpu9250_write_gyro_range(dev, (uint8_t)arg);
+ mpu9250_gyro_scale(priv, (enum gyro_config_bit)arg);
+ }
+ break;
+
+ /* Set accelerometer full scale */
+
+ case SENSOR_TYPE_ACCELEROMETER:
+ {
+ ret = mpu9250_write_accel_range(dev, (uint8_t)arg);
+ mpu9250_accel_scale(priv, (enum accel_config_bit)arg);
+ }
+ break;
+
+ default:
+ snerr("ERROR: Unrecognized type: %d\n", priv->lower.type);
+ ret = -ENOTTY;
+ break;
+ }
+ }
+ break;
+
+ default:
+ snerr("ERROR: Unrecognized cmd: %d\n", cmd);
+ ret = -ENOTTY;
+ break;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: mpu9250_accel_scale
+ *
+ * Description: Set scale of accelerometer.
+ *
+ * AFS_SEL | Full Scale Range | LSB Sensitivity
+ * --------+------------------+----------------
+ * 0 | +/- 2g | 16384 LSB/mg
+ * 1 | +/- 4g | 8192 LSB/mg
+ * 2 | +/- 8g | 4096 LSB/mg
+ * 3 | +/- 16g | 2048 LSB/mg
+ ****************************************************************************/
+
+static void mpu9250_accel_scale(FAR struct mpu9250_sensor_s *priv,
+ enum accel_config_bit scale)
+{
+ switch (scale)
+ {
+ case ACCEL_FS_SEL_2G:
+ priv->scale = CONSTANTS_ONE_G / 16384.f;
+ break;
+
+ case ACCEL_FS_SEL_4G:
+ priv->scale = CONSTANTS_ONE_G / 8192.f;
+ break;
+
+ case ACCEL_FS_SEL_8G:
+ priv->scale = CONSTANTS_ONE_G / 4096.f;
+ break;
+
+ case ACCEL_FS_SEL_16G:
+ priv->scale = CONSTANTS_ONE_G / 2048.f;
+ break;
+ default:
+ break;
+ }
+}
+
+/****************************************************************************
+ * Name: mpu9250_gyro_scale
+ *
+ * Description: Set scale of accelerometer.
+ *
+ * FS_SEL | Full Scale Range | LSB Sensitivity
+ * -------+--------------------+----------------
+ * 0 | +/- 250 degrees/s | 131 LSB/deg/s
+ * 1 | +/- 500 degrees/s | 65.5 LSB/deg/s
+ * 2 | +/- 1000 degrees/s | 32.8 LSB/deg/s
+ * 3 | +/- 2000 degrees/s | 16.4 LSB/deg/s
+ ****************************************************************************/
+
+static void mpu9250_gyro_scale(FAR struct mpu9250_sensor_s *priv,
+ enum gyro_config_bit scale)
+{
+ switch (scale)
+ {
+ case GYRO_FS_SEL_250_DPS:
+ priv->scale = (M_PI / 180.0f) * 250.f / 32768.f;
+ break;
+
+ case GYRO_FS_SEL_500_DPS:
+ priv->scale = (M_PI / 180.0f) * 500.f / 32768.f;
+ break;
+
+ case GYRO_FS_SEL_1000_DPS:
+ priv->scale = (M_PI / 180.0f) * 1000.f / 32768.f;
+ break;
+
+ case GYRO_FS_SEL_2000_DPS:
+ priv->scale = (M_PI / 180.0f) * 2000.f / 32768.f;
+ break;
+ default:
+ break;
+ }
+}
+
+/* NOTE :
+ *
+ * In all of the following code, functions named with a double leading
+ * underscore '_' must be invoked ONLY if the mpu9250_dev_s lock is
+ * already held. Failure to do this might cause the transaction to get
+ * interrupted, which will likely confuse the data you get back.
+ *
+ * The mpu9250_dev_s lock is NOT the same thing as, i.e. the SPI master
+ * interface lock: the latter protects the bus interface hardware
+ * (which may have other SPI devices attached), the former protects
+ * the chip and its associated data.
+ */
+
+#ifdef CONFIG_MPU9250_SPI
+/* mpu9250_read_reg(), but for spi-connected devices. See that function
+ * for documentation.
+ */
+
+static int mpu9250_read_reg_spi(FAR struct mpu9250_dev_s *dev,
+ enum mpu9250_regaddr_e reg_addr,
+ FAR uint8_t *buf, uint8_t len)
+{
+ FAR struct spi_dev_s *spi = dev->config.spi;
+ int id = dev->config.spi_devid;
+ int ret;
+
+ /* We'll probably return the number of bytes asked for. */
+
+ ret = len;
+
+ /* Grab and configure the SPI master device: always mode 0, 20MHz if it's a
+ * data register, 1MHz otherwise (per datasheet).
+ */
+
+ SPI_LOCK(spi, true);
+ SPI_SETMODE(spi, SPIDEV_MODE0);
+
+ if ((reg_addr >= ACCEL_XOUT_H) && ((reg_addr + len) <= I2C_SLV0_DO))
+ {
+ SPI_SETFREQUENCY(spi, 20000000);
+ }
+ else
+ {
+ SPI_SETFREQUENCY(spi, 1000000);
+ }
+
+ /* Select the chip. */
+
+ SPI_SELECT(spi, id, true);
+
+ /* Send the read request. */
+
+ SPI_SEND(spi, reg_addr | MPU_REG_READ);
+
+ /* Clock in the data. */
+
+ while (0 != len--)
+ {
+ *buf++ = (uint8_t) (SPI_SEND(spi, 0xff));
+ }
+
+ /* Deselect the chip, release the SPI master. */
+
+ SPI_SELECT(spi, id, false);
+ SPI_LOCK(spi, false);
+
+ return ret;
+}
+
+/* mpu9250_write_reg(), but for SPI connections. */
+
+static int mpu9250_write_reg_spi(FAR struct mpu9250_dev_s *dev,
+ enum mpu9250_regaddr_e reg_addr,
+ FAR const uint8_t * buf, uint8_t len)
+{
+ FAR struct spi_dev_s *spi = dev->config.spi;
+ int id = dev->config.spi_devid;
+ int ret;
+
+ /* Hopefully, we'll return all the bytes they're asking for. */
+
+ ret = len;
+
+ /* Grab and configure the SPI master device. */
+
+ SPI_LOCK(spi, true);
+ SPI_SETMODE(spi, SPIDEV_MODE0);
+ SPI_SETFREQUENCY(spi, 1000000);
+
+ /* Select the chip. */
+
+ SPI_SELECT(spi, id, true);
+
+ /* Send the write request. */
+
+ SPI_SEND(spi, reg_addr | MPU_REG_WRITE);
+
+ /* Send the data. */
+
+ while (0 != len--)
+ {
+ SPI_SEND(spi, *buf++);
+ }
+
+ /* Release the chip and SPI master. */
+
+ SPI_SELECT(spi, id, false);
+ SPI_LOCK(spi, false);
+
+ return ret;
+}
+
+#else
+
+/* mpu9250_read_reg(), but for i2c-connected devices. */
+
+static int mpu9250_read_reg_i2c(FAR struct mpu9250_dev_s *dev,
+ uint8_t reg_addr,
+ FAR uint8_t *buf, uint8_t len)
+{
+ int ret;
+ struct i2c_msg_s msg[2];
+
+ msg[0].frequency = CONFIG_MPU9250_I2C_FREQ;
+ msg[0].addr = dev->config.addr;
+ msg[0].flags = I2C_M_NOSTOP;
+ msg[0].buffer = ®_addr;
+ msg[0].length = 1;
+
+ msg[1].frequency = CONFIG_MPU9250_I2C_FREQ;
+ msg[1].addr = dev->config.addr;
+ msg[1].flags = I2C_M_READ;
+ msg[1].buffer = buf;
+ msg[1].length = len;
+
+ ret = I2C_TRANSFER(dev->config.i2c, msg, 2);
+ if (ret < 0)
+ {
+ snerr("ERROR: I2C_TRANSFER(read) failed: %d\n", ret);
+ return ret;
+ }
+
+ return OK;
+}
+
+/* mpu9250_write_reg(), but for I2C connections. */
+
+static int mpu9250_write_reg_i2c(FAR struct mpu9250_dev_s *dev,
+ uint8_t reg_addr,
+ FAR const uint8_t *buf, uint8_t len)
+{
+ int ret;
+ struct i2c_msg_s msg[2];
+
+ msg[0].frequency = CONFIG_MPU9250_I2C_FREQ;
+ msg[0].addr = dev->config.addr;
+ msg[0].flags = I2C_M_NOSTOP;
+ msg[0].buffer = ®_addr;
+ msg[0].length = 1;
+ msg[1].frequency = CONFIG_MPU9250_I2C_FREQ;
+ msg[1].addr = dev->config.addr;
+ msg[1].flags = I2C_M_NOSTART;
+ msg[1].buffer = (FAR uint8_t *)buf;
+ msg[1].length = len;
+ ret = I2C_TRANSFER(dev->config.i2c, msg, 2);
+ if (ret < 0)
+ {
+ snerr("ERROR: I2C_TRANSFER(write) failed: %d\n", ret);
+ return ret;
+ }
+
+ return OK;
+}
+#endif /* CONFIG_MPU9250_SPI */
+
+/* mpu9250_read_reg()
+ *
+ * Reads a block of @len byte-wide registers, starting at @reg_addr,
+ * from the device connected to @dev. Bytes are returned in @buf,
+ * which must have a capacity of at least @len bytes.
+ *
+ * Note: The caller must hold @dev->lock before calling this function.
+ *
+ * Returns number of bytes read, or a negative errno.
+ */
+
+static inline int mpu9250_read_reg(FAR struct mpu9250_dev_s *dev,
+ enum mpu9250_regaddr_e reg_addr,
+ FAR uint8_t *buf, uint8_t len)
+{
+#ifdef CONFIG_MPU9250_SPI
+ /* If we're wired to SPI, use that function. */
+
+ if (dev->config.spi != NULL)
+ {
+ return mpu9250_read_reg_spi(dev, reg_addr, buf, len);
+ }
+#else
+ /* If we're wired to I2C, use that function. */
+
+ if (dev->config.i2c != NULL)
+ {
+ return mpu9250_read_reg_i2c(dev, reg_addr, buf, len);
+ }
+#endif
+
+ /* If we get this far, it's because we can't "find" our device. */
+
+ return -ENODEV;
+}
+
+/* mpu9250_write_reg()
+ *
+ * Writes a block of @len byte-wide registers, starting at @reg_addr,
+ * using the values in @buf to the device connected to @dev. Register
+ * values are taken in numerical order from @buf, i.e.:
+ *
+ * buf[0] -> register[@reg_addr]
+ * buf[1] -> register[@reg_addr + 1]
+ * ...
+ *
+ * Note: The caller must hold @dev->lock before calling this function.
+ *
+ * Returns number of bytes written, or a negative errno.
+ */
+
+static inline int mpu9250_write_reg(FAR struct mpu9250_dev_s *dev,
+ enum mpu9250_regaddr_e reg_addr,
+ FAR const uint8_t *buf, uint8_t len)
+{
+#ifdef CONFIG_MPU9250_SPI
+ /* If we're connected to SPI, use that function. */
+
+ if (dev->config.spi != NULL)
+ {
+ return mpu9250_write_reg_spi(dev, reg_addr, buf, len);
+ }
+#else
+ if (dev->config.i2c != NULL)
+ {
+ return mpu9250_write_reg_i2c(dev, reg_addr, buf, len);
+ }
+#endif
+
+ /* If we get this far, it's because we can't "find" our device. */
+
+ return -ENODEV;
+}
+
+/****************************************************************************
+ * mpu9250_write_reg_verify
+ *
+ * Description:
+ * write a 8-bit register value by address
+ * read back a 8-bit register value by address to verify
+ *
+ * Returns number of bytes read, or a negative errno.
+ ****************************************************************************/
+
+static int mpu9250_write_reg_verify(FAR struct mpu9250_dev_s *dev,
+ enum mpu9250_regaddr_e reg_addr,
+ uint8_t val, uint8_t mask)
+{
+ int ret;
+ uint8_t b;
+ int retry = 5;
+ bool err;
+
+ while (retry)
+ {
+ err = false;
+ --retry;
+ ret = mpu9250_write_reg(dev, reg_addr, &val, sizeof(val));
+ if (ret < 0)
+ {
+ err = true;
+ continue;
+ }
+
+ ret = mpu9250_read_reg(dev, reg_addr, &b, sizeof(b));
+ if (ret < 0)
+ {
+ err = true;
+ continue;
+ }
+
+ if ((b & mask) != val)
+ {
+ continue;
+ }
+ else
+ {
+ return 0;
+ }
+ }
+
+ if (err)
+ {
+ snerr("write_reg_verify failed at reg %d. Error %d.", reg_addr, ret);
+ }
+ else
+ {
+ snerr("write_reg_verify failed at reg %d. %d!=%d", reg_addr, val, b);
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * mpu9250_modify_reg()
+ *
+ * Description:
+ * Modify a 8-bit register value by address
+ * mpu9250_modify_reg(d,v,m,a) defined as:
+ * mpu9250_write_reg(d,(mpu9250_read_reg(d,a) & ~(m)) | ((v) & (m)), (a))
+ *
+ * Note: The caller must hold @dev->lock before calling this function.
+ *
+ * Returns number of bytes written, or a negative errno.
+ ****************************************************************************/
+
+static inline int mpu9250_modify_reg(FAR struct mpu9250_dev_s *dev,
+ enum mpu9250_regaddr_e reg_addr,
+ uint8_t clearbits, uint8_t setbits)
+{
+ uint8_t buf = 0xff;
+
+ mpu9250_read_reg(dev, reg_addr, &buf, sizeof(buf));
+
+ buf = (buf & ~clearbits) | (setbits & clearbits);
+
+ return mpu9250_write_reg(dev, reg_addr, &buf, sizeof(buf));
+}
+
+/****************************************************************************
+ * mpu9250_read_imu()
+ *
+ * Reads the whole IMU data file from @dev in one uninterrupted pass,
+ * placing the sampled values into @buf. This function is the only way
+ * to guarantee that the measured values are sampled as closely-spaced
+ * in time as the hardware permits, which is almost always what you
+ * want.
+ ****************************************************************************/
+
+static inline int mpu9250_read_imu(FAR struct mpu9250_dev_s *dev,
+ FAR struct sensor_data_s *buf)
+{
+ return mpu9250_read_reg(dev, ACCEL_XOUT_H, (uint8_t *) buf, sizeof(*buf));
+}
+
+/* mpu9250_read_pwr_mgmt_1()
+ *
+ * Returns the value of the PWR_MGMT_1 register from @dev.
+ */
+
+static inline uint8_t mpu9250_read_pwr_mgmt_1(FAR struct mpu9250_dev_s *dev)
+{
+ uint8_t buf = 0xff;
+
+ mpu9250_read_reg(dev, PWR_MGMT_1, &buf, sizeof(buf));
+ return buf;
+}
+
+static inline int mpu9250_write_signal_reset(FAR struct mpu9250_dev_s *dev,
+ uint8_t val)
+{
+ return mpu9250_write_reg(dev, SIGNAL_PATH_RESET, &val, sizeof(val));
+}
+
+static inline int mpu9250_write_int_pin_cfg(FAR struct mpu9250_dev_s *dev,
+ uint8_t val)
+{
+ return mpu9250_write_reg(dev, INT_PIN_CFG, &val, sizeof(val));
+}
+
+static inline int mpu9250_write_pwr_mgmt_1(FAR struct mpu9250_dev_s *dev,
+ uint8_t val)
+{
+ return mpu9250_write_reg(dev, PWR_MGMT_1, &val, sizeof(val));
+}
+
+static inline int mpu9250_write_pwr_mgmt_2(FAR struct mpu9250_dev_s *dev,
+ uint8_t val)
+{
+ return mpu9250_write_reg(dev, PWR_MGMT_2, &val, sizeof(val));
+}
+
+static inline int mpu9250_write_user_ctrl(FAR struct mpu9250_dev_s *dev,
+ uint8_t val)
+{
+ return mpu9250_write_reg(dev, USER_CTRL, &val, sizeof(val));
+}
+
+static inline int mpu9250_write_fifo_en(FAR struct mpu9250_dev_s *dev,
+ uint8_t val)
+{
+ return mpu9250_write_reg(dev, FIFO_EN, &val, sizeof(val));
+}
+
+/****************************************************************************
+ * mpu9250_write_gyro_range() :
+ *
+ * Sets the @fs_sel bit in GYRO_CONFIG to the value provided. Per the
+ * datasheet, the meaning of @fs_sel is as follows:
+ *
+ * GYRO_CONFIG(0x1b) : XG_ST YG_ST ZG_ST FS_SEL1 FS_SEL0 x x x
+ *
+ * XG_ST, YG_ST, ZG_ST : self-test (unsupported in this driver)
+ * 1 -> activate self-test on X, Y, and/or Z gyros
+ *
+ * FS_SEL[10] : full-scale range select
+ * 0 -> ± 250 deg/sec
+ * 1 -> ± 500 deg/sec
+ * 2 -> ± 1000 deg/sec
+ * 3 -> ± 2000 deg/sec
+ ****************************************************************************/
+
+static inline int mpu9250_write_gyro_range(FAR struct mpu9250_dev_s *dev,
+ uint8_t fs_sel)
+{
+ uint8_t val = TO_BITFIELD(GYRO_CONFIG_FS_SEL, fs_sel);
+ return mpu9250_write_reg(dev, GYRO_CONFIG, &val, sizeof(val));
+}
+
+/****************************************************************************
+ * mpu9250_write_accel_range() :
+ *
+ * Sets the @afs_sel bit in ACCEL_CONFIG to the value provided. Per
+ * the datasheet, the meaning of @afs_sel is as follows:
+ *
+ * ACCEL_CONFIG(0x1c) : XA_ST YA_ST ZA_ST AFS_SEL1 AFS_SEL0 x x x
+ *
+ * XA_ST, YA_ST, ZA_ST : self-test (unsupported in this driver)
+ * 1 -> activate self-test on X, Y, and/or Z accelerometers
+ *
+ * AFS_SEL[10] : full-scale range select
+ * 0 -> ± 2 g
+ * 1 -> ± 4 g
+ * 2 -> ± 8 g
+ * 3 -> ± 16 g
+ ****************************************************************************/
+
+static inline int mpu9250_write_accel_range(FAR struct mpu9250_dev_s *dev,
+ uint8_t afs_sel)
+{
+ uint8_t val = TO_BITFIELD(ACCEL_CONFIG_AFS_SEL, afs_sel);
+ return mpu9250_write_reg(dev, ACCEL_CONFIG, &val, sizeof(val));
+}
+
+/****************************************************************************
+ * CONFIG (0x1a) : x x EXT_SYNC_SET[2..0] DLPF_CFG[2..0]
+ *
+ * EXT_SYNC_SET : frame sync bit position
+ * DLPF_CFG : digital low-pass filter bandwidth
+ ****************************************************************************/
+
+static inline int mpu9250_write_config(FAR struct mpu9250_dev_s *dev,
+ uint8_t ext_sync_set, uint8_t dlpf_cfg)
+{
+ uint8_t val = TO_BITFIELD(CONFIG_EXT_SYNC_SET, ext_sync_set) |
+ TO_BITFIELD(CONFIG_DLPF_CFG, dlpf_cfg);
+ return mpu9250_write_reg(dev, MPU9250_CONFIG, &val, sizeof(val));
+}
+
+/****************************************************************************
+ * CONFIG2 (0x1d) : x x accel_fchoice_b A_DLPF_CFG[2..0]
+ *
+ * accel_fchoice_b : he inverted version of accel_fchoice
+ * A_DLPF_CFG : Accelerometer low pass filter setting
+ ****************************************************************************/
+
+static inline int mpu9250_write_config2(FAR struct mpu9250_dev_s *dev,
+ uint8_t acce_fchoice_b,
+ uint8_t a_dlpf_cfg)
+{
+ uint8_t val = acce_fchoice_b | TO_BITFIELD(CONFIG2_A_DLPF_CFG, a_dlpf_cfg);
+ return mpu9250_write_reg(dev, ACCEL_CONFIG2, &val, sizeof(val));
+}
+
+/****************************************************************************
+ * Name: mpu9250_initialize
+ *
+ * Description: Initialize IMU and AK8963 magnetometer inside the MPU9250
+ *
+ * Parameter:
+ * dev - Internal private lower half driver instance
+ *
+ * Return:
+ * OK - on success
+ ****************************************************************************/
+
+static int mpu9250_initialize(FAR struct mpu9250_dev_s *dev)
+{
+ int ret = OK;
+
+#ifdef CONFIG_MPU9250_SPI
+ if (dev->config.spi == NULL)
+ {
+ return -EINVAL;
+ }
+#else
+ if (dev->config.i2c == NULL)
+ {
+ return -EINVAL;
+ }
+#endif
+
+ nxmutex_lock(&dev->lock);
+
+ /* Awaken chip, issue hardware reset */
+
+ ret = mpu9250_write_pwr_mgmt_1(dev, PWR_MGMT_1_DEVICE_RESET);
+ if (ret < 0)
+ {
+ snerr("mpu9250 write_pwr_mgmt_1 error!\n");
+ goto errout;
+ }
+
+ /* Wait for reset cycle to finish (note: per the datasheet, we don't need
+ * to hold NSS for this)
+ */
+
+ do
+ {
+ nxsig_usleep(50000); /* usecs (arbitrary) */
+ }
+ while (mpu9250_read_pwr_mgmt_1(dev) & PWR_MGMT_1_DEVICE_RESET);
+
+ /* Reset signal paths */
+
+ ret = mpu9250_write_signal_reset(dev, SIGNAL_PATH_RESET_ALL_RESET);
+ if (ret < 0)
+ {
+ snerr("mpu9250 write_signal_path_reset error!\n");
+ goto errout;
+ }
+
+ nxsig_usleep(1000);
+
+ /* Disable SLEEP, use PLL with z-axis clock source */
+
+ ret = mpu9250_write_pwr_mgmt_1(dev, 3);
+ if (ret < 0)
+ {
+ snerr("mpu9250 write_pwr_mgmt_1 error!\n");
+ goto errout;
+ }
+
+ nxsig_usleep(1000);
+
+ /* Disable low-power mode, enable all gyros and accelerometers */
+
+ ret = mpu9250_write_pwr_mgmt_2(dev, 0);
+ if (ret < 0)
+ {
+ snerr("mpu9250 write_pwr_mgmt_2 error!\n");
+ goto errout;
+ }
+
+ nxsig_usleep(1000);
+
+ /* clear first, and separate *_RST from *_EN */
+
+ ret = mpu9250_write_user_ctrl(dev, 0);
+ if (ret < 0)
+ {
+ snerr("mpu9250 write_user_ctrl error!\n");
+ goto errout;
+ }
+
+ nxsig_usleep(1000);
+
+ ret = mpu9250_write_fifo_en(dev, 0);
+ if (ret < 0)
+ {
+ snerr("mpu9250 write_fifo_en error!\n");
+ goto errout;
+ }
+
+ nxsig_usleep(1000);
+
+ /* Reset I2C Master module. */
+
+ ret = mpu9250_write_user_ctrl(dev, USER_CTRL_FIFO_RST |
+ USER_CTRL_I2C_MST_RST |
+ USER_CTRL_SIG_COND_RST);
+ if (ret < 0)
+ {
+ snerr("mpu9250 write_user_ctrl error!\n");
+ goto errout;
+ }
+
+ nxsig_usleep(1000);
+
+ /* Disable i2c if we're on spi. */
+
+#ifdef CONFIG_MPU9250_SPI
+ if (dev->config.spi)
+ {
+ ret = mpu9250_write_user_ctrl(dev, USER_CTRL_I2C_MST_EN |
+ USER_CTRL_I2C_IF_DIS);
+ }
+#else
+ if (dev->config.i2c)
+ {
+ ret = mpu9250_write_user_ctrl(dev, USER_CTRL_I2C_MST_EN);
+ }
+
+#endif
+ if (ret < 0)
+ {
+ snerr("mpu9250 write_user_ctrl error!\n");
+ goto errout;
+ }
+
+ nxsig_usleep(1000);
+
+ /* default No FSYNC, set accel LPF at 184 Hz, gyro LPF at 188 Hz in
+ * menuconfig
+ */
+
+ ret = mpu9250_write_config(dev, CONFIG_MPU9250_EXT_SYNC_SET,
+ CONFIG_MPU9250_DLPF_CFG);
+ if (ret < 0)
+ {
+ snerr("mpu9250 write_config error!\n");
+ goto errout;
+ }
+
+ nxsig_usleep(1000);
+
+ /* default ± 1000 deg/sec in menuconfig */
+
+ ret = mpu9250_write_gyro_range(dev, CONFIG_MPU9250_GYRO_FS_SEL);
+ if (ret < 0)
+ {
+ snerr("mpu9250 write_gyro_range error!\n");
+ goto errout;
+ }
+
+ nxsig_usleep(1000);
+
+ /* default ± 8g in menuconfig */
+
+ ret = mpu9250_write_accel_range(dev, CONFIG_MPU9250_ACCEL_AFS_SEL);
+ if (ret < 0)
+ {
+ snerr("mpu9250 write_accel_range error!\n");
+ goto errout;
+ }
+
+ nxsig_usleep(1000);
+
+ /* Accelerometer low pass filter setting */
+
+ ret = mpu9250_write_config2(dev, CONFIG_MPU9250_ACCEL_FCHOICE_B,
+ CONFIG_MPU9250_A_DLPF_CFG);
+ if (ret < 0)
+ {
+ snerr("mpu9250 write_config2 error!\n");
+ goto errout;
+ }
+
+ nxsig_usleep(1000);
+
+ /* clear INT on any read (we aren't using that pin right now) */
+
+ ret = mpu9250_write_int_pin_cfg(dev, INT_PIN_CFG_INT_RD_CLEAR);
+ if (ret < 0)
+ {
+ snerr("mpu9250 write int pin cfg error!\n");
+ goto errout;
+ }
+
+ nxsig_usleep(1000);
+
+ /* Initialize ak8963 magnetometer inside the IMU */
+
+ ret = ak8963_initialize(dev, CONFIG_MPU9250_MEASURE_FREQ);
+ if (ret < 0)
+ {
+ snerr("ak8963 initialize error!\n");
+ goto errout;
+ }
+
+errout:
+ nxmutex_unlock(&dev->lock);
+ return ret;
+}
+
+/****************************************************************************
+ * Name: ak8963_initialize
+ *
+ * Description: Initialize AK8963 magnetometer inside the IMU
+ *
+ * Parameter:
+ * dev - Internal private lower half driver instance
+ * measure_freq - Data output_rate_in_hz
+ *
+ * Return:
+ * OK - on success
+ ****************************************************************************/
+
+static int ak8963_initialize(FAR struct mpu9250_dev_s *dev,
+ int measure_freq)
+{
+ uint8_t val = 0;
+ int ret = 0;
+
+ /* Enable the IMU extended I2C master. */
+
+ ret = mpu9250_modify_reg(dev, USER_CTRL, USER_CTRL_I2C_MST_EN,
+ USER_CTRL_I2C_MST_EN);
+ if (ret < 0)
+ {
+ snerr("MPU9250 I2C master bus enable failed: %d\n", ret);
+ return ret;
+ }
+
+ /* Configure the IMU as an I2C master at 400 KHz. */
+
+ val = 0x0d;
+ ret = mpu9250_write_reg(dev, I2C_MST_CTRL, &val, sizeof(val));
+ if (ret < 0)
+ {
+ snerr("MPU9250 I2C master bus clock set failed: %d\n", ret);
+ return ret;
+ }
+
+ nxsig_usleep(1000);
+
+ /* First set power-down mode */
+
+ val = BIT_MAG_CNTL2_SOFT_RESET;
+ ret = mpu9250_write_reg(dev, MPU9250_MAG_REG_CNTL2, &val, sizeof(val));
+ if (ret < 0)
+ {
+ snerr("MPU9250 soft reset failed: %d\n", ret);
+
+ /* Reset i2c master */
+
+ mpu9250_modify_reg(dev, USER_CTRL, USER_CTRL_I2C_MST_RST,
+ USER_CTRL_I2C_MST_RST);
+ return ret;
+ }
+
+ nxsig_usleep(1000);
+
+ /* get mag version ID */
+
+ ret = read_ak8963_reg(dev, MPU9250_MAG_REG_WIA, &val);
+ if (ret < 0)
+ {
+ snerr("error reading mag whoami reg: %d", ret);
+ return ret;
+ }
+
+ /* Detect mag presence by reading whoami register */
+
+ if (val != MPU9250_AKM_DEV_ID)
+ {
+ snerr("wrong mag ID %u (expected %u)", val, MPU9250_AKM_DEV_ID);
+ return -1;
+ }
+
+ /* Get mag calibraion data from Fuse ROM */
+
+ ret = get_mag_adjustment(dev);
+ if (ret < 0)
+ {
+ snerr("Unable to read mag sensitivity adjustment");
+ return ret;
+ }
+
+ /* Power on and configure the mag in 100Hz measurement mode */
+
+ ret = write_ak8963_reg(dev, MPU9250_MAG_REG_CNTL1, BIT_MAG_CNTL1_16_BITS |
+ BIT_MAG_CNTL1_MODE_CONTINUOUS_MEASURE_MODE_2);
+ if (ret < 0)
+ {
+ snerr("Unable to configure the magnetometer mode.");
+ return ret;
+ }
+
+ nxsig_usleep(1000);
+
+ /* Slave 0 provides ST1, mag data, and ST2 data in a bulk transfer of
+ * 8 bytes of data. Use the address of ST1 in SLV0_REG as the beginning
+ * register of the 8 byte bulk transfer.
+ */
+
+ val = MPU9250_AK8963_I2C_ADDR | MPU9250_AK8963_I2C_READ;
+ ret = mpu9250_write_reg(dev, I2C_SLV0_ADDR, &val, sizeof(val));
+ if (ret < 0)
+ {
+ snerr("MPU9250 I2C slave 0 address configuration failed.");
+ return ret;
+ }
+
+ val = MPU9250_MAG_REG_ST1;
+ ret = mpu9250_write_reg(dev, I2C_SLV0_REG, &val, sizeof(val));
+ if (ret < 0)
+ {
+ snerr("MPU9250 I2C slave 0 register configuration failed.");
+ return ret;
+ }
+
+ val = BITS_I2C_SLV0_EN | BITS_I2C_SLV0_READ_8BYTES;
+ ret = mpu9250_write_reg(dev, I2C_SLV0_CTRL, &val, sizeof(val));
+ if (ret < 0)
+ {
+ snerr("MPU9250 I2C slave 0 control configuration failed.");
+ return ret;
+ }
+
+ nxsig_usleep(1000);
+
+ /* Enable reading of the mag every n samples, dividing down from the
+ * output data rate provided by the caller.
+ */
+
+ val = measure_freq / 100;
+ ret = mpu9250_write_reg(dev, I2C_SLV4_CTRL, &val, sizeof(val));
+ if (ret < 0)
+ {
+ snerr("Unable to configure I2C delay from given output data rate.");
+ return ret;
+ }
+
+ nxsig_usleep(1000);
+
+ /* Enable delayed I2C transfers for the mag on Slave 0 registers. */
+
+ val = BITS_SLV0_DLY_EN;
+ ret = mpu9250_write_reg(dev, I2C_MST_DELAY_CTRL, &val, sizeof(val));
+ if (ret < 0)
+ {
+ snerr("Unable to enable the I2C delay on slave 0.");
+ return ret;
+ }
+
+ nxsig_usleep(1000);
+
+ return 0;
+}
+
+/****************************************************************************
+ * Name: get_mag_adjustment
+ *
+ * Description: Get mag calibraion data from Fuse ROM
+ *
+ * Parameter:
+ * dev - Internal private lower half driver instance
+ *
+ * Return:
+ * OK - on success
+ ****************************************************************************/
+
+static int get_mag_adjustment(FAR struct mpu9250_dev_s *dev)
+{
+ FAR struct mpu9250_sensor_s *priv = &dev->priv[MPU9250_MAG_IDX];
+ int ret = 0;
+ uint8_t asa;
+ int i = 0;
+
+ /* First set power-down mode */
+
+ ret = write_ak8963_reg(dev, MPU9250_MAG_REG_CNTL1,
+ BIT_MAG_CNTL1_MODE_POWER_DOWN);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ nxsig_usleep(10000);
+
+ /* Enable FUSE ROM, since the sensitivity adjustment data is stored in
+ * compass registers 0x10, 0x11 and 0x12 which is only accessible in Fuse
+ * access mode.
+ */
+
+ ret = write_ak8963_reg(dev, MPU9250_MAG_REG_CNTL1, BIT_MAG_CNTL1_16_BITS |
+ BIT_MAG_CNTL1_FUSE_ROM_ACCESS_MODE);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ nxsig_usleep(10000);
+
+ /* Get compass calibration register 0x10, 0x11, 0x12 store into context */
+
+ for (i = 0; i < 3; ++i)
+ {
+ ret = read_ak8963_reg(dev, MPU9250_MAG_REG_ASAX + i, &asa);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* H_adj = H * ((ASA-128)*0.5/128 + 1)
+ * = H * ((ASA-128) / 256 + 1)
+ * H is the raw compass reading.
+ */
+
+ priv->adj[i] = (((float)asa - 128.0f) / 256.0f) + 1.0f;
+ }
+
+ /* Leave in a power-down mode */
+
+ ret = write_ak8963_reg(dev, MPU9250_MAG_REG_CNTL1,
+ BIT_MAG_CNTL1_MODE_POWER_DOWN);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ nxsig_usleep(10000);
+
+ return 0;
+}
+
+/****************************************************************************
+ * Name: read_ak8963_reg
+ *
+ * Description: Read register of AK8963 by extended I2C bus
+ *
+ * Parameter:
+ * dev - Internal private lower half driver instance
+ * reg - register address of AK8963
+ * val - Point to the data read from AK8963
+ *
+ * Return:
+ * OK - on success
+ ****************************************************************************/
+
+static int read_ak8963_reg(FAR struct mpu9250_dev_s *dev,
+ enum mpu9250_regaddr_e reg, FAR uint8_t *val)
+{
+ int ret = 0;
+ uint8_t b = 0;
+ int loop_ctrl = 1000; /* wait up to 1000 * 1ms for completion */
+
+ /* Read operation on the mag using the slave 4 registers */
+
+ ret = mpu9250_write_reg_verify(dev, I2C_SLV4_ADDR, MPU9250_AK8963_I2C_ADDR
+ | MPU9250_AK8963_I2C_READ, 0xff);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Set the mag register to read from */
+
+ ret = mpu9250_write_reg_verify(dev, I2C_SLV4_REG, reg, 0xff);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Read the existing value of the SLV4 control register */
+
+ ret = mpu9250_read_reg(dev, I2C_SLV4_CTRL, &b, sizeof(b));
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Set the I2C_SLV4_EN bit in I2C_SL4_CTRL register without overwriting
+ * other bits. Enable data transfer, a read transfer as configured above.
+ */
+
+ b |= BITS_I2C_SLV4_EN;
+
+ /* Trigger the data transfer */
+
+ ret = mpu9250_write_reg(dev, I2C_SLV4_CTRL, &b, sizeof(b));
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Continuously check I2C_MST_STATUS register value for the completion
+ * of I2C transfer until timeout
+ */
+
+ do
+ {
+ nxsig_usleep(1000);
+ ret = mpu9250_read_reg(dev, I2C_MST_STATUS, &b, sizeof(b));
+ if (ret < 0)
+ {
+ return ret;
+ }
+ }
+ while (((b & BITS_I2C_SLV4_DONE) == 0x00) && (--loop_ctrl));
+
+ if (loop_ctrl == 0)
+ {
+ snerr("I2C transfer timed out");
+ return -1;
+ }
+
+ /* Read the value received from the mag */
+
+ ret = mpu9250_read_reg(dev, I2C_SLV4_DI, val, sizeof(*val));
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ return 0;
+}
+
+/****************************************************************************
+ * Name: write_ak8963_reg
+ *
+ * Description: Write register of AK8963 by extended I2C bus
+ *
+ * Parameter:
+ * dev - Internal private lower half driver instance
+ * reg - register address of AK8963
+ * val - 8bit data will be write into AK8963 register
+ *
+ * Return:
+ * OK - on success
+ ****************************************************************************/
+
+static int write_ak8963_reg(FAR struct mpu9250_dev_s *dev,
+ enum mpu9250_regaddr_e reg, uint8_t val)
+{
+ int ret = 0;
+ uint8_t b = 0;
+ int loop_ctrl = 1000; /* wait up to 1000 * 1ms for completion */
+
+ /* Configure a write operation to the mag using Slave 4 */
+
+ ret = mpu9250_write_reg_verify(dev, I2C_SLV4_ADDR,
+ MPU9250_AK8963_I2C_ADDR, 0xff);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Set the mag register address to write to using Slave 4 */
+
+ ret = mpu9250_write_reg_verify(dev, I2C_SLV4_REG, reg, 0xff);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Set the value to write in the I2C_SLV4_DO register */
+
+ ret = mpu9250_write_reg_verify(dev, I2C_SLV4_DO, val, 0xff);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Read the current value of the Slave 4 control register */
+
+ ret = mpu9250_read_reg(dev, I2C_SLV4_CTRL, &b, sizeof(b));
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Set I2C_SLV4_EN bit in I2C_SL4_CTRL register without overwriting other
+ * bits.
+ */
+
+ b |= BITS_I2C_SLV4_EN;
+
+ /* Trigger the data transfer from the byte now stored in the SLV4_DO
+ * register.
+ */
+
+ ret = mpu9250_write_reg(dev, I2C_SLV4_CTRL, &b, sizeof(b));
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Continuously check I2C_MST_STATUS regsiter value for the completion
+ * of I2C transfer until timeout.
+ */
+
+ do
+ {
+ nxsig_usleep(1000);
+ ret = mpu9250_read_reg(dev, I2C_MST_STATUS, &b, sizeof(b));
+ if (ret < 0)
+ {
+ return ret;
+ }
+ }
+ while (((b & BITS_I2C_SLV4_DONE) == 0x00) && (--loop_ctrl));
+
+ if (loop_ctrl == 0)
+ {
+ snerr("I2C transfer to mag timed out");
+ return -1;
+ }
+
+ return 0;
+}
+
+/****************************************************************************
+ * Name: swap16
+ *
+ * Description: swap H and L byte of a 16bit Data
+ *
+ * Parameter:
+ * val - Big endian Data
+ *
+ * Return:
+ * do nothing if Big endian, swap H and L byte if little endian
+ ****************************************************************************/
+
+static uint16_t swap16(uint16_t val)
+{
+#ifdef CONFIG_ENDIAN_BIG
+ return val;
+#else
+ return (val >> 8) | (val << 8);
+#endif
+}
+
+/****************************************************************************
+ * Name: mpu9250_accel_data
+ *
+ * Description: get and push accel data from struct sensor_data_s
+ *
+ * Parameter:
+ * priv - Internal private lower half driver instance
+ * buf - Point to struct sensor_data_s
+ *
+ * Return:
+ * OK - on success
+ ****************************************************************************/
+
+static void mpu9250_accel_data(FAR struct mpu9250_sensor_s *priv,
+ FAR struct sensor_data_s *buf)
+{
+ FAR struct sensor_lowerhalf_s *lower = &priv->lower;
+ uint64_t now = sensor_get_timestamp();
+ struct sensor_accel accel;
+
+ if (!priv->enabled || now - priv->last_update < priv->interval)
+ {
+ return;
+ }
+
+ priv->last_update = now;
+
+ accel.timestamp = now;
+ accel.x = (int16_t)swap16(buf->x_accel) * priv->scale;
+ accel.y = (int16_t)swap16(buf->y_accel) * priv->scale;
+ accel.z = (int16_t)swap16(buf->z_accel) * priv->scale;
+ accel.temperature = swap16(buf->temp) / 333.87f + 21.0f;
+
+ lower->push_event(lower->priv, &accel, sizeof(accel));
+ sninfo("Accel: %.3fm/s^2 %.3fm/s^2 %.3fm/s^2, t:%.1f\n",
+ accel.x, accel.y, accel.z, accel.temperature);
+}
+
+/****************************************************************************
+ * Name: mpu9250_gyro_data
+ *
+ * Description: get and push gyro data from struct sensor_data_s
+ *
+ * Parameter:
+ * priv - Internal private lower half driver instance
+ * buf - Point to struct sensor_data_s
+ *
+ * Return:
+ * OK - on success
+ ****************************************************************************/
+
+static void mpu9250_gyro_data(FAR struct mpu9250_sensor_s *priv,
+ FAR struct sensor_data_s *buf)
+{
+ FAR struct sensor_lowerhalf_s *lower = &priv->lower;
+ uint64_t now = sensor_get_timestamp();
+ struct sensor_gyro gyro;
+
+ if (!priv->enabled || now - priv->last_update < priv->interval)
+ {
+ return;
+ }
+
+ priv->last_update = now;
+
+ gyro.timestamp = now;
+ gyro.x = (int16_t)swap16(buf->x_gyro) * priv->scale;
+ gyro.y = (int16_t)swap16(buf->y_gyro) * priv->scale;
+ gyro.z = (int16_t)swap16(buf->z_gyro) * priv->scale;
+ gyro.temperature = swap16(buf->temp) / 333.87f + 21.0f;
+
+ lower->push_event(lower->priv, &gyro, sizeof(gyro));
+ sninfo("Gyro: %.3frad/s %.3frad/s %.3frad/s, t:%.1f\n",
+ gyro.x, gyro.y, gyro.z, gyro.temperature);
+}
+
+/****************************************************************************
+ * Name: mpu9250_mag_data
+ *
+ * Description: get and push magnetometer data from struct sensor_data_s
+ *
+ * Parameter:
+ * priv - Internal private lower half driver instance
+ * buf - Point to struct sensor_data_s
+ *
+ * Return:
+ * OK - on success
+ ****************************************************************************/
+
+static void mpu9250_mag_data(FAR struct mpu9250_sensor_s *priv,
+ FAR struct sensor_data_s *buf)
+{
+ FAR struct sensor_lowerhalf_s *lower = &priv->lower;
+ uint64_t now = sensor_get_timestamp();
+ struct sensor_mag mag;
+ float temp_mag_x;
+
+ if (!priv->enabled || now - priv->last_update < priv->interval)
+ {
+ return;
+ }
+
+ priv->last_update = now;
+
+ mag.timestamp = now;
+
+ mag.x = (int16_t)buf->x_mag * priv->adj[0] * priv->scale;
+ mag.y = (int16_t)buf->y_mag * priv->adj[1] * priv->scale;
+ mag.z = (int16_t)buf->z_mag * priv->adj[2] * priv->scale;
+
+ /* Swap magnetometer x and y axis, and invert z because internal mag in
+ * MPU9250 has a different orientation.
+ * Magnetometer X axis = Gyro and Accel Y axis
+ * Magnetometer Y axis = Gyro and Accel X axis
+ * Magnetometer Z axis = -Gyro and Accel Z axis
+ */
+
+ temp_mag_x = mag.x;
+ mag.x = mag.y;
+ mag.y = temp_mag_x;
+ mag.z = -mag.z;
+
+ mag.temperature = swap16(buf->temp) / 333.87f + 21.0f;
+
+ lower->push_event(lower->priv, &mag, sizeof(mag));
+ sninfo("Mag: %.3fuT %.3fuT %.3fuT, t:%.1f\n",
+ mag.x, mag.y, mag.z, mag.temperature);
+}
+
+/****************************************************************************
+ * Name: mpu9250_thread
+ *
+ * Description: Thread for performing interval measurement cycle and data
+ * read.
+ *
+ * Parameter:
+ * argc - Number opf arguments
+ * argv - Pointer to argument list
+ ****************************************************************************/
+
+static int mpu9250_thread(int argc, FAR char **argv)
+{
+ FAR struct mpu9250_dev_s *dev = (FAR struct mpu9250_dev_s *)
+ ((uintptr_t)strtoul(argv[1], NULL, 0));
+ FAR struct mpu9250_sensor_s *accel = &dev->priv[MPU9250_ACCEL_IDX];
+ FAR struct mpu9250_sensor_s *gyro = &dev->priv[MPU9250_GYRO_IDX];
+ FAR struct mpu9250_sensor_s *mag = &dev->priv[MPU9250_MAG_IDX];
+ struct sensor_data_s buf; /* temporary buffer (for read(), etc.) */
+ unsigned long min_interval;
+ int ret;
+
+ while (true)
+ {
+ if ((!accel->enabled) && (!gyro->enabled) && (!mag->enabled))
+ {
+ /* Waiting to be woken up */
+
+ ret = nxsem_wait(&dev->run);
+ if (ret < 0)
+ {
+ continue;
+ }
+ }
+
+ /* Returns a snapshot of the accelerometer, temperature, and gyro
+ * registers.
+ *
+ * Note: the chip uses traditional, twos-complement notation, i.e. "0"
+ * is encoded as 0, and full-scale-negative is 0x8000, and
+ * full-scale-positive is 0x7fff. If we read the registers
+ * sequentially and directly into memory (as we do), the measurements
+ * from each sensor are captured as big endian words.
+ */
+
+ ret = mpu9250_read_imu(dev, &buf);
+ if (ret < 0)
+ {
+ continue;
+ }
+
+ mpu9250_accel_data(accel, &buf);
+
+ mpu9250_gyro_data(gyro, &buf);
+
+ mpu9250_mag_data(mag, &buf);
+
+ /* Sleeping thread before fetching the next sensor data */
+
+ min_interval = MIN(accel->interval, gyro->interval);
+ min_interval = MIN(min_interval, mag->interval);
+ nxsig_usleep(min_interval);
+ }
+
+ return OK;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: mpu9250_register
+ *
+ * Description:
+ * Registers the mpu9250 character device
+ *
+ * Input Parameters:
+ * devno - Instance number for driver
+ * config - Configuration information
+ *
+ * Returned Value:
+ * Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+int mpu9250_register(int devno, FAR struct mpu9250_config_s *config)
+{
+ FAR struct mpu9250_dev_s *dev;
+ FAR struct mpu9250_sensor_s *tmp;
+ FAR char *argv[2];
+ char arg1[32];
+ int ret;
+
+ /* Without config info, we can't do anything. */
+
+ if (config == NULL)
+ {
+ return -EINVAL;
+ }
+
+ /* Initialize the device structure. */
+
+ dev = (FAR struct mpu9250_dev_s *)kmm_malloc(sizeof(struct mpu9250_dev_s));
+ if (dev == NULL)
+ {
+ snerr("ERROR: Failed to allocate mpu9250 device instance\n");
+ return -ENOMEM;
+ }
+
+ memset(dev, 0, sizeof(*dev));
+ nxmutex_init(&dev->lock);
+
+ /* Keep a copy of the config structure, in case the caller discards
+ * theirs.
+ */
+
+ dev->config = *config;
+
+ /* Accelerometer register */
+
+ tmp = &dev->priv[MPU9250_ACCEL_IDX];
+ tmp->lower.ops = &g_mpu9250_ops;
+ tmp->lower.type = SENSOR_TYPE_ACCELEROMETER;
+ tmp->lower.nbuffer = 1;
+ tmp->dev = dev;
+ tmp->interval = 1000000 / CONFIG_MPU9250_MEASURE_FREQ;
+ tmp->enabled = true;
+ ret = sensor_register(&tmp->lower, devno);
+ if (ret < 0)
+ {
+ goto accel_err;
+ }
+
+ mpu9250_accel_scale(tmp, CONFIG_MPU9250_ACCEL_AFS_SEL);
+
+ /* Gyroscope register */
+
+ tmp = &dev->priv[MPU9250_GYRO_IDX];
+ tmp->lower.ops = &g_mpu9250_ops;
+ tmp->lower.type = SENSOR_TYPE_GYROSCOPE;
+ tmp->lower.nbuffer = 1;
+ tmp->dev = dev;
+ tmp->interval = 1000000 / CONFIG_MPU9250_MEASURE_FREQ;
+ tmp->enabled = false;
+ ret = sensor_register(&tmp->lower, devno);
+ if (ret < 0)
+ {
+ goto gyro_err;
+ }
+
+ mpu9250_gyro_scale(tmp, CONFIG_MPU9250_GYRO_FS_SEL);
+
+ /* Magnetic register */
+
+ tmp = &dev->priv[MPU9250_MAG_IDX];
+ tmp->lower.ops = &g_mpu9250_ops;
+ tmp->lower.type = SENSOR_TYPE_MAGNETIC_FIELD;
+ tmp->lower.nbuffer = 1;
+ tmp->dev = dev;
+ tmp->interval = 1000000 / CONFIG_MPU9250_MEASURE_FREQ;
+ ret = sensor_register(&tmp->lower, devno);
+ tmp->enabled = false;
+ if (ret < 0)
+ {
+ goto mag_err;
+ }
+
+ tmp->scale = MAG_RAW_TO_GAUSS;
+
+ /* Reset the chip, to give it an initial configuration. */
+
+ ret = mpu9250_initialize(dev);
+ if (ret < 0)
+ {
+ snerr("ERROR: Failed to configure mpu9250: %d\n", ret);
+
+ nxmutex_destroy(&dev->lock);
+ kmm_free(dev);
+ return ret;
+ }
+
+ /* Create thread for polling sensor data */
+
+ snprintf(arg1, 16, "0x%" PRIxPTR, (uintptr_t)dev);
+ argv[0] = arg1;
+ argv[1] = NULL;
+
+ ret = kthread_create("mpu9250_thread", SCHED_PRIORITY_DEFAULT,
+ CONFIG_MPU9250_THREAD_STACKSIZE,
+ mpu9250_thread, argv);
+ if (ret < 0)
+ {
+ goto thr_err;
+ }
+
+ return ret;
+
+thr_err:
+ sensor_unregister(&dev->priv[MPU9250_MAG_IDX].lower, devno);
+mag_err:
+ sensor_unregister(&dev->priv[MPU9250_GYRO_IDX].lower, devno);
+gyro_err:
+ sensor_unregister(&dev->priv[MPU9250_ACCEL_IDX].lower, devno);
+accel_err:
+ kmm_free(dev);
+ return ret;
+}
diff --git a/include/nuttx/sensors/mpu9250.h b/include/nuttx/sensors/mpu9250.h
new file mode 100644
index 0000000000..7c4445a097
--- /dev/null
+++ b/include/nuttx/sensors/mpu9250.h
@@ -0,0 +1,170 @@
+/****************************************************************************
+ * include/nuttx/sensors/mpu9250.h
+ *
+ * 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_MPU9250_H
+#define __INCLUDE_NUTTX_SENSORS_MPU9250_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/sensors/sensor.h>
+#include <nuttx/sensors/ioctl.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Sets bit @n */
+
+#define BIT(n) (1 << (n))
+
+/* Creates a mask of @m bits, i.e. MASK(2) -> 00000011 */
+
+#define MASK(m) (BIT(m) - 1)
+
+/* Masks and shifts @v into bit field @m */
+
+#define TO_BITFIELD(m,v) (((v) & MASK(m ##_WIDTH)) << (m ##_SHIFT))
+
+/* Un-masks and un-shifts bit field @m from @v */
+
+#define FROM_BITFIELD(m,v) (((v) >> (m ##_SHIFT)) & MASK(m ##_WIDTH))
+
+/* SPI read/write codes */
+
+#define MPU_REG_READ 0x80
+#define MPU_REG_WRITE 0
+#define CONSTANTS_ONE_G 9.8f
+
+/****************************************************************************
+ * Public Types
+ ****************************************************************************/
+
+/* GYRO_CONFIG */
+
+enum gyro_config_bit
+{
+ /* GYRO_FS_SEL [4:3] */
+
+ GYRO_FS_SEL_250_DPS = 0, /* 0b00000 */
+ GYRO_FS_SEL_500_DPS = 1, /* 0b01000 */
+ GYRO_FS_SEL_1000_DPS = 2, /* 0b10000 */
+ GYRO_FS_SEL_2000_DPS = 3, /* 0b11000 */
+
+ /* FCHOICE_B [1:0] */
+
+ FCHOICE_B_BYPASS_DLPF = BIT(1) | BIT(0),
+};
+
+/* ACCEL_CONFIG */
+
+enum accel_config_bit
+{
+ /* ACCEL_FS_SEL [4:3] */
+
+ ACCEL_FS_SEL_2G = 0, /* 0b00000 */
+ ACCEL_FS_SEL_4G = 1, /* 0b01000 */
+ ACCEL_FS_SEL_8G = 2, /* 0b10000 */
+ ACCEL_FS_SEL_16G = 3, /* 0b11000 */
+};
+
+/* These structures are defined elsewhere, and we don't need their
+ * definitions here.
+ */
+
+#ifdef CONFIG_MPU9250_SPI
+struct spi_dev_s;
+#else
+struct i2c_master_s;
+#endif
+
+/* Specifies the initial chip configuration and location.
+ *
+ * The chip supports both SPI and I2C interfaces, but you wouldn't use
+ * both at the same time on the same chip. It isn't an error to have
+ * one chip of each flavor in the system, though, so it's not an
+ * either-or configuration item.
+ *
+ * Important note :
+ *
+ * The driver determines which interface type to use according to
+ * which of the two groups of fields is non-NULL. Since support for
+ * I2C and SPI are individually configurable, however, users should
+ * let the compiler clear unused fields instead of setting unused
+ * fields to NULL directly. For example, if using SPI and a
+ * stack-allocated instance:
+ *
+ * struct mpu_config_s mpuc;
+ * memset(&mpuc, 0, sizeof(mpuc)); * sets i2c to NULL, if present *
+ * mpuc.spi = ...;
+ *
+ * Or, if using dynamic memory allocation and I2C:
+ *
+ * struct mpu_config_s* mpuc;
+ * mpuc = kmm_malloc(sizeof(*mpuc));
+ * memset(mpuc, 0, sizeof(*mpuc)); * sets spi to NULL, if present *
+ * mpuc.i2c = ...;
+ *
+ * The above examples will avoid compile-time errors unless the user
+ * forgets to enable their preferred interface type, and will allow
+ * them to disable or enable the unused interface type without
+ * changing their code.
+ *
+ */
+
+struct mpu9250_config_s
+{
+#ifdef CONFIG_MPU9250_SPI
+ /* For users on SPI.
+ *
+ * spi_devid : the SPI master's slave-select number
+ * for the chip, as used in SPI_SELECT(..., dev_id, ...)
+ * spi : the SPI master device, as used in SPI_SELECT(spi, ..., ...)
+ */
+
+ FAR struct spi_dev_s *spi;
+ int spi_devid;
+#else
+ /* For users on I2C.
+ *
+ * i2c : the I2C master device
+ * addr : the I2C address.
+ */
+
+ FAR struct i2c_master_s *i2c;
+ int addr;
+#endif
+ };
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/* Declares the existence of an mpu9250 chip, wired according to
+ * config; creates an interface to it at path.
+ *
+ * Returns 0 on success, or negative errno.
+ */
+
+int mpu9250_register(int devno, FAR struct mpu9250_config_s *config);
+
+#endif /* __INCLUDE_NUTTX_SENSORS_MPU9250_H */