sergiotarxz/nuttx
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
2cda16b606
nuttx/drivers/sensors/mpu60x0.c
Brennan Ashton 54832f37f2 sim: Initial Linux i2c bus support 
This adds the inital wiring for i2c bus support in the sim target
and for Linux host adds the lower half that uses the i2c chardev.

Signed-off-by: Brennan Ashton <bashton@brennanashton.com>
2020-11-04 23:51:09 -08:00

1013 lines
28 KiB
C
Raw Blame History

/****************************************************************************
* 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>
#ifdef CONFIG_MPU60X0_SPI
#include <nuttx/spi/spi.h>
#else
#include <nuttx/i2c/i2c_master.h>
#endif
#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,
NULL
#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_MPU60X0_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;
}
#else
/* __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)
{
int ret;
struct i2c_msg_s msg[2];
msg[0].frequency = CONFIG_MPU60X0_I2C_FREQ;
msg[0].addr = dev->config.addr;
msg[0].flags = I2C_M_NOSTOP;
msg[0].buffer = &reg_addr;
msg[0].length = 1;
msg[1].frequency = CONFIG_MPU60X0_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;
}
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)
{
int ret;
struct i2c_msg_s msg[2];
msg[0].frequency = CONFIG_MPU60X0_I2C_FREQ;
msg[0].addr = dev->config.addr;
msg[0].flags = I2C_M_NOSTOP;
msg[0].buffer = &reg_addr;
msg[0].length = 1;
msg[1].frequency = CONFIG_MPU60X0_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_MPU60X0_SPI */
/* __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_MPU60X0_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);
}
#else
/* 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_MPU60X0_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);
}
#else
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)
{
#ifdef CONFIG_MPU60X0_SPI
if (dev->config.spi == NULL)
{
return -EINVAL;
}
#else
if (dev->config.i2c == NULL)
{
return -EINVAL;
}
#endif
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
{
nxsig_usleep(50000); /* usecs (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);
nxsig_usleep(2000);
/* Disable SLEEP, use PLL with z-axis clock source */
__mpu_write_pwr_mgmt_1(dev, 3);
nxsig_usleep(2000);
/* Disable i2c if we're on spi. */
#ifdef CONFIG_MPU60X0_SPI
if (dev->config.spi)
{
__mpu_write_user_ctrl(dev, USER_CTRL__I2C_IF_DIS);
}
#endif
/* 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("ERROR: %p %p %d\n", 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("ERROR: %p %p\n", 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("ERROR: %p %p\n", inode, dev);
/* ENOTTY is the standard return if an IOCTL command is not supported. */
return -ENOTTY;
}
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* 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);
}
Reference in New Issue View Git Blame Copy Permalink