/**************************************************************************** * drivers/sensors/mpu60x0.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. * ****************************************************************************/ /**************************************************************************** * TODO: Theory of Operation ****************************************************************************/ /**************************************************************************** * Included Files ****************************************************************************/ #include #include #include #include #include #include #include #include #include #include #ifdef CONFIG_MPU60X0_SPI #include #else #include #endif #include #include #include /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ /* 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 /**************************************************************************** * 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 = 3, CONFIG__DLPF_CFG__SHIFT = 0, CONFIG__DLPF_CFG__WIDTH = 3, GYRO_CONFIG = 0x1b, 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, FIFO_EN__TEMP = BIT(7), FIFO_EN__XG = BIT(6), FIFO_EN__YG = BIT(5), FIFO_EN__ZG = BIT(4), FIFO_EN__ACCEL = BIT(3), 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 (!) */ uint8_t smplrt_div; /* divider to control sample rate */ uint8_t afs_sel; /* full scale range of the accelerometer */ uint8_t dlpf_config; /* digital low pass filter configuration */ bool fifo_enabled; /* current enable state of FIFO buffer */ float sample_rate; /* current sample rate */ }; /**************************************************************************** * 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, /* open */ mpu_close, /* close */ mpu_read, /* read */ mpu_write, /* write */ mpu_seek, /* seek */ mpu_ioctl, /* ioctl */ }; /**************************************************************************** * 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, uint8_t 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 = ®_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, uint8_t 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 = ®_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) { if (dev->fifo_enabled) { return __mpu_read_reg(dev, FIFO_R_W, (FAR uint8_t *)buf, sizeof(*buf)); } return __mpu_read_reg(dev, ACCEL_XOUT_H, (FAR 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)); } #ifdef CONFIG_MPU60X0_SPI 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)); } #endif /* __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; if (afs_sel > 3) { snerr("ERROR: Invalid AFS_SEL value\n"); return -EINVAL; } 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)); } /* Sets the SMPLRT_DIV that controls the sample rate. */ static inline int __mpu_set_sample_rate_divider(FAR struct mpu_dev_s *dev, uint8_t val) { return __mpu_write_reg(dev, SMPLRT_DIV, &val, sizeof(val)); } /* Reads current sample rate. Value is updated to mpu_dev_s->sample_rate. */ static inline int __mpu_read_sample_rate(FAR struct mpu_dev_s *dev) { int ret; float gyro_output_rate = 1000.0f; ret = __mpu_read_reg(dev, SMPLRT_DIV, &dev->smplrt_div, sizeof(dev->smplrt_div)); if (ret < 0) { return ret; } ret = __mpu_read_reg(dev, CONFIG, &dev->dlpf_config, sizeof(dev->dlpf_config)); if (ret < 0) { return ret; } dev->dlpf_config = TO_BITFIELD(CONFIG__DLPF_CFG, dev->dlpf_config); /* This condition verifies if DLPF is disabled */ if ((dev->dlpf_config == 0) || (dev->dlpf_config == 7)) { gyro_output_rate = 8000.0f; } dev->sample_rate = gyro_output_rate / (float)(1 + dev->smplrt_div); return OK; } /* Read the number of bytes currently in FIFO buffer. */ static inline int __mpu_read_fifo_count(FAR struct mpu_dev_s *dev, uint16_t *buf) { int ret; uint8_t fifo_counter[2]; ret = __mpu_read_reg(dev, FIFO_COUNTH, fifo_counter, sizeof(fifo_counter)); if (ret < 0) { snerr("ERROR: Failed to read FIFO counter\n"); *buf = 0; } else { *buf = (fifo_counter[0] << 8) | fifo_counter[1]; } return ret; } /* Enables or disables FIFO loading a specific sensor. * It may receive a OR combination of multiple sensors. * Example: * __mpu_set_fifo(priv, FIFO_EN__TEMP | FIFO_EN__YG | FIFO_EN__ACCEL); */ static inline int __mpu_set_fifo(FAR struct mpu_dev_s *dev, uint8_t val) { return __mpu_write_reg(dev, FIFO_EN, &val, sizeof(val)); } /* Sets USER CONTROL register. It may receive an OR combination of multiple * bitfields. * Example: * __mpu_user_control(priv, USER_CTRL__FIFO_EN | USER_CTRL__I2C_MST_RESET); */ static inline int __mpu_user_control(FAR struct mpu_dev_s *dev, uint8_t val) { return __mpu_write_reg(dev, USER_CTRL, &val, sizeof(val)); } /* Resets the mpu60x0, sets it to a default configuration. */ static int mpu_reset(FAR struct mpu_dev_s *dev) { int ret; #ifdef CONFIG_MPU60X0_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 = __mpu_write_pwr_mgmt_1(dev, PWR_MGMT_1__DEVICE_RESET); if (ret < 0) { nxmutex_unlock(&dev->lock); snerr("Could not find mpu60x0!\n"); return ret; } /* 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); /* default No FSYNC, set accel LPF at 184 Hz, gyro LPF at 188 Hz in * menuconfig */ __mpu_write_config(dev, CONFIG_MPU60X0_EXT_SYNC_SET, CONFIG_MPU60X0_DLPF_CFG); dev->dlpf_config = CONFIG_MPU60X0_DLPF_CFG; /* default ± 1000 deg/sec in menuconfig */ __mpu_write_gyro_config(dev, CONFIG_MPU60X0_GYRO_FS_SEL); /* default ± 8g in menuconfig */ __mpu_write_accel_config(dev, CONFIG_MPU60X0_ACCEL_AFS_SEL); dev->afs_sel = CONFIG_MPU60X0_ACCEL_AFS_SEL; /* 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); /* Disable use of FIFO buffer */ __mpu_set_fifo(dev, 0); dev->fifo_enabled = false; nxmutex_unlock(&dev->lock); 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 */ nxmutex_lock(&dev->lock); dev->bufpos = 0; nxmutex_unlock(&dev->lock); 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. */ nxmutex_lock(&dev->lock); dev->bufpos = 0; nxmutex_unlock(&dev->lock); 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; nxmutex_lock(&dev->lock); /* 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, ((FAR 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; } nxmutex_unlock(&dev->lock); 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 %zu\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 *priv = inode->i_private; uint8_t write_data = (uint8_t)arg; int ret = OK; switch (cmd) { /* Sets the accelerometer full scale range. Arg: uin8_t value */ case SNIOC_SET_AFS_SEL: ret = __mpu_write_accel_config(priv, write_data); if (ret < 0) { snerr("ERROR: SNIOC_SET_AFS_SEL fails. Returns: %d\n", ret); } else { priv->afs_sel = write_data; sninfo("SNIOC_SET_AFS_SEL: %d Returns: %d\n", priv->afs_sel, ret); } break; /* Sets the sample rate divider. Arg: uin8_t value */ case SNIOC_SMPLRT_DIV: ret = __mpu_set_sample_rate_divider(priv, write_data); priv->smplrt_div = write_data; sninfo("SNIOC_SMPLRT_DIV: %d Returns: %d\n", priv->smplrt_div, ret); break; /* Read current sample rate. Arg: uin32_t* pointer */ case SNIOC_READ_SAMPLE_RATE: { FAR uint32_t *ptr = (FAR uint32_t *)((uintptr_t)arg); ret = __mpu_read_sample_rate(priv); sninfo("SNIOC_READ_SAMPLE_RATE: Returns: %d. Read: %f\n", ret, priv->sample_rate); *ptr = (uint32_t)priv->sample_rate; break; } /* Read current number of bytes in FIFO buffer. Arg: uin16_t* */ case SNIOC_READ_FIFO_COUNT: { FAR uint16_t *ptr = (FAR uint16_t *)((uintptr_t)arg); uint16_t fifo_count = 0; ret = __mpu_read_fifo_count(priv, &fifo_count); *ptr = fifo_count; sninfo("SNIOC_READ_FIFO_COUNT: Returns: %d. Read: 0x%x\n", ret, fifo_count); break; } /* Enable or disable the use of FIFO buffer. Arg: bool* */ case SNIOC_ENABLE_FIFO: if (!write_data) { ret = __mpu_set_fifo(priv, 0); if (ret < 0) { sninfo("SNIOC_ENABLE_FIFO failed. Returns: %d\n", ret); } ret = __mpu_user_control(priv, 0); priv->fifo_enabled = false; } else { ret = __mpu_user_control(priv, USER_CTRL__FIFO_EN); if (ret < 0) { sninfo("SNIOC_ENABLE_FIFO failed. Returns: %d\n", ret); } /* This configuration enables temperature, accelerometer and * gyro on all three axis. Each read requires 14 bytes, allowing * the FIFO to store 1024/14 = 73 samples. */ ret = __mpu_set_fifo(priv, FIFO_EN__TEMP | FIFO_EN__XG | FIFO_EN__YG | FIFO_EN__ZG | FIFO_EN__ACCEL); priv->fifo_enabled = true; } sninfo("SNIOC_ENABLE_FIFO: %d Returns: %d\n", write_data, ret); break; default: sninfo("Unrecognized IOCTL command: 0x%04x\n", cmd); ret = -ENOTTY; break; } return ret; } /**************************************************************************** * 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 = 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; /* Reset the chip, to give it an initial configuration. */ ret = mpu_reset(priv); if (ret < 0) { snerr("ERROR: Failed to configure mpu60x0: %d\n", ret); nxmutex_destroy(&priv->lock); kmm_free(priv); return ret; } /* 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; } return OK; }