/**************************************************************************** * 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 #include #include #include #include #include #include #include #include #include #include #include #ifdef CONFIG_MPU9250_SPI #include #else #include #endif #include #include /**************************************************************************** * 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; }