sergiotarxz/nuttx
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

drivers/sensors/mpu60x0.c: Initial support for TDK InvenSense MPU6000 and MPU6050 6-axis gyroscope and accelerometers. This commit supports SPI media only.

Browse Source
This commit is contained in:
Bill Gatliff 2019-03-07 16:50:37 -06:00 committed by Gregory Nutt
parent 868796acac
commit a8744defe2
4 changed files with 1136 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
drivers/sensors/Kconfig
View File

@ -431,6 +431,48 @@ config SENSORS_ADXL372
---help---
Enable driver support for the Analog Devices ADXL372 Sensor.
config SENSORS_MPU60X0
bool "Invensense MPU60x0 Sensor support"
default n
---help---
Enable driver support for Invensense MPU60x0 MotionTracker(tm) device.
if SENSORS_MPU60X0
choice
prompt "MPU60x0 Interface"
default MPU60X0_SPI
config MPU60X0_SPI
bool "MPU6000 SPI Interface"
select SPI
---help---
Enables support for the SPI interface (MPU6000 only)
config MPU60X0_I2C
bool "MPU60x0 I2C Interface"
select I2C
---help---
Enables support for the I2C interface (MPU6000
or MPU6050, autodetected during driver initialization)
endchoice
config MPU60X0_EXTI
bool "Enable interrupts"
default n
---help---
Select this when the MPU60x0's INT pin is connected to an
interrupt line on the host processor, and identify the pin
during device registration. The driver will use interrupts to
manage FIFO-full and/or sample-ready events, depending on how
the device is being used. The interface will block until the
next sample is ready, which will naturally synchronize the user
to the sampling rate chosen during operation.
Default: No interrupts or blocking, i.e. user-driven sampling.
endif # SENSORS_MPU60X0
config SENSORS_MAX44009
bool "Maxim MAX44009 ALS sensor"
default n

5
drivers/sensors/Make.defs
View File

@ -182,6 +182,7 @@ endif # CONFIG_I2C
# These drivers depend on SPI support
ifeq ($(CONFIG_SPI),y)
ifeq ($(CONFIG_ADXL345_SPI),y)
CSRCS += adxl345_spi.c
endif
@ -198,6 +199,10 @@ ifeq ($(CONFIG_LIS3DH),y)
CSRCS += lis3dh.c
endif
ifeq ($(CONFIG_SENSORS_MPU60X0),y)
CSRCS += mpu60x0.c
endif
ifeq ($(CONFIG_SENSORS_MAX31855),y)
CSRCS += max31855.c
endif

962
drivers/sensors/mpu60x0.c Normal file
View File

@ -0,0 +1,962 @@
/****************************************************************************
* drivers/sensors/mpu60x0.c
*
* Support for the Invensense MPU6000 and MPU6050 MotionTracking(tm)
* 6-axis accelerometer and gyroscope.
*
* Copyright (C) 2019 Bill Gatliff. All rights reserved.
* Author: Bill Gatliff <bgat@billgatliff.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright+
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*****************************************************************************/
/****************************************************************************
* TODO: Theory of Operation
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <errno.h>
#include <debug.h>
#include <string.h>
#include <limits.h>
#include <nuttx/mutex.h>
#include <nuttx/compiler.h>
#include <nuttx/kmalloc.h>
#include <nuttx/spi/spi.h>
#include <nuttx/fs/fs.h>
#include <nuttx/sensors/mpu60x0.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) - 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
/****************************************************************************
* Private Types
****************************************************************************/
enum mpu_regaddr_e
{
SELF_TEST_X = 0x0d,
SELF_TEST_Y = 0x0e,
SELF_TEST_Z = 0x0f,
SELF_TEST_A = 0x10,
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
*/
CONFIG = 0x1a,
CONFIG__EXT_SYNC_SET__SHIFT = 3,
CONFIG__EXT_SYNC_SET__WIDTH = 2,
CONFIG__DLPF_CFG__SHIFT = 0,
CONFIG__DLPF_CFG__WIDTH = 2,
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,
MOT_THR = 0x1f,
FIFO_EN = 0x23,
I2C_MST_CTRL = 0x24,
I2C_SLV0_ADDR = 0x25,
I2C_SLV0_REG = 0x26,
I2C_SLV0_CTRL = 0x27,
I2C_SLV1_ADDR = 0x28,
I2C_SLV1_REG = 0x29,
I2C_SLV1_CTRL = 0x2a,
I2C_SLV2_ADDR = 0x2b,
I2C_SLV2_REG = 0x2c,
I2C_SLV2_CTRL = 0x2d,
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,
I2C_SLV4_DI = 0x35, /* RO */
I2C_MST_STATUS = 0x36, /* RO */
INT_PIN_CFG = 0x37,
INT_PIN_CFG__INT_LEVEL = 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,
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_RESET = BIT(2),
USER_CTRL__I2C_MST_RESET = BIT(1),
USER_CTRL__SIG_COND_RESET = 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__TEMP_DIS = 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 */
};
/* Describes the mpu60x0 sensor register file. This structure reflects
* the underlying hardware, so don't change it!
*/
begin_packed_struct struct sensor_data_s
{
int16_t x_accel;
int16_t y_accel;
int16_t z_accel;
int16_t temp;
int16_t x_gyro;
int16_t y_gyro;
int16_t z_gyro;
} end_packed_struct;
/* Used by the driver to manage the device */
struct mpu_dev_s
{
mutex_t lock; /* mutex for this structure */
struct mpu_config_s config; /* board-specific information */
struct sensor_data_s buf; /* temporary buffer (for read(), etc.) */
size_t bufpos; /* cursor into @buf, in bytes (!) */
};
/****************************************************************************
* Private Function Function Prototypes
****************************************************************************/
static int mpu_open(FAR struct file *filep);
static int mpu_close(FAR struct file *filep);
static ssize_t mpu_read(FAR struct file *filep, FAR char *buf, size_t len);
static ssize_t mpu_write(FAR struct file *filep, FAR const char *buf,
size_t len);
static off_t mpu_seek(FAR struct file *filep, off_t offset, int whence);
static int mpu_ioctl(FAR struct file *filep, int cmd, unsigned long arg);
/****************************************************************************
* Private Data
****************************************************************************/
static const struct file_operations g_mpu_fops =
{
mpu_open,
mpu_close,
mpu_read,
mpu_write,
mpu_seek,
mpu_ioctl
#ifndef CONFIG_DISABLE_POLL
, NULL
#endif
#ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS
, NULL
#endif
};
/****************************************************************************
* Private Functions
****************************************************************************/
/* NOTE :
*
* In all of the following code, functions named with a double leading
* underscore '__' must be invoked ONLY if the mpu_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 mpu_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_SPI
/* __mpu_read_reg(), but for spi-connected devices. See that function
* for documentation.
*/
static int __mpu_read_reg_spi(FAR struct mpu_dev_s *dev,
enum mpu_regaddr_e reg_addr,
FAR uint8_t *buf, uint8_t len)
{
int ret;
FAR struct spi_dev_s *spi = dev->config.spi;
int id = dev->config.spi_devid;
/* 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;
}
/* __mpu_write_reg(), but for SPI connections. */
static int __mpu_write_reg_spi(FAR struct mpu_dev_s *dev,
enum mpu_regaddr_e reg_addr,
FAR const uint8_t * buf, uint8_t len)
{
int ret;
FAR struct spi_dev_s *spi = dev->config.spi;
int id = dev->config.spi_devid;
/* 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;
}
#endif
#ifdef CONFIG_I2C
/* __mpu_read_reg(), but for i2c-connected devices. */
static int __mpu_read_reg_i2c(FAR struct mpu_dev_s *dev,
enum mpu_regaddr_e reg_addr,
FAR uint8_t *buf, uint8_t len)
{
/* We don't support i2c yet. */
return -EINVAL;
}
static int __mpu_write_reg_i2c(FAR struct mpu_dev_s *dev,
enum mpu_regaddr_e reg_addr,
FAR const uint8_t *buf, uint8_t len)
{
/* We don't support i2c yet. */
return -EINVAL;
}
#endif
/* __mpu_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 __mpu_read_reg(FAR struct mpu_dev_s *dev,
enum mpu_regaddr_e reg_addr,
FAR uint8_t *buf, uint8_t len)
{
#ifdef CONFIG_SPI
/* If we're wired to SPI, use that function. */
if (dev->config.spi != NULL)
{
return __mpu_read_reg_spi(dev, reg_addr, buf, len);
}
#endif
#ifdef CONFIG_I2C
/* If we're wired to I2C, use that function. */
if (dev->config.i2c != NULL)
{
return __mpu_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;
}
/* __mpu_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 __mpu_write_reg(FAR struct mpu_dev_s *dev,
enum mpu_regaddr_e reg_addr,
FAR const uint8_t *buf, uint8_t len)
{
#ifdef CONFIG_SPI
/* If we're connected to SPI, use that function. */
if (dev->config.spi != NULL)
{
return __mpu_write_reg_spi(dev, reg_addr, buf, len);
}
#endif
#ifdef CONFIG_I2C
if (dev->config.i2c != NULL)
{
return __mpu_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;
}
/* __mpu_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 __mpu_read_imu(FAR struct mpu_dev_s *dev,
FAR struct sensor_data_s *buf)
{
return __mpu_read_reg(dev, ACCEL_XOUT_H, (uint8_t *) buf, sizeof(*buf));
}
/* __mpu_read_pwr_mgmt_1()
*
* Returns the value of the PWR_MGMT_1 register from @dev.
*/
static inline uint8_t __mpu_read_pwr_mgmt_1(FAR struct mpu_dev_s *dev)
{
uint8_t buf = 0xff;
__mpu_read_reg(dev, PWR_MGMT_1, &buf, sizeof(buf));
return buf;
}
static inline int __mpu_write_signal_path_reset(FAR struct mpu_dev_s *dev,
uint8_t val)
{
return __mpu_write_reg(dev, SIGNAL_PATH_RESET, &val, sizeof(val));
}
static inline int __mpu_write_int_pin_cfg(FAR struct mpu_dev_s *dev,
uint8_t val)
{
return __mpu_write_reg(dev, INT_PIN_CFG, &val, sizeof(val));
}
static inline int __mpu_write_pwr_mgmt_1(FAR struct mpu_dev_s *dev,
uint8_t val)
{
return __mpu_write_reg(dev, PWR_MGMT_1, &val, sizeof(val));
}
static inline int __mpu_write_pwr_mgmt_2(FAR struct mpu_dev_s *dev,
uint8_t val)
{
return __mpu_write_reg(dev, PWR_MGMT_2, &val, sizeof(val));
}
static inline int __mpu_write_user_ctrl(FAR struct mpu_dev_s *dev,
uint8_t val)
{
return __mpu_write_reg(dev, USER_CTRL, &val, sizeof(val));
}
/* __mpu_write_gyro_config() :
*
* 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 __mpu_write_gyro_config(FAR struct mpu_dev_s *dev,
uint8_t fs_sel)
{
uint8_t val = TO_BITFIELD(GYRO_CONFIG__FS_SEL, fs_sel);
return __mpu_write_reg(dev, GYRO_CONFIG, &val, sizeof(val));
}
/* __mpu_write_accel_config() :
*
* 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 __mpu_write_accel_config(FAR struct mpu_dev_s *dev,
uint8_t afs_sel)
{
uint8_t val = TO_BITFIELD(ACCEL_CONFIG__AFS_SEL, afs_sel);
return __mpu_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
* (see datasheet, it's ... complicated)
*/
static inline int __mpu_write_config(FAR struct mpu_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 __mpu_write_reg(dev, CONFIG, &val, sizeof(val));
}
/* WHO_AM_I (0x75) : read-only, always returns 0x68 for mpu60x0 */
static inline uint8_t __mpu_read_who_am_i(FAR struct mpu_dev_s *dev)
{
uint8_t val = 0xff;
__mpu_read_reg(dev, WHO_AM_I, &val, sizeof(val));
return val;
}
/* Locks and unlocks the @dev data structure (mutex).
*
* Use these functions any time you call one of the lock-dependent
* helper functions defined above.
*/
static void inline mpu_lock(FAR struct mpu_dev_s *dev)
{
nxmutex_lock(&dev->lock);
}
static void inline mpu_unlock(FAR struct mpu_dev_s *dev)
{
nxmutex_unlock(&dev->lock);
}
/* Resets the mpu60x0, sets it to a default configuration. */
static int mpu_reset(FAR struct mpu_dev_s *dev)
{
/* We support only SPI right now. */
if (dev->config.spi == NULL)
{
return -EINVAL;
}
mpu_lock(dev);
/* Awaken chip, issue hardware reset */
__mpu_write_pwr_mgmt_1(dev, PWR_MGMT_1__DEVICE_RESET);
/* Wait for reset cycle to finish (note: per the datasheet, we don't need to
* hold NSS for this)
*/
do
{
up_mdelay(50); /* msecs (arbitrary) */
}
while (__mpu_read_pwr_mgmt_1(dev) & PWR_MGMT_1__DEVICE_RESET);
/* Reset signal paths */
__mpu_write_signal_path_reset(dev, SIGNAL_PATH_RESET__ALL_RESET);
up_mdelay(2);
/* Disable SLEEP, use PLL with z-axis clock source */
__mpu_write_pwr_mgmt_1(dev, 3);
up_mdelay(2);
/* Disable i2c if we're on spi. */
if (dev->config.spi)
{
__mpu_write_user_ctrl(dev, USER_CTRL__I2C_IF_DIS);
}
/* Disable low-power mode, enable all gyros and accelerometers */
__mpu_write_pwr_mgmt_2(dev, 0);
/* No FSYNC, set accel LPF at 184 Hz, gyro LPF at 188 Hz */
__mpu_write_config(dev, 0, 1);
/* ± 1000 deg/sec */
__mpu_write_gyro_config(dev, 2);
/* ± 8g */
__mpu_write_accel_config(dev, 2);
/* clear INT on any read (we aren't using that pin right now) */
__mpu_write_int_pin_cfg(dev, INT_PIN_CFG__INT_RD_CLEAR);
mpu_unlock(dev);
return 0;
}
/****************************************************************************
* Name: mpu_open
*
* Note: we don't deal with multiple users trying to access this interface at
* the same time. Until further notice, don't do that.
*
* And no, it's not as simple as just prohibiting concurrent opens or
* reads with a mutex: there are legit reasons for truy concurrent
* access, but they must be treated carefully in this interface lest a
* partial reader end up with a mixture of old and new samples. This
* will make some users unhappy.
*
****************************************************************************/
static int mpu_open(FAR struct file *filep)
{
FAR struct inode *inode = filep->f_inode;
FAR struct mpu_dev_s *dev = inode->i_private;
/* Reset the register cache */
mpu_lock(dev);
dev->bufpos = 0;
mpu_unlock(dev);
return 0;
}
/****************************************************************************
* Name: mpu_close
****************************************************************************/
static int mpu_close(FAR struct file *filep)
{
FAR struct inode *inode = filep->f_inode;
FAR struct mpu_dev_s *dev = inode->i_private;
/* Reset (clear) the register cache. */
mpu_lock(dev);
dev->bufpos = 0;
mpu_unlock(dev);
return 0;
}
/****************************************************************************
* Name: mpu_read
*
* 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.
*
* In contrast, ASN.1 maps "0" to 0x8000, full-scale-negative to 0,
* and full-scale-positive to 0xffff. So if we want to send in a
* format that an ASN.1 PER-decoder would recognize, must:
*
* 1. Treat the register data/measurements as unsigned,
* 2. Add 0x8000 to each measurement, and then,
* 3. Send each word in big-endian order.
*
* The result of the above will be something you could neatly describe
* like this (confirmed with asn1scc):
*
* Sint16 ::= INTEGER(-32768..32767)
*
* Mpu60x0Sample ::= SEQUENCE
* {
* accel-X Sint16,
* accel-Y Sint16,
* accel-Z Sint16,
* temp Sint16,
* gyro-X Sint16,
* gyro-Y Sint16,
* gyro-Z Sint16
* }
*
****************************************************************************/
static ssize_t mpu_read(FAR struct file *filep, FAR char *buf, size_t len)
{
FAR struct inode *inode = filep->f_inode;
FAR struct mpu_dev_s *dev = inode->i_private;
size_t send_len = 0;
mpu_lock(dev);
/* Populate the register cache if it seems empty. */
if (!dev->bufpos)
{
__mpu_read_imu(dev, &dev->buf);
}
/* Send the lesser of: available bytes, or amount requested. */
send_len = sizeof(dev->buf) - dev->bufpos;
if (send_len > len)
{
send_len = len;
}
if (send_len)
{
memcpy(buf, ((uint8_t *) & dev->buf) + dev->bufpos, send_len);
}
/* Move the cursor, to mark them as sent. */
dev->bufpos += send_len;
/* If we've sent the last byte, reset the buffer. */
if (dev->bufpos >= sizeof(dev->buf))
{
dev->bufpos = 0;
}
mpu_unlock(dev);
return send_len;
}
/****************************************************************************
* Name: mpu_write
****************************************************************************/
static ssize_t mpu_write(FAR struct file *filep, FAR const char *buf,
size_t len)
{
FAR struct inode *inode = filep->f_inode;
FAR struct mpu_dev_s *dev = inode->i_private;
UNUSED(inode);
UNUSED(dev);
snerr("%s: %p %p %d\n", __FUNCTION__, inode, dev, len);
return len;
}
/****************************************************************************
* Name: mpu60x0_seek
****************************************************************************/
static off_t mpu_seek(FAR struct file *filep, off_t offset, int whence)
{
FAR struct inode *inode = filep->f_inode;
FAR struct mpu_dev_s *dev = inode->i_private;
UNUSED(inode);
UNUSED(dev);
snerr("%s: %p %p\n", __FUNCTION__, inode, dev);
return 0;
}
/****************************************************************************
* Name: mpu60x0_ioctl
****************************************************************************/
static int mpu_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
{
FAR struct inode *inode = filep->f_inode;
FAR struct mpu_dev_s *dev = inode->i_private;
UNUSED(inode);
UNUSED(dev);
snerr("%s: %p %p\n", __FUNCTION__, inode, dev);
return -EINVAL;
}
/****************************************************************************
* Name: mpu60x0_register
*
* Description:
* Registers the mpu60x0 interface as 'devpath'
*
* Input Parameters:
* devpath - The full path to the interface to register. E.g., "/dev/imu0"
* spi - SPI interface for chip communications
* config - Configuration information
*
* Returned Value:
* Zero (OK) on success; a negated errno value on failure.
*
****************************************************************************/
int mpu60x0_register(FAR const char *path, FAR struct mpu_config_s *config)
{
FAR struct mpu_dev_s *priv;
int ret;
/* Without config info, we can't do anything. */
if (config == NULL)
{
return -EINVAL;
}
/* Initialize the device structure. */
priv = (FAR struct mpu_dev_s *)kmm_malloc(sizeof(struct mpu_dev_s));
if (priv == NULL)
{
snerr("ERROR: Failed to allocate mpu60x0 device instance\n");
return -ENOMEM;
}
memset(priv, 0, sizeof(*priv));
nxmutex_init(&priv->lock);
/* Keep a copy of the config structure, in case the caller discards theirs. */
priv->config = *config;
/* Register the device node. */
ret = register_driver(path, &g_mpu_fops, 0666, priv);
if (ret < 0)
{
snerr("ERROR: Failed to register mpu60x0 interface: %d\n", ret);
nxmutex_destroy(&priv->lock);
kmm_free(priv);
return ret;
}
/* Reset the chip, to give it an initial configuration. */
return mpu_reset(priv);
}

127
include/nuttx/sensors/mpu60x0.h Normal file
View File

@ -0,0 +1,127 @@
/****************************************************************************
* include/nuttx/sensors/mpu60x0.h
*
* Support for the Invensense MPU6000 and MPU6050 MotionTracking(tm)
* 6-axis accelerometer and gyroscope.
*
* Copyright (C) 2019 Bill Gatliff. All rights reserved.
* Author: Bill Gatliff <bgat@billgatliff.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright+
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*****************************************************************************/
#ifndef __INCLUDE_NUTTX_SENSORS_MPU60X0_H
#define __INCLUDE_NUTTX_SENSORS_MPU60X0_H
/****************************************************************************
* Included Files
*****************************************************************************/
#include <nuttx/config.h>
/****************************************************************************
* Public Types
*****************************************************************************/
/* These structures are defined elsewhere, and we don't need their definitions
* here.
*/
struct spi_dev_s;
struct i2c_master_s;
/* 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 = 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.
*
* Note, I2C support is unimplemented at present.
*/
struct mpu_config_s
{
#ifdef CONFIG_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;
#endif
#ifdef CONFIG_I2C
/* For users on I2C. (Unimplemented.) */
FAR struct i2c_master_s *i2c;
#endif
};
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
/* Declares the existence of an mpu60x0 chip, wired according to
* config; creates an interface to it at path.
*
* Returns 0 on success, or negative errno.
*/
int mpu60x0_register(FAR const char *path, FAR struct mpu_config_s *config);
#endif /* __INCLUDE_NUTTX_SENSORS_MPU60X0_H */